[med-svn] [opensurgsim] 01/02: Imported Upstream version 0.6.0
Paul Novotny
paulnovo-guest at moszumanska.debian.org
Thu Jan 22 22:41:04 UTC 2015
This is an automated email from the git hooks/post-receive script.
paulnovo-guest pushed a commit to branch master
in repository opensurgsim.
commit 9cc9611dec16001ce75a58750a6be038e356eb81
Author: Paul Novotny <paul at paulnovo.us>
Date: Thu Jan 15 15:23:02 2015 -0500
Imported Upstream version 0.6.0
---
.gitattributes | 123 +
.gitignore | 94 +
CMake/DashboardTemplate.cmake | 131 +
CMake/External_yamlcpp.cmake | 47 +
CMake/FindEigen3.cmake | 58 +
CMake/FindGlutFromOsg.cmake | 49 +
CMake/FindGoogleMock.cmake | 34 +
CMake/FindLabJack.cmake | 45 +
CMake/FindNovintHdalSdk.cmake | 116 +
CMake/FindOpenHaptics.cmake | 80 +
CMake/FindOptiTrack.cmake | 115 +
CMake/FindSixenseSdk.cmake | 190 +
CMake/FindWDK.cmake | 115 +
CMake/OpenSurgSimConfig.cmake.in | 18 +
CMake/OpenSurgSimConfigVersion.cmake.in | 11 +
CMake/SurgSimBuildFlags.cmake | 145 +
CMake/SurgSimUtilities.cmake | 269 +
CMake/tryCompileWdk/CMakeLists.txt | 23 +
CMake/tryCompileWdk/main.cpp | 33 +
CMakeLists.txt | 168 +
CTestConfig.cmake | 22 +
CTestCustom.cmake.in | 52 +
Data/Shaders/README.txt | 10 +
Data/Shaders/basic_lit.frag | 25 +
Data/Shaders/basic_lit.vert | 40 +
Data/Shaders/basic_unlit.frag | 25 +
Data/Shaders/basic_unlit.vert | 26 +
Data/Shaders/depth_map.frag | 31 +
Data/Shaders/depth_map.vert | 23 +
Data/Shaders/ds_mapping_material.frag | 61 +
Data/Shaders/ds_mapping_material.vert | 74 +
Data/Shaders/material.frag | 52 +
Data/Shaders/material.vert | 74 +
Data/Shaders/osg_statshandler.frag | 29 +
Data/Shaders/osg_statshandler.vert | 33 +
Data/Shaders/s_mapping.frag | 35 +
Data/Shaders/s_mapping.vert | 58 +
Data/Shaders/shadow_map.frag | 45 +
Data/Shaders/shadow_map.vert | 39 +
Data/Shaders/unlit_texture.frag | 27 +
Data/Shaders/unlit_texture.vert | 29 +
Data/Shaders/unlit_texture_rectangle.frag | 10 +
Data/Textures/black.png | Bin 0 -> 140 bytes
Data/Textures/checkered.png | Bin 0 -> 1176 bytes
Data/Textures/white.png | Bin 0 -> 153 bytes
Documentation/CMakeLists.txt | 103 +
Documentation/Doxyfile.in | 1880 +++++
Documentation/FileFormatConversion.dox | 17 +
Documentation/OpenSurgSim-dox.png | Bin 0 -> 9859 bytes
Documentation/footer.html | 21 +
Documentation/header.html | 63 +
Documentation/mainPage.dox | 50 +
Examples/AddSphereFromInput/AddSphereBehavior.cpp | 81 +
Examples/AddSphereFromInput/AddSphereBehavior.h | 57 +
Examples/AddSphereFromInput/AddSphereFromInput.cpp | 180 +
Examples/AddSphereFromInput/CMakeLists.txt | 52 +
Examples/CMakeLists.txt | 27 +
Examples/DroppingBalls/AddRandomSphereBehavior.cpp | 96 +
Examples/DroppingBalls/AddRandomSphereBehavior.h | 68 +
Examples/DroppingBalls/CMakeLists.txt | 57 +
Examples/DroppingBalls/Data/Earth.png | Bin 0 -> 768083 bytes
Examples/DroppingBalls/DroppingBalls.cpp | 293 +
Examples/DroppingBalls/config.txt.in | 1 +
Examples/GraphicsScene/CMakeLists.txt | 56 +
.../GraphicsScene/Data/Textures/CheckerBoard.png | Bin 0 -> 4009 bytes
Examples/GraphicsScene/GraphicsScene.cpp | 463 ++
Examples/GraphicsScene/config.txt.in | 2 +
Examples/InputVtc/CMakeLists.txt | 58 +
Examples/InputVtc/DeviceFactory.cpp | 73 +
Examples/InputVtc/DeviceFactory.h | 45 +
Examples/InputVtc/InputVtc.cpp | 255 +
Examples/Stapling/CMakeLists.txt | 67 +
Examples/Stapling/Data/Geometry/arm_collision.ply | 2125 +++++
Examples/Stapling/Data/Geometry/forearm.osgb | Bin 0 -> 179237 bytes
Examples/Stapling/Data/Geometry/forearm.png | Bin 0 -> 387104 bytes
Examples/Stapling/Data/Geometry/forearm_normal.png | Bin 0 -> 210860 bytes
Examples/Stapling/Data/Geometry/staple.mtl | 10 +
Examples/Stapling/Data/Geometry/staple.obj | 236 +
.../Stapling/Data/Geometry/staple_collision.ply | 741 ++
.../Stapling/Data/Geometry/stapler_collision.ply | 679 ++
Examples/Stapling/Data/Geometry/stapler_handle.mtl | 11 +
Examples/Stapling/Data/Geometry/stapler_handle.obj | 544 ++
.../Stapling/Data/Geometry/stapler_indicator.mtl | 11 +
.../Stapling/Data/Geometry/stapler_indicator.obj | 34 +
.../Stapling/Data/Geometry/stapler_markings.mtl | 11 +
.../Stapling/Data/Geometry/stapler_markings.obj | 8114 ++++++++++++++++++++
.../Stapling/Data/Geometry/stapler_trigger.mtl | 11 +
.../Stapling/Data/Geometry/stapler_trigger.obj | 357 +
Examples/Stapling/Data/Geometry/upperarm.osgb | Bin 0 -> 30306 bytes
Examples/Stapling/Data/Geometry/upperarm.png | Bin 0 -> 2879974 bytes
.../Stapling/Data/Geometry/upperarm_normal.png | Bin 0 -> 1044033 bytes
.../Stapling/Data/Geometry/virtual_staple_1.ply | 28 +
.../Stapling/Data/Geometry/virtual_staple_2.ply | 28 +
Examples/Stapling/Data/Geometry/wound.png | Bin 0 -> 1761647 bytes
.../Stapling/Data/Geometry/wound_deformable.ply | 2391 ++++++
Examples/Stapling/Data/StaplingDemo.yaml | 463 ++
Examples/Stapling/SerializedStapling.cpp | 121 +
Examples/Stapling/StapleElement.cpp | 70 +
Examples/Stapling/StapleElement.h | 44 +
Examples/Stapling/StaplerBehavior.cpp | 394 +
Examples/Stapling/StaplerBehavior.h | 181 +
Examples/Stapling/Stapling.cpp | 486 ++
Examples/Stapling/config.txt.in | 2 +
LICENSE | 202 +
NOTICE | 48 +
README | 143 +
SurgSim/Blocks/CMakeLists.txt | 70 +
SurgSim/Blocks/DriveElementFromInputBehavior.cpp | 102 +
SurgSim/Blocks/DriveElementFromInputBehavior.h | 87 +
.../Blocks/KeyboardTogglesComponentBehavior.cpp | 109 +
SurgSim/Blocks/KeyboardTogglesComponentBehavior.h | 107 +
SurgSim/Blocks/MassSpring1DRepresentation.cpp | 142 +
SurgSim/Blocks/MassSpring1DRepresentation.h | 64 +
SurgSim/Blocks/MassSpring2DRepresentation.cpp | 176 +
SurgSim/Blocks/MassSpring2DRepresentation.h | 89 +
SurgSim/Blocks/MassSpring3DRepresentation.cpp | 302 +
SurgSim/Blocks/MassSpring3DRepresentation.h | 100 +
SurgSim/Blocks/MassSpringNDRepresentationUtils.cpp | 47 +
SurgSim/Blocks/MassSpringNDRepresentationUtils.h | 51 +
SurgSim/Blocks/PoseInterpolator.cpp | 149 +
SurgSim/Blocks/PoseInterpolator.h | 125 +
SurgSim/Blocks/SphereElement.cpp | 93 +
SurgSim/Blocks/SphereElement.h | 54 +
.../TransferPhysicsToGraphicsMeshBehavior.cpp | 107 +
.../Blocks/TransferPhysicsToGraphicsMeshBehavior.h | 86 +
.../Blocks/TransferPhysicsToPointCloudBehavior.cpp | 108 +
.../Blocks/TransferPhysicsToPointCloudBehavior.h | 86 +
SurgSim/Blocks/UnitTests/CMakeLists.txt | 53 +
.../UnitTests/Data/Geometry/wound_deformable.ply | 2389 ++++++
.../KeyboardTogglesComponentBehaviorTests.cpp | 178 +
.../UnitTests/MassSpring1DRepresentationTests.cpp | 110 +
.../UnitTests/MassSpring2DRepresentationTests.cpp | 185 +
.../UnitTests/MassSpring3DRepresentationTests.cpp | 334 +
.../MassSpringNDRepresentationUtilsTests.cpp | 79 +
SurgSim/Blocks/UnitTests/PoseInterpolatorTests.cpp | 178 +
SurgSim/Blocks/UnitTests/SpringTestUtils.cpp | 49 +
SurgSim/Blocks/UnitTests/SpringTestUtils.h | 49 +
.../TransferPhysicsToGraphicsMeshBehaviorTests.cpp | 152 +
.../TransferPhysicsToPointCloudBehaviorTests.cpp | 151 +
.../UnitTests/VisualizeContactsBehaviorTests.cpp | 81 +
SurgSim/Blocks/UnitTests/config.txt.in | 1 +
SurgSim/Blocks/VisualizeContactsBehavior.cpp | 141 +
SurgSim/Blocks/VisualizeContactsBehavior.h | 99 +
SurgSim/CMakeLists.txt | 28 +
SurgSim/Collision/BoxCapsuleDcdContact.cpp | 194 +
SurgSim/Collision/BoxCapsuleDcdContact.h | 53 +
.../Collision/BoxDoubleSidedPlaneDcdContact.cpp | 166 +
SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.h | 52 +
SurgSim/Collision/BoxPlaneDcdContact.cpp | 88 +
SurgSim/Collision/BoxPlaneDcdContact.h | 53 +
SurgSim/Collision/BoxSphereDcdContact.cpp | 131 +
SurgSim/Collision/BoxSphereDcdContact.h | 55 +
SurgSim/Collision/CMakeLists.txt | 76 +
SurgSim/Collision/CapsuleSphereDcdContact.cpp | 78 +
SurgSim/Collision/CapsuleSphereDcdContact.h | 52 +
SurgSim/Collision/CollisionPair.cpp | 132 +
SurgSim/Collision/CollisionPair.h | 141 +
SurgSim/Collision/ContactCalculation.cpp | 63 +
SurgSim/Collision/ContactCalculation.h | 64 +
SurgSim/Collision/DcdCollision.h | 33 +
SurgSim/Collision/DefaultContactCalculation.cpp | 52 +
SurgSim/Collision/DefaultContactCalculation.h | 59 +
SurgSim/Collision/OctreeDcdContact.cpp | 109 +
SurgSim/Collision/OctreeDcdContact.h | 77 +
SurgSim/Collision/Representation.cpp | 44 +
SurgSim/Collision/Representation.h | 90 +
SurgSim/Collision/ShapeCollisionRepresentation.cpp | 87 +
SurgSim/Collision/ShapeCollisionRepresentation.h | 67 +
.../Collision/SphereDoubleSidedPlaneDcdContact.cpp | 85 +
.../Collision/SphereDoubleSidedPlaneDcdContact.h | 52 +
SurgSim/Collision/SpherePlaneDcdContact.cpp | 82 +
SurgSim/Collision/SpherePlaneDcdContact.h | 52 +
SurgSim/Collision/SphereSphereDcdContact.cpp | 66 +
SurgSim/Collision/SphereSphereDcdContact.h | 50 +
SurgSim/Collision/TriangleMeshPlaneDcdContact.cpp | 91 +
SurgSim/Collision/TriangleMeshPlaneDcdContact.h | 52 +
.../TriangleMeshTriangleMeshDcdContact.cpp | 229 +
.../Collision/TriangleMeshTriangleMeshDcdContact.h | 50 +
.../BoxCapsuleContactCalculationTests.cpp | 318 +
.../BoxDoubleSidedPlaneContactCalculationTests.cpp | 232 +
.../UnitTests/BoxPlaneContactCalculationTests.cpp | 215 +
.../UnitTests/BoxSphereContactCalculationTests.cpp | 328 +
SurgSim/Collision/UnitTests/CMakeLists.txt | 65 +
.../CapsuleSphereContactCalculationTests.cpp | 102 +
SurgSim/Collision/UnitTests/CollisionPairTests.cpp | 159 +
.../UnitTests/ContactCalculationTests.cpp | 75 +
.../UnitTests/ContactCalculationTestsCommon.cpp | 205 +
.../UnitTests/ContactCalculationTestsCommon.h | 151 +
.../UnitTests/DefaultContactCalculationTests.cpp | 49 +
.../UnitTests/OctreeContactCalculationTests.cpp | 427 +
SurgSim/Collision/UnitTests/RepresentationTest.cpp | 151 +
.../UnitTests/RepresentationUtilities.cpp | 87 +
.../Collision/UnitTests/RepresentationUtilities.h | 55 +
.../UnitTests/ShapeCollisionRepresentationTest.cpp | 109 +
...hereDoubleSidedPlaneContactCalculationTests.cpp | 168 +
.../SpherePlaneContactCalculationTests.cpp | 121 +
.../SphereSphereContactCalculationTests.cpp | 100 +
.../TriangleMeshPlaneContactCalculationTests.cpp | 416 +
...ngleMeshTriangleMeshContactCalculationTests.cpp | 489 ++
SurgSim/Collision/UnitTests/config.txt.in | 1 +
SurgSim/DataStructures/AabbTree.cpp | 117 +
SurgSim/DataStructures/AabbTree.h | 90 +
SurgSim/DataStructures/AabbTreeData.cpp | 133 +
SurgSim/DataStructures/AabbTreeData.h | 97 +
.../DataStructures/AabbTreeIntersectionVisitor.cpp | 97 +
.../DataStructures/AabbTreeIntersectionVisitor.h | 76 +
SurgSim/DataStructures/AabbTreeNode.cpp | 115 +
SurgSim/DataStructures/AabbTreeNode.h | 77 +
SurgSim/DataStructures/BufferedValue-inl.h | 65 +
SurgSim/DataStructures/BufferedValue.h | 78 +
SurgSim/DataStructures/CMakeLists.txt | 108 +
SurgSim/DataStructures/DataGroup.cpp | 173 +
SurgSim/DataStructures/DataGroup.h | 229 +
SurgSim/DataStructures/DataGroupBuilder.cpp | 194 +
SurgSim/DataStructures/DataGroupBuilder.h | 206 +
SurgSim/DataStructures/DataGroupCopier.cpp | 76 +
SurgSim/DataStructures/DataGroupCopier.h | 73 +
SurgSim/DataStructures/DataStructuresConvert-inl.h | 187 +
SurgSim/DataStructures/DataStructuresConvert.h | 71 +
SurgSim/DataStructures/EmptyData.h | 39 +
SurgSim/DataStructures/Image-inl.h | 150 +
SurgSim/DataStructures/Image.h | 120 +
SurgSim/DataStructures/IndexDirectory.cpp | 67 +
SurgSim/DataStructures/IndexDirectory.h | 140 +
SurgSim/DataStructures/IndexedLocalCoordinate.cpp | 35 +
SurgSim/DataStructures/IndexedLocalCoordinate.h | 51 +
SurgSim/DataStructures/Location.h | 68 +
SurgSim/DataStructures/MeshElement.h | 98 +
SurgSim/DataStructures/NamedData-inl.h | 346 +
SurgSim/DataStructures/NamedData.h | 305 +
SurgSim/DataStructures/NamedDataBuilder.h | 163 +
SurgSim/DataStructures/NamedVariantData-inl.h | 96 +
SurgSim/DataStructures/NamedVariantData.h | 101 +
SurgSim/DataStructures/OctreeNode-inl.h | 266 +
SurgSim/DataStructures/OctreeNode.cpp | 275 +
SurgSim/DataStructures/OctreeNode.h | 256 +
SurgSim/DataStructures/OptionalValue.h | 173 +
.../DataStructures/PerformanceTests/CMakeLists.txt | 34 +
.../PerformanceTests/NamedDataPerformanceTest.cpp | 124 +
SurgSim/DataStructures/PlyReader.cpp | 365 +
SurgSim/DataStructures/PlyReader.h | 269 +
SurgSim/DataStructures/PlyReaderDelegate.h | 55 +
SurgSim/DataStructures/TetrahedronMesh-inl.h | 292 +
SurgSim/DataStructures/TetrahedronMesh.h | 207 +
SurgSim/DataStructures/Tree.cpp | 55 +
SurgSim/DataStructures/Tree.h | 77 +
SurgSim/DataStructures/TreeData.cpp | 37 +
SurgSim/DataStructures/TreeData.h | 65 +
SurgSim/DataStructures/TreeNode.cpp | 112 +
SurgSim/DataStructures/TreeNode.h | 124 +
SurgSim/DataStructures/TreeVisitor.h | 59 +
SurgSim/DataStructures/TriangleMesh-inl.h | 35 +
SurgSim/DataStructures/TriangleMesh.cpp | 102 +
SurgSim/DataStructures/TriangleMesh.h | 107 +
SurgSim/DataStructures/TriangleMeshBase-inl.h | 263 +
SurgSim/DataStructures/TriangleMeshBase.h | 211 +
.../TriangleMeshPlyReaderDelegate-inl.h | 159 +
.../DataStructures/TriangleMeshPlyReaderDelegate.h | 135 +
SurgSim/DataStructures/TriangleMeshUtilities-inl.h | 35 +
SurgSim/DataStructures/TriangleMeshUtilities.cpp | 30 +
SurgSim/DataStructures/TriangleMeshUtilities.h | 43 +
.../DataStructures/UnitTests/AabbTreeDataTests.cpp | 181 +
.../DataStructures/UnitTests/AabbTreeNodeTests.cpp | 102 +
SurgSim/DataStructures/UnitTests/AabbTreeTests.cpp | 283 +
.../UnitTests/BufferedValueTests.cpp | 62 +
SurgSim/DataStructures/UnitTests/CMakeLists.txt | 67 +
.../UnitTests/Data/PlyReaderTests/Cube.ply | 51 +
.../UnitTests/Data/PlyReaderTests/Testdata.ply | 23 +
.../DataStructures/UnitTests/DataGroupTests.cpp | 464 ++
.../UnitTests/DataStructuresConvertTests.cpp | 257 +
SurgSim/DataStructures/UnitTests/ImageTest.cpp | 212 +
.../UnitTests/IndexDirectoryTests.cpp | 107 +
.../UnitTests/IndexedLocalCoordinateTest.cpp | 65 +
SurgSim/DataStructures/UnitTests/LocationTests.cpp | 70 +
.../DataStructures/UnitTests/MeshElementTest.cpp | 145 +
SurgSim/DataStructures/UnitTests/MeshTest.cpp | 274 +
.../DataStructures/UnitTests/MeshVertexTest.cpp | 65 +
SurgSim/DataStructures/UnitTests/MockObjects.h | 524 ++
.../DataStructures/UnitTests/NamedDataTests.cpp | 405 +
.../UnitTests/NamedVariantDataTests.cpp | 176 +
.../DataStructures/UnitTests/OctreeNodeTests.cpp | 622 ++
.../UnitTests/OptionalValueTests.cpp | 189 +
.../DataStructures/UnitTests/PlyReaderTests.cpp | 278 +
.../UnitTests/TetrahedronMeshTest.cpp | 663 ++
.../UnitTests/TriangleMeshBaseTest.cpp | 655 ++
.../DataStructures/UnitTests/TriangleMeshTest.cpp | 141 +
SurgSim/DataStructures/UnitTests/config.txt.in | 2 +
SurgSim/DataStructures/Vertex.h | 112 +
SurgSim/DataStructures/Vertices.h | 205 +
SurgSim/DataStructures/ply.c | 2589 +++++++
SurgSim/DataStructures/ply.h | 183 +
SurgSim/Devices/CMakeLists.txt | 44 +
SurgSim/Devices/DeviceFilters/CMakeLists.txt | 49 +
SurgSim/Devices/DeviceFilters/ForceScale.cpp | 141 +
SurgSim/Devices/DeviceFilters/ForceScale.h | 122 +
SurgSim/Devices/DeviceFilters/PoseIntegrator.cpp | 178 +
SurgSim/Devices/DeviceFilters/PoseIntegrator.h | 113 +
SurgSim/Devices/DeviceFilters/PoseTransform.cpp | 279 +
SurgSim/Devices/DeviceFilters/PoseTransform.h | 161 +
.../Devices/DeviceFilters/UnitTests/CMakeLists.txt | 33 +
.../DeviceFilters/UnitTests/ForceScaleTest.cpp | 240 +
.../DeviceFilters/UnitTests/PoseIntegratorTest.cpp | 287 +
.../DeviceFilters/UnitTests/PoseTransformTest.cpp | 290 +
SurgSim/Devices/IdentityPoseDevice/CMakeLists.txt | 49 +
.../IdentityPoseDevice/IdentityPoseDevice.cpp | 84 +
.../IdentityPoseDevice/IdentityPoseDevice.h | 60 +
.../IdentityPoseDevice/UnitTests/CMakeLists.txt | 31 +
.../UnitTests/IdentityPoseDeviceTest.cpp | 129 +
SurgSim/Devices/Keyboard/CMakeLists.txt | 55 +
SurgSim/Devices/Keyboard/KeyCode.h | 249 +
SurgSim/Devices/Keyboard/KeyboardDevice.cpp | 73 +
SurgSim/Devices/Keyboard/KeyboardDevice.h | 79 +
SurgSim/Devices/Keyboard/KeyboardScaffold.cpp | 138 +
SurgSim/Devices/Keyboard/KeyboardScaffold.h | 105 +
SurgSim/Devices/Keyboard/OsgKeyboardHandler.cpp | 60 +
SurgSim/Devices/Keyboard/OsgKeyboardHandler.h | 50 +
SurgSim/Devices/Keyboard/UnitTests/CMakeLists.txt | 36 +
.../Keyboard/UnitTests/KeyboardDeviceTest.cpp | 83 +
.../Keyboard/UnitTests/KeyboardScaffoldTest.cpp | 55 +
.../Devices/Keyboard/VisualTests/CMakeLists.txt | 35 +
.../Keyboard/VisualTests/KeyboardVisualTests.cpp | 325 +
SurgSim/Devices/LabJack/CMakeLists.txt | 85 +
SurgSim/Devices/LabJack/LabJack.dox | 20 +
SurgSim/Devices/LabJack/LabJackDevice.cpp | 306 +
SurgSim/Devices/LabJack/LabJackDevice.h | 523 ++
SurgSim/Devices/LabJack/LabJackScaffold.h | 150 +
SurgSim/Devices/LabJack/LabJackThread.cpp | 49 +
SurgSim/Devices/LabJack/LabJackThread.h | 51 +
SurgSim/Devices/LabJack/UnitTests/CMakeLists.txt | 45 +
.../LabJack/UnitTests/LabJackChecksumsTest.cpp | 95 +
.../LabJack/UnitTests/LabJackDeviceTest.cpp | 359 +
.../LabJack/UnitTests/LabJackScaffoldTest.cpp | 185 +
.../UnitTests/LabJackTypeConvertersTest.cpp | 79 +
SurgSim/Devices/LabJack/VisualTest/CMakeLists.txt | 50 +
SurgSim/Devices/LabJack/VisualTest/main.cpp | 312 +
SurgSim/Devices/LabJack/linux/LabJackChecksums.cpp | 77 +
SurgSim/Devices/LabJack/linux/LabJackChecksums.h | 70 +
SurgSim/Devices/LabJack/linux/LabJackConstants.h | 35 +
SurgSim/Devices/LabJack/linux/LabJackScaffold.cpp | 1514 ++++
.../LabJack/linux/LabJackTypeConverters.cpp | 71 +
.../Devices/LabJack/linux/LabJackTypeConverters.h | 60 +
SurgSim/Devices/LabJack/win32/LabJackScaffold.cpp | 1031 +++
SurgSim/Devices/Mouse/CMakeLists.txt | 54 +
SurgSim/Devices/Mouse/MouseDevice.cpp | 72 +
SurgSim/Devices/Mouse/MouseDevice.h | 86 +
SurgSim/Devices/Mouse/MouseScaffold.cpp | 153 +
SurgSim/Devices/Mouse/MouseScaffold.h | 108 +
SurgSim/Devices/Mouse/OsgMouseHandler.cpp | 79 +
SurgSim/Devices/Mouse/OsgMouseHandler.h | 59 +
SurgSim/Devices/Mouse/UnitTests/CMakeLists.txt | 35 +
.../Devices/Mouse/UnitTests/MouseDeviceTest.cpp | 88 +
.../Devices/Mouse/UnitTests/MouseScaffoldTest.cpp | 55 +
SurgSim/Devices/Mouse/VisualTests/CMakeLists.txt | 35 +
.../Devices/Mouse/VisualTests/MouseVisualTests.cpp | 112 +
SurgSim/Devices/MultiAxis/BitSetBuffer.h | 133 +
SurgSim/Devices/MultiAxis/CMakeLists.txt | 123 +
.../Devices/MultiAxis/CreateInputDeviceHandle.h | 51 +
SurgSim/Devices/MultiAxis/GetSystemError.h | 43 +
SurgSim/Devices/MultiAxis/MultiAxis.dox | 23 +
SurgSim/Devices/MultiAxis/MultiAxisDevice.cpp | 153 +
SurgSim/Devices/MultiAxis/MultiAxisDevice.h | 164 +
SurgSim/Devices/MultiAxis/README.linux | 14 +
SurgSim/Devices/MultiAxis/RawMultiAxisDevice.cpp | 132 +
SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h | 127 +
SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.cpp | 622 ++
SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h | 168 +
SurgSim/Devices/MultiAxis/RawMultiAxisThread.cpp | 56 +
SurgSim/Devices/MultiAxis/RawMultiAxisThread.h | 54 +
.../Devices/MultiAxis/SystemInputDeviceHandle.cpp | 38 +
.../Devices/MultiAxis/SystemInputDeviceHandle.h | 88 +
SurgSim/Devices/MultiAxis/UnitTests/CMakeLists.txt | 37 +
.../MultiAxis/UnitTests/MultiAxisDeviceTest.cpp | 204 +
.../MultiAxis/UnitTests/RawMultiAxisDeviceTest.cpp | 214 +
.../UnitTests/RawMultiAxisScaffoldTest.cpp | 185 +
.../Devices/MultiAxis/VisualTest/CMakeLists.txt | 68 +
SurgSim/Devices/MultiAxis/VisualTest/main.cpp | 48 +
.../Devices/MultiAxis/VisualTest/raw_test_main.cpp | 48 +
.../MultiAxis/linux/CreateInputDeviceHandle.cpp | 40 +
SurgSim/Devices/MultiAxis/linux/FileDescriptor.cpp | 166 +
SurgSim/Devices/MultiAxis/linux/FileDescriptor.h | 113 +
SurgSim/Devices/MultiAxis/linux/GetSystemError.cpp | 70 +
.../Devices/MultiAxis/linux/InputDeviceHandle.cpp | 342 +
.../Devices/MultiAxis/linux/InputDeviceHandle.h | 93 +
.../MultiAxis/udev-rules/90-3dconnexion.rules | 17 +
.../MultiAxis/win32/CreateInputDeviceHandle.cpp | 40 +
SurgSim/Devices/MultiAxis/win32/FileHandle.cpp | 186 +
SurgSim/Devices/MultiAxis/win32/FileHandle.h | 126 +
SurgSim/Devices/MultiAxis/win32/GetSystemError.cpp | 66 +
.../Devices/MultiAxis/win32/WdkHidDeviceHandle.cpp | 578 ++
.../Devices/MultiAxis/win32/WdkHidDeviceHandle.h | 120 +
SurgSim/Devices/Novint/CMakeLists.txt | 64 +
SurgSim/Devices/Novint/Novint.dox | 16 +
SurgSim/Devices/Novint/Novint7DofDevice.cpp | 62 +
SurgSim/Devices/Novint/Novint7DofDevice.h | 72 +
SurgSim/Devices/Novint/NovintCommonDevice.cpp | 132 +
SurgSim/Devices/Novint/NovintCommonDevice.h | 97 +
SurgSim/Devices/Novint/NovintDevice.cpp | 56 +
SurgSim/Devices/Novint/NovintDevice.h | 74 +
SurgSim/Devices/Novint/NovintScaffold.cpp | 1198 +++
SurgSim/Devices/Novint/NovintScaffold.h | 227 +
SurgSim/Devices/Novint/UnitTests/CMakeLists.txt | 38 +
.../Novint/UnitTests/Novint7DofDeviceTest.cpp | 219 +
.../Devices/Novint/UnitTests/NovintDeviceTest.cpp | 209 +
.../Novint/UnitTests/NovintScaffoldTest.cpp | 190 +
SurgSim/Devices/Novint/VisualTest/CMakeLists.txt | 67 +
.../Devices/Novint/VisualTest/falcon_7dof_main.cpp | 51 +
SurgSim/Devices/Novint/VisualTest/main.cpp | 53 +
SurgSim/Devices/Phantom/CMakeLists.txt | 62 +
SurgSim/Devices/Phantom/Phantom.dox | 14 +
SurgSim/Devices/Phantom/PhantomDevice.cpp | 83 +
SurgSim/Devices/Phantom/PhantomDevice.h | 93 +
SurgSim/Devices/Phantom/PhantomScaffold.cpp | 744 ++
SurgSim/Devices/Phantom/PhantomScaffold.h | 175 +
SurgSim/Devices/Phantom/UnitTests/CMakeLists.txt | 37 +
.../Phantom/UnitTests/PhantomDeviceTest.cpp | 199 +
.../Phantom/UnitTests/PhantomScaffoldTest.cpp | 185 +
SurgSim/Devices/Phantom/VisualTest/CMakeLists.txt | 52 +
SurgSim/Devices/Phantom/VisualTest/main.cpp | 45 +
SurgSim/Devices/Sixense/CMakeLists.txt | 60 +
SurgSim/Devices/Sixense/Sixense.dox | 11 +
SurgSim/Devices/Sixense/SixenseDevice.cpp | 77 +
SurgSim/Devices/Sixense/SixenseDevice.h | 79 +
SurgSim/Devices/Sixense/SixenseScaffold.cpp | 496 ++
SurgSim/Devices/Sixense/SixenseScaffold.h | 154 +
SurgSim/Devices/Sixense/SixenseThread.cpp | 46 +
SurgSim/Devices/Sixense/SixenseThread.h | 57 +
SurgSim/Devices/Sixense/UnitTests/CMakeLists.txt | 47 +
.../Sixense/UnitTests/SixenseDeviceTest.cpp | 217 +
.../Sixense/UnitTests/SixenseScaffoldTest.cpp | 185 +
SurgSim/Devices/Sixense/VisualTest/CMakeLists.txt | 65 +
SurgSim/Devices/Sixense/VisualTest/main.cpp | 44 +
SurgSim/Devices/TrackIR/CMakeLists.txt | 68 +
SurgSim/Devices/TrackIR/TrackIR.dox | 22 +
SurgSim/Devices/TrackIR/TrackIRDevice.cpp | 112 +
SurgSim/Devices/TrackIR/TrackIRDevice.h | 95 +
SurgSim/Devices/TrackIR/TrackIRScaffold.h | 147 +
SurgSim/Devices/TrackIR/TrackIRThread.cpp | 52 +
SurgSim/Devices/TrackIR/TrackIRThread.h | 64 +
SurgSim/Devices/TrackIR/UnitTests/CMakeLists.txt | 59 +
.../TrackIR/UnitTests/TrackIRDeviceTest.cpp | 109 +
.../TrackIR/UnitTests/TrackIRScaffoldTest.cpp | 193 +
SurgSim/Devices/TrackIR/VisualTest/CMakeLists.txt | 50 +
SurgSim/Devices/TrackIR/VisualTest/main.cpp | 45 +
SurgSim/Devices/TrackIR/linux/TrackIRScaffold.cpp | 399 +
SurgSim/Devices/TrackIR/win32/TrackIRScaffold.cpp | 473 ++
SurgSim/Devices/devices.dox | 15 +
SurgSim/Framework/Accessible-inl.h | 63 +
SurgSim/Framework/Accessible.cpp | 200 +
SurgSim/Framework/Accessible.h | 223 +
SurgSim/Framework/ApplicationData.cpp | 171 +
SurgSim/Framework/ApplicationData.h | 103 +
SurgSim/Framework/Assert.h | 106 +
SurgSim/Framework/AssertMessage.cpp | 59 +
SurgSim/Framework/AssertMessage.h | 124 +
SurgSim/Framework/Asset.cpp | 74 +
SurgSim/Framework/Asset.h | 80 +
SurgSim/Framework/Barrier.cpp | 59 +
SurgSim/Framework/Barrier.h | 76 +
SurgSim/Framework/BasicSceneElement.cpp | 39 +
SurgSim/Framework/BasicSceneElement.h | 50 +
SurgSim/Framework/BasicThread.cpp | 255 +
SurgSim/Framework/BasicThread.h | 169 +
SurgSim/Framework/Behavior.h | 61 +
SurgSim/Framework/BehaviorManager.cpp | 75 +
SurgSim/Framework/BehaviorManager.h | 54 +
SurgSim/Framework/CMakeLists.txt | 107 +
SurgSim/Framework/Clock.h | 36 +
SurgSim/Framework/Component.cpp | 192 +
SurgSim/Framework/Component.h | 187 +
SurgSim/Framework/ComponentManager-inl.h | 83 +
SurgSim/Framework/ComponentManager.cpp | 215 +
SurgSim/Framework/ComponentManager.h | 182 +
SurgSim/Framework/Documentation/CMakeLists.txt | 22 +
SurgSim/Framework/Documentation/framework.dox | 8 +
SurgSim/Framework/Documentation/serialization.dox | 81 +
SurgSim/Framework/FrameworkConvert-inl.h | 48 +
SurgSim/Framework/FrameworkConvert.cpp | 164 +
SurgSim/Framework/FrameworkConvert.h | 117 +
SurgSim/Framework/LockedContainer.h | 202 +
SurgSim/Framework/Log.h | 41 +
SurgSim/Framework/LogMacros.h | 217 +
SurgSim/Framework/LogMessage.h | 61 +
SurgSim/Framework/LogMessageBase.cpp | 56 +
SurgSim/Framework/LogMessageBase.h | 105 +
SurgSim/Framework/LogOutput.cpp | 90 +
SurgSim/Framework/LogOutput.h | 98 +
SurgSim/Framework/Logger.cpp | 36 +
SurgSim/Framework/Logger.h | 142 +
SurgSim/Framework/LoggerManager.cpp | 111 +
SurgSim/Framework/LoggerManager.h | 93 +
SurgSim/Framework/Macros.h | 47 +
SurgSim/Framework/ObjectFactory-inl.h | 100 +
SurgSim/Framework/ObjectFactory.h | 152 +
SurgSim/Framework/PoseComponent.cpp | 63 +
SurgSim/Framework/PoseComponent.h | 74 +
SurgSim/Framework/Representation.cpp | 71 +
SurgSim/Framework/Representation.h | 65 +
SurgSim/Framework/ReuseFactory.h | 123 +
SurgSim/Framework/Runtime.cpp | 311 +
SurgSim/Framework/Runtime.h | 156 +
SurgSim/Framework/Scene.cpp | 144 +
SurgSim/Framework/Scene.h | 92 +
SurgSim/Framework/SceneElement-inl.h | 47 +
SurgSim/Framework/SceneElement.cpp | 295 +
SurgSim/Framework/SceneElement.h | 198 +
SurgSim/Framework/SharedInstance-inl.h | 81 +
SurgSim/Framework/SharedInstance.h | 166 +
SurgSim/Framework/Timer.cpp | 135 +
SurgSim/Framework/Timer.h | 119 +
SurgSim/Framework/TransferPropertiesBehavior.cpp | 127 +
SurgSim/Framework/TransferPropertiesBehavior.h | 105 +
SurgSim/Framework/UnitTests/AccessibleTests.cpp | 442 ++
.../Framework/UnitTests/AccessibleTypeTests.cpp | 251 +
.../Framework/UnitTests/ApplicationDataTest.cpp | 254 +
SurgSim/Framework/UnitTests/AssertTest.cpp | 202 +
SurgSim/Framework/UnitTests/AssetTests.cpp | 101 +
SurgSim/Framework/UnitTests/BarrierTest.cpp | 64 +
.../Framework/UnitTests/BasicSceneElementTests.cpp | 110 +
SurgSim/Framework/UnitTests/BasicThreadTests.cpp | 304 +
.../Framework/UnitTests/BehaviorManagerTest.cpp | 94 +
SurgSim/Framework/UnitTests/CMakeLists.txt | 65 +
.../Framework/UnitTests/ComponentManagerTests.cpp | 217 +
SurgSim/Framework/UnitTests/ComponentTest.cpp | 536 ++
.../Directory1/duplicatedFile.txt | 0
.../ApplicationDataTest/Directory1/uniqueFile1.txt | 0
.../Directory2/duplicatedFile.txt | 0
.../ApplicationDataTest/Directory2/uniqueFile2.txt | 0
.../Test Directory/uniqueFile.txt | 1 +
.../Data/ApplicationDataTest/testFile1.txt | 2 +
.../UnitTests/Data/AssetTestData/DummyFile.txt | 0
.../Framework/UnitTests/LockedContainerTest.cpp | 657 ++
SurgSim/Framework/UnitTests/LoggerManagerTest.cpp | 140 +
SurgSim/Framework/UnitTests/LoggerTest.cpp | 347 +
SurgSim/Framework/UnitTests/MockObjects.cpp | 68 +
SurgSim/Framework/UnitTests/MockObjects.h | 329 +
SurgSim/Framework/UnitTests/ObjectFactoryTests.cpp | 122 +
SurgSim/Framework/UnitTests/ReuseFactoryTest.cpp | 110 +
SurgSim/Framework/UnitTests/RuntimeTest.cpp | 240 +
SurgSim/Framework/UnitTests/SceneElementTest.cpp | 192 +
SurgSim/Framework/UnitTests/SceneTest.cpp | 124 +
SurgSim/Framework/UnitTests/SharedInstanceTest.cpp | 294 +
SurgSim/Framework/UnitTests/TimerTest.cpp | 95 +
.../UnitTests/TransferPropertiesBehaviorTests.cpp | 97 +
SurgSim/Framework/UnitTests/config.txt.in | 1 +
SurgSim/Graphics/AxesRepresentation.h | 52 +
SurgSim/Graphics/BoxRepresentation.h | 89 +
SurgSim/Graphics/CMakeLists.txt | 185 +
SurgSim/Graphics/Camera.cpp | 80 +
SurgSim/Graphics/Camera.h | 145 +
SurgSim/Graphics/CapsuleRepresentation.h | 77 +
SurgSim/Graphics/CylinderRepresentation.h | 78 +
SurgSim/Graphics/Group.cpp | 88 +
SurgSim/Graphics/Group.h | 92 +
SurgSim/Graphics/Light.h | 121 +
SurgSim/Graphics/Manager.cpp | 214 +
SurgSim/Graphics/Manager.h | 142 +
SurgSim/Graphics/Material.h | 108 +
SurgSim/Graphics/Mesh-inl.h | 36 +
SurgSim/Graphics/Mesh.cpp | 92 +
SurgSim/Graphics/Mesh.h | 91 +
SurgSim/Graphics/MeshPlyReaderDelegate.cpp | 56 +
SurgSim/Graphics/MeshPlyReaderDelegate.h | 50 +
SurgSim/Graphics/MeshRepresentation.h | 83 +
SurgSim/Graphics/MeshUtilities.cpp | 28 +
SurgSim/Graphics/MeshUtilities.h | 39 +
SurgSim/Graphics/OctreeRepresentation.h | 63 +
SurgSim/Graphics/OsgAxesRepresentation.cpp | 60 +
SurgSim/Graphics/OsgAxesRepresentation.h | 70 +
SurgSim/Graphics/OsgBoxRepresentation.cpp | 104 +
SurgSim/Graphics/OsgBoxRepresentation.h | 113 +
SurgSim/Graphics/OsgCamera.cpp | 314 +
SurgSim/Graphics/OsgCamera.h | 145 +
SurgSim/Graphics/OsgCapsuleRepresentation.cpp | 122 +
SurgSim/Graphics/OsgCapsuleRepresentation.h | 108 +
SurgSim/Graphics/OsgConversions.h | 27 +
SurgSim/Graphics/OsgCylinderRepresentation.cpp | 97 +
SurgSim/Graphics/OsgCylinderRepresentation.h | 99 +
SurgSim/Graphics/OsgGroup.cpp | 109 +
SurgSim/Graphics/OsgGroup.h | 86 +
SurgSim/Graphics/OsgLight.cpp | 234 +
SurgSim/Graphics/OsgLight.h | 141 +
SurgSim/Graphics/OsgLog.cpp | 60 +
SurgSim/Graphics/OsgLog.h | 60 +
SurgSim/Graphics/OsgManager.cpp | 187 +
SurgSim/Graphics/OsgManager.h | 106 +
SurgSim/Graphics/OsgMaterial.cpp | 217 +
SurgSim/Graphics/OsgMaterial.h | 107 +
SurgSim/Graphics/OsgMatrixConversions.h | 147 +
SurgSim/Graphics/OsgMeshRepresentation.cpp | 283 +
SurgSim/Graphics/OsgMeshRepresentation.h | 126 +
SurgSim/Graphics/OsgOctreeRepresentation.cpp | 127 +
SurgSim/Graphics/OsgOctreeRepresentation.h | 97 +
SurgSim/Graphics/OsgPlane.h | 65 +
SurgSim/Graphics/OsgPlaneRepresentation.cpp | 48 +
SurgSim/Graphics/OsgPlaneRepresentation.h | 70 +
SurgSim/Graphics/OsgPointCloudRepresentation.cpp | 133 +
SurgSim/Graphics/OsgPointCloudRepresentation.h | 111 +
SurgSim/Graphics/OsgQuaternionConversions.h | 61 +
SurgSim/Graphics/OsgRenderTarget-inl.h | 182 +
SurgSim/Graphics/OsgRenderTarget.h | 146 +
SurgSim/Graphics/OsgRepresentation.cpp | 139 +
SurgSim/Graphics/OsgRepresentation.h | 100 +
SurgSim/Graphics/OsgRigidTransformConversions.h | 79 +
SurgSim/Graphics/OsgSceneryRepresentation.cpp | 67 +
SurgSim/Graphics/OsgSceneryRepresentation.h | 73 +
SurgSim/Graphics/OsgScreenSpacePass.cpp | 85 +
SurgSim/Graphics/OsgScreenSpacePass.h | 73 +
.../Graphics/OsgScreenSpaceQuadRepresentation.cpp | 288 +
.../Graphics/OsgScreenSpaceQuadRepresentation.h | 159 +
SurgSim/Graphics/OsgShader.cpp | 259 +
SurgSim/Graphics/OsgShader.h | 150 +
SurgSim/Graphics/OsgSphereRepresentation.cpp | 61 +
SurgSim/Graphics/OsgSphereRepresentation.h | 75 +
SurgSim/Graphics/OsgTexture.cpp | 45 +
SurgSim/Graphics/OsgTexture.h | 63 +
SurgSim/Graphics/OsgTexture1d.cpp | 36 +
SurgSim/Graphics/OsgTexture1d.h | 69 +
SurgSim/Graphics/OsgTexture2d.cpp | 64 +
SurgSim/Graphics/OsgTexture2d.h | 84 +
SurgSim/Graphics/OsgTexture3d.cpp | 110 +
SurgSim/Graphics/OsgTexture3d.h | 79 +
SurgSim/Graphics/OsgTextureCubeMap.cpp | 143 +
SurgSim/Graphics/OsgTextureCubeMap.h | 107 +
SurgSim/Graphics/OsgTextureRectangle.cpp | 41 +
SurgSim/Graphics/OsgTextureRectangle.h | 73 +
SurgSim/Graphics/OsgTextureUniform-inl.h | 121 +
SurgSim/Graphics/OsgTextureUniform.h | 137 +
SurgSim/Graphics/OsgTrackballZoomManipulator.cpp | 214 +
SurgSim/Graphics/OsgTrackballZoomManipulator.h | 131 +
SurgSim/Graphics/OsgUniform-inl.h | 117 +
SurgSim/Graphics/OsgUniform.h | 99 +
SurgSim/Graphics/OsgUniformBase.cpp | 36 +
SurgSim/Graphics/OsgUniformBase.h | 72 +
SurgSim/Graphics/OsgUniformFactory.cpp | 72 +
SurgSim/Graphics/OsgUniformFactory.h | 42 +
SurgSim/Graphics/OsgUniformTypes.h | 176 +
SurgSim/Graphics/OsgUnitAxes.cpp | 72 +
SurgSim/Graphics/OsgUnitAxes.h | 49 +
SurgSim/Graphics/OsgUnitBox.h | 60 +
SurgSim/Graphics/OsgUnitCylinder.h | 60 +
SurgSim/Graphics/OsgUnitSphere.h | 67 +
SurgSim/Graphics/OsgVectorConversions.h | 120 +
SurgSim/Graphics/OsgVectorFieldRepresentation.cpp | 173 +
SurgSim/Graphics/OsgVectorFieldRepresentation.h | 113 +
SurgSim/Graphics/OsgView.cpp | 454 ++
SurgSim/Graphics/OsgView.h | 202 +
SurgSim/Graphics/OsgViewElement.cpp | 104 +
SurgSim/Graphics/OsgViewElement.h | 93 +
SurgSim/Graphics/PlaneRepresentation.h | 43 +
SurgSim/Graphics/PointCloudRepresentation.cpp | 36 +
SurgSim/Graphics/PointCloudRepresentation.h | 68 +
SurgSim/Graphics/RenderPass.cpp | 150 +
SurgSim/Graphics/RenderPass.h | 131 +
SurgSim/Graphics/RenderTarget.h | 74 +
SurgSim/Graphics/RenderTests/CMakeLists.txt | 63 +
.../Data/OsgMeshRepresentationRenderTests/cube.png | Bin 0 -> 751 bytes
.../OsgMeshRepresentationRenderTests/wound.png | Bin 0 -> 1761647 bytes
.../wound_deformable.ply | 2391 ++++++
.../OsgScreenSpaceQuadRenderTests/CheckerBoard.png | Bin 0 -> 4009 bytes
.../OsgScreenSpaceQuadRenderTests/Rectangle.png | Bin 0 -> 1674 bytes
.../Data/OsgShaderRenderTests/shader.frag | 7 +
.../Data/OsgShaderRenderTests/shader.geom | 30 +
.../Data/OsgShaderRenderTests/shader.vert | 9 +
.../OsgBoxRepresentationRenderTests.cpp | 180 +
.../Graphics/RenderTests/OsgCameraRenderTests.cpp | 178 +
.../OsgCapsuleRepresentationRenderTests.cpp | 153 +
.../OsgCylinderRepresentationRenderTests.cpp | 123 +
.../Graphics/RenderTests/OsgManagerRenderTests.cpp | 59 +
.../OsgMeshRepresentationRenderTests.cpp | 400 +
.../OsgOctreeRepresentationRenderTests.cpp | 122 +
.../OsgPlaneRepresentationRenderTests.cpp | 114 +
.../OsgPointCloudRepresentationRenderTests.cpp | 205 +
.../RenderTests/OsgRepresentationRenderTests.cpp | 163 +
.../OsgSceneryRepresentationRenderTests.cpp | 92 +
.../RenderTests/OsgScreenSpaceQuadRenderTests.cpp | 224 +
.../Graphics/RenderTests/OsgShaderRenderTests.cpp | 253 +
.../OsgSphereRepresentationRenderTests.cpp | 114 +
.../OsgVectorFieldRepresentationRenderTests.cpp | 218 +
.../RenderTests/OsgViewElementRenderTests.cpp | 196 +
SurgSim/Graphics/RenderTests/RenderTest.cpp | 79 +
SurgSim/Graphics/RenderTests/RenderTest.h | 70 +
SurgSim/Graphics/RenderTests/config.txt.in | 3 +
SurgSim/Graphics/Representation.cpp | 132 +
SurgSim/Graphics/Representation.h | 139 +
SurgSim/Graphics/SceneryRepresentation.h | 53 +
SurgSim/Graphics/ScreenSpaceQuadRepresentation.h | 77 +
SurgSim/Graphics/Shader.h | 123 +
SurgSim/Graphics/SphereRepresentation.h | 50 +
SurgSim/Graphics/Texture.h | 49 +
SurgSim/Graphics/Texture1d.h | 47 +
SurgSim/Graphics/Texture2d.h | 49 +
SurgSim/Graphics/Texture3d.h | 59 +
SurgSim/Graphics/TextureCubeMap.h | 60 +
SurgSim/Graphics/TextureRectangle.h | 49 +
SurgSim/Graphics/TriangleNormalGenerator.cpp | 96 +
SurgSim/Graphics/TriangleNormalGenerator.h | 80 +
SurgSim/Graphics/Uniform.h | 79 +
SurgSim/Graphics/UniformBase.h | 48 +
SurgSim/Graphics/UnitTests/CMakeLists.txt | 92 +
.../Data/OsgMeshRepresentationTests/Cube.ply | 53 +
.../UnitTests/Data/OsgShaderTests/shader.frag | 7 +
.../UnitTests/Data/OsgShaderTests/shader.geom | 21 +
.../UnitTests/Data/OsgShaderTests/shader.vert | 9 +
.../UnitTests/Data/OsgTextureTests/Brdf0.png | Bin 0 -> 1736 bytes
.../UnitTests/Data/OsgTextureTests/Brdf1.png | Bin 0 -> 15269 bytes
.../Data/OsgTextureTests/CheckerBoard.png | Bin 0 -> 4009 bytes
.../UnitTests/Data/OsgTextureTests/CubeMap.png | Bin 0 -> 20360 bytes
.../UnitTests/Data/OsgTextureTests/Gradient.png | Bin 0 -> 271 bytes
.../UnitTests/Data/OsgTextureTests/NegativeX.png | Bin 0 -> 6133 bytes
.../UnitTests/Data/OsgTextureTests/NegativeY.png | Bin 0 -> 5056 bytes
.../UnitTests/Data/OsgTextureTests/NegativeZ.png | Bin 0 -> 4700 bytes
.../UnitTests/Data/OsgTextureTests/PositiveX.png | Bin 0 -> 6225 bytes
.../UnitTests/Data/OsgTextureTests/PositiveY.png | Bin 0 -> 5012 bytes
.../UnitTests/Data/OsgTextureTests/PositiveZ.png | Bin 0 -> 4775 bytes
SurgSim/Graphics/UnitTests/GroupTests.cpp | 153 +
SurgSim/Graphics/UnitTests/ManagerTests.cpp | 240 +
SurgSim/Graphics/UnitTests/MeshTests.cpp | 136 +
SurgSim/Graphics/UnitTests/MockObjects.h | 597 ++
SurgSim/Graphics/UnitTests/MockOsgObjects.h | 177 +
.../UnitTests/OsgBoxRepresentationTests.cpp | 237 +
SurgSim/Graphics/UnitTests/OsgCameraTests.cpp | 257 +
.../UnitTests/OsgCapsuleRepresentationTests.cpp | 112 +
.../UnitTests/OsgCylinderRepresentationTests.cpp | 116 +
SurgSim/Graphics/UnitTests/OsgGroupTests.cpp | 225 +
SurgSim/Graphics/UnitTests/OsgLightTests.cpp | 174 +
SurgSim/Graphics/UnitTests/OsgLogTests.cpp | 52 +
SurgSim/Graphics/UnitTests/OsgManagerTests.cpp | 266 +
SurgSim/Graphics/UnitTests/OsgMaterialTests.cpp | 240 +
.../UnitTests/OsgMatrixConversionsTests.cpp | 114 +
.../UnitTests/OsgMeshRepresentationTests.cpp | 168 +
.../UnitTests/OsgOctreeRepresentationTests.cpp | 123 +
.../UnitTests/OsgPlaneRepresentationTests.cpp | 194 +
SurgSim/Graphics/UnitTests/OsgPlaneTests.cpp | 77 +
.../UnitTests/OsgPointCloudRepresentationTests.cpp | 118 +
.../UnitTests/OsgQuaternionConversionsTests.cpp | 45 +
.../Graphics/UnitTests/OsgRenderTargetTests.cpp | 86 +
.../Graphics/UnitTests/OsgRepresentationTests.cpp | 241 +
.../OsgRigidTransformConversionsTests.cpp | 118 +
.../UnitTests/OsgSceneryRepresentationTests.cpp | 106 +
.../Graphics/UnitTests/OsgScreenSpaceQuadTests.cpp | 135 +
SurgSim/Graphics/UnitTests/OsgShaderTests.cpp | 297 +
.../UnitTests/OsgSphereRepresentationTests.cpp | 210 +
SurgSim/Graphics/UnitTests/OsgTexture1dTests.cpp | 98 +
SurgSim/Graphics/UnitTests/OsgTexture2dTests.cpp | 97 +
SurgSim/Graphics/UnitTests/OsgTexture3dTests.cpp | 151 +
.../Graphics/UnitTests/OsgTextureCubeMapTests.cpp | 163 +
.../UnitTests/OsgTextureRectangleTests.cpp | 97 +
SurgSim/Graphics/UnitTests/OsgTextureTests.cpp | 81 +
.../Graphics/UnitTests/OsgTextureUniformTests.cpp | 129 +
SurgSim/Graphics/UnitTests/OsgUniformBaseTests.cpp | 73 +
SurgSim/Graphics/UnitTests/OsgUniformTests.cpp | 430 ++
.../Graphics/UnitTests/OsgUniformTypesTests.cpp | 104 +
SurgSim/Graphics/UnitTests/OsgUnitSphereTests.cpp | 75 +
.../UnitTests/OsgVectorConversionsTests.cpp | 72 +
.../OsgVectorFieldRepresentationTests.cpp | 63 +
SurgSim/Graphics/UnitTests/OsgViewElementTests.cpp | 60 +
SurgSim/Graphics/UnitTests/OsgViewTests.cpp | 229 +
SurgSim/Graphics/UnitTests/RenderPassTests.cpp | 59 +
SurgSim/Graphics/UnitTests/ViewElementTests.cpp | 147 +
SurgSim/Graphics/UnitTests/ViewTests.cpp | 113 +
SurgSim/Graphics/UnitTests/config.txt.in | 3 +
SurgSim/Graphics/VectorField.h | 64 +
SurgSim/Graphics/VectorFieldRepresentation.h | 72 +
SurgSim/Graphics/View.cpp | 173 +
SurgSim/Graphics/View.h | 204 +
SurgSim/Graphics/ViewElement.cpp | 71 +
SurgSim/Graphics/ViewElement.h | 97 +
SurgSim/Input/CMakeLists.txt | 54 +
SurgSim/Input/CommonDevice.cpp | 215 +
SurgSim/Input/CommonDevice.h | 150 +
SurgSim/Input/DeviceInterface.h | 90 +
SurgSim/Input/InputComponent.cpp | 133 +
SurgSim/Input/InputComponent.h | 99 +
SurgSim/Input/InputConsumerInterface.h | 76 +
SurgSim/Input/InputManager.cpp | 188 +
SurgSim/Input/InputManager.h | 101 +
SurgSim/Input/OutputComponent.cpp | 130 +
SurgSim/Input/OutputComponent.h | 96 +
SurgSim/Input/OutputProducerInterface.h | 73 +
SurgSim/Input/UnitTests/CMakeLists.txt | 38 +
SurgSim/Input/UnitTests/CommonDeviceTests.cpp | 248 +
SurgSim/Input/UnitTests/InputComponentTest.cpp | 70 +
SurgSim/Input/UnitTests/InputManagerTest.cpp | 214 +
SurgSim/Input/UnitTests/OutputComponentTest.cpp | 70 +
SurgSim/Input/UnitTests/TestDevice.cpp | 93 +
SurgSim/Input/UnitTests/TestDevice.h | 95 +
SurgSim/Math/Aabb.h | 84 +
SurgSim/Math/BoxShape.cpp | 129 +
SurgSim/Math/BoxShape.h | 116 +
SurgSim/Math/CMakeLists.txt | 123 +
SurgSim/Math/CapsuleShape.cpp | 125 +
SurgSim/Math/CapsuleShape.h | 98 +
SurgSim/Math/CylinderShape.cpp | 88 +
SurgSim/Math/CylinderShape.h | 90 +
SurgSim/Math/DoubleSidedPlaneShape.cpp | 64 +
SurgSim/Math/DoubleSidedPlaneShape.h | 71 +
SurgSim/Math/GaussLegendreQuadrature.cpp | 165 +
SurgSim/Math/GaussLegendreQuadrature.h | 96 +
SurgSim/Math/Geometry.h | 1619 ++++
SurgSim/Math/LinearSolveAndInverse-inl.h | 201 +
SurgSim/Math/LinearSolveAndInverse.cpp | 68 +
SurgSim/Math/LinearSolveAndInverse.h | 134 +
SurgSim/Math/MathConvert-inl.h | 194 +
SurgSim/Math/MathConvert.cpp | 117 +
SurgSim/Math/MathConvert.h | 98 +
SurgSim/Math/Matrix.h | 223 +
SurgSim/Math/MeshShape-inl.h | 42 +
SurgSim/Math/MeshShape.cpp | 212 +
SurgSim/Math/MeshShape.h | 142 +
SurgSim/Math/MlcpConstraintType.h | 50 +
SurgSim/Math/MlcpConstraintTypeName.h | 100 +
SurgSim/Math/MlcpGaussSeidelSolver.cpp | 1765 +++++
SurgSim/Math/MlcpGaussSeidelSolver.h | 194 +
SurgSim/Math/MlcpProblem.cpp | 49 +
SurgSim/Math/MlcpProblem.h | 119 +
SurgSim/Math/MlcpSolution.h | 68 +
SurgSim/Math/MlcpSolver.h | 58 +
SurgSim/Math/OctreeShape-inl.h | 34 +
SurgSim/Math/OctreeShape.cpp | 87 +
SurgSim/Math/OctreeShape.h | 98 +
SurgSim/Math/OdeEquation.cpp | 32 +
SurgSim/Math/OdeEquation.h | 95 +
SurgSim/Math/OdeSolver.cpp | 66 +
SurgSim/Math/OdeSolver.h | 139 +
SurgSim/Math/OdeSolverEulerExplicit.cpp | 64 +
SurgSim/Math/OdeSolverEulerExplicit.h | 52 +
SurgSim/Math/OdeSolverEulerExplicitModified.cpp | 64 +
SurgSim/Math/OdeSolverEulerExplicitModified.h | 49 +
SurgSim/Math/OdeSolverEulerImplicit.cpp | 75 +
SurgSim/Math/OdeSolverEulerImplicit.h | 52 +
SurgSim/Math/OdeSolverLinearEulerExplicit.cpp | 51 +
SurgSim/Math/OdeSolverLinearEulerExplicit.h | 50 +
.../Math/OdeSolverLinearEulerExplicitModified.cpp | 51 +
.../Math/OdeSolverLinearEulerExplicitModified.h | 50 +
SurgSim/Math/OdeSolverLinearEulerImplicit.cpp | 53 +
SurgSim/Math/OdeSolverLinearEulerImplicit.h | 51 +
SurgSim/Math/OdeSolverLinearRungeKutta4.cpp | 87 +
SurgSim/Math/OdeSolverLinearRungeKutta4.h | 47 +
SurgSim/Math/OdeSolverLinearStatic.cpp | 53 +
SurgSim/Math/OdeSolverLinearStatic.h | 47 +
SurgSim/Math/OdeSolverRungeKutta4.cpp | 95 +
SurgSim/Math/OdeSolverRungeKutta4.h | 76 +
SurgSim/Math/OdeSolverStatic.cpp | 65 +
SurgSim/Math/OdeSolverStatic.h | 46 +
SurgSim/Math/OdeState.cpp | 201 +
SurgSim/Math/OdeState.h | 162 +
SurgSim/Math/PlaneShape.cpp | 64 +
SurgSim/Math/PlaneShape.h | 77 +
SurgSim/Math/Quaternion.h | 162 +
SurgSim/Math/RigidTransform.h | 165 +
SurgSim/Math/Shape.cpp | 46 +
SurgSim/Math/Shape.h | 100 +
SurgSim/Math/Shapes.h | 30 +
SurgSim/Math/SphereShape.cpp | 73 +
SurgSim/Math/SphereShape.h | 77 +
SurgSim/Math/SurfaceMeshShape-inl.h | 41 +
SurgSim/Math/SurfaceMeshShape.cpp | 197 +
SurgSim/Math/SurfaceMeshShape.h | 137 +
.../Math/TriangleTriangleContactCalculation-inl.h | 306 +
SurgSim/Math/TriangleTriangleIntersection-inl.h | 185 +
SurgSim/Math/UnitTests/AabbTests.cpp | 75 +
SurgSim/Math/UnitTests/CMakeLists.txt | 89 +
SurgSim/Math/UnitTests/GeometryTests.cpp | 1563 ++++
.../Math/UnitTests/LinearSolveAndInverseTests.cpp | 279 +
SurgSim/Math/UnitTests/MakeRigidTransformTests.cpp | 104 +
SurgSim/Math/UnitTests/MatrixTests.cpp | 1368 ++++
SurgSim/Math/UnitTests/MeshShapeTests.cpp | 315 +
.../Math/UnitTests/MlcpGaussSeidelSolverTests.cpp | 220 +
.../UnitTests/MlcpTestData/mlcpOriginalTest.txt | 28 +
.../Math/UnitTests/MlcpTestData/mlcpTest000.txt | 12 +
.../Math/UnitTests/MlcpTestData/mlcpTest001.txt | 42 +
.../Math/UnitTests/MlcpTestData/mlcpTest002.txt | 30 +
.../Math/UnitTests/MlcpTestData/mlcpTest003.txt | 30 +
.../Math/UnitTests/MlcpTestData/mlcpTest004.txt | 22 +
.../Math/UnitTests/MlcpTestData/mlcpTest005.txt | 18 +
.../Math/UnitTests/MlcpTestData/mlcpTest006.txt | 22 +
.../Math/UnitTests/MlcpTestData/mlcpTest007.txt | 18 +
.../Math/UnitTests/MlcpTestData/mlcpTest008.txt | 24 +
.../Math/UnitTests/MlcpTestData/mlcpTest009.txt | 21 +
SurgSim/Math/UnitTests/MockObject.h | 227 +
SurgSim/Math/UnitTests/MockTriangle.h | 106 +
SurgSim/Math/UnitTests/OdeEquationTests.cpp | 117 +
.../OdeSolverEulerExplicitModifiedTests.cpp | 123 +
.../Math/UnitTests/OdeSolverEulerExplicitTests.cpp | 123 +
.../Math/UnitTests/OdeSolverEulerImplicitTests.cpp | 122 +
.../Math/UnitTests/OdeSolverRungeKutta4Tests.cpp | 252 +
SurgSim/Math/UnitTests/OdeSolverStaticTests.cpp | 85 +
SurgSim/Math/UnitTests/OdeSolverTests.cpp | 112 +
SurgSim/Math/UnitTests/OdeStateTests.cpp | 400 +
SurgSim/Math/UnitTests/QuaternionTests.cpp | 674 ++
SurgSim/Math/UnitTests/RigidTransformTests.cpp | 199 +
SurgSim/Math/UnitTests/ShapeTests.cpp | 602 ++
SurgSim/Math/UnitTests/SurfaceMeshShapeTests.cpp | 144 +
.../TriangleTriangleContactCalculationTests.cpp | 184 +
.../TriangleTriangleIntersectionTests.cpp | 50 +
.../UnitTests/TriangleTriangleTestParameters.h | 244 +
SurgSim/Math/UnitTests/ValidTests.cpp | 779 ++
SurgSim/Math/UnitTests/VectorTests.cpp | 1379 ++++
SurgSim/Math/UnitTests/config.txt.in | 1 +
SurgSim/Math/Valid-inl.h | 275 +
SurgSim/Math/Valid.h | 237 +
SurgSim/Math/Vector.h | 199 +
SurgSim/Physics/BuildMlcp.cpp | 119 +
SurgSim/Physics/BuildMlcp.h | 46 +
SurgSim/Physics/CMakeLists.txt | 172 +
SurgSim/Physics/Computation.cpp | 99 +
SurgSim/Physics/Computation.h | 75 +
SurgSim/Physics/Constraint.cpp | 160 +
SurgSim/Physics/Constraint.h | 132 +
SurgSim/Physics/ConstraintComponent.cpp | 52 +
SurgSim/Physics/ConstraintComponent.h | 66 +
SurgSim/Physics/ConstraintData.cpp | 33 +
SurgSim/Physics/ConstraintData.h | 40 +
SurgSim/Physics/ConstraintImplementation.cpp | 36 +
SurgSim/Physics/ConstraintImplementation.h | 116 +
.../Physics/ConstraintImplementationFactory.cpp | 62 +
SurgSim/Physics/ConstraintImplementationFactory.h | 70 +
SurgSim/Physics/ContactConstraintData.h | 82 +
SurgSim/Physics/ContactConstraintGeneration.cpp | 187 +
SurgSim/Physics/ContactConstraintGeneration.h | 99 +
SurgSim/Physics/DcdCollision.cpp | 160 +
SurgSim/Physics/DcdCollision.h | 85 +
.../Physics/DeformableCollisionRepresentation.cpp | 147 +
.../Physics/DeformableCollisionRepresentation.h | 104 +
SurgSim/Physics/DeformableRepresentation.cpp | 327 +
SurgSim/Physics/DeformableRepresentation.h | 192 +
SurgSim/Physics/Fem1DElementBeam.cpp | 390 +
SurgSim/Physics/Fem1DElementBeam.h | 208 +
SurgSim/Physics/Fem1DPlyReaderDelegate.cpp | 96 +
SurgSim/Physics/Fem1DPlyReaderDelegate.h | 64 +
SurgSim/Physics/Fem1DRepresentation.cpp | 163 +
SurgSim/Physics/Fem1DRepresentation.h | 75 +
.../Physics/Fem1DRepresentationLocalization.cpp | 98 +
SurgSim/Physics/Fem1DRepresentationLocalization.h | 72 +
SurgSim/Physics/Fem2DElementTriangle.cpp | 624 ++
SurgSim/Physics/Fem2DElementTriangle.h | 249 +
SurgSim/Physics/Fem2DPlyReaderDelegate.cpp | 96 +
SurgSim/Physics/Fem2DPlyReaderDelegate.h | 63 +
SurgSim/Physics/Fem2DRepresentation.cpp | 145 +
SurgSim/Physics/Fem2DRepresentation.h | 68 +
.../Physics/Fem2DRepresentationLocalization.cpp | 98 +
SurgSim/Physics/Fem2DRepresentationLocalization.h | 72 +
.../Fem3DElementCorotationalTetrahedron.cpp | 268 +
.../Physics/Fem3DElementCorotationalTetrahedron.h | 83 +
SurgSim/Physics/Fem3DElementCube.cpp | 469 ++
SurgSim/Physics/Fem3DElementCube.h | 308 +
SurgSim/Physics/Fem3DElementTetrahedron.cpp | 447 ++
SurgSim/Physics/Fem3DElementTetrahedron.h | 206 +
SurgSim/Physics/Fem3DPlyReaderDelegate.cpp | 61 +
SurgSim/Physics/Fem3DPlyReaderDelegate.h | 46 +
SurgSim/Physics/Fem3DRepresentation.cpp | 301 +
SurgSim/Physics/Fem3DRepresentation.h | 92 +
SurgSim/Physics/Fem3DRepresentationBilateral3D.cpp | 128 +
SurgSim/Physics/Fem3DRepresentationBilateral3D.h | 74 +
SurgSim/Physics/Fem3DRepresentationContact.cpp | 130 +
SurgSim/Physics/Fem3DRepresentationContact.h | 73 +
.../Physics/Fem3DRepresentationLocalization.cpp | 98 +
SurgSim/Physics/Fem3DRepresentationLocalization.h | 73 +
SurgSim/Physics/FemElement.cpp | 119 +
SurgSim/Physics/FemElement.h | 224 +
SurgSim/Physics/FemPlyReaderDelegate.cpp | 205 +
SurgSim/Physics/FemPlyReaderDelegate.h | 149 +
SurgSim/Physics/FemRepresentation.cpp | 434 ++
SurgSim/Physics/FemRepresentation.h | 202 +
SurgSim/Physics/FemRepresentationParameters.cpp | 198 +
SurgSim/Physics/FemRepresentationParameters.h | 171 +
SurgSim/Physics/FixedRepresentation.cpp | 50 +
SurgSim/Physics/FixedRepresentation.h | 55 +
SurgSim/Physics/FixedRepresentationBilateral3D.cpp | 77 +
SurgSim/Physics/FixedRepresentationBilateral3D.h | 74 +
SurgSim/Physics/FixedRepresentationContact.cpp | 84 +
SurgSim/Physics/FixedRepresentationContact.h | 76 +
SurgSim/Physics/FixedRepresentationLocalization.h | 105 +
SurgSim/Physics/FreeMotion.cpp | 61 +
SurgSim/Physics/FreeMotion.h | 56 +
SurgSim/Physics/LinearSpring.cpp | 268 +
SurgSim/Physics/LinearSpring.h | 128 +
SurgSim/Physics/Localization.cpp | 38 +
SurgSim/Physics/Localization.h | 92 +
SurgSim/Physics/Mass.h | 74 +
SurgSim/Physics/MassSpringRepresentation.cpp | 469 ++
SurgSim/Physics/MassSpringRepresentation.h | 202 +
.../Physics/MassSpringRepresentationContact.cpp | 109 +
SurgSim/Physics/MassSpringRepresentationContact.h | 79 +
.../MassSpringRepresentationLocalization.cpp | 88 +
.../Physics/MassSpringRepresentationLocalization.h | 81 +
SurgSim/Physics/MlcpMapping.h | 68 +
SurgSim/Physics/MlcpPhysicsProblem-inl.h | 57 +
SurgSim/Physics/MlcpPhysicsProblem.cpp | 48 +
SurgSim/Physics/MlcpPhysicsProblem.h | 94 +
SurgSim/Physics/MlcpPhysicsSolution.h | 43 +
SurgSim/Physics/PerformanceTests/CMakeLists.txt | 45 +
.../Data/Fem3DPerformanceTest/wound_deformable.ply | 2389 ++++++
.../PerformanceTests/Fem3DPerformanceTest.cpp | 309 +
SurgSim/Physics/PerformanceTests/config.txt.in | 1 +
SurgSim/Physics/PhysicsConvert.cpp | 48 +
SurgSim/Physics/PhysicsConvert.h | 42 +
SurgSim/Physics/PhysicsManager.cpp | 176 +
SurgSim/Physics/PhysicsManager.h | 147 +
SurgSim/Physics/PhysicsManagerState.cpp | 208 +
SurgSim/Physics/PhysicsManagerState.h | 212 +
SurgSim/Physics/PostUpdate.cpp | 54 +
SurgSim/Physics/PostUpdate.h | 50 +
SurgSim/Physics/PreUpdate.cpp | 54 +
SurgSim/Physics/PreUpdate.h | 48 +
SurgSim/Physics/PushResults.cpp | 67 +
SurgSim/Physics/PushResults.h | 49 +
SurgSim/Physics/RenderTests/CMakeLists.txt | 54 +
SurgSim/Physics/RenderTests/Data/box.ply | 30 +
SurgSim/Physics/RenderTests/Data/sphere.ply | 1932 +++++
.../RenderTests/Data/uniaxial_positions.csv | 350 +
.../Physics/RenderTests/Fem3DMeshRenderTest.cpp | 97 +
.../RenderTests/Fem3DvsTruthCubeRenderTest.cpp | 711 ++
SurgSim/Physics/RenderTests/RenderTest.cpp | 67 +
SurgSim/Physics/RenderTests/RenderTest.h | 56 +
SurgSim/Physics/RenderTests/RenderTestFem1D.cpp | 163 +
SurgSim/Physics/RenderTests/RenderTestFem2D.cpp | 250 +
SurgSim/Physics/RenderTests/RenderTestFem3D.cpp | 262 +
.../RenderTests/RenderTestFem3DCorotational.cpp | 166 +
.../Physics/RenderTests/RenderTestMassSprings.cpp | 339 +
.../Physics/RenderTests/RenderTestRigidBodies.cpp | 405 +
SurgSim/Physics/RenderTests/config.txt.in | 2 +
SurgSim/Physics/Representation.cpp | 133 +
SurgSim/Physics/Representation.h | 178 +
SurgSim/Physics/RigidCollisionRepresentation.cpp | 97 +
SurgSim/Physics/RigidCollisionRepresentation.h | 83 +
SurgSim/Physics/RigidRepresentation.cpp | 465 ++
SurgSim/Physics/RigidRepresentation.h | 139 +
SurgSim/Physics/RigidRepresentationBase-inl.h | 44 +
SurgSim/Physics/RigidRepresentationBase.cpp | 240 +
SurgSim/Physics/RigidRepresentationBase.h | 179 +
SurgSim/Physics/RigidRepresentationBilateral3D.cpp | 136 +
SurgSim/Physics/RigidRepresentationBilateral3D.h | 74 +
SurgSim/Physics/RigidRepresentationContact.cpp | 115 +
SurgSim/Physics/RigidRepresentationContact.h | 75 +
.../Physics/RigidRepresentationLocalization.cpp | 88 +
SurgSim/Physics/RigidRepresentationLocalization.h | 78 +
SurgSim/Physics/RigidRepresentationState.cpp | 114 +
SurgSim/Physics/RigidRepresentationState.h | 108 +
...dRepresentation_addExternalGeneralizedForce.dox | 101 +
SurgSim/Physics/SolveMlcp.cpp | 65 +
SurgSim/Physics/SolveMlcp.h | 64 +
SurgSim/Physics/Spring.h | 120 +
SurgSim/Physics/UnitTests/BuildMlcpTests.cpp | 523 ++
SurgSim/Physics/UnitTests/CMakeLists.txt | 108 +
SurgSim/Physics/UnitTests/CommonTests.h | 158 +
SurgSim/Physics/UnitTests/ComputationTests.cpp | 146 +
.../Physics/UnitTests/ConstraintComponentTests.cpp | 52 +
SurgSim/Physics/UnitTests/ConstraintTests.cpp | 397 +
.../UnitTests/ContactConstraintDataTests.cpp | 48 +
.../UnitTests/ContactConstraintGenerationTests.cpp | 206 +
.../UnitTests/Data/PlyReaderTests/Fem1D.ply | 34 +
.../UnitTests/Data/PlyReaderTests/Fem2D.ply | 31 +
.../UnitTests/Data/PlyReaderTests/Fem3DCube.ply | 32 +
.../UnitTests/Data/PlyReaderTests/Tetrahedron.ply | 63 +
.../Data/PlyReaderTests/WrongDataTetrahedron.ply | 64 +
.../Data/PlyReaderTests/WrongPlyTetrahedron.ply | 63 +
SurgSim/Physics/UnitTests/DcdCollisionTests.cpp | 124 +
.../DeformableCollisionRepresentationTest.cpp | 144 +
.../UnitTests/DeformableRepresentationTest.cpp | 531 ++
SurgSim/Physics/UnitTests/EigenGtestAsserts.cpp | 81 +
SurgSim/Physics/UnitTests/EigenGtestAsserts.h | 44 +
.../Physics/UnitTests/Fem1DElementBeamTests.cpp | 634 ++
.../UnitTests/Fem1DMechanicalValidationTests.cpp | 397 +
.../UnitTests/Fem1DPlyReaderDelegateTests.cpp | 90 +
.../Fem1DRepresentationLocalizationTest.cpp | 228 +
.../Physics/UnitTests/Fem1DRepresentationTests.cpp | 239 +
.../UnitTests/Fem2DElementTriangleTests.cpp | 1128 +++
.../UnitTests/Fem2DMechanicalValidationTests.cpp | 698 ++
.../UnitTests/Fem2DPlyReaderDelegateTests.cpp | 91 +
.../Fem2DRepresentationLocalizationTest.cpp | 235 +
.../Physics/UnitTests/Fem2DRepresentationTests.cpp | 216 +
.../Fem3DElementCorotationalTetrahedronTests.cpp | 531 ++
.../Physics/UnitTests/Fem3DElementCubeTests.cpp | 915 +++
.../UnitTests/Fem3DElementTetrahedronTests.cpp | 535 ++
.../UnitTests/Fem3DPlyReaderDelegateTests.cpp | 158 +
.../Fem3DRepresentationBilateral3DTests.cpp | 237 +
.../UnitTests/Fem3DRepresentationContactTests.cpp | 275 +
.../Fem3DRepresentationLocalizationTest.cpp | 371 +
.../Physics/UnitTests/Fem3DRepresentationTests.cpp | 375 +
SurgSim/Physics/UnitTests/FemElementTests.cpp | 226 +
.../UnitTests/FemRepresentationParametersTest.cpp | 183 +
.../Physics/UnitTests/FemRepresentationTests.cpp | 562 ++
.../FixedRepresentationBilateral3DTests.cpp | 116 +
.../UnitTests/FixedRepresentationContactTests.cpp | 87 +
.../FixedRepresentationLocalizationTest.cpp | 99 +
.../Physics/UnitTests/FixedRepresentationTest.cpp | 229 +
SurgSim/Physics/UnitTests/FreeMotionTests.cpp | 67 +
SurgSim/Physics/UnitTests/LinearSpringTest.cpp | 485 ++
.../MassSpringMechanicalValidationTests.cpp | 318 +
.../MassSpringRepresentationContactTest.cpp | 259 +
.../MassSpringRepresentationLocalizationTest.cpp | 107 +
.../UnitTests/MassSpringRepresentationTests.cpp | 595 ++
SurgSim/Physics/UnitTests/MassTest.cpp | 64 +
SurgSim/Physics/UnitTests/MockObjects.cpp | 585 ++
SurgSim/Physics/UnitTests/MockObjects.h | 428 ++
.../Physics/UnitTests/PhysicsManagerStateTests.cpp | 346 +
SurgSim/Physics/UnitTests/PhysicsManagerTests.cpp | 183 +
SurgSim/Physics/UnitTests/PostUpdateTests.cpp | 66 +
SurgSim/Physics/UnitTests/PreUpdateTests.cpp | 66 +
SurgSim/Physics/UnitTests/PushResultsTests.cpp | 492 ++
SurgSim/Physics/UnitTests/RepresentationTest.cpp | 186 +
.../UnitTests/RigidCollisionRepresentationTest.cpp | 215 +
.../RigidRepresentationBilateral3DTests.cpp | 163 +
.../UnitTests/RigidRepresentationContactTests.cpp | 99 +
.../RigidRepresentationLocalizationTest.cpp | 123 +
.../UnitTests/RigidRepresentationStateTest.cpp | 134 +
.../Physics/UnitTests/RigidRepresentationTest.cpp | 1131 +++
SurgSim/Physics/UnitTests/SolveMlcpTests.cpp | 91 +
.../Physics/UnitTests/VirtualToolCouplerTest.cpp | 566 ++
SurgSim/Physics/UnitTests/config.txt.in | 2 +
SurgSim/Physics/UpdateCollisionRepresentations.cpp | 53 +
SurgSim/Physics/UpdateCollisionRepresentations.h | 45 +
SurgSim/Physics/VirtualToolCoupler.cpp | 481 ++
SurgSim/Physics/VirtualToolCoupler.h | 269 +
SurgSim/Serialize/CMakeLists.txt | 51 +
SurgSim/Serialize/GraphicsConvert.cpp | 67 +
SurgSim/Serialize/GraphicsConvert.h | 46 +
SurgSim/Serialize/ShapesFactory-inl.h | 37 +
SurgSim/Serialize/ShapesFactory.cpp | 48 +
SurgSim/Serialize/ShapesFactory.h | 64 +
SurgSim/Serialize/UnitTests/CMakeLists.txt | 45 +
.../Serialize/UnitTests/GraphicsConvertTest.cpp | 77 +
SurgSim/Serialize/UnitTests/ShapesConvertTest.cpp | 259 +
SurgSim/Serialize/UnitTests/ShapesFactoryTest.cpp | 138 +
SurgSim/Testing/CMakeLists.txt | 62 +
SurgSim/Testing/Data/Geometry/arm_collision.ply | 2125 +++++
SurgSim/Testing/Data/Geometry/wound_deformable.ply | 2389 ++++++
SurgSim/Testing/Data/Textures/CheckerBoard.png | Bin 0 -> 4009 bytes
SurgSim/Testing/Data/Textures/Testpattern.png | Bin 0 -> 16992 bytes
SurgSim/Testing/MathUtilities.cpp | 67 +
SurgSim/Testing/MathUtilities.h | 106 +
SurgSim/Testing/MeshShapeData/InvalidMesh.ply | 12 +
SurgSim/Testing/MeshShapeData/staple_collision.ply | 741 ++
SurgSim/Testing/MlcpIO/CMakeLists.txt | 55 +
SurgSim/Testing/MlcpIO/MlcpTestData.cpp | 36 +
SurgSim/Testing/MlcpIO/MlcpTestData.h | 80 +
SurgSim/Testing/MlcpIO/ReadText.cpp | 509 ++
SurgSim/Testing/MlcpIO/ReadText.h | 24 +
SurgSim/Testing/MlcpIO/TextLabels.cpp | 27 +
SurgSim/Testing/MlcpIO/TextLabels.h | 30 +
SurgSim/Testing/MlcpIO/WriteText.cpp | 151 +
SurgSim/Testing/MlcpIO/WriteText.h | 24 +
SurgSim/Testing/MockInputOutput.cpp | 56 +
SurgSim/Testing/MockInputOutput.h | 47 +
SurgSim/Testing/MockPhysicsManager.h | 59 +
SurgSim/Testing/OctreeShapeData/invalid-staple.vox | 26 +
SurgSim/Testing/OctreeShapeData/staple.vox | 26 +
.../OsgSceneryRepresentationTests/Torus.mtl | 11 +
.../OsgSceneryRepresentationTests/Torus.obj | 2694 +++++++
.../OsgSceneryRepresentationTests/Torus.osgb | Bin 0 -> 84162 bytes
SurgSim/Testing/SerializationMockComponent.cpp | 71 +
SurgSim/Testing/SerializationMockComponent.h | 51 +
SurgSim/Testing/TestCube.cpp | 158 +
SurgSim/Testing/TestCube.h | 53 +
SurgSim/Testing/TestingMain.cpp | 30 +
SurgSim/Testing/TriangleMeshBaseTests/Cube.ply | 51 +
SurgSim/Testing/VisualTestCommon/CMakeLists.txt | 55 +
SurgSim/Testing/VisualTestCommon/GlutRenderer.cpp | 187 +
SurgSim/Testing/VisualTestCommon/GlutRenderer.h | 232 +
.../Testing/VisualTestCommon/MovingSquareForce.cpp | 147 +
.../Testing/VisualTestCommon/MovingSquareForce.h | 102 +
.../VisualTestCommon/MovingSquareGlutWindow.cpp | 145 +
.../VisualTestCommon/MovingSquareGlutWindow.h | 75 +
.../Testing/VisualTestCommon/ToolSquareTest.cpp | 79 +
SurgSim/Testing/VisualTestCommon/ToolSquareTest.h | 39 +
ThirdParty/README_THIRD_PARTY | 3 +
ThirdParty/google-style-lint/cpplint.py | 3361 ++++++++
Tools/Fragments/README.txt | 4 +
.../Fragments/merge-testdata/expected-testout.txt | 9 +
Tools/Fragments/merge-testdata/testobj-indices.txt | 8 +
Tools/Fragments/merge-testdata/testobj.txt | 7 +
Tools/Fragments/merge-testdata/testvertices.txt | 4 +
Tools/Fragments/merge.py | 128 +
Tools/alphabetize_cmakelists.py | 109 +
Tools/fix-style.py | 70 +
Tools/fix_header_guards.py | 144 +
Tools/memcheck.supp | 7 +
Tools/remove-trailing-whitespace.py | 80 +
Tools/run-lint.py | 496 ++
1180 files changed, 199687 insertions(+)
diff --git a/.gitattributes b/.gitattributes
new file mode 100644
index 0000000..6eb22f6
--- /dev/null
+++ b/.gitattributes
@@ -0,0 +1,123 @@
+##### Global default #####
+
+## Any file not listed below will not have EOL characters normalized.
+## (Diffing and merging will depend on whether Git thinks the file is really
+## text or binary.)
+* -text
+
+## Alternatively, we could let Git auto-detect text files and normalize EOL,
+## by setting:
+## * text=auto
+
+
+##### Source code #####
+
+## C++ and C source files
+*.h text diff=cpp
+*.cc text diff=cpp
+*.c text diff=cpp
+## (Discouraged C++ extensions)
+*.cpp text diff=cpp
+*.cxx text diff=cpp
+*.hh text diff=cpp
+
+## Python scripts
+*.py text eol=lf diff=python
+## Perl scripts/libraries/modules
+*.perl text eol=lf diff=perl
+*.pl text eol=lf diff=perl
+*.pm text eol=lf diff=perl
+## Shell scripts
+*.sh text eol=lf
+*.bash text eol=lf
+## Windows batch scripts
+*.bat text eol=crlf
+
+## Shader program source
+*.frag text diff=cpp
+*.vert text diff=cpp
+
+
+##### Other file types #####
+
+## Text files and documentation
+*.txt text
+README* text
+INSTALL* text
+LICENSE* text
+NOTICE* text
+## Non-text documentation
+*.html text diff=html
+*.pdf binary
+*.rtf binary
+
+## Doxygen documentation configuration files
+Doxyfile text
+Doxyfile.in text
+*.dox text
+
+## Image files
+*.png binary
+*.PNG binary
+*.jpg binary
+*.JPG binary
+*.gif binary
+*.GIF binary
+*.bmp binary
+*.BMP binary
+*.ico binary
+*.ICO binary
+*.ppm binary
+*.pgm binary
+*.pbm binary
+*.xpm -text diff -merge
+## Vector graphics
+*.svg -text diff -merge
+
+## Model files
+*.obj text diff -merge
+*.mtl text diff -merge
+*.osg text diff -merge
+*.osgt text diff -merge
+*.ply text diff -merge
+*.stl text diff -merge
+*.osgb binary
+*.oct binary
+*.phi binary
+*.rgd binary
+*.tet binary
+*.tri binary
+
+## CMake files
+CMakeLists.txt text
+*.cmake text
+
+## Makefiles
+Makefile text
+makefile text
+GNUmakefile text
+*.mk text
+
+## Various IDE project files, etc
+*.sln -text diff merge
+*.vcxproj -text diff -merge
+*.vcxproj.filters -text diff -merge
+*.props -text diff -merge
+*.pbxproj -text diff -merge
+
+## Resource files and UI design descriptions
+*.qrc text
+*.ui text
+*.rc text
+
+## Diff/patch files
+*.diff text diff -merge
+*.patch text diff -merge
+
+## XML and configuration
+*.xml text
+*.cfg text
+
+## Self-reference =)
+.gitignore text
+.gitattributes text
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..317be26
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,94 @@
+# This file specifies intentionally untracked files that git should ignore.
+# For details, see documentation, e.g. by running "git help ignore".
+#
+# To list the files in your tree that are being ignored (i.e. files which
+# are not already under version control and match an ignore pattern), run
+# git ls-files --others -i
+
+##### generic file types #####
+
+## object files and other build output
+*.o
+*.obj
+*.pdb
+*.so
+*.a
+*.dll
+*.lib
+*.exe
+*.pyc
+
+## editor etc. back-up files
+*~
+.*~
+*.bak
+.#*
+
+## archive file formats
+*.tar
+*.gz
+*.bz2
+*.zip
+*.7z
+
+## CMake build directories
+CMakeFiles
+
+## files left by CMake
+CMakeCache.txt
+Makefile
+cmake_install.cmake
+
+## Visual Studio build directories
+Debug
+Release
+ReleaseUnoptimized
+Debug-vs20[01][0-9]
+Release-vs20[01][0-9]
+ReleaseUnoptimized-vs20[01][0-9]
+
+## files left by Visual Studio
+*.sdf
+*.opensdf
+*.suo
+*.ncb
+*.bsc
+*.pch
+*.ilk
+*.idb
+*.exp
+*.vcproj.*.user
+*.vcxproj.user
+
+## Visual Studio solution and project files
+# (Currently always generated from CMake, so we just ignore them all.
+# If that changes in the future, this will have to change as well.)
+*.sln
+*.vcxproj
+*.vcxproj.filters
+
+## files left by other development tools
+ipch
+VTune
+My Amplifier XE Results*
+*.ampl.cfg
+
+## files left around after manually running 'patch'
+*.rej
+*.orig
+
+## other files we don't generally care about
+*.log
+
+##### specific files/paths to ignore #####
+
+## build directories
+build/
+build-*/
+build_*/
+
+## binaries
+# (only if building directly inside the tree)
+/tests/unit_tests
+/tests/unit_tests.exe
+*.smproj
diff --git a/CMake/DashboardTemplate.cmake b/CMake/DashboardTemplate.cmake
new file mode 100644
index 0000000..bbec878
--- /dev/null
+++ b/CMake/DashboardTemplate.cmake
@@ -0,0 +1,131 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+cmake_minimum_required(VERSION 2.8)
+
+##############################################################################
+#
+# Copy this script onto the dashboard machine and then customize the variables
+# OSS_* below. Run the script using the command
+#
+# ctest -VV -O <logfile> -S <script>
+#
+# or for generators that require a configuration to be chosen
+#
+# ctest -VV -O <logfile> -S <script> -C <configuration>
+#
+# Run ctest --help for more options.
+#
+##############################################################################
+
+set(OSS_SOURCE_NAME "OpenSurgSim")
+set(OSS_BUILD_TYPE "Experimental") # Continuous, Experimental, or Nightly
+set(OSS_BUILD_CONFIGURATION "Debug")
+set(OSS_CMAKE_GENERATOR "Unix Makefiles")
+set(OSS_DASHBOARD_ROOT "$ENV{HOME}/Dashboards/OpenSurgSim")
+set(OSS_CXX_COMPILER "g++")
+set(OSS_CXX_COMPILER_VERSION "4.8")
+set(OSS_ARCHITECTURE "x86_64")
+set(OSS_GIT_REPOSITORY "git://git.assembla.com/OpenSurgSim.git")
+set(OSS_WITH_COVERAGE TRUE)
+set(OSS_COVERAGE_EXTRA_FLAGS "-g -fprofile-arcs -ftest-coverage")
+set(OSS_WITH_MEMCHECK TRUE)
+set(OSS_MEMORYCHECK_SUPPRESSIONS_FILE "${OSS_DASHBOARD_ROOT}/${OSS_SOURCE_NAME}/Tools/memcheck.supp")
+set(OSS_INITIAL_CACHE "")
+
+##############################################################################
+
+set(CTEST_BUILD_CONFIGURATION "${OSS_BUILD_CONFIGURATION}")
+set(CTEST_CMAKE_GENERATOR "${OSS_CMAKE_GENERATOR}")
+
+set(CTEST_SOURCE_NAME "${OSS_SOURCE_NAME}")
+set(CTEST_BINARY_NAME "${CTEST_SOURCE_NAME}-${CTEST_BUILD_CONFIGURATION}")
+
+set(CTEST_DASHBOARD_ROOT "${OSS_DASHBOARD_ROOT}")
+set(CTEST_SOURCE_DIRECTORY "${CTEST_DASHBOARD_ROOT}/${CTEST_SOURCE_NAME}")
+set(CTEST_BINARY_DIRECTORY "${CTEST_DASHBOARD_ROOT}/${CTEST_BINARY_NAME}")
+
+cmake_host_system_information(RESULT CTEST_SITE QUERY FQDN)
+set(CTEST_BUILD_NAME "${CMAKE_SYSTEM}-${OSS_CXX_COMPILER}-${OSS_CXX_COMPILER_VERSION}-${OSS_ARCHITECTURE}-${CTEST_BUILD_CONFIGURATION}")
+
+find_program(CTEST_GIT_COMMAND NAMES git)
+
+if(NOT EXISTS "${CTEST_SOURCE_DIRECTORY}")
+ set(CTEST_CHECKOUT_COMMAND "${CTEST_GIT_COMMAND} clone -b CTestCDash ${OSS_GIT_REPOSITORY} ${CTEST_SOURCE_DIRECTORY}")
+endif()
+
+set(CTEST_UPDATE_COMMAND "${CTEST_GIT_COMMAND}")
+
+if(NOT "${CTEST_SOURCE_DIRECTORY}" STREQUAL "${CTEST_BINARY_DIRECTORY}")
+ ctest_empty_binary_directory("${CTEST_BINARY_DIRECTORY}")
+endif()
+
+if(OSS_WITH_COVERAGE)
+ find_program(CTEST_COVERAGE_COMMAND NAMES gcov llvm-cov)
+
+ if(CTEST_COVERAGE_COMMAND)
+ set(OSS_INITIAL_CACHE "
+${OSS_INITIAL_CACHE}
+CMAKE_CXX_FLAGS:STRING=${OSS_COVERAGE_EXTRA_FLAGS} ${CMAKE_CXX_FLAGS}
+CMAKE_C_FLAGS:STRING=${OSS_COVERAGE_EXTRA_FLAGS} ${CMAKE_C_FLAGS}
+")
+ endif()
+endif()
+
+if(OSS_WITH_MEMCHECK)
+ find_program(CTEST_MEMORYCHECK_COMMAND NAMES valgrind purify)
+
+ if(CTEST_MEMORYCHECK_COMMAND)
+ set(CTEST_MEMORYCHECK_SUPPRESSIONS_FILE "${OSS_MEMORYCHECK_SUPPRESSIONS_FILE}")
+ endif()
+endif()
+
+file(WRITE "${CTEST_BINARY_DIRECTORY}/CMakeCache.txt" "${OSS_INITIAL_CACHE}")
+set(CTEST_NOTES_FILES "${CTEST_BINARY_DIRECTORY}/CMakeCache.txt")
+
+ctest_start(${OSS_BUILD_TYPE})
+
+message("-------------------- [ Update ] --------------------")
+ctest_update()
+
+message("-------------------- [ Configure ] --------------------")
+ctest_configure()
+ctest_submit(PARTS Update Configure)
+
+message("-------------------- [ Build ] --------------------")
+ctest_read_custom_files("${CTEST_BINARY_DIRECTORY}")
+ctest_build(APPEND)
+ctest_submit(PARTS Build)
+
+message("-------------------- [ Test ] --------------------")
+ctest_test(APPEND)
+ctest_submit(PARTS Test)
+
+if(OSS_WITH_COVERAGE AND CTEST_COVERAGE_COMMAND)
+ message("-------------------- [ Coverage ] --------------------")
+ ctest_coverage()
+ ctest_submit(PARTS Coverage)
+endif()
+
+if(OSS_WITH_MEMCHECK AND CTEST_MEMORYCHECK_COMMAND)
+ message("-------------------- [ MemCheck ] --------------------")
+ set(ENV{GTEST_DEATH_TEST_USE_FORK} 1)
+ ctest_memcheck()
+ ctest_submit(PARTS MemCheck)
+ unset(ENV{GTEST_DEATH_TEST_USE_FORK})
+endif()
+
+ctest_submit(PARTS Notes)
+message("-------------------- [ Done ] --------------------")
diff --git a/CMake/External_yamlcpp.cmake b/CMake/External_yamlcpp.cmake
new file mode 100644
index 0000000..d08e19a
--- /dev/null
+++ b/CMake/External_yamlcpp.cmake
@@ -0,0 +1,47 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+set(file_id "bAvdlSnfqr5ikadmr6bg7m")
+ExternalProject_Add(yaml-cpp
+ URL "https://www.assembla.com/spaces/OpenSurgSim/documents/${file_id}/download/yaml-cpp.tar.gz"
+ URL_MD5 "6bd2a7b4cc31ad0b65209a8030dda7ed"
+ PREFIX yaml-cpp
+ CMAKE_ARGS
+ -DCMAKE_INSTALL_PREFIX:PATH=<INSTALL_DIR>
+ -DYAML_CPP_BUILD_TOOLS:BOOL=OFF
+ -DBUILD_SHARED_LIBS:BOOL=${BUILD_SHARED_LIBS}
+)
+
+ExternalProject_Get_Property(yaml-cpp install_dir)
+
+if(MSVC)
+ set(YAML_CPP_LIBRARIES
+ debug ${install_dir}/lib/libyaml-cppmdd${CMAKE_STATIC_LIBRARY_SUFFIX}
+ optimized ${install_dir}/lib/libyaml-cppmd${CMAKE_STATIC_LIBRARY_SUFFIX}
+ CACHE INTERNAL "")
+else()
+ if(BUILD_SHARED_LIBS)
+ set(YAML_CPP_LIBRARIES
+ "${install_dir}/lib/${CMAKE_SHARED_LIBRARY_PREFIX}yaml-cpp${CMAKE_SHARED_LIBRARY_SUFFIX}"
+ CACHE INTERNAL "")
+ else()
+ set(YAML_CPP_LIBRARIES
+ "${install_dir}/lib/${CMAKE_STATIC_LIBRARY_PREFIX}yaml-cpp${CMAKE_STATIC_LIBRARY_SUFFIX}"
+ CACHE INTERNAL "")
+ endif()
+endif()
+
+set(YAML_CPP_INCLUDE_DIR "${install_dir}/include" CACHE INTERNAL "")
+
diff --git a/CMake/FindEigen3.cmake b/CMake/FindEigen3.cmake
new file mode 100644
index 0000000..1090f3c
--- /dev/null
+++ b/CMake/FindEigen3.cmake
@@ -0,0 +1,58 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+
+
+# - Try to find Eigen 3.x
+#
+# Once done this will define
+# EIGEN3_FOUND - system has eigen lib with correct version
+# EIGEN3_INCLUDE_DIR - the eigen include directory
+#
+# Rewritten from scratch to keep things simple and flexible.
+
+# Attempt to define EIGEN3_INCLUDE_DIR if undefined
+if(NOT EIGEN3_INCLUDE_DIR)
+ if(DEFINED ENV{EIGEN_DIR})
+ find_path(EIGEN3_INCLUDE_DIR
+ NAMES signature_of_eigen3_matrix_library
+ PATHS "$ENV{EIGEN_DIR}"
+ )
+ else(DEFINED ENV{EIGEN_DIR})
+ find_path(EIGEN3_INCLUDE_DIR
+ NAMES signature_of_eigen3_matrix_library
+ PATHS ${KDE4_INCLUDE_DIR} /usr/include
+ PATH_SUFFIXES eigen3 eigen
+ )
+ endif(DEFINED ENV{EIGEN_DIR})
+endif(NOT EIGEN3_INCLUDE_DIR)
+
+set(EIGEN3_MACROS_H "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h")
+
+if(EXISTS "${EIGEN3_MACROS_H}")
+ set(EIGEN3_MACROS_H_FOUND TRUE)
+else(EXISTS "${EIGEN3_MACROS_H}")
+ set(EIGEN3_MACROS_H_FOUND FALSE)
+endif(EXISTS "${EIGEN3_MACROS_H}")
+
+if(EIGEN3_MACROS_H_FOUND)
+ file(READ "${EIGEN3_MACROS_H}" _eigen3_version_header)
+
+ string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+ set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+
+ string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+
+ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+ set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+endif(EIGEN3_MACROS_H_FOUND)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Eigen3
+ REQUIRED_VARS EIGEN3_MACROS_H EIGEN3_MACROS_H_FOUND
+ VERSION_VAR EIGEN3_VERSION
+)
+
+mark_as_advanced(EIGEN3_INCLUDE_DIR)
diff --git a/CMake/FindGlutFromOsg.cmake b/CMake/FindGlutFromOsg.cmake
new file mode 100644
index 0000000..05b9677
--- /dev/null
+++ b/CMake/FindGlutFromOsg.cmake
@@ -0,0 +1,49 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+
+
+# - Try to find GLUT via OpenSceneGraph.
+#
+# Once done this will define
+# GLUT_FOUND - true if the system has GLUT
+# GLUT_INCLUDE_DIR - the GLUT include directory (not including the GL/)
+# GLUT_LIBRARIES - the GLUT libraries
+#
+# Written from scratch to keep things simple; but does not support OS
+# X or Solaris or anything else exciting. If you need support for
+# such things, you may want something like
+#
+# find_package(GlutFromOsg) # this file
+# find_package(GLUT) # the version that ships with CMake
+#
+# where several methods will be tried and the first successful one "wins".
+
+
+find_package(OpenSceneGraph)
+
+if(OPENSCENEGRAPH_FOUND)
+ find_path(GLUT_INCLUDE_DIR
+ NAMES GL/glut.h
+ PATHS ${OSG_INCLUDE_DIR}
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH)
+
+ osg_find_library(OSG_GLUT glut32)
+ if(OSG_GLUT_LIBRARIES)
+ set(GLUT_glut_LIBRARY ${OSG_GLUT_LIBRARIES}
+ CACHE FILEPATH "GLUT main library." FORCE)
+ endif(OSG_GLUT_LIBRARIES)
+endif(OPENSCENEGRAPH_FOUND)
+
+include(FindPackageHandleStandardArgs)
+# NB: use the same names here as FindGLUT.cmake, so you cam use both
+# interchangeably as described in the comments above. --advornik
+find_package_handle_standard_args(GLUT
+ DEFAULT_MSG GLUT_glut_LIBRARY GLUT_INCLUDE_DIR)
+
+if(GLUT_FOUND)
+ # What if GLUT needs additional libraries to work on this system?
+ # (I guess we can figure it out when we run across a system where it
+ # does; the GLUT implementations we use on Linux and Windows don't.)
+ set(GLUT_LIBRARIES ${GLUT_glut_LIBRARY})
+endif(GLUT_FOUND)
+mark_as_advanced(GLUT_LIBRARIES)
diff --git a/CMake/FindGoogleMock.cmake b/CMake/FindGoogleMock.cmake
new file mode 100644
index 0000000..810c08e
--- /dev/null
+++ b/CMake/FindGoogleMock.cmake
@@ -0,0 +1,34 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2014, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+# - Try to find the Google Mock Source directory
+#
+# Once done this will define
+# GOOGLEMOCK_FOUND
+# GOOGLEMOCK_DIR
+#
+
+if(NOT GOOGLEMOCK_DIR)
+ find_path(GOOGLEMOCK_DIR
+ NAMES src/gmock.cc
+ PATHS "$ENV{GOOGLEMOCK_DIR}" "/usr/src/gmock/"
+ )
+endif(NOT GOOGLEMOCK_DIR)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(GoogleMock DEFAULT_MSG GOOGLEMOCK_DIR)
+
+mark_as_advanced(GOOGLEMOCK_DIR)
diff --git a/CMake/FindLabJack.cmake b/CMake/FindLabJack.cmake
new file mode 100644
index 0000000..bc67fa9
--- /dev/null
+++ b/CMake/FindLabJack.cmake
@@ -0,0 +1,45 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+
+
+# - Try to find LabJack
+#
+# Once done this will define
+# LABJACK_FOUND - system has LabJack lib with correct version
+# LABJACK_INCLUDE_DIR - the LabJack include directory
+
+# Attempt to define LABJACK_INCLUDE_DIR if undefined
+find_path(LABJACK_INCLUDE_DIR
+ NAMES LabJackUD.h labjackusb.h
+ PATHS "$ENV{LABJACK_SDK}" "/usr/local" "/usr"
+ PATH_SUFFIXES "include"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+)
+
+if(LABJACK_INCLUDE_DIR)
+ set(LABJACK_ROOT_DIR ${LABJACK_INCLUDE_DIR})
+endif(LABJACK_INCLUDE_DIR)
+
+if(WIN32 AND CMAKE_CL_64)
+ find_library(LABJACK_LIBRARY
+ NAMES LabJackUD
+ PATHS ${LABJACK_ROOT_DIR}/64bit
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+endif(WIN32 AND CMAKE_CL_64)
+
+find_library(LABJACK_LIBRARY
+ NAMES LabJackUD labjackusb
+ HINTS ${LABJACK_ROOT_DIR} "/usr/local/lib" "/usr/lib"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+)
+mark_as_advanced(LABJACK_LIBRARY)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(LABJACK
+ DEFAULT_MSG LABJACK_ROOT_DIR LABJACK_INCLUDE_DIR
+ LABJACK_LIBRARY)
+
+if(LABJACK_FOUND)
+ set(LABJACK_LIBRARIES ${LABJACK_LIBRARY})
+endif(LABJACK_FOUND)
diff --git a/CMake/FindNovintHdalSdk.cmake b/CMake/FindNovintHdalSdk.cmake
new file mode 100644
index 0000000..9d604c1
--- /dev/null
+++ b/CMake/FindNovintHdalSdk.cmake
@@ -0,0 +1,116 @@
+# - Try to find the Novint HDAL SDK, used by the Novint Falcon haptic devices.
+#
+# Once done this will define
+# NOVINT_HDAL_SDK_FOUND - system has the Novint HDAL SDK directory
+# NOVINT_HDAL_SDK_INCLUDE_DIR - the Novint HDAL SDK include directory
+# NOVINT_HDAL_SDK_LIBRARIES - the Novint HDAL SDK libraries
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+
+
+# Cache settings and Novint HDAL SDK environment variables take
+# precedence, or we try to fall back to the default search.
+
+find_path(NOVINT_HDAL_SDK_INCLUDE_DIR
+ NAMES hdl/hdl.h
+ PATHS "$ENV{NOVINT_DEVICE_SUPPORT}"
+ PATH_SUFFIXES "include"
+ # hack for getting the HDAL from the SimQuest-only SutureSim source tree
+ "SurgTool2003/DeviceDrivers/hdal/include"
+ DOC "Path in which the file hdl/hdl.h is located. File is part of HDAL SDK."
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+)
+
+# TODO(advornik): Shake down some usual suspects under Windows/Linux?
+find_path(NOVINT_HDAL_SDK_INCLUDE_DIR
+ NAMES hdl/hdl.h
+ PATH_SUFFIXES "include"
+)
+
+if(NOVINT_HDAL_SDK_INCLUDE_DIR)
+ get_filename_component(PARENT_DIR "${NOVINT_HDAL_SDK_INCLUDE_DIR}" PATH)
+ set(NOVINT_HDAL_SDK_ROOT_DIRS "${PARENT_DIR}")
+
+ # include all parent directories, too
+ get_filename_component(PARENT_DIR "${PARENT_DIR}" PATH)
+ get_filename_component(NEXT_PARENT_DIR "${PARENT_DIR}" PATH)
+ while(NOT "${NEXT_PARENT_DIR}" STREQUAL "${PARENT_DIR}")
+ # use PARENT_DIR, nor NEXT_PARENT_DIR, to stay out of the root directory
+ list(APPEND NOVINT_HDAL_SDK_ROOT_DIRS "${PARENT_DIR}")
+ set(PARENT_DIR "${NEXT_PARENT_DIR}")
+ get_filename_component(NEXT_PARENT_DIR "${PARENT_DIR}" PATH)
+ endwhile()
+endif(NOVINT_HDAL_SDK_INCLUDE_DIR)
+
+# Look for (shared) libraries.
+set(LIB_SUFFIX "")
+if(WIN32 AND MSVC)
+ if(CMAKE_CL_64)
+ set(LIB_SUFFIX "${LIB_SUFFIX}_64")
+ endif()
+endif(WIN32 AND MSVC)
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ if("${CMAKE_SIZEOF_VOID_P}" STREQUAL "64")
+ set(LIB_SUFFIX "${LIB_SUFFIX}_64")
+ endif()
+endif(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+
+
+macro(novint_shared_from_link OUTPUT)
+ set(SHARED_LIST)
+ foreach(FILE ${ARGN})
+ if(WIN32)
+ string(REPLACE "/lib/" "/bin/" SHARED "${FILE}")
+ string(REPLACE ".lib" ".dll" SHARED "${SHARED}")
+ else()
+ set(SHARED "${FILE}")
+ endif()
+ if(EXISTS "${SHARED}")
+ list(APPEND SHARED_LIST "${SHARED}")
+ else()
+ message(SEND_ERROR "Could not find dynamic library for ${FILE}")
+ endif()
+ endforeach(FILE ${ARGN})
+ set(${OUTPUT} ${SHARED_LIST}
+ CACHE STRING "DLLs/SOs from the Novint HDAL SDK.")
+ mark_as_advanced(${OUTPUT})
+endmacro()
+
+macro(novint_find_library LIB_NAME)
+ find_library(NOVINT_HDAL_SDK_${LIB_NAME}_LIBRARY
+ NAMES "${LIB_NAME}${LIB_SUFFIX}"
+ HINTS ${NOVINT_HDAL_SDK_ROOT_DIRS}
+ PATH_SUFFIXES "lib"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+ find_library(NOVINT_HDAL_SDK_${LIB_NAME}_LIBRARY
+ NAMES "${LIB_NAME}${LIB_SUFFIX}"
+ PATH_SUFFIXES "lib"
+ )
+ mark_as_advanced(NOVINT_HDAL_SDK_${LIB_NAME}_LIBRARY)
+
+ if(NOVINT_HDAL_SDK_${LIB_NAME}_LIBRARY AND
+ NOT NOVINT_HDAL_SDK_${LIB_NAME}_SHARED)
+ novint_shared_from_link(NOVINT_HDAL_SDK_${LIB_NAME}_SHARED
+ "${NOVINT_HDAL_SDK_${LIB_NAME}_LIBRARY}")
+ endif()
+endmacro()
+
+novint_find_library(hdl)
+
+# In order to use a hardware device, the binary needs the HDAL shared
+# library (hdl.dll on Windows). The HDAL will in turn need to load
+# the hdal.ini configuration file, a number of auxiliary DLLs, .bin
+# files for the hardware type, etc. All of those will be found
+# automatically based on the NOVINT_DEVICE_SUPPORT environment
+# variable, so the HDAL library is the only thing we need to copy.
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Novint_HDAL_SDK
+ DEFAULT_MSG NOVINT_HDAL_SDK_INCLUDE_DIR NOVINT_HDAL_SDK_hdl_LIBRARY)
+
+if(NOVINT_HDAL_SDK_FOUND)
+ set(NOVINT_HDAL_SDK_LIBRARIES ${NOVINT_HDAL_SDK_hdl_LIBRARY})
+endif(NOVINT_HDAL_SDK_FOUND)
+mark_as_advanced(NOVINT_HDAL_SDK_LIBRARIES)
diff --git a/CMake/FindOpenHaptics.cmake b/CMake/FindOpenHaptics.cmake
new file mode 100644
index 0000000..86ab2e2
--- /dev/null
+++ b/CMake/FindOpenHaptics.cmake
@@ -0,0 +1,80 @@
+# - Try to find the Sensable OpenHaptics (specifically, the HDAPI portion).
+#
+# Once done this will define
+# OPENHAPTICS_FOUND - system has the OpenHaptics directory
+# OPENHAPTICS_INCLUDE_DIR - the OpenHaptics include directory
+# OPENHAPTICS_LIBRARIES - the OpenHaptics HDAPI libraries
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+
+
+# Cache settings and OpenHaptics environment variables take
+# precedence, or we try to fall back to the default search.
+
+find_path(OPENHAPTICS_INCLUDE_DIR
+ NAMES HD/hd.h
+ PATHS "$ENV{3DTOUCH_BASE}" "$ENV{OH_SDK_BASE}"
+ PATH_SUFFIXES "include"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+)
+find_path(OPENHAPTICS_INCLUDE_DIR
+ NAMES include/HD/hd.h
+ PATH_SUFFIXES "include"
+)
+
+
+set(LIB_ARCH "unknown")
+if(WIN32 AND MSVC)
+ if(NOT CMAKE_CL_64)
+ set(LIB_ARCH "win32")
+ else()
+ set(LIB_ARCH "x64")
+ endif()
+else(WIN32 AND MSVC)
+ if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ # On Linux, the installation puts things into /usr/lib64 or
+ # /usr/lib as appropriate.
+ # http://www.sensable.com/documents/documents/HW_userguide_Linux.pdf
+ # If you only have one library flavor installed, things should
+ # work out of the box. (If you have both, things get messy fast,
+ # and we don't have a Linux version of OpenHaptics to test against,
+ # so this is likely broken for now.)
+ set(LIB_ARCH "")
+ else(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ message(STATUS "Could not determine the OpenHaptics architecture!")
+ endif(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+endif(WIN32 AND MSVC)
+
+if(OPENHAPTICS_INCLUDE_DIR)
+ get_filename_component(OPENHAPTICS_ROOT_DIR
+ ${OPENHAPTICS_INCLUDE_DIR} PATH)
+endif(OPENHAPTICS_INCLUDE_DIR)
+
+
+find_library(OPENHAPTICS_HD_LIBRARY
+ NAMES HD # note: case doesn't matter on Windows, uppercase needed for Linux
+ HINTS "${OPENHAPTICS_ROOT_DIR}"
+ PATH_SUFFIXES "lib/${LIB_ARCH}" "lib"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+)
+find_library(OPENHAPTICS_HD_LIBRARY
+ NAMES HD
+ HINTS "${OPENHAPTICS_ROOT_DIR}"
+ PATH_SUFFIXES "lib/${LIB_ARCH}" "lib"
+)
+
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(OpenHaptics
+ DEFAULT_MSG OPENHAPTICS_ROOT_DIR OPENHAPTICS_INCLUDE_DIR
+ OPENHAPTICS_HD_LIBRARY)
+mark_as_advanced(OPENHAPTICS_INCLUDE_DIR)
+mark_as_advanced(OPENHAPTICS_HD_LIBRARY)
+
+
+# Big assumption: will be using the HDAPI, without the HDU utility library.
+if(OPENHAPTICS_FOUND)
+ set(OPENHAPTICS_LIBRARIES ${OPENHAPTICS_HD_LIBRARY})
+endif(OPENHAPTICS_FOUND)
+mark_as_advanced(OPENHAPTICS_LIBRARIES)
diff --git a/CMake/FindOptiTrack.cmake b/CMake/FindOptiTrack.cmake
new file mode 100644
index 0000000..469549a
--- /dev/null
+++ b/CMake/FindOptiTrack.cmake
@@ -0,0 +1,115 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+
+
+# - Try to find OptiTrack
+#
+# Once done this will define
+# OPTITRACK_FOUND - system has OptiTrack lib with correct version
+# OPTITRACK_INCLUDE_DIR - the OptiTrack include directory
+#
+# Rewritten from scratch to keep things simple and flexible.
+
+# Attempt to define OPTITRACK_INCLUDE_DIR if undefined
+find_path(OPTITRACK_INCLUDE_DIR
+ NAMES cameralibrary.h linuxtrack.h
+ PATHS "$ENV{NP_CAMERASDK}" "/opt/linuxtrack" "/usr/local"
+ PATH_SUFFIXES "include"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+)
+
+if(OPTITRACK_INCLUDE_DIR)
+ get_filename_component(OPTITRACK_ROOT_DIR
+ ${OPTITRACK_INCLUDE_DIR} PATH)
+endif(OPTITRACK_INCLUDE_DIR)
+
+macro(optitrack_shared_from_link OUTPUT)
+ set(SHARED_LIST)
+ foreach(FILE ${ARGN})
+ if(WIN32)
+ string(REPLACE ".lib" ".dll" SHARED "${FILE}")
+ else()
+ string(REPLACE ".a" ".so" SHARED "${FILE}")
+ endif()
+
+ if(EXISTS "${SHARED}")
+ list(APPEND SHARED_LIST "${SHARED}")
+ else()
+ message(SEND_ERROR "Could not find dynamic library for ${FILE}")
+ endif()
+ endforeach(FILE ${ARGN})
+
+ set(${OUTPUT} ${SHARED_LIST}
+ CACHE STRING "DLLs/SOs from the OptiTrack SDK.")
+ mark_as_advanced(${OUTPUT})
+endmacro()
+
+macro(optiTrack_find_library LIB_NAME)
+ if(WIN32)
+ if(CMAKE_CL_64)
+ set(LIB_SUFFIX "x64")
+ endif()
+ find_library(OPTITRACK_LIBRARY_RELEASE
+ NAMES "${LIB_NAME}2010${LIB_SUFFIX}S"
+ HINTS ${OPTITRACK_ROOT_DIR}
+ PATH_SUFFIXES "lib"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+ find_library(OPTITRACK_LIBRARY_DEBUG
+ NAMES "${LIB_NAME}2010${LIB_SUFFIX}D"
+ HINTS ${OPTITRACK_ROOT_DIR}
+ PATH_SUFFIXES "lib"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+ else()
+ find_library(OPTITRACK_LIBRARY_RELEASE
+ NAMES "${LIB_NAME}.a"
+ HINTS ${OPTITRACK_ROOT_DIR}
+ PATH_SUFFIXES "lib/linuxtrack"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+ find_library(OPTITRACK_LIBRARY_DEBUG
+ NAMES "${LIB_NAME}.a"
+ HINTS ${OPTITRACK_ROOT_DIR}
+ PATH_SUFFIXES "lib/linuxtrack"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+ endif()
+
+ if(OPTITRACK_LIBRARY_RELEASE AND
+ OPTITRACK_LIBRARY_DEBUG AND
+ NOT OPTITRACK_LIBRARY)
+ set(OPTITRACK_LIBRARY
+ optimized ${OPTITRACK_LIBRARY_RELEASE}
+ debug ${OPTITRACK_LIBRARY_DEBUG}
+ CACHE STRING "The ${LIB_NAME} library from the OptiTrack SDK.")
+ mark_as_advanced(OPTITRACK_LIBRARY)
+ endif()
+
+ if(OPTITRACK_LIBRARY_RELEASE AND
+ NOT OPTITRACK_SHARED_RELEASE)
+ optitrack_shared_from_link(OPTITRACK_SHARED_RELEASE
+ "${OPTITRACK_LIBRARY_RELEASE}")
+ endif()
+
+ if(OPTITRACK_LIBRARY_DEBUG AND
+ NOT OPTITRACK_SHARED_DEBUG)
+ optitrack_shared_from_link(OPTITRACK_SHARED_DEBUG
+ "${OPTITRACK_LIBRARY_DEBUG}")
+ endif()
+endmacro()
+
+if(WIN32)
+ optiTrack_find_library(cameralibrary)
+else()
+ optiTrack_find_library(liblinuxtrack)
+endif()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(OPTITRACK
+ DEFAULT_MSG OPTITRACK_ROOT_DIR OPTITRACK_INCLUDE_DIR
+ OPTITRACK_LIBRARY)
+
+if(OPTITRACK_FOUND)
+ set(OPTITRACK_LIBRARIES ${OPTITRACK_LIBRARY})
+endif(OPTITRACK_FOUND)
diff --git a/CMake/FindSixenseSdk.cmake b/CMake/FindSixenseSdk.cmake
new file mode 100644
index 0000000..f6aba21
--- /dev/null
+++ b/CMake/FindSixenseSdk.cmake
@@ -0,0 +1,190 @@
+# - Try to find the Sixense SDK, used by the Razer Hydra gaming controller.
+#
+# Once done this will define
+# SIXENSE_SDK_FOUND - system has the Sixense SDK directory
+# SIXENSE_SDK_INCLUDE_DIR - the Sixense SDK include directory
+# SIXENSE_SDK_LIBRARIES - the Sixense SDK libraries
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+
+
+# Cache settings and Sixense SDK environment variables take
+# precedence, or we try to fall back to the default search.
+
+find_path(SIXENSE_SDK_INCLUDE_DIR
+ NAMES sixense.h
+ PATHS "$ENV{SIXENSE_SDK_PATH}" "$ENV{SIXENSE_ROOT}"
+ PATH_SUFFIXES "include"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+)
+if(WIN32)
+ # Shake down some usual suspects under Windows.
+ #
+ # This is far from exhaustive-- Steam may not be installed in
+ # C:\Program Files, and even if it is, you can install apps into
+ # alternate locations. But this does covers the most common case.
+ #
+ # Alternately, we could look at examining the registry; on my
+ # machine, there's some interesting stuff in
+ # HKLM\SOFTWARE\Wow6432Node\Microsoft\Windows\CurrentVersion\Uninstall\Steam App 42300.
+ find_path(SIXENSE_SDK_INCLUDE_DIR
+ NAMES sixense.h
+ PATHS
+ "C:/Program Files (x86)/Steam/steamapps/common/sixense sdk/SixenseSDK"
+ "C:/Program Files/Steam/steamapps/common/sixense sdk/SixenseSDK"
+ PATH_SUFFIXES "include"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+endif(WIN32)
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ # Shake down some usual suspects under Linux.
+ # This is largely hypothetical.
+ find_path(SIXENSE_SDK_INCLUDE_DIR
+ NAMES sixense.h
+ PATHS
+ "$ENV{HOME}/.local/share/Steam/SteamApps/common/sixense sdk/SixenseSDK"
+ "/usr/local"
+ PATH_SUFFIXES "include"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+endif(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+find_path(SIXENSE_SDK_INCLUDE_DIR
+ NAMES sixense.h
+ PATH_SUFFIXES "include"
+)
+
+
+# Look for shared libraries. Static linking is not currently supported.
+set(LIB_SUFFIX "")
+#set(LIB_SUFFIX "_s")
+
+set(LIB_ARCH "unknown")
+set(LIB_SYSDIR "lib_unknown")
+if(WIN32 AND MSVC)
+ if(CMAKE_CL_64)
+ set(LIB_ARCH "x64")
+ set(LIB_SUFFIX "${LIB_SUFFIX}_x64")
+ else()
+ set(LIB_ARCH "win32")
+ endif()
+endif(WIN32 AND MSVC)
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ if("${CMAKE_SIZEOF_VOID_P}" STREQUAL "64")
+ set(LIB_ARCH "linux_x64")
+ set(LIB_SYSDIR "lib64")
+ set(LIB_SUFFIX "${LIB_SUFFIX}_x64")
+ else("${CMAKE_SIZEOF_VOID_P}" STREQUAL "64")
+ set(LIB_ARCH "linux")
+ set(LIB_SYSDIR "lib")
+ endif("${CMAKE_SIZEOF_VOID_P}" STREQUAL "64")
+endif(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+
+if(SIXENSE_SDK_INCLUDE_DIR)
+ get_filename_component(SIXENSE_SDK_ROOT_DIR
+ ${SIXENSE_SDK_INCLUDE_DIR} PATH)
+endif(SIXENSE_SDK_INCLUDE_DIR)
+
+
+macro(sixense_shared_from_link OUTPUT)
+ set(SHARED_LIST)
+ foreach(FILE ${ARGN})
+ if(WIN32)
+ string(REPLACE "/lib/" "/bin/" SHARED "${FILE}")
+ string(REPLACE ".lib" ".dll" SHARED "${SHARED}")
+ else()
+ set(SHARED "${FILE}")
+ endif()
+ if(EXISTS "${SHARED}")
+ list(APPEND SHARED_LIST "${SHARED}")
+ else()
+ message(SEND_ERROR "Could not find dynamic library for ${FILE}")
+ endif()
+ endforeach(FILE ${ARGN})
+ set(${OUTPUT} ${SHARED_LIST}
+ CACHE STRING "DLLs/SOs from the Sixense SDK.")
+ mark_as_advanced(${OUTPUT})
+endmacro()
+
+macro(sixense_find_library LIB_NAME)
+ find_library(SIXENSE_SDK_${LIB_NAME}_LIBRARY_RELEASE
+ NAMES ${LIB_NAME}${LIB_SUFFIX}
+ HINTS "${SIXENSE_SDK_ROOT_DIR}"
+ PATH_SUFFIXES "lib/${LIB_ARCH}/release_dll" "lib/${LIB_ARCH}/release"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+ find_library(SIXENSE_SDK_${LIB_NAME}_LIBRARY_RELEASE
+ NAMES ${LIB_NAME}${LIB_SUFFIX}
+ PATH_SUFFIXES "lib/${LIB_ARCH}/release_dll" "lib/${LIB_ARCH}/release"
+ "${LIB_SYSDIR}"
+ )
+
+ find_library(SIXENSE_SDK_${LIB_NAME}_LIBRARY_DEBUG
+ NAMES ${LIB_NAME}d${LIB_SUFFIX}
+ HINTS "${SIXENSE_SDK_ROOT_DIR}"
+ PATH_SUFFIXES "lib/${LIB_ARCH}/debug_dll" "lib/${LIB_ARCH}/debug"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+ find_library(SIXENSE_SDK_${LIB_NAME}_LIBRARY_DEBUG
+ NAMES ${LIB_NAME}d${LIB_SUFFIX}
+ PATH_SUFFIXES "lib/${LIB_ARCH}/debug_dll" "lib/${LIB_ARCH}/debug"
+ "${LIB_SYSDIR}"
+ )
+
+ if(SIXENSE_SDK_${LIB_NAME}_LIBRARY_RELEASE AND
+ SIXENSE_SDK_${LIB_NAME}_LIBRARY_DEBUG AND
+ NOT SIXENSE_SDK_${LIB_NAME}_LIBRARY)
+ set(SIXENSE_SDK_${LIB_NAME}_LIBRARY
+ optimized ${SIXENSE_SDK_${LIB_NAME}_LIBRARY_RELEASE}
+ debug ${SIXENSE_SDK_${LIB_NAME}_LIBRARY_DEBUG}
+ CACHE STRING "The ${LIB_NAME} library from the Sixense SDK.")
+ mark_as_advanced(SIXENSE_SDK_${LIB_NAME}_LIBRARY)
+ endif()
+
+ if(SIXENSE_SDK_${LIB_NAME}_LIBRARY_RELEASE AND
+ NOT SIXENSE_SDK_${LIB_NAME}_SHARED_RELEASE)
+ sixense_shared_from_link(SIXENSE_SDK_${LIB_NAME}_SHARED_RELEASE
+ "${SIXENSE_SDK_${LIB_NAME}_LIBRARY_RELEASE}")
+ endif()
+ if(SIXENSE_SDK_${LIB_NAME}_LIBRARY_DEBUG AND
+ NOT SIXENSE_SDK_${LIB_NAME}_SHARED_DEBUG)
+ sixense_shared_from_link(SIXENSE_SDK_${LIB_NAME}_SHARED_DEBUG
+ "${SIXENSE_SDK_${LIB_NAME}_LIBRARY_DEBUG}")
+ endif()
+endmacro()
+
+sixense_find_library(sixense)
+sixense_find_library(sixense_utils)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Sixense_SDK
+ DEFAULT_MSG SIXENSE_SDK_ROOT_DIR SIXENSE_SDK_INCLUDE_DIR
+ SIXENSE_SDK_sixense_LIBRARY SIXENSE_SDK_sixense_utils_LIBRARY)
+
+if(SIXENSE_SDK_FOUND)
+ set(SIXENSE_SDK_LIBRARIES
+ ${SIXENSE_SDK_sixense_LIBRARY} ${SIXENSE_SDK_sixense_utils_LIBRARY})
+endif(SIXENSE_SDK_FOUND)
+mark_as_advanced(SIXENSE_SDK_LIBRARIES)
+
+if(SIXENSE_SDK_FOUND)
+ # HACK: for debug, we may also need the mysterious DeviceDLL.dll on Windows.
+ if(WIN32)
+ find_file(SIXENSE_SDK_DeviceDLL_SHARED_DEBUG
+ NAMES DeviceDLL.dll
+ HINTS "${SIXENSE_SDK_ROOT_DIR}"
+ PATH_SUFFIXES "samples/${LIB_ARCH}/sixense_simple3d"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+ if(NOT SIXENSE_SDK_DeviceDLL_SHARED_DEBUG)
+ # if not found, clear it and hope for the best...
+ message("Warning: DeviceDLL.dll (from Sixense) not found; continuing.")
+ set(SIXENSE_SDK_DeviceDLL_SHARED_DEBUG
+ CACHE PATH "Path to DeviceDLL, if any." FORCE)
+ endif(NOT SIXENSE_SDK_DeviceDLL_SHARED_DEBUG)
+ else()
+ set(SIXENSE_SDK_DeviceDLL_SHARED_DEBUG
+ CACHE PATH "Path to DeviceDLL, if any.")
+ mark_as_advanced(SIXENSE_SDK_DeviceDLL_SHARED_DEBUG)
+ endif()
+endif(SIXENSE_SDK_FOUND)
diff --git a/CMake/FindWDK.cmake b/CMake/FindWDK.cmake
new file mode 100644
index 0000000..660e636
--- /dev/null
+++ b/CMake/FindWDK.cmake
@@ -0,0 +1,115 @@
+# - Try to find the Windows Driver Kit (WDK, formerly WinDDK).
+#
+# Once done this will define
+# WDK_FOUND - true if the system has WDK
+# WDK_INCLUDE_DIR - the WDK include directory or set to "." if no include directory is needed
+# WDK_LIBRARIES - the WDK libraries
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+
+
+# We try finding the necessary files in the compiler's default include
+# and library paths. If not found, then cache settings and WinDDK
+# environment variables take precedence, or we try to fall back to the
+# default search.
+
+if(NOT DEFINED WDK_CAN_BUILD_DIRECTLY)
+ # Note: check_include_files will NOT find hidsdi.h, even if it's
+ # present in the compiler's default search path! Sigh.
+ try_compile(WDK_CAN_BUILD_DIRECTLY
+ ${CMAKE_BINARY_DIR}/CMakeFiles/tryCompileWdk
+ ${CMAKE_SOURCE_DIR}/CMake/tryCompileWdk tryCompileWdk
+ OUTPUT_VARIABLE dbg_tryCompileWdk
+ )
+ #message("OUT:${dbg_tryCompileWdk}")
+endif(NOT DEFINED WDK_CAN_BUILD_DIRECTLY)
+
+if(WDK_CAN_BUILD_DIRECTLY)
+ # No include directory is needed. Set it to . for convenience in other files.
+ set(WDK_INCLUDE_DIR . CACHE PATH "Include directory for the WDK.")
+ set(WDK_location provided_by_compiler)
+else()
+ # Find the include directory.
+ find_path(WDK_INCLUDE_DIR
+ NAMES hidsdi.h
+ PATHS "$ENV{WINDDK_PATH}" "$ENV{WINDDK_ROOT}" "$ENV{WDK_PATH}"
+ PATH_SUFFIXES inc/api
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+ find_path(WDK_INCLUDE_DIR
+ NAMES hidsdi.h
+ PATH_SUFFIXES inc/api
+ )
+ set(WDK_location ${WDK_INCLUDE_DIR})
+ if(WDK_INCLUDE_DIR)
+ get_filename_component(WDK_DIR_UP1
+ ${WDK_INCLUDE_DIR} PATH)
+ get_filename_component(WDK_ROOT_DIR
+ ${WDK_DIR_UP1} PATH)
+ endif(WDK_INCLUDE_DIR)
+endif()
+mark_as_advanced(WDK_INCLUDE_DIR)
+
+# magical WinDDK system subdirectory names
+set(WDK710_os_winXP "wxp")
+set(WDK710_os_server2003 "wnet")
+set(WDK710_os_winVista "wlh")
+set(WDK710_os_server2008 "wlh")
+set(WDK710_os_win7 "win7")
+
+set(WDK710_LIB_ARCH "unknown")
+if(CMAKE_CL_64)
+ set(WDK710_LIB_ARCH "amd64")
+else()
+ set(WDK710_LIB_ARCH "i386")
+endif()
+
+
+
+macro(winddk_find_library LIB_NAME)
+ if(WDK_CAN_BUILD_DIRECTLY)
+ # just assume we can get at this library.
+ # Note: find_library and check_library_exists will NOT find the
+ # WDK 8 libraries that are present in the compiler's default
+ # search path! Sigh.
+ set(WDK_${LIB_NAME}_LIBRARY ${LIB_NAME}
+ CACHE FILEPATH "Library for the WDK.")
+ else()
+ # find the library in the oldest available OS version directory
+ find_library(WDK_${LIB_NAME}_LIBRARY
+ NAMES ${LIB_NAME}
+ HINTS "${WDK_ROOT_DIR}"
+ PATH_SUFFIXES "lib/${WDK710_os_winXP}/${WDK710_LIB_ARCH}"
+ "lib/${WDK710_os_server2003}/${WDK710_LIB_ARCH}"
+ "lib/${WDK710_os_winVista}/${WDK710_LIB_ARCH}"
+ "lib/${WDK710_os_server2008}/${WDK710_LIB_ARCH}"
+ "lib/${WDK710_os_win7}/${WDK710_LIB_ARCH}"
+ NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
+ )
+ find_library(WDK_${LIB_NAME}_LIBRARY
+ NAMES ${LIB_NAME}
+ HINTS "${WDK_ROOT_DIR}"
+ PATH_SUFFIXES "lib/${WDK710_os_winXP}/${WDK710_LIB_ARCH}"
+ "lib/${WDK710_os_server2003}/${WDK710_LIB_ARCH}"
+ "lib/${WDK710_os_winVista}/${WDK710_LIB_ARCH}"
+ "lib/${WDK710_os_server2008}/${WDK710_LIB_ARCH}"
+ "lib/${WDK710_os_win7}/${WDK710_LIB_ARCH}"
+ )
+ endif()
+ mark_as_advanced(WDK_${LIB_NAME}_LIBRARY)
+endmacro()
+
+winddk_find_library(hid)
+winddk_find_library(setupapi)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(WDK
+ DEFAULT_MSG WDK_location
+ WDK_INCLUDE_DIR WDK_hid_LIBRARY WDK_setupapi_LIBRARY)
+
+if(WDK_FOUND)
+ set(WDK_LIBRARIES
+ ${WDK_hid_LIBRARY} ${WDK_setupapi_LIBRARY})
+endif(WDK_FOUND)
+mark_as_advanced(WDK_LIBRARIES)
diff --git a/CMake/OpenSurgSimConfig.cmake.in b/CMake/OpenSurgSimConfig.cmake.in
new file mode 100644
index 0000000..565b1c7
--- /dev/null
+++ b/CMake/OpenSurgSimConfig.cmake.in
@@ -0,0 +1,18 @@
+# - Config file for the OpenSurgSim package
+# It defines the following variables
+# OPENSURGSIM_INCLUDE_DIRS - include directories
+# OPENSURGSIM_LIBRARIES - libraries to link against
+# OPENSURGSIM_DATA_DIR - directory to use for default data
+
+# Compute paths
+get_filename_component(OPENSURGSIM_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+set(OPENSURGSIM_INCLUDE_DIRS "@CONFIG_INCLUDE_DIRS@")
+
+# Our library dependencies (contains definitions for IMPORTED targets)
+include("${OPENSURGSIM_CMAKE_DIR}/OpenSurgSimTargets.cmake")
+
+# These are IMPORTED targets created by OpenSurgSimTargets.cmake
+set(OPENSURGSIM_LIBRARIES "@EXPORT_TARGETS@")
+
+# This is the default data directory
+set(OPENSURGSIM_DATA_DIR "@CONFIG_DATA_DIR@")
\ No newline at end of file
diff --git a/CMake/OpenSurgSimConfigVersion.cmake.in b/CMake/OpenSurgSimConfigVersion.cmake.in
new file mode 100644
index 0000000..c55f5d8
--- /dev/null
+++ b/CMake/OpenSurgSimConfigVersion.cmake.in
@@ -0,0 +1,11 @@
+set(PACKAGE_VERSION "@OPENSURGSIM_VERSION@")
+
+# Check whether the requested PACKAGE_FIND_VERSION is compatible
+if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
+ set(PACKAGE_VERSION_COMPATIBLE FALSE)
+else()
+ set(PACKAGE_VERSION_COMPATIBLE TRUE)
+ if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}")
+ set(PACKAGE_VERSION_EXACT TRUE)
+ endif()
+endif()
diff --git a/CMake/SurgSimBuildFlags.cmake b/CMake/SurgSimBuildFlags.cmake
new file mode 100644
index 0000000..9de6014
--- /dev/null
+++ b/CMake/SurgSimBuildFlags.cmake
@@ -0,0 +1,145 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+## Set build flags through CMake.
+## Splitting this out removes excessive verbiage from CMakeLists.txt.
+
+# If no build type is specified, default to "Release".
+# Note that this does nothing for VS and the like (but no harm either).
+if("${CMAKE_BUILD_TYPE}" STREQUAL "")
+ set(CMAKE_BUILD_TYPE "Release")
+endif("${CMAKE_BUILD_TYPE}" STREQUAL "")
+
+# We always want to use defines from <math.h>.
+if(MSVC)
+ add_definitions( -D_USE_MATH_DEFINES )
+endif()
+
+# Define our own debug symbol
+set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -DOSS_DEBUG")
+
+option(SURGSIM_WARNINGS_AS_ERRORS "Treat warnings as errors in the compilation process" OFF)
+
+# G++ (C++ compilation) specific settings
+if(CMAKE_COMPILER_IS_GNUCXX)
+ # default G++ compilation flags
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall ")
+
+ if (SURGSIM_WARNINGS_AS_ERRORS)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror")
+ endif()
+
+ # Enable support for C++0x/C++11 for G++ if available
+ include(CheckCXXCompilerFlag)
+ check_cxx_compiler_flag(-std=gnu++11 HAVE_FLAG_STD_GNUXX11)
+ if(HAVE_FLAG_STD_GNUXX11)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11")
+ else(HAVE_FLAG_STD_GNUXX11)
+ check_cxx_compiler_flag(-std=gnu++0x HAVE_FLAG_STD_GNUXX0X)
+ if(HAVE_FLAG_STD_GNUXX0X)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++0x")
+ else(HAVE_FLAG_STD_GNUXX0X)
+ message(WARNING "G++ is missing C++0x/C++11 support; trying anyway.")
+ endif(HAVE_FLAG_STD_GNUXX0X)
+ endif(HAVE_FLAG_STD_GNUXX11)
+
+endif(CMAKE_COMPILER_IS_GNUCXX)
+
+# GCC (C compilation) specific settings
+if(CMAKE_COMPILER_IS_GNUCC) # "CC"? really? sigh.
+ # default GCC compilation flags
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall")
+
+ if (SURGSIM_WARNINGS_AS_ERRORS)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror")
+ endif(SURGSIM_WARNINGS_AS_ERRORS)
+
+endif(CMAKE_COMPILER_IS_GNUCC)
+
+# Visual Studio C/C++ specific settings
+if(MSVC)
+ # Sanity check the version. (You can see this problem if you delete
+ # the cache file on disk while cmake-gui has the data in memory.)
+ if("${MSVC_VERSION}" STREQUAL "")
+ message(FATAL_ERROR
+ "MSVC_VERSION isn't correctly set; delete the cache and try again!")
+ endif("${MSVC_VERSION}" STREQUAL "")
+
+ # default VC++ compilation flags
+ add_definitions( -D_CRT_SECURE_NO_WARNINGS )
+ add_definitions( -D_SCL_SECURE_NO_WARNINGS )
+
+ # Set the iterator debug level consistently for Debug builds.
+ # [Note that you can't add_definitions() conditionally for debug only,
+ # because for VS add_definitions() affects ALL build types in the project.
+ # But this works, and gets put in the proper place in the project.]
+ set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_ITERATOR_DEBUG_LEVEL=2")
+ # Enable parallel builds:
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP")
+
+ if (SURGSIM_WARNINGS_AS_ERRORS)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /WX")
+ endif(SURGSIM_WARNINGS_AS_ERRORS)
+
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP") # is this needed?
+ set(CMAKE_DEBUG_POSTFIX "d")
+
+ # Work around a stupid template argument limitation in VS 2012
+ if(MSVC_VERSION EQUAL 1700)
+ add_definitions( -D_VARIADIC_MAX=10 )
+ endif(MSVC_VERSION EQUAL 1700)
+
+ if(BUILD_SHARED_LIBS)
+ message(FATAL_ERROR "Please turn off BUILD_SHARED_LIBS. Shared libraries on Windows is currently unsupported.")
+ endif()
+endif(MSVC)
+
+# Settings for LLVM Clang.
+if("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang")
+ if(APPLE)
+ set(CMAKE_XCODE_ATTRIBUTE_CLANG_CXX_LANGUAGE_STANDARD "c++11")
+ set(CMAKE_XCODE_ATTRIBUTE_CLANG_CXX_LIBRARY "libc++")
+ endif()
+
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11 -stdlib=libc++")
+endif()
+
+# Windows-specific settings
+if(WIN32)
+ add_definitions( -D_WIN32_WINNT=0x0501 ) # request compatibility with WinXP
+endif(WIN32)
+
+# Linux-specific settings
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ add_definitions( -D_POSIX_C_SOURCE=200809L ) # request POSIX APIs
+endif(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+
+set(DEFAULT_EIGEN_ALIGNMENT OFF)
+# Enable alignement on linux by default
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ set(DEFAULT_EIGEN_ALIGNMENT ON)
+endif()
+
+# Enable alignement on Windows 64bit by default
+if(${CMAKE_CL_64})
+ set(DEFAULT_EIGEN_ALIGNMENT ON)
+endif()
+
+option(EIGEN_ALIGNMENT "Enable alignment in Eigen" ${DEFAULT_EIGEN_ALIGNMENT})
+mark_as_advanced(EIGEN_ALIGNMENT)
+if(NOT EIGEN_ALIGNMENT)
+ add_definitions( -DEIGEN_DONT_ALIGN )
+endif()
diff --git a/CMake/SurgSimUtilities.cmake b/CMake/SurgSimUtilities.cmake
new file mode 100644
index 0000000..09a61f9
--- /dev/null
+++ b/CMake/SurgSimUtilities.cmake
@@ -0,0 +1,269 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+## Options related to integration with Visual Studio and other IDEs.
+
+# options related to source file organization
+#
+option(SURGSIM_IDE_SEPARATE_SOURCES_HEADERS
+ "If turned on, separate header files from the source files in the IDE. " OFF)
+if(SURGSIM_IDE_SEPARATE_SOURCES_HEADERS)
+ set(SURGSIM_IDE_SOURCE_PREFIX "Source Files/")
+ set(SURGSIM_IDE_HEADER_PREFIX "Header Files/")
+else()
+ set(SURGSIM_IDE_SOURCE_PREFIX "Files/")
+ set(SURGSIM_IDE_HEADER_PREFIX "Files/")
+endif()
+
+# Sets up the source file organization, for Visual Studio etc.
+# This will group the source and header files in folders by directory.
+#
+# Note the use of optional arguments:
+# surgsim_set_source_groups(PREFIX FILES...)
+#
+function(surgsim_source_hierarchy PREFIX)
+ foreach(FILE ${ARGN})
+ # Get the directory name. Note that for files in the current
+ # directory (and empty prefix), you will get "" rather than ".".
+ get_filename_component(FILE_PREFIXED_DIR "${PREFIX}${FILE}" PATH)
+ # Apparently, the hierarchical separator is "\" (per CMake docs).
+ # Hopefully that works in Linux IDEs too...
+ string(REGEX REPLACE "/" "\\\\" FILE_GROUP "${FILE_PREFIXED_DIR}")
+ # Set the IDE source group. Note that top level is "", not ".".
+ source_group("${FILE_GROUP}" FILES "${FILE}")
+ #message("SRC ${FILE}|${FILE_GROUP}")
+ endforeach(FILE ${ARGN})
+endfunction()
+
+# Sets up the folder organization for sources and headers in Visual Studio etc.
+function(surgsim_show_ide_folders SOURCES HEADERS)
+ surgsim_source_hierarchy("${SURGSIM_IDE_SOURCE_PREFIX}" ${SOURCES})
+ surgsim_source_hierarchy("${SURGSIM_IDE_HEADER_PREFIX}" ${HEADERS})
+endfunction()
+
+set(SURGSIM_COPY_WARNING_ONCE TRUE)
+
+# Copy zero or more files to the location of a built target, after the
+# target is built successfully, but only if the condition (which
+# should be a add_custom_command "generator expression") evaluates to 1.
+#
+# Note the use of optional arguments:
+# surgsim_copy_to_target_directory_if(<condition> <target> [<file>...])
+#
+function(surgsim_copy_to_target_directory_if CONDITION TARGET)
+ foreach(FILE ${ARGN})
+ if(NOT "${FILE}" STREQUAL "")
+ # After much experimentation (and cmake-gui crashes), I learned
+ # that as of CMake 2.8.10, you can't have line breaks or
+ # multiple tokens within the body of a $<{0,1}:...> generator
+ # expression. So to conditionalize the command "foo bar", you
+ # can't do "$<COND:foo bar>"; you need "$<COND:foo> $<COND:bar>"...
+ # And on top of that, we can't use VERBATIM or each of the
+ # conditional tokens will become "" rather than being skipped
+ # altogether.
+ get_filename_component(FNAME "${FILE}" NAME)
+ set(CMD_COPY copy_if_different "${FILE}" $<TARGET_FILE_DIR:${TARGET}>)
+ set(CMD_SKIP echo "${TARGET}: Not copying ${FNAME} for $<CONFIGURATION>")
+ # Build a list of properly conditionalized list elements.
+ set(CONDITIONAL_COMMAND ${CMAKE_COMMAND} -E)
+ if(CMAKE_VERSION VERSION_LESS 2.8.10)
+ if(SURGSIM_COPY_WARNING_ONCE)
+ # Only show the message once *per directory*.
+ set(SURGSIM_COPY_WARNING_ONCE FALSE)
+ message("Will copy target libraries unconditionally. Please consider upgrading to CMake 2.8.10 or later.")
+ endif(SURGSIM_COPY_WARNING_ONCE)
+ list(APPEND CONDITIONAL_COMMAND ${CMD_COPY})
+ else()
+ foreach(TOKEN ${CMD_COPY})
+ list(APPEND CONDITIONAL_COMMAND $<${CONDITION}:${TOKEN}>)
+ endforeach(TOKEN ${CMD_COPY})
+ foreach(TOKEN ${CMD_SKIP})
+ list(APPEND CONDITIONAL_COMMAND $<$<NOT:${CONDITION}>:${TOKEN}>)
+ endforeach(TOKEN ${CMD_SKIP})
+ endif()
+ # Now use the conditional command we built. (Can't use VERBATIM!)
+ add_custom_command(TARGET ${TARGET} POST_BUILD
+ COMMAND ${CONDITIONAL_COMMAND})
+ endif(NOT "${FILE}" STREQUAL "")
+ endforeach(FILE ${ARGN})
+endfunction()
+
+# Copy zero or more files to the location of a built target, after the
+# target is built successfully.
+#
+# Note the use of optional arguments:
+# surgsim_copy_to_target_directory(<target> [<file>...])
+#
+macro(surgsim_copy_to_target_directory TARGET)
+ surgsim_copy_to_target_directory_if(1 "${TARGET}" ${ARGN})
+endmacro()
+
+# Copy zero or more files to the location of a built *Debug* target,
+# after the target is built successfully.
+#
+# Note the use of optional arguments:
+# surgsim_copy_to_target_directory_for_debug(<target> [<file>...])
+#
+macro(surgsim_copy_to_target_directory_for_debug TARGET)
+ surgsim_copy_to_target_directory_if($<CONFIG:Debug> "${TARGET}" ${ARGN})
+endmacro()
+
+# Copy zero or more files to the location of a built *non-Debug*
+# target, after the target is built successfully.
+#
+# Note the use of optional arguments:
+# surgsim_copy_to_target_directory_for_release(<target> [<file>...])
+#
+macro(surgsim_copy_to_target_directory_for_release TARGET)
+ surgsim_copy_to_target_directory_if($<NOT:$<CONFIG:Debug>>
+ "${TARGET}" ${ARGN})
+endmacro()
+
+option(SURGSIM_RUN_TEST_WITHIN_BUILD "This exectutes the tests directly from the chosen build system." OFF)
+# Build the unit test executable.
+# Uses UNIT_TEST_SOURCES, UNIT_TEST_HEADERS, LIBS, UNIT_TEST_SHARED_LIBS,
+# UNIT_TEST_SHARED_RELEASE_LIBS and UNIT_TEST_SHARED_DEBUG_LIBS.
+#
+macro(surgsim_add_unit_tests TESTNAME)
+ add_executable(${TESTNAME} ${UNIT_TEST_SOURCES} ${UNIT_TEST_HEADERS})
+ target_link_libraries(${TESTNAME} SurgSimTesting gmock ${LIBS})
+ add_test(NAME ${TESTNAME} COMMAND ${TESTNAME} "--gtest_output=xml")
+
+ if(SURGSIM_RUN_TEST_WITHIN_BUILD)
+ add_custom_command(TARGET ${TESTNAME} POST_BUILD
+ COMMAND ${SURGSIM_TEST_RUN_PREFIX} $<TARGET_FILE:${TESTNAME}>
+ ${SURGSIM_TEST_RUN_SUFFIX}
+ VERBATIM)
+ endif()
+
+
+ # copy all ${UNIT_TEST_SHARED..._LIBS} to the test executable directory:
+ surgsim_copy_to_target_directory(${TESTNAME}
+ ${UNIT_TEST_SHARED_LIBS})
+ surgsim_copy_to_target_directory_for_release(${TESTNAME}
+ ${UNIT_TEST_SHARED_RELEASE_LIBS})
+ surgsim_copy_to_target_directory_for_debug(${TESTNAME}
+ ${UNIT_TEST_SHARED_DEBUG_LIBS})
+ surgsim_show_ide_folders("${UNIT_TEST_SOURCES}" "${UNIT_TEST_HEADERS}")
+endmacro()
+
+# Do all the work to add a library to the system
+# Works with the install system and detects whether the library is
+# header only or has source files, for header only the headers are copied into
+# the appropriate directory.
+# Note that when calling this the parameters should be quoted to separate lists
+function(surgsim_add_library LIBRARY_NAME SOURCE_FILES HEADER_FILES HEADER_DIRECTORY)
+ if (SOURCE_FILES)
+ add_library(${LIBRARY_NAME} ${SOURCE_FILES} ${HEADER_FILES})
+
+ set_target_properties(${LIBRARY_NAME} PROPERTIES PUBLIC_HEADER "${HEADER_FILES}")
+ install(TARGETS ${LIBRARY_NAME}
+ EXPORT ${PROJECT_NAME}Targets
+ RUNTIME DESTINATION "${INSTALL_BIN_DIR}"
+ LIBRARY DESTINATION "${INSTALL_LIB_DIR}"
+ ARCHIVE DESTINATION "${INSTALL_LIB_DIR}"
+ PUBLIC_HEADER DESTINATION ${INSTALL_INCLUDE_DIR}/${HEADER_DIRECTORY})
+
+ set(EXPORT_TARGETS ${LIBRARY_NAME} ${EXPORT_TARGETS} CACHE INTERNAL "export targets")
+ surgsim_show_ide_folders("${SOURCE_FILES}" "${HEADER_FILES}")
+ else()
+ install(FILES ${HEADER_FILES} DESTINATION ${INSTALL_INCLUDE_DIR}/${HEADER_DIRECTORY})
+ surgsim_show_ide_folders("" "${HEADER_FILES}")
+ endif()
+endfunction()
+
+
+## CMake support for running Google's cpplint over the C++ source.
+
+# Should we bother with cpplint at all?
+option(SURGSIM_CPPLINT "Include cpplint in the build" ON)
+
+# Running cpplint requires Python
+if(SURGSIM_CPPLINT)
+ find_package(PythonInterp)
+ if(NOT PYTHON_EXECUTABLE)
+ set(SURGSIM_CPPLINT OFF)
+ endif(NOT PYTHON_EXECUTABLE)
+endif(SURGSIM_CPPLINT)
+
+# Extra flags to pass to run-lint
+set(RUNLINT_DEFAULT_EXTRA_FLAGS)
+if(MSVC)
+ set(RUNLINT_DEFAULT_EXTRA_FLAGS --vs)
+endif()
+
+set(SURGSIM_RUNLINT_EXTRA_FLAGS
+ ${RUNLINT_DEFAULT_EXTRA_FLAGS}
+ CACHE STRING "Extra flags to pass to run-lint."
+)
+mark_as_advanced(SURGSIM_RUNLINT_EXTRA_FLAGS)
+
+# Default filter settings for cpplint
+set(CPPLINT_DEFAULT_FILTER_LIST
+ # our coding standards differ from Google's when it comes to whitespace:
+ -whitespace/tab -whitespace/braces -whitespace/line_length
+ -whitespace/ending_newline
+ # these flag some useful stuff, but also currently produce a lot of noise:
+ -whitespace/operators -whitespace/newline -whitespace/blank_line
+ -whitespace/comma -whitespace/parens -whitespace/comments
+ # this is just broken if a comment is preceded by a tab and ends in ":":
+ -whitespace/labels
+ # our preferred include guard format differs from Google's:
+ -build/header_guard
+ # potentially useful, but generates a lot of noise:
+ -build/include_what_you_use
+ # potentially useful, but generates some crazy false positives
+ # (it claims <unordered_map> is a C header!?):
+ -build/include_order
+ # things disallowed by Google's coding standards, but not ours:
+ -runtime/rtti -readability/streams
+)
+string(REPLACE ";" "," CPPLINT_DEFAULT_FILTERS
+ "--filter=${CPPLINT_DEFAULT_FILTER_LIST}")
+
+set(SURGSIM_CPPLINT_FILTERS
+ "${CPPLINT_DEFAULT_FILTERS}"
+ CACHE STRING "Filter settings to pass to cpplint via run-cpplint."
+)
+mark_as_advanced(SURGSIM_CPPLINT_FILTERS)
+
+
+# Run cpplint (from Google's coding standards project) on specified files.
+#
+# Note the use of optional arguments:
+# surgsim_run_cpplint(<testname> [<file|arg>...])
+#
+macro(surgsim_run_cpplint TARGET)
+ if(SURGSIM_CPPLINT AND PYTHON_EXECUTABLE)
+ add_custom_target("${TARGET}"
+ ${PYTHON_EXECUTABLE}
+ ${SURGSIM_TOOLS_DIR}/run-lint.py
+ --cpplint-script
+ ${SURGSIM_THIRD_PARTY_DIR}/google-style-lint/cpplint.py
+ ${SURGSIM_CPPLINT_FILTERS} ${SURGSIM_RUNLINT_EXTRA_FLAGS} ${ARGN}
+ WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+ COMMENT "Checking C++ sources with cpplint" VERBATIM)
+ endif(SURGSIM_CPPLINT AND PYTHON_EXECUTABLE)
+endmacro()
+
+# Run cpplint (from Google's coding standards project) on the source
+# files in the current directory and all of its subdirectories.
+macro(surgsim_cpplint_this_tree TARGET)
+ if(SURGSIM_CPPLINT AND PYTHON_EXECUTABLE)
+ surgsim_run_cpplint("${TARGET}" "--traverse" "${CMAKE_CURRENT_SOURCE_DIR}")
+ endif(SURGSIM_CPPLINT AND PYTHON_EXECUTABLE)
+endmacro()
+
diff --git a/CMake/tryCompileWdk/CMakeLists.txt b/CMake/tryCompileWdk/CMakeLists.txt
new file mode 100644
index 0000000..0b8cda1
--- /dev/null
+++ b/CMake/tryCompileWdk/CMakeLists.txt
@@ -0,0 +1,23 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+project(tryCompileWdk)
+set(TRYCOMPILEWDK_SOURCE
+ main.cpp
+)
+add_executable(tryCompileWdk
+ ${TRYCOMPILEWDK_SOURCE}
+)
+target_link_libraries(tryCompileWdk hid setupapi)
diff --git a/CMake/tryCompileWdk/main.cpp b/CMake/tryCompileWdk/main.cpp
new file mode 100644
index 0000000..3ce5a12
--- /dev/null
+++ b/CMake/tryCompileWdk/main.cpp
@@ -0,0 +1,33 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#undef _WIN32_WINNT
+#define _WIN32_WINNT 0x0501 // request Windows XP-compatible SDK APIs
+#undef WIN32_LEAN_AND_MEAN
+#define WIN32_LEAN_AND_MEAN // do not automatically include WinSock 1 and some other header files
+#include <windows.h>
+
+#include <setupapi.h>
+extern "C" { // sigh...
+#include <hidsdi.h>
+}
+
+
+int main(int argc, char** argv)
+{
+ GUID hidGuid;
+ HidD_GetHidGuid(&hidGuid);
+ return 0;
+}
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..875d89d
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,168 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+cmake_minimum_required(VERSION 2.8)
+project(OpenSurgSim)
+set_property(GLOBAL PROPERTY USE_FOLDERS ON)
+
+set(OPENSURGSIM_MAJOR_VERSION 0)
+set(OPENSURGSIM_MINOR_VERSION 0)
+set(OPENSURGSIM_PATCH_VERSION 0)
+set(OPENSURGSIM_VERSION
+ ${OPENSURGSIM_MAJOR_VERSION}.${OPENSURGSIM_MINOR_VERSION}.${OPENSURGSIM_PATCH_VERSION})
+
+# Install Directories
+set(INSTALL_LIB_DIR ${CMAKE_INSTALL_PREFIX}/lib)
+set(INSTALL_BIN_DIR ${CMAKE_INSTALL_PREFIX}/bin)
+set(INSTALL_INCLUDE_DIR ${CMAKE_INSTALL_PREFIX}/include)
+if(WIN32 AND NOT CYGWIN)
+ set(INSTALL_CMAKE_DIR ${CMAKE_INSTALL_PREFIX}/CMake)
+else()
+ set(INSTALL_CMAKE_DIR ${CMAKE_INSTALL_PREFIX}/lib/cmake/${PROJECT_NAME})
+endif()
+
+# Use SURGSIM_SOURCE_DIR SURGSIM_SOURCE_DIR will always point to the top of the OSS Tree
+set(SURGSIM_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR})
+
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/CMake)
+set(SURGSIM_TOOLS_DIR "${SURGSIM_SOURCE_DIR}/Tools")
+set(SURGSIM_THIRD_PARTY_DIR "${SURGSIM_SOURCE_DIR}/ThirdParty")
+set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} ${SURGSIM_THIRD_PARTY_DIR})
+
+include(ExternalProject)
+include(SurgSimBuildFlags)
+include(SurgSimUtilities)
+
+
+### Find required libraries
+
+find_package(OpenSceneGraph 3.2 COMPONENTS osg osgViewer osgText osgUtil osgDB osgGA osgAnimation REQUIRED)
+find_package(OpenThreads)
+find_package(OpenGL)
+find_package(Eigen3 3.2.0 REQUIRED)
+
+if(WIN32)
+ set(GLUT_ROOT_PATH "$ENV{GLUT_ROOT_PATH}")
+endif(WIN32)
+find_package(GLUT) # only needed for visual device tests
+find_package(GlutFromOsg) # look for GLUT in the OSG tree if not found above
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ # The standard FindGLUT code in CMake 2.8 includes some annoying old
+ # junk (-lXmu -lXi) in GLUT_LIBRARIES for Unix/Linux, which is
+ # neither needed nor usually available. We only need GLUT itself.
+ set(GLUT_LIBRARIES ${GLUT_glut_LIBRARY})
+endif(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+
+if(${BUILD_SHARED_LIBS})
+ set(Boost_USE_STATIC_LIBS OFF)
+ add_definitions("-DBOOST_ALL_DYN_LINK")
+else()
+ set(Boost_USE_STATIC_LIBS ON)
+endif()
+set(Boost_USE_MULTITHREADED ON)
+set(Boost_USE_STATIC_RUNTIME OFF)
+find_package(Boost 1.54 COMPONENTS chrono date_time filesystem system thread REQUIRED)
+mark_as_advanced(Boost_DIR)
+
+# Look for platform specific pthread.
+find_package(Threads REQUIRED)
+# Append pthread and rt to Boost_LIBRARIES since they are boost dependancies
+list(APPEND Boost_LIBRARIES ${CMAKE_THREAD_LIBS_INIT})
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ list(APPEND Boost_LIBRARIES rt)
+endif()
+
+option(USE_SYSTEM_YAMLCPP "Should we use the system yaml-cpp?" OFF)
+if(USE_SYSTEM_YAMLCPP)
+ find_package(yaml-cpp 0.5.1.2 EXACT REQUIRED)
+else(USE_SYSTEM_YAMLCPP)
+ include(External_yamlcpp)
+endif(USE_SYSTEM_YAMLCPP)
+
+include_directories(${SURGSIM_SOURCE_DIR})
+include_directories(SYSTEM
+ ${Boost_INCLUDE_DIR}
+ ${EIGEN3_INCLUDE_DIR}
+ ${YAML_CPP_INCLUDE_DIR}
+)
+
+include(CTest)
+configure_file(CTestCustom.cmake.in CTestCustom.cmake)
+
+option(BUILD_RENDER_TESTING "Build the render testing tree." OFF)
+option(BUILD_PERFORMANCE_TESTING "Build the performance testing tree." OFF)
+
+if(BUILD_TESTING)
+ set(gtest_force_shared_crt ON CACHE BOOL "Use shared CRT for Google Test [OpenSurgSim forces this to true!]" FORCE)
+ find_package(GoogleMock)
+ add_subdirectory(${GOOGLEMOCK_DIR} GoogleMock)
+endif()
+
+unset(EXPORT_TARGETS CACHE)
+add_subdirectory(SurgSim)
+
+option(BUILD_EXAMPLES "Build the examples." ON)
+
+if(BUILD_EXAMPLES)
+ add_subdirectory(Examples)
+endif()
+
+option(BUILD_DOCUMENTATION "Build the documentation using Doxygen." OFF)
+
+if(BUILD_DOCUMENTATION)
+ add_subdirectory(Documentation)
+endif()
+
+#OpenSurgSimConfig For the Build Tree
+set(CONFIG_INCLUDE_DIRS "${SURGSIM_SOURCE_DIR}")
+set(CONFIG_DATA_DIR "${SURGSIM_SOURCE_DIR}/Data")
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/CMake/OpenSurgSimConfig.cmake.in
+ "${PROJECT_BINARY_DIR}/OpenSurgSimConfig.cmake" @ONLY)
+
+#OpenSurgSimConfig For the Install Tree
+file(RELATIVE_PATH REL_INCLUDE_DIR "${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/include")
+set(CONFIG_INCLUDE_DIRS "\${OPENSURGSIM_CMAKE_DIR}/${REL_INCLUDE_DIR}")
+
+if (WIN32)
+file(RELATIVE_PATH REL_DATA_DIR "${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/Data")
+install(DIRECTORY Data/ DESTINATION ${CMAKE_INSTALL_PREFIX}/Data)
+else (WIN32)
+file(RELATIVE_PATH REL_DATA_DIR "${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}")
+install(DIRECTORY Data/ DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME})
+endif (WIN32)
+set(CONFIG_DATA_DIR "\${OPENSURGSIM_CMAKE_DIR}/${REL_DATA_DIR}")
+
+
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/CMake/OpenSurgSimConfig.cmake.in
+ "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/OpenSurgSimConfig.cmake" @ONLY)
+
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/CMake/OpenSurgSimConfigVersion.cmake.in
+ "${PROJECT_BINARY_DIR}/OpenSurgSimConfigVersion.cmake" @ONLY)
+
+
+install(FILES
+ "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/OpenSurgSimConfig.cmake"
+ "${PROJECT_BINARY_DIR}/OpenSurgSimConfigVersion.cmake"
+ DESTINATION "${INSTALL_CMAKE_DIR}" COMPONENT dev)
+install(EXPORT ${PROJECT_NAME}Targets DESTINATION ${INSTALL_CMAKE_DIR} COMPONENT dev)
+
+surgsim_cpplint_this_tree(cpplint)
+
+# Set these variables for Projects using Oss inside their Project structure rather than
+# Through the installed version
+set(OPENSURGSIM_INCLUDE_DIRS ${SURGSIM_SOURCE_DIR} CACHE INTERNAL "OSS Include Directories")
+set(OPENSURGSIM_DATA_DIR ${SURGSIM_SOURCE_DIR}/Data CACHE INTERNAL "OSS Data Directory")
+set(OPENSURGSIM_LIBRARIES ${EXPORT_TARGETS} CACHE INTERNAL "OSS Exported Libraries")
diff --git a/CTestConfig.cmake b/CTestConfig.cmake
new file mode 100644
index 0000000..822ce41
--- /dev/null
+++ b/CTestConfig.cmake
@@ -0,0 +1,22 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+set(CTEST_PROJECT_NAME "OpenSurgSim")
+set(CTEST_NIGHTLY_START_TIME "00:00:00 EST")
+
+set(CTEST_DROP_METHOD "http")
+set(CTEST_DROP_SITE "open.cdash.org")
+set(CTEST_DROP_LOCATION "/submit.php?project=OpenSurgSim")
+set(CTEST_DROP_SITE_CDASH TRUE)
diff --git a/CTestCustom.cmake.in b/CTestCustom.cmake.in
new file mode 100644
index 0000000..3162ecc
--- /dev/null
+++ b/CTestCustom.cmake.in
@@ -0,0 +1,52 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS 50)
+set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS 500)
+
+set(CTEST_CUSTOM_ERROR_MATCH
+ ${CTEST_CUSTOM_ERROR_MATCH}
+)
+
+set(CTEST_CUSTOM_ERROR_EXCEPTION
+ ${CTEST_CUSTOM_ERROR_EXCEPTION}
+)
+
+set(CTEST_CUSTOM_WARNING_MATCH
+ ${CTEST_CUSTOM_WARNING_MATCH}
+)
+
+set(CTEST_CUSTOM_WARNING_EXCEPTION
+ ${CTEST_CUSTOM_WARNING_EXCEPTION}
+ "osgUtil::GLObjectsVisitor::apply"
+ "osgUtil::StateToCompile::apply"
+)
+
+set(CTEST_CUSTOM_TESTS_IGNORE
+ ${CTEST_CUSTOM_TESTS_IGNORE}
+)
+
+set(CTEST_CUSTOM_MEMCHECK_IGNORE
+ ${CTEST_CUSTOM_MEMCHECK_IGNORE}
+)
+
+set(CTEST_CUSTOM_COVERAGE_EXCLUDE
+ ${CTEST_CUSTOM_COVERAGE_EXCLUDE}
+ ".*/ThirdParty/.*"
+)
+
+set(CTEST_CUSTOM_MAXIMUM_PASSED_TEST_OUTPUT_SIZE 32768)
+set(CTEST_CUSTOM_MAXIMUM_FAILED_TEST_OUTPUT_SIZE 524288)
+
diff --git a/Data/Shaders/README.txt b/Data/Shaders/README.txt
new file mode 100644
index 0000000..ea2436a
--- /dev/null
+++ b/Data/Shaders/README.txt
@@ -0,0 +1,10 @@
+Some of the shader encapsulate various pieces of functionality, to let the names not get too long but also
+stay descriptive, the following abbreviations are being used when naming shaders.
+
+Naming conventions for shaders
+
+*_mapping_* means that various maps are available in the shader to be used
+
+s shadow map
+n normal map
+d diffuse map
\ No newline at end of file
diff --git a/Data/Shaders/basic_lit.frag b/Data/Shaders/basic_lit.frag
new file mode 100644
index 0000000..51018af
--- /dev/null
+++ b/Data/Shaders/basic_lit.frag
@@ -0,0 +1,25 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 basic_lit.frag
+/// Basic fragment shader just passes through the color from the vertex shader
+/// onto the fragment
+
+varying vec4 color;
+
+void main(void)
+{
+ gl_FragColor = color;
+}
diff --git a/Data/Shaders/basic_lit.vert b/Data/Shaders/basic_lit.vert
new file mode 100644
index 0000000..12ee7e4
--- /dev/null
+++ b/Data/Shaders/basic_lit.vert
@@ -0,0 +1,40 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 basic_lit.vert
+/// Basic vertex shader with one light, only diffuse term is used for
+/// lighting, lighting is per vertex, light is considered to be a point light
+
+varying vec4 color;
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+
+ vec4 eyeDir4 = gl_ModelViewMatrix * gl_Vertex;
+ vec3 eyeDir = normalize(eyeDir4.xyz);
+
+ vec3 lightDir = gl_LightSource[0].position.xyz - eyeDir4.xyz;
+ float lightDistance = length(lightDir);
+ lightDir = normalize(lightDir);
+
+ vec3 normal = normalize(gl_NormalMatrix * gl_Normal);
+
+ float attenuation = 1.0 / (gl_LightSource[0].constantAttenuation + gl_LightSource[0].linearAttenuation*lightDistance +
+ gl_LightSource[0].quadraticAttenuation*lightDistance*lightDistance);
+
+ color.rgb = attenuation * dot(lightDir, normal) * gl_Color.rgb * gl_LightSource[0].diffuse.rgb + gl_LightSource[0].ambient.rgb;
+ color.a = gl_Color.a;
+}
diff --git a/Data/Shaders/basic_unlit.frag b/Data/Shaders/basic_unlit.frag
new file mode 100644
index 0000000..51018af
--- /dev/null
+++ b/Data/Shaders/basic_unlit.frag
@@ -0,0 +1,25 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 basic_lit.frag
+/// Basic fragment shader just passes through the color from the vertex shader
+/// onto the fragment
+
+varying vec4 color;
+
+void main(void)
+{
+ gl_FragColor = color;
+}
diff --git a/Data/Shaders/basic_unlit.vert b/Data/Shaders/basic_unlit.vert
new file mode 100644
index 0000000..d4d70d4
--- /dev/null
+++ b/Data/Shaders/basic_unlit.vert
@@ -0,0 +1,26 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 basic_unlit.vert
+/// Basic vertex shader, transform vertex and pass through the color
+
+varying vec4 color;
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+
+ color = gl_Color;
+}
diff --git a/Data/Shaders/depth_map.frag b/Data/Shaders/depth_map.frag
new file mode 100644
index 0000000..1f164b5
--- /dev/null
+++ b/Data/Shaders/depth_map.frag
@@ -0,0 +1,31 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 depth.frag
+/// Encode the z-depth of the fragment into rgba values, see
+/// http://www.ozone3d.net/blogs/lab/20080604/glsl-float-to-rgba8-encoder/
+
+vec4 encodeDepth(float depth)
+{
+ vec4 encoded = vec4(1.0, 256.0, 256.0*256.0, 256.0*256.0*256.0) * depth;
+ encoded = fract(encoded);
+ encoded -= encoded.yzww * vec4(1.0/256.0, 1.0/256.0, 1.0/256.0, 0.0);
+ return encoded;
+}
+
+void main(void)
+{
+ gl_FragColor.rgba = encodeDepth(gl_FragCoord.z);
+}
\ No newline at end of file
diff --git a/Data/Shaders/depth_map.vert b/Data/Shaders/depth_map.vert
new file mode 100644
index 0000000..078dd36
--- /dev/null
+++ b/Data/Shaders/depth_map.vert
@@ -0,0 +1,23 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 depth.vert
+/// Passthrough shader, transform the vertex into camera ModelView space
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+}
+
\ No newline at end of file
diff --git a/Data/Shaders/ds_mapping_material.frag b/Data/Shaders/ds_mapping_material.frag
new file mode 100644
index 0000000..2a440b9
--- /dev/null
+++ b/Data/Shaders/ds_mapping_material.frag
@@ -0,0 +1,61 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 ds_mapping_material.frag
+/// Phong material with diffuse and shadow map
+
+// These are 'free' uniforms to be set for this shader, they won't be provided by OSS
+uniform float shininess;
+uniform sampler2D diffuseMap;
+uniform sampler2D shadowMap;
+
+// Oss provided uniforms
+uniform vec4 ambientColor;
+
+// Incoming from the vertex shader
+varying vec3 lightDir;
+varying vec3 eyeDir;
+varying vec3 normalDir;
+
+varying vec2 texCoord0;
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+
+varying vec4 clipCoord;
+
+void main(void)
+{
+ vec3 vAmbient = ambientColor.xyz; // Old Term ... osg_ambientColor * _lightColor;
+
+ vec2 shadowCoord = clipCoord.xy / clipCoord.w * vec2(0.5) + vec2(0.5);
+ float shadowAmount = 1.0 - texture2D(shadowMap, shadowCoord).r;
+
+ // Disable shadows for now
+ //shadowAmount = 1.0;
+
+ float diffuse = max(dot(lightDir, normalDir), 0.0);
+ vec3 vDiffuse = vertexDiffuseColor * diffuse * shadowAmount;
+
+ float temp = max(dot(reflect(lightDir, normalDir), eyeDir), 0.0);
+ float specular = temp / (shininess - temp * shininess + temp);
+ vec3 vSpecular = vertexSpecularColor * specular * shadowAmount;
+
+ vec3 base = texture2D(diffuseMap, texCoord0).rgb;
+ vec3 color = (vAmbient + vDiffuse) * base + vSpecular;
+
+ gl_FragColor.rgb = color;
+ gl_FragColor.a = 1.0;
+}
\ No newline at end of file
diff --git a/Data/Shaders/ds_mapping_material.vert b/Data/Shaders/ds_mapping_material.vert
new file mode 100644
index 0000000..2d88948
--- /dev/null
+++ b/Data/Shaders/ds_mapping_material.vert
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 ds_mapping_material.frag
+/// Phong material with diffuse and shadow map
+
+// These are 'free' uniforms to be set for this shader, they won't be provided by OSS
+uniform vec4 diffuseColor;
+uniform vec4 specularColor;
+
+// Oss provided uniforms
+uniform mat4 viewMatrix;
+
+
+struct LightSource {
+ vec4 diffuse;
+ vec4 specular;
+ vec4 position;
+ float constantAttenuation;
+ float linearAttenuation;
+ float quadraticAttenuation;
+};
+
+uniform LightSource lightSource;
+
+// Outgoing Values
+// Normalized direction vectors
+varying vec3 lightDir;
+varying vec3 eyeDir;
+varying vec3 normalDir;
+
+varying vec2 texCoord0; ///< Texture unit 0 texture coordinates
+varying vec4 clipCoord; ///< Projected and transformed vertex coordinates
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+ clipCoord = gl_Position;
+
+ vec4 eyeDir4 = gl_ModelViewMatrix * gl_Vertex;
+ eyeDir = eyeDir4.xyz;
+
+ texCoord0 = gl_MultiTexCoord0.xy;
+
+ lightDir = ( viewMatrix * lightSource.position - eyeDir4).xyz;
+ float lightDistance = length(lightDir);
+ lightDir = normalize(lightDir);
+
+ float eyeDistance = length(eyeDir);
+ eyeDir = normalize(eyeDir);
+
+ normalDir = normalize(gl_NormalMatrix * gl_Normal);
+
+ float attenuation = 1.0 / (lightSource.constantAttenuation + lightSource.linearAttenuation*lightDistance + lightSource.quadraticAttenuation*lightDistance*lightDistance);
+
+ vertexDiffuseColor = (attenuation * diffuseColor * lightSource.diffuse).xyz;
+ vertexSpecularColor = (attenuation * specularColor * lightSource.specular).xyz;
+}
diff --git a/Data/Shaders/material.frag b/Data/Shaders/material.frag
new file mode 100644
index 0000000..1a16d53
--- /dev/null
+++ b/Data/Shaders/material.frag
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 material.frag
+/// Phong material, using material colors
+
+// These are 'free' uniforms to be set for this shader, they won't be provided by OSS
+uniform float shininess;
+
+// Oss provided uniforms
+uniform vec4 ambientColor;
+
+// Incoming from the vertex shader
+varying vec3 lightDir;
+varying vec3 eyeDir;
+varying vec3 normalDir;
+
+varying vec2 texCoord0;
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+
+varying vec4 clipCoord;
+
+void main(void)
+{
+ vec3 vAmbient = ambientColor.xyz; // Old Term ... osg_ambientColor * _lightColor;
+
+ float diffuse = max(dot(lightDir, normalDir), 0.0);
+ vec3 vDiffuse = vertexDiffuseColor * diffuse;
+
+ float temp = max(dot(reflect(lightDir, normalDir), eyeDir), 0.0);
+ float specular = temp / (shininess - temp * shininess + temp);
+ vec3 vSpecular = vertexSpecularColor * specular;
+
+ vec3 color = vAmbient + vDiffuse + vSpecular;
+
+ gl_FragColor.rgb = color;
+ gl_FragColor.a = 1.0;
+}
\ No newline at end of file
diff --git a/Data/Shaders/material.vert b/Data/Shaders/material.vert
new file mode 100644
index 0000000..08c4513
--- /dev/null
+++ b/Data/Shaders/material.vert
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 material.vert
+/// Phong material, using material colors
+
+// These are 'free' uniforms to be set for this shader, they won't be provided by OSS
+uniform vec4 diffuseColor;
+uniform vec4 specularColor;
+
+// Oss provided uniforms
+uniform mat4 viewMatrix;
+
+
+struct LightSource {
+ vec4 diffuse;
+ vec4 specular;
+ vec4 position;
+ float constantAttenuation;
+ float linearAttenuation;
+ float quadraticAttenuation;
+};
+
+uniform LightSource lightSource;
+
+// Outgoing Values
+// Normalized direction vectors
+varying vec3 lightDir;
+varying vec3 eyeDir;
+varying vec3 normalDir;
+
+varying vec2 texCoord0; ///< Texture unit 0 texture coordinates
+varying vec4 clipCoord; ///< Projected and transformed vertex coordinates
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+ clipCoord = gl_Position;
+
+ vec4 eyeDir4 = gl_ModelViewMatrix * gl_Vertex;
+ eyeDir = eyeDir4.xyz;
+
+ texCoord0 = gl_MultiTexCoord0.xy;
+
+ lightDir = ( viewMatrix * lightSource.position - eyeDir4).xyz;
+ float lightDistance = length(lightDir);
+ lightDir = normalize(lightDir);
+
+ float eyeDistance = length(eyeDir);
+ eyeDir = normalize(eyeDir);
+
+ normalDir = normalize(gl_NormalMatrix * gl_Normal);
+
+ float attenuation = 1.0 / (lightSource.constantAttenuation + lightSource.linearAttenuation*lightDistance + lightSource.quadraticAttenuation*lightDistance*lightDistance);
+
+ vertexDiffuseColor = (attenuation * diffuseColor * lightSource.diffuse).xyz;
+ vertexSpecularColor = (attenuation * specularColor * lightSource.specular).xyz;
+}
diff --git a/Data/Shaders/osg_statshandler.frag b/Data/Shaders/osg_statshandler.frag
new file mode 100644
index 0000000..679481e
--- /dev/null
+++ b/Data/Shaders/osg_statshandler.frag
@@ -0,0 +1,29 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 osg_statshandler.frag
+/// Under modelview transform and attribute replacement the osg statshandler does not render correctly
+/// this vertex shader fixes this problems
+
+uniform sampler2D osg_TextTexture;
+
+varying vec4 color;
+varying vec2 textureCoords;
+
+void main()
+{
+ vec4 textColor = vec4(1.0, 1.0, 1.0, texture2D(osg_TextTexture, textureCoords).a);
+ gl_FragColor = color * textColor;
+}
\ No newline at end of file
diff --git a/Data/Shaders/osg_statshandler.vert b/Data/Shaders/osg_statshandler.vert
new file mode 100644
index 0000000..56a0378
--- /dev/null
+++ b/Data/Shaders/osg_statshandler.vert
@@ -0,0 +1,33 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 osg_statshandler.vert
+/// Under modelview transform and attribute replacement the osg statshandler does not render correctly
+/// this vertex shader fixes this problems
+
+attribute vec4 osg_Vertex;
+attribute vec4 osg_Color;
+attribute vec4 osg_MultiTexCoord0;
+uniform mat4 osg_ModelViewProjectionMatrix;
+
+varying vec4 color;
+varying vec2 textureCoords;
+
+void main()
+{
+ color = osg_Color;
+ textureCoords = osg_MultiTexCoord0.xy;
+ gl_Position = osg_ModelViewProjectionMatrix * osg_Vertex;
+}
\ No newline at end of file
diff --git a/Data/Shaders/s_mapping.frag b/Data/Shaders/s_mapping.frag
new file mode 100644
index 0000000..b94c07d
--- /dev/null
+++ b/Data/Shaders/s_mapping.frag
@@ -0,0 +1,35 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 s_mapping.frag
+/// Modulate the outgoing color by the amount fetched from the shadowMap, intended for use
+/// with simple vertex colors, does not do any texturing
+
+/// map for modulation, needs to be rendered with the same modelview and projection
+/// matrices as the ones that are used for this shader.
+uniform sampler2D shadowMap;
+
+/// incoming color for this fragment
+varying vec4 color;
+
+/// incoming projected coordinates of the current fragment
+varying vec4 clipCoord;
+
+void main(void)
+{
+ vec2 shadowCoord = clipCoord.xy / clipCoord.w * vec2(0.5) + vec2(0.5);
+ float shadowAmount = 1.0 - texture2D(shadowMap, shadowCoord).r;
+ gl_FragColor = color * shadowAmount;
+}
diff --git a/Data/Shaders/s_mapping.vert b/Data/Shaders/s_mapping.vert
new file mode 100644
index 0000000..1e3b835
--- /dev/null
+++ b/Data/Shaders/s_mapping.vert
@@ -0,0 +1,58 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 s_mapping.vert
+/// Vertex shader for simple default material, lighting from a single light source, per vertex lighing
+
+uniform mat4 viewMatrix;
+
+/// outgoing calculated color for this vertex
+varying vec4 color;
+
+/// outgoing calculated projected coordinates for this vertex
+varying vec4 clipCoord;
+
+struct LightSource {
+ vec4 ambient;
+ vec4 diffuse;
+ vec4 specular;
+ vec4 position;
+ float constantAttenuation;
+ float linearAttenuation;
+ float quadraticAttenuation;
+};
+
+uniform LightSource lightSource;
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+ clipCoord = gl_Position;
+
+ vec4 eyeDir4 = gl_ModelViewMatrix * gl_Vertex;
+ vec3 eyeDir = normalize(eyeDir4.xyz);
+
+ vec3 lightDir = (viewMatrix * lightSource.position).xyz - eyeDir4.xyz;
+ float lightDistance = length(lightDir);
+ lightDir = normalize(lightDir);
+
+ vec3 normal = normalize(gl_NormalMatrix * gl_Normal);
+
+ float attenuation = 1.0 / (lightSource.constantAttenuation + lightSource.linearAttenuation*lightDistance +
+ lightSource.quadraticAttenuation*lightDistance*lightDistance);
+
+ color.rgb = attenuation * dot(lightDir, normal) * gl_Color.rgb * lightSource.diffuse.rgb;
+ color.a = gl_Color.a;
+}
diff --git a/Data/Shaders/shadow_map.frag b/Data/Shaders/shadow_map.frag
new file mode 100644
index 0000000..6a34ebc
--- /dev/null
+++ b/Data/Shaders/shadow_map.frag
@@ -0,0 +1,45 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 shadow_map.frag
+/// Calculate a shadow map that can be used for modulating the color of each
+/// fragment by the amount of shadow that should be applied to that color,
+/// the outgoing image will have indicate the amount of shadowing from 1 .. 0
+/// 1 being fully in the shadow, 0 being full light
+
+/// Texture rendered from the view of the light, containing the distance of each fragment
+/// from the light, the z value of the incoming fragment projected into
+/// light space is close to the value in the texture map then the fragment is lit, otherwise
+/// it is not
+uniform sampler2D encodedLightDepthMap;
+
+
+/// The coordinates of the fragment in the space of the projected depth map
+varying vec4 lightCoord;
+
+float decodeDepth(vec4 color)
+{
+ return dot(color, vec4(1.0, 1.0/256.0, 1.0/(256.0*256.0), 1.0/(256.0*256.0*256.0)));
+}
+
+void main(void)
+{
+ // Calculate texture coordinates from incoming point
+ vec3 lightCoord3 = (lightCoord.xyz / lightCoord.w) * vec3(0.5) + vec3(0.5);
+
+ float depth = decodeDepth(texture2D(encodedLightDepthMap, lightCoord3.xy));
+
+ gl_FragColor = vec4(depth + 0.00001 > lightCoord3.z ? 0.0 : 1.0);
+}
diff --git a/Data/Shaders/shadow_map.vert b/Data/Shaders/shadow_map.vert
new file mode 100644
index 0000000..8001d2d
--- /dev/null
+++ b/Data/Shaders/shadow_map.vert
@@ -0,0 +1,39 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 shadow_map.vert
+/// Calculate a shadow map that can be used for modulating the color of each
+/// fragment by the amount of shadow that should be applied to that color
+
+/// inverse view matrix from the rendering camera
+uniform mat4 inverseViewMatrix;
+
+/// view matrix for the camera that rendered the original depth map (see shadow_map.frag)
+uniform mat4 lightViewMatrix;
+
+/// projection matrix for the camera that rendered the original depth map (see shadow_map.frag)
+uniform mat4 lightProjectionMatrix;
+
+varying vec4 lightCoord;
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+
+ // compute the coordinates of the incoming point in the space of the camera used
+ // to render the depth map
+ lightCoord = lightProjectionMatrix * lightViewMatrix * inverseViewMatrix * gl_ModelViewMatrix * gl_Vertex;
+}
+
\ No newline at end of file
diff --git a/Data/Shaders/unlit_texture.frag b/Data/Shaders/unlit_texture.frag
new file mode 100644
index 0000000..cc5cd27
--- /dev/null
+++ b/Data/Shaders/unlit_texture.frag
@@ -0,0 +1,27 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 unlit_texture.frag
+/// Fragment Shader to do simple unlit texture mapping
+
+uniform sampler2D texture;
+
+varying vec2 texCoord0;
+varying vec4 color;
+
+void main(void)
+{
+ gl_FragColor = texture2D(texture, texCoord0);
+}
\ No newline at end of file
diff --git a/Data/Shaders/unlit_texture.vert b/Data/Shaders/unlit_texture.vert
new file mode 100644
index 0000000..3697813
--- /dev/null
+++ b/Data/Shaders/unlit_texture.vert
@@ -0,0 +1,29 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 unlit_texture.vert
+/// Grabs the texture coordinate of the first unit and the current color to
+/// pass it to the fragment shader
+
+varying vec2 texCoord0;
+varying vec4 color;
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+
+ texCoord0 = gl_MultiTexCoord0.xy;
+ color = gl_Color;
+}
diff --git a/Data/Shaders/unlit_texture_rectangle.frag b/Data/Shaders/unlit_texture_rectangle.frag
new file mode 100644
index 0000000..c06ed98
--- /dev/null
+++ b/Data/Shaders/unlit_texture_rectangle.frag
@@ -0,0 +1,10 @@
+#extension GL_ARB_texture_rectangle : enable
+
+uniform sampler2DRect texture;
+
+varying vec2 texCoord0;
+
+void main(void)
+{
+ gl_FragColor = texture2DRect(texture, texCoord0);
+}
\ No newline at end of file
diff --git a/Data/Textures/black.png b/Data/Textures/black.png
new file mode 100644
index 0000000..2ca5ad9
Binary files /dev/null and b/Data/Textures/black.png differ
diff --git a/Data/Textures/checkered.png b/Data/Textures/checkered.png
new file mode 100644
index 0000000..535a8d1
Binary files /dev/null and b/Data/Textures/checkered.png differ
diff --git a/Data/Textures/white.png b/Data/Textures/white.png
new file mode 100644
index 0000000..d062ac5
Binary files /dev/null and b/Data/Textures/white.png differ
diff --git a/Documentation/CMakeLists.txt b/Documentation/CMakeLists.txt
new file mode 100644
index 0000000..44ca678
--- /dev/null
+++ b/Documentation/CMakeLists.txt
@@ -0,0 +1,103 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+# Make it easier to tell CMake where to find dot for doxygen. You can
+# set the environment variable DOXYGEN_DOT_PATH to a directory path to
+# where the dot binary lives. (You don't need this on Linux if you
+# install Graphviz in a standard location.)
+#
+# Note that the Windows installer for Graphviz 2.30 doesn't seem to
+# write the install directory into the registry.
+find_program(DOXYGEN_DOT_EXECUTABLE
+ NAMES dot
+ PATHS "$ENV{DOXYGEN_DOT_PATH}" "${DOXYGEN_DOT_PATH}"
+ DOC "Graphviz Dot tool for using Doxygen")
+
+find_package(Doxygen REQUIRED)
+
+if(EXISTS "${SURGSIM_THIRD_PARTY_DIR}/mathjax")
+ set(MATHJAX_LOCAL "${SURGSIM_THIRD_PARTY_DIR}/mathjax")
+else()
+ set(MATHJAX_LOCAL "")
+endif()
+
+set(SURGSIM_DOCUMENTATION_EXCLUDE_LIST
+ */UnitTests/*
+ CACHE STRING "Directories and files to ignore when generating documentation")
+mark_as_advanced(SURGSIM_DOCUMENTATION_EXCLUDE_LIST)
+
+set(SURGSIM_DOCUMENTATION_MATHJAX_LOCAL
+ "${MATHJAX_LOCAL}"
+ CACHE PATH "A local copy of MathJax to be used for HTML documentation")
+mark_as_advanced(SURGSIM_DOCUMENTATION_LOCAL_MATHJAX)
+
+set(PROJECT_LOGO "${CMAKE_CURRENT_SOURCE_DIR}/OpenSurgSim-dox.png")
+set(HTML_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/header.html")
+set(HTML_FOOTER "${CMAKE_CURRENT_SOURCE_DIR}/footer.html")
+
+# Set DOT_PATH to DOXYGEN_DOT_EXECUTABLE, or empty if not found.
+# Eliminates whining from Doxygen.
+if(DOXYGEN_DOT_FOUND)
+ set(DOT_PATH "${DOXYGEN_DOT_EXECUTABLE}")
+else(DOXYGEN_DOT_FOUND)
+ set(DOT_PATH "")
+endif(DOXYGEN_DOT_FOUND)
+
+# Tie the documentation build to the project build.
+set(DOCUMENTATION_TARGET_FLAGS ALL)
+
+# Control the output format so warnings pop up in the Visual Studio list
+if(MSVC)
+ set(WARN_FORMAT "\"$file($line): warning DOC1: $text\"")
+else(MSVC)
+ set(WARN_FORMAT "\"$file:$line: $text\"")
+endif(MSVC)
+
+if(SURGSIM_DOCUMENTATION_MATHJAX_LOCAL STREQUAL "")
+ # Use MathJax directly from the web
+ set(MATHJAX_RELPATH http://www.mathjax.org/mathjax)
+else()
+ # Copy MathJax directly into the generated documentation
+ if(NOT EXISTS "${CMAKE_CURRENT_BINARY_DIR}/html/mathjax")
+ message("Copying local MathJax (please be patient)...")
+ file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html/")
+ file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html/mathjax/")
+ file(COPY "${SURGSIM_THIRD_PARTY_DIR}/mathjax/."
+ DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/html/")
+ message("...done!")
+ endif()
+ set(MATHJAX_RELPATH ./mathjax)
+endif()
+
+# Generate Doxyfile from Doxyfile.in, with @VARIABLE@ substitution.
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in
+ ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile @ONLY)
+
+add_custom_target(documentation ${DOCUMENTATION_TARGET_FLAGS}
+ ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile
+ SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in
+ WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
+ COMMENT "Generating HTML documentation" VERBATIM)
+# Put the Doxyfile.in at the top of the VS project, not "Header Files":
+source_group("" FILES "${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in")
+
+if(UNIX)
+ install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/html/
+ DESTINATION share/doc/${CMAKE_PROJECT_NAME})
+else(UNIX)
+ install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/html/
+ DESTINATION documentation)
+endif(UNIX)
diff --git a/Documentation/Doxyfile.in b/Documentation/Doxyfile.in
new file mode 100644
index 0000000..2f4bb7a
--- /dev/null
+++ b/Documentation/Doxyfile.in
@@ -0,0 +1,1880 @@
+# Doxyfile 1.8.3.1
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a 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.
+
+DOXYFILE_ENCODING = UTF-8
+
+# The PROJECT_NAME tag is a single word (or sequence of words) that should
+# identify the project. Note that if you do not use Doxywizard you need
+# to put quotes around the project name if it contains spaces.
+
+PROJECT_NAME = "OpenSurgSim"
+
+# 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 =
+
+# 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 = "A free platform for open surgery simulation"
+
+# 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 *89* pixels (when using the OpenSurgSim custom HTML_HEADER).
+# Doxygen will copy the logo to the output directory.
+
+PROJECT_LOGO = @PROJECT_LOGO@
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute)
+# base path where the generated documentation will be put.
+# 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 cause performance problems for the file system.
+
+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.
+# The default language is English, other supported languages are:
+# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional,
+# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German,
+# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English
+# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian,
+# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak,
+# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese.
+
+OUTPUT_LANGUAGE = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES (the default) 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.
+
+BRIEF_MEMBER_DESC = YES
+
+# If the REPEAT_BRIEF tag is set to YES (the default) 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.
+
+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" "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.
+
+ALWAYS_DETAILED_SEC = YES
+
+# 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.
+
+INLINE_INHERITED_MEMB = NO
+
+# If the FULL_PATH_NAMES tag is set to YES then 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.
+
+FULL_PATH_NAMES = YES
+
+# If the FULL_PATH_NAMES tag is set to YES then 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 specify absolute paths here, but also
+# relative paths, which will be relative from the directory where doxygen is
+# started.
+
+STRIP_FROM_PATH = @PROJECT_SOURCE_DIR@
+
+# 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 include paths that
+# are normally passed to the compiler using the -I flag.
+
+STRIP_FROM_INC_PATH = @PROJECT_SOURCE_DIR@
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter
+# (but less readable) file names. This can be useful if your file system
+# doesn't support long names like on DOS, Mac, or CD-ROM.
+
+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
+# comments will behave just like regular Qt-style comments
+# (thus requiring an explicit @brief command for a brief description.)
+
+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 comments
+# will behave just like regular Qt-style comments (thus requiring
+# an explicit \brief command for a brief description.)
+
+QT_AUTOBRIEF = YES
+
+# 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 behaviour.
+# 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 behaviour instead.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented
+# member inherits the documentation from any documented member that it
+# re-implements.
+
+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.
+
+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.
+
+TAB_SIZE = 4
+
+# This tag can be used to specify a number of aliases that acts
+# 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.
+
+OPTIMIZE_OUTPUT_FOR_C = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java
+# sources only. Doxygen will then generate output that is more tailored for
+# Java. For instance, namespaces will be presented as packages, qualified
+# scopes will look different, etc.
+
+OPTIMIZE_OUTPUT_JAVA = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources only. Doxygen will then generate output that is more tailored for
+# Fortran.
+
+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.
+
+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, CSharp, C, C++, D, PHP, Objective-C, Python, Fortran, VHDL, C,
+# C++. 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
+# that for custom extensions you also need to set FILE_PATTERNS otherwise the
+# files are not read by doxygen.
+
+EXTENSION_MAPPING =
+
+# If MARKDOWN_SUPPORT is enabled (the default) 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.
+
+MARKDOWN_SUPPORT = YES
+
+# 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.
+
+#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); v.s.
+# func(std::string) {}). This also makes the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+
+BUILTIN_STL_SUPPORT = YES
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+
+CPP_CLI_SUPPORT = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip 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.
+
+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 (the
+# default) will make doxygen 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.
+
+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.
+
+DISTRIBUTE_GROUP_DOC = YES
+
+# Set the SUBGROUPING tag to YES (the default) 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.
+
+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).
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and
+# unions with only public data 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 (the default),
+# structs, classes, and unions are shown on a separate page (for HTML and Man
+# pages) or section (for LaTeX and RTF).
+
+INLINE_SIMPLE_STRUCTS = NO
+
+# When TYPEDEF_HIDES_STRUCT 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.
+
+TYPEDEF_HIDES_STRUCT = NO
+
+# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to
+# determine which symbols to keep in memory and which to flush to disk.
+# When the cache is full, less often used symbols will be written to disk.
+# For small to medium size projects (<1000 input files) the default value is
+# probably good enough. For larger projects a too small cache size can cause
+# doxygen to be busy swapping symbols to and from disk most of the time
+# causing a significant performance penalty.
+# If the system has enough physical memory increasing the cache will improve the
+# performance by keeping more symbols in memory. Note that the value works on
+# a logarithmic scale so increasing the size by one will roughly double the
+# memory usage. The cache size is given by this formula:
+# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0,
+# corresponding to a cache size of 2^16 = 65536 symbols.
+
+# Obsolete
+#SYMBOL_CACHE_SIZE = 0
+
+# Similar to the SYMBOL_CACHE_SIZE 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 appear 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.
+
+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 and EXTRACT_STATIC tags are set to YES
+
+EXTRACT_ALL = YES
+
+# If the EXTRACT_PRIVATE tag is set to YES all private members of a class
+# will be included in the documentation.
+
+#EXTRACT_PRIVATE = NO
+EXTRACT_PRIVATE = YES
+
+# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal
+# scope will be included in the documentation.
+
+#EXTRACT_PACKAGE = NO
+EXTRACT_PACKAGE = YES
+
+# If the EXTRACT_STATIC tag is set to YES all static members of a file
+# will be included in the documentation.
+
+EXTRACT_STATIC = NO
+
+# 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.
+
+EXTRACT_LOCAL_CLASSES = YES
+
+# 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 (the default) only methods in the interface are included.
+
+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 namespaces are hidden.
+
+EXTRACT_ANON_NSPACES = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all
+# undocumented members of documented classes, files or namespaces.
+# If set to NO (the default) 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.
+
+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 (the default) these classes will be included in the various
+# overviews. This option has no effect if EXTRACT_ALL is enabled.
+
+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 (the default) these declarations will be included in the
+# documentation.
+
+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 (the default) these blocks will be appended to the
+# function's detailed documentation block.
+
+HIDE_IN_BODY_DOCS = NO
+
+# The INTERNAL_DOCS tag determines if documentation
+# that is typed after a \internal command is included. If the tag is set
+# to NO (the default) then the documentation will be excluded.
+# Set it to YES to include the internal documentation.
+
+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.
+
+CASE_SENSE_NAMES = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen
+# will show members with their full class and namespace scopes in the
+# documentation. If set to YES the scope will be hidden.
+
+HIDE_SCOPE_NAMES = NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen
+# will put a list of the files that are included by a file in the documentation
+# of that file.
+
+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.
+
+FORCE_LOCAL_INCLUDES = NO
+
+# If the INLINE_INFO tag is set to YES (the default) then a tag [inline]
+# is inserted in the documentation for inline members.
+
+INLINE_INFO = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES (the default) 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.
+
+SORT_MEMBER_DOCS = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the
+# brief documentation of file, namespace and class members alphabetically
+# by member name. If set to NO (the default) the members will appear in
+# declaration order.
+
+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 default)
+# the constructors will appear in the respective orders defined by
+# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS.
+# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO
+# and ignored for detailed docs if SORT_MEMBER_DOCS is set to 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 default)
+# the group names will appear in their defined order.
+
+SORT_GROUP_NAMES = NO
+
+# 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 default), 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.
+
+SORT_BY_SCOPE_NAME = NO
+
+# 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.
+
+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.
+
+GENERATE_TODOLIST = YES
+
+# 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.
+
+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.
+
+GENERATE_BUGLIST = YES
+
+# 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.
+
+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
+# the initial value of a variable or macro consists of 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 initializer of individual variables and macros in the
+# documentation can be controlled using \showinitializer or \hideinitializer
+# command in the documentation regardless of this setting.
+
+#MAX_INITIALIZER_LINES = 30
+MAX_INITIALIZER_LINES = 1
+
+# 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.
+
+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 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 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. See the manual for examples.
+
+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.
+
+LAYOUT_FILE =
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files
+# containing the references data. This must be a list of .bib files. The
+# .bib extension is automatically appended if omitted. Using this command
+# 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.
+
+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
+# by doxygen. Possible values are YES and NO. If left blank NO is used.
+
+QUIET = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated by doxygen. Possible values are YES and NO. If left blank
+# NO is used.
+
+WARNINGS = YES
+
+# If WARN_IF_UNDOCUMENTED 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.
+
+WARN_IF_UNDOCUMENTED = YES
+
+# If WARN_IF_DOC_ERROR 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.
+
+WARN_IF_DOC_ERROR = YES
+
+# The 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 (the default) doxygen will only warn about
+# wrong or incomplete parameter documentation, but not about the absence of
+# documentation.
+
+#WARN_NO_PARAMDOC = NO
+WARN_NO_PARAMDOC = YES
+
+# 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)
+
+#WARN_FORMAT = "$file:$line: $text"
+WARN_FORMAT = @WARN_FORMAT@
+
+# 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 stderr.
+
+WARN_LOGFILE =
+
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag can be 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.
+
+INPUT = @PROJECT_SOURCE_DIR@/SurgSim @PROJECT_SOURCE_DIR@/Documentation
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is
+# also the default input encoding. 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.
+
+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 pattern (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++ *.d *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh
+# *.hxx *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.dox *.py
+# *.f90 *.f *.for *.vhd *.vhdl
+
+FILE_PATTERNS =
+
+# The RECURSIVE tag can be used to turn specify whether or not subdirectories
+# should be searched for input files as well. Possible values are YES and NO.
+# If left blank NO is used.
+
+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.
+
+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 = @SURGSIM_DOCUMENTATION_EXCLUDE_LIST@
+
+# 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
+
+EXCLUDE_SYMBOLS = *::internal
+
+# 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.
+# Possible values are YES and NO. If left blank NO is used.
+
+EXAMPLE_RECURSIVE = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or
+# directories that contain image that are included in the documentation (see
+# the \image command).
+
+IMAGE_PATH =
+
+# 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.
+
+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
+# info on how filters are used. If FILTER_PATTERNS is empty or if
+# non 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 be used to filter the input files when producing source
+# files to browse (i.e. when SOURCE_BROWSER is set to YES).
+
+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 option only has effect when
+# FILTER_SOURCE_FILES is enabled.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MD_FILE_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 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 also
+# VERBATIM_HEADERS is set to NO.
+
+SOURCE_BROWSER = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body
+# of functions and classes directly in the documentation.
+
+INLINE_SOURCES = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct
+# doxygen to hide any special comment blocks from generated source code
+# fragments. Normal C, C++ and Fortran comments will always remain visible.
+
+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.
+
+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.
+
+REFERENCES_RELATION = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES (the default)
+# 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.
+
+REFERENCES_LINK_SOURCE = 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.
+
+USE_HTAGS = NO
+
+# If the VERBATIM_HEADERS tag is set to YES (the default) 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.
+
+VERBATIM_HEADERS = YES
+
+#---------------------------------------------------------------------------
+# 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.
+
+ALPHABETICAL_INDEX = YES
+
+# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then
+# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns
+# in which this list will be split (can be a number in the range [1..20])
+
+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 one or more prefixes that
+# should be ignored while generating the index headers.
+
+IGNORE_PREFIX =
+
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES (the default) Doxygen will
+# generate HTML output.
+
+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. If left blank `html' will be used as the default path.
+
+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). If it is left blank
+# doxygen will generate files with .html extension.
+
+HTML_FILE_EXTENSION = .html
+
+# The HTML_HEADER tag can be used to specify a personal HTML header for
+# each generated HTML page. If it is left blank doxygen will generate a
+# standard header. Note that when using a custom header you are responsible
+# for the proper inclusion of any scripts and style sheets that doxygen
+# needs, which is dependent on the configuration options used.
+# It is advised to generate a default header using "doxygen -w html
+# header.html footer.html stylesheet.css YourConfigFile" and then modify
+# that header. Note that the header is subject to change so you typically
+# have to redo this when upgrading to a newer version of doxygen or when
+# changing the value of configuration settings such as GENERATE_TREEVIEW!
+
+HTML_HEADER = @HTML_HEADER@
+
+# The HTML_FOOTER tag can be used to specify a personal HTML footer for
+# each generated HTML page. If it is left blank doxygen will generate a
+# standard footer.
+
+HTML_FOOTER = @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. Note that it is recommended to use
+# HTML_EXTRA_STYLESHEET instead of this one, as it is more robust and this
+# tag will in the future become obsolete.
+
+#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.
+
+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.
+
+HTML_EXTRA_FILES =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output.
+# Doxygen will adjust the colors in the style sheet 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.
+# The allowed range is 0 to 359.
+
+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.
+
+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.
+
+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.
+
+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.
+
+HTML_DYNAMIC_SECTIONS = NO
+
+# 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.
+
+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, 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.
+
+GENERATE_DOCSET = NO
+
+# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the
+# 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.
+
+DOCSET_FEEDNAME = "Doxygen generated docs"
+
+# When GENERATE_DOCSET tag is set to YES, 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.
+
+DOCSET_BUNDLE_ID = org.doxygen.Project
+
+# When GENERATE_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.
+
+DOCSET_PUBLISHER_ID = org.doxygen.Publisher
+
+# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher.
+
+DOCSET_PUBLISHER_NAME = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES, additional index files
+# will be generated that can be used as input for tools like the
+# Microsoft HTML help workshop to generate a compiled HTML help file (.chm)
+# of the generated HTML documentation.
+
+GENERATE_HTMLHELP = NO
+
+# If the GENERATE_HTMLHELP tag is set to YES, 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.
+
+CHM_FILE =
+
+# If the GENERATE_HTMLHELP tag is set to YES, 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.
+
+HHC_LOCATION =
+
+# If the GENERATE_HTMLHELP tag is set to YES, 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).
+
+GENERATE_CHI = NO
+
+# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING
+# is used to encode HtmlHelp index (hhk), content (hhc) and project file
+# content.
+
+CHM_INDEX_ENCODING =
+
+# If the GENERATE_HTMLHELP tag is set to YES, 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.
+
+BINARY_TOC = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members
+# to the contents of the HTML help documentation and to the tree view.
+
+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.
+
+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.
+
+QCH_FILE =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating
+# Qt Help Project output. For more information please see
+# http://doc.trolltech.com/qthelpproject.html#namespace
+
+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
+# http://doc.trolltech.com/qthelpproject.html#virtual-folders
+
+QHP_VIRTUAL_FOLDER = doc
+
+# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to
+# add. For more information please see
+# http://doc.trolltech.com/qthelpproject.html#custom-filters
+
+QHP_CUST_FILTER_NAME =
+
+# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see
+# <a href="http://doc.trolltech.com/qthelpproject.html#custom-filters">
+# Qt Help Project / Custom Filters</a>.
+
+QHP_CUST_FILTER_ATTRS =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's
+# filter section matches.
+# <a href="http://doc.trolltech.com/qthelpproject.html#filter-attributes">
+# Qt Help Project / Filter Attributes</a>.
+
+QHP_SECT_FILTER_ATTRS =
+
+# If the GENERATE_QHP tag is set to YES, 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.
+
+QHG_LOCATION =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files
+# will be generated, which together with the HTML files, 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.
+
+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.
+
+ECLIPSE_DOC_ID = org.doxygen.Project
+
+# The DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs)
+# at top of each HTML page. The value NO (the default) enables the index and
+# the value YES disables it. Since the tabs have the same information as the
+# navigation tree you can set this option to NO if you already set
+# GENERATE_TREEVIEW 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.
+# Since the tree basically has the same information as the tab index you
+# could consider to set DISABLE_INDEX to NO when enabling this option.
+
+GENERATE_TREEVIEW = NO
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values
+# (range [0,1..20]) 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.
+
+ENUM_VALUES_PER_LINE = 4
+
+# 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.
+
+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.
+
+EXT_LINKS_IN_WINDOW = NO
+
+# Use this tag to change the font size of Latex formulas included
+# as images in the HTML documentation. The default is 10. Note that
+# 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.
+
+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 before the changes have effect.
+
+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.
+
+USE_MATHJAX = YES
+
+# When MathJax is enabled you can set the default output format to be used for
+# thA MathJax output. Supported types are HTML-CSS, NativeMML (i.e. MathML) and
+# SVG. The default value is HTML-CSS, which is slower, but has the best
+# compatibility.
+
+#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.
+
+#MATHJAX_RELPATH = http://www.mathjax.org/mathjax
+MATHJAX_RELPATH = @MATHJAX_RELPATH@
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension
+# names that should be enabled during MathJax rendering.
+
+MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+
+# 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.
+
+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 search 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 manual for details.
+
+SERVER_BASED_SEARCH = NO
+
+# When EXTERNAL_SEARCH 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 the manual for configuration details.
+
+#EXTERNAL_SEARCH = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will returned the search results when EXTERNAL_SEARCH is enabled.
+# Doxygen ships with an example search engine (doxysearch) which is based on
+# the open source search engine library Xapian. See the manual for configuration
+# details.
+
+#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.
+
+#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.
+
+#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 = id1=loc1 id2=loc2 ...
+
+#EXTRA_SEARCH_MAPPINGS =
+
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will
+# generate Latex output.
+
+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. If left blank `latex' will be used as the default path.
+
+LATEX_OUTPUT = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked. If left blank `latex' will be used as the default command name.
+# 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.
+
+LATEX_CMD_NAME = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to
+# generate index for LaTeX. If left blank `makeindex' will be used as the
+# default command name.
+
+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.
+
+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, letter, legal and
+# executive. If left blank a4wide will be used.
+
+PAPER_TYPE = a4
+
+# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX
+# packages that should be included in the LaTeX output.
+
+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. Notice: only use this tag if you know what you are doing!
+
+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. Notice: only use this tag if you know what you are doing!
+
+LATEX_FOOTER =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated
+# is prepared for conversion to pdf (using ps2pdf). 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.
+
+PDF_HYPERLINKS = YES
+
+# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of
+# plain latex in the generated Makefile. Set this option to YES to get a
+# higher quality PDF documentation.
+
+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.
+
+LATEX_BATCHMODE = NO
+
+# If LATEX_HIDE_INDICES is set to YES then doxygen will not
+# include the index chapters (such as File Index, Compound Index, etc.)
+# in the output.
+
+LATEX_HIDE_INDICES = NO
+
+# If LATEX_SOURCE_CODE 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.
+
+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. The default style is "plain". See
+# http://en.wikipedia.org/wiki/BibTeX for more info.
+
+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 very pretty with
+# other RTF readers or editors.
+
+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. If left blank `rtf' will be used as the default path.
+
+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.
+
+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 other
+# programs which support those fields.
+# Note: wordpad (write) and others do not support links.
+
+RTF_HYPERLINKS = NO
+
+# Load style sheet 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.
+
+RTF_STYLESHEET_FILE =
+
+# Set optional variables used in the generation of an rtf document.
+# Syntax is similar to doxygen's config file.
+
+RTF_EXTENSIONS_FILE =
+
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES (the default) Doxygen will
+# generate man pages
+
+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. If left blank `man' will be used as the default path.
+
+MAN_OUTPUT = man
+
+# The MAN_EXTENSION tag determines the extension that is added to
+# the generated man pages (default is the subroutine's section .3)
+
+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 is NO.
+
+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.
+
+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. If left blank `xml' will be used as the default path.
+
+XML_OUTPUT = xml
+
+# The XML_SCHEMA tag can be used to specify an XML schema,
+# which can be used by a validating XML parser to check the
+# syntax of the XML files.
+
+# Obsolete
+#XML_SCHEMA =
+
+# The XML_DTD tag can be used to specify an XML DTD,
+# which can be used by a validating XML parser to check the
+# syntax of the XML files.
+
+# Obsolete
+#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.
+
+XML_PROGRAMLISTING = YES
+
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will
+# generate an AutoGen Definitions (see 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.
+
+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.
+
+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.
+
+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.
+
+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.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will
+# evaluate all C-preprocessor directives found in the sources and include
+# files.
+
+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 (the default) only conditional
+# compilation will be performed. Macro expansion can be done in a controlled
+# way by setting EXPAND_ONLY_PREDEF to YES.
+
+MACRO_EXPANSION = NO
+
+# 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.
+
+EXPAND_ONLY_PREDEF = NO
+
+# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files
+# pointed to by INCLUDE_PATH will be searched when a #include is found.
+
+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.
+
+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.
+
+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
+# 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.
+
+PREDEFINED = SURGSIM_PARSED_BY_DOXYGEN
+
+# 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.
+
+EXPAND_AS_DEFINED =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then
+# doxygen's preprocessor will remove all references to function-like macros
+# that are alone on a line, have an all uppercase name, and do not end with a
+# semicolon, because these will confuse the parser if not removed.
+
+SKIP_FUNCTION_MACROS = YES
+
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES option can be used to specify one or more tagfiles. 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. Note that each tag file must have a 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.
+
+GENERATE_TAGFILE =
+
+# If the ALLEXTERNALS tag is set to YES all external classes will be listed
+# in the class index. If set to NO only the inherited external classes
+# will be listed.
+
+ALLEXTERNALS = NO
+
+# 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.
+
+EXTERNAL_GROUPS = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of `which perl').
+
+PERL_PATH = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will
+# generate a inheritance diagram (in HTML, RTF 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.
+
+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.
+
+HIDE_UNDOC_RELATIONS = YES
+#HIDE_UNDOC_RELATIONS = NO
+
+# 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, 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)
+
+HAVE_DOT = @DOXYGEN_DOT_FOUND@
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is
+# allowed to run in parallel. When set to 0 (the default) 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.
+
+DOT_NUM_THREADS = 0
+
+# By default doxygen will use the Helvetica font for all dot files that
+# doxygen generates. When you want a differently looking font 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.
+
+DOT_FONTNAME = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs.
+# The default size is 10pt.
+
+DOT_FONTSIZE = 10
+
+# By default doxygen will tell dot to use the Helvetica font.
+# If you specify a different font using DOT_FONTNAME you can use DOT_FONTPATH to
+# set the path where dot can find it.
+
+DOT_FONTPATH =
+
+# If the CLASS_GRAPH and HAVE_DOT tags are 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.
+
+CLASS_GRAPH = YES
+
+# If the COLLABORATION_GRAPH and HAVE_DOT tags are 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.
+
+COLLABORATION_GRAPH = YES
+
+# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen
+# will generate a graph for groups, showing the direct groups dependencies
+
+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.
+
+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
+# managable. Set this to 0 for no limit. Note that the threshold may be
+# exceeded by 50% before the limit is enforced.
+
+UML_LIMIT_NUM_FIELDS = 10
+
+# If set to YES, the inheritance and collaboration graphs will show the
+# relations between templates and their instances.
+
+TEMPLATE_RELATIONS = NO
+
+# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT
+# 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.
+
+INCLUDE_GRAPH = YES
+
+# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and
+# HAVE_DOT tags are set to YES then doxygen will generate a graph for each
+# documented header file showing the documented files that directly or
+# indirectly include this file.
+
+INCLUDED_BY_GRAPH = YES
+
+# If the CALL_GRAPH and HAVE_DOT options are 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.
+
+CALL_GRAPH = NO
+
+# If the CALLER_GRAPH and HAVE_DOT tags are 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.
+
+CALLER_GRAPH = NO
+
+# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen
+# will generate a graphical hierarchy of all classes instead of a textual one.
+
+GRAPHICAL_HIERARCHY = YES
+
+# If the DIRECTORY_GRAPH and HAVE_DOT tags are 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.
+
+DIRECTORY_GRAPH = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot. Possible values are svg, png, jpg, or gif.
+# If left blank png will be used. 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).
+
+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. 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.
+
+INTERACTIVE_SVG = NO
+
+# The tag DOT_PATH 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.
+
+DOT_PATH = @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).
+
+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.
+
+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.
+
+MAX_DOT_GRAPH_DEPTH = 0
+#MAX_DOT_GRAPH_DEPTH = 3
+
+# 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).
+
+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.
+
+DOT_MULTI_TARGETS = YES
+
+# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will
+# generate a legend page explaining the meaning of the various boxes and
+# arrows in the dot generated graphs.
+
+GENERATE_LEGEND = YES
+
+# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will
+# remove the intermediate dot files that are used to generate
+# the various graphs.
+
+DOT_CLEANUP = YES
diff --git a/Documentation/FileFormatConversion.dox b/Documentation/FileFormatConversion.dox
new file mode 100644
index 0000000..b4f734e
--- /dev/null
+++ b/Documentation/FileFormatConversion.dox
@@ -0,0 +1,17 @@
+/*!
+
+\page Conversion File Format Conversion
+
+In OpenSurgSim(OSS), we load graphical meshes from obj(3D model format) files and meshes for physics and collision from ply(Polygon File Format) files. ply files are converted from obj files using Blender software. obj files can be either created by Blender or obtained somewhere else.
+
+## Note
+
+1. A obj file can contain geometry information for multiple objects
+2. A ply file can contain geometry information for exactly one object
+3. Blender use mouse right click to 'select' instead of left click.
+
+## Steps
+Load the obj file to be converted by selecting 'File->Import->Wavefront(.obj). After imported the obj file in Blender, to export objects as ply files, one must first right click, i.e. select, the object to be exported. Then the option in menu 'File->Export->Stanford(.ply)' will become available. In one obj file, there might be multiple objects. To convert them to ply files, the user must do the previous steps for each object.
+
+
+*/
\ No newline at end of file
diff --git a/Documentation/OpenSurgSim-dox.png b/Documentation/OpenSurgSim-dox.png
new file mode 100644
index 0000000..32950cd
Binary files /dev/null and b/Documentation/OpenSurgSim-dox.png differ
diff --git a/Documentation/footer.html b/Documentation/footer.html
new file mode 100644
index 0000000..d2068a5
--- /dev/null
+++ b/Documentation/footer.html
@@ -0,0 +1,21 @@
+<!-- HTML footer for doxygen 1.8.3.1-->
+<!-- start footer part -->
+<!--BEGIN GENERATE_TREEVIEW-->
+<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
+ <ul>
+ $navpath
+ <li class="footer">$generatedby
+ <a href="http://www.doxygen.org/index.html">
+ <img class="footer" src="$relpath^doxygen.png" alt="doxygen"/></a> $doxygenversion </li>
+ </ul>
+</div>
+<!--END GENERATE_TREEVIEW-->
+<!--BEGIN !GENERATE_TREEVIEW-->
+<hr class="footer"/><address class="footer"><small>
+$generatedby <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="$relpath^doxygen.png" alt="doxygen"/>
+</a> $doxygenversion
+</small></address>
+<!--END !GENERATE_TREEVIEW-->
+</body>
+</html>
diff --git a/Documentation/header.html b/Documentation/header.html
new file mode 100644
index 0000000..4b0738a
--- /dev/null
+++ b/Documentation/header.html
@@ -0,0 +1,63 @@
+<!-- HTML header for doxygen 1.8.3.1-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen $doxygenversion"/>
+<!--BEGIN PROJECT_NAME--><title>$projectname: $title</title><!--END PROJECT_NAME-->
+<!--BEGIN !PROJECT_NAME--><title>$title</title><!--END !PROJECT_NAME-->
+<link href="$relpath^tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="$relpath^jquery.js"></script>
+<script type="text/javascript" src="$relpath^dynsections.js"></script>
+$treeview
+$search
+$mathjax
+<link href="$relpath^$stylesheet" rel="stylesheet" type="text/css" />
+$extrastylesheet
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+
+<!--BEGIN TITLEAREA-->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 90px;">
+ <!--BEGIN PROJECT_LOGO-->
+ <td id="projectlogo">
+ <!--BEGIN PROJECT_NAME-->
+ <!--BEGIN PROJECT_BRIEF--><img alt="$projectname: $projectbrief" src="$relpath^$projectlogo"/><!--END PROJECT_BRIEF-->
+ <!--BEGIN !PROJECT_BRIEF--><img alt="$projectname" src="$relpath^$projectlogo"/><!--END !PROJECT_BRIEF-->
+ <!--END PROJECT_NAME-->
+ <!--BEGIN !PROJECT_NAME--><img alt="Logo" src="$relpath^$projectlogo"/><!--END !PROJECT_NAME-->
+ </td>
+ <!--END PROJECT_LOGO-->
+ <!--BEGIN !PROJECT_LOGO-->
+ <!--BEGIN PROJECT_NAME-->
+ <td style="padding-left: 0.5em;">
+ <div id="projectname">$projectname
+ <!--BEGIN PROJECT_NUMBER--> <span id="projectnumber">$projectnumber</span><!--END PROJECT_NUMBER-->
+ </div>
+ <!--BEGIN PROJECT_BRIEF--><div id="projectbrief">$projectbrief</div><!--END PROJECT_BRIEF-->
+ </td>
+ <!--END PROJECT_NAME-->
+ <!--BEGIN !PROJECT_NAME-->
+ <!--BEGIN PROJECT_BRIEF-->
+ <td style="padding-left: 0.5em;">
+ <div id="projectbrief">$projectbrief</div>
+ </td>
+ <!--END PROJECT_BRIEF-->
+ <!--END !PROJECT_NAME-->
+ <!--END !PROJECT_LOGO-->
+ <!--BEGIN DISABLE_INDEX-->
+ <!--BEGIN SEARCHENGINE-->
+ <td>$searchbox</td>
+ <!--END SEARCHENGINE-->
+ <!--END DISABLE_INDEX-->
+ </tr>
+ </tbody>
+</table>
+</div>
+<!--END TITLEAREA-->
+<!-- end header part -->
diff --git a/Documentation/mainPage.dox b/Documentation/mainPage.dox
new file mode 100644
index 0000000..8bc4f51
--- /dev/null
+++ b/Documentation/mainPage.dox
@@ -0,0 +1,50 @@
+// This documentation file is formatted as C++ because Doxygen wants that.
+// NB: "\mainpage notitle" skips the title.
+/**
+
+\mainpage Welcome to the OpenSurgSim Project!
+
+\section mainIntro Introduction
+
+OpenSurgSim is a new (2013) effort to build an open source surgical
+simulation framework. The foundation of the framework draws on many
+years of proprietary simulation development that has matured to the
+point of being generally useful for a wide range if simulators. Over
+the course of the year the released framework will grow from the
+minimal infrastructure that is currently available to a system capable
+of creating complete simulations. While the initial core is being
+contributed by <a href="http://www.simquest.com/">SimQuest</a>, the
+intent is that other groups join in the effort to from an active
+community with contributions coming from a variety of places.
+
+\section mainContrib Contributing
+
+We encourage all members of the surgical simulation community to get
+involved with the project at any level. The code is beginning to be
+added and so now is a great time to weigh in with thoughts and ideas
+about the basic architecture and API's. If you see something you like,
+or don't like, or could be more useful to you if it were done
+differently speak up. This goes for the code as well as the rest of
+the project. We are open to ideas of all sorts.
+
+\section mainResources Resources
+
+<ul>
+<li><a href="https://www.assembla.com/spaces/OpenSurgSim/wiki">OpenSurgSim
+ project wiki</a><br> </li>
+<li>the <i>openSurgSim</i> mailing list/forum:<ul><li>subscribe by sending a
+ <a href="mailto:opensurgsim+subscribe at simquest.com">blank
+ e-mail to opensurgsim+subscribe at simquest.com</a></li>
+ <li>unsubscribe by sending a
+ <a href="mailto:opensurgsim+unsubscribe at simquest.com">blank
+ e-mail to opensurgsim+unsubscribe at simquest.com</a></li>
+ <li>view the archives, subscribe, unsubscribe, manage settings and
+ send messages using
+ <a href="http://groups.google.com/a/simquest.com/group/opensurgsim/?hl=en">the
+ Google Groups web interface</a> (requires Google account)</li>
+ </ul></li><br>
+<li>subscribe to the <a href="http://eepurl.com/vzzdv">OpenSurgSim
+ newsletter</a> for low-volume periodic updates</li>
+</ul>
+
+*/
diff --git a/Examples/AddSphereFromInput/AddSphereBehavior.cpp b/Examples/AddSphereFromInput/AddSphereBehavior.cpp
new file mode 100644
index 0000000..e117842
--- /dev/null
+++ b/Examples/AddSphereFromInput/AddSphereBehavior.cpp
@@ -0,0 +1,81 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "Examples/AddSphereFromInput/AddSphereBehavior.h"
+
+#include "SurgSim/Blocks/SphereElement.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Framework/Scene.h"
+
+#include <sstream>
+
+using SurgSim::Math::RigidTransform3d;
+
+AddSphereFromInputBehavior::AddSphereFromInputBehavior(
+ const std::string& name):
+ SurgSim::Framework::Behavior(name), m_numElements(0), m_buttonPreviouslyPressed(false)
+{
+}
+
+void AddSphereFromInputBehavior::setInputComponent(std::shared_ptr<SurgSim::Input::InputComponent> sender)
+{
+ m_from = sender;
+}
+
+void AddSphereFromInputBehavior::update(double dt)
+{
+ // Get the pose information from input device
+ // Then use the pose as the location to add sphere
+ SurgSim::DataStructures::DataGroup dataGroup;
+ m_from->getData(&dataGroup);
+
+ RigidTransform3d pose;
+ dataGroup.poses().get(SurgSim::DataStructures::Names::POSE, &pose);
+
+ // Add sphere to the scene from input
+ bool button1 = false;
+ dataGroup.booleans().get(SurgSim::DataStructures::Names::BUTTON_1, &button1);
+
+ if (button1 && ! m_buttonPreviouslyPressed)
+ {
+ std::stringstream elementCount;
+ elementCount << ++ m_numElements;
+
+ std::string name = "sphereId_" + elementCount.str();
+
+ std::shared_ptr<SurgSim::Framework::SceneElement> element =
+ std::make_shared<SurgSim::Blocks::SphereElement>(name);
+ element->setPose(pose);
+
+ getScene()->addSceneElement(element);
+ }
+ m_buttonPreviouslyPressed = button1;
+}
+
+int AddSphereFromInputBehavior::getTargetManagerType() const
+{
+ return SurgSim::Framework::MANAGER_TYPE_INPUT;
+}
+
+bool AddSphereFromInputBehavior::doInitialize()
+{
+ return true;
+}
+
+bool AddSphereFromInputBehavior::doWakeUp()
+{
+ return true;
+}
diff --git a/Examples/AddSphereFromInput/AddSphereBehavior.h b/Examples/AddSphereFromInput/AddSphereBehavior.h
new file mode 100644
index 0000000..91563e7
--- /dev/null
+++ b/Examples/AddSphereFromInput/AddSphereBehavior.h
@@ -0,0 +1,57 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef EXAMPLES_ADDSPHEREFROMINPUT_ADDSPHEREBEHAVIOR_H
+#define EXAMPLES_ADDSPHEREFROMINPUT_ADDSPHEREBEHAVIOR_H
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Input/InputComponent.h"
+
+class AddSphereFromInputBehavior: public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit AddSphereFromInputBehavior(const std::string& name);
+
+ /// Set input component to get the pose
+ /// \param sender Input component to get the pose
+ void setInputComponent(std::shared_ptr<SurgSim::Input::InputComponent> sender);
+
+ /// Update the behavior
+ /// \param dt The length of time (seconds) between update calls.
+ virtual void update(double dt) override;
+
+ /// Return the type of manager that should be responsible for this behavior
+ virtual int getTargetManagerType() const override;
+
+protected:
+ /// Initialize the behavior
+ virtual bool doInitialize() override;
+ /// Wakeup the behavior
+ virtual bool doWakeUp() override;
+
+private:
+ /// Input component to get the pose
+ std::shared_ptr<SurgSim::Input::InputComponent> m_from;
+
+ /// Used to record the number of spheres added
+ int m_numElements;
+
+ /// Used to record if button was previously pressed
+ bool m_buttonPreviouslyPressed;
+};
+
+#endif // EXAMPLES_ADDSPHEREFROMINPUT_ADDSPHEREBEHAVIOR_H
diff --git a/Examples/AddSphereFromInput/AddSphereFromInput.cpp b/Examples/AddSphereFromInput/AddSphereFromInput.cpp
new file mode 100644
index 0000000..07575bf
--- /dev/null
+++ b/Examples/AddSphereFromInput/AddSphereFromInput.cpp
@@ -0,0 +1,180 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <boost/thread.hpp>
+
+#include "Examples/AddSphereFromInput/AddSphereBehavior.h"
+
+#include "SurgSim/Blocks/DriveElementFromInputBehavior.h"
+#include "SurgSim/Devices/MultiAxis/MultiAxisDevice.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgPlaneRepresentation.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/ViewElement.h"
+#include "SurgSim/Input/InputManager.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+
+using SurgSim::Blocks::DriveElementFromInputBehavior;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Framework::Logger;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Graphics::OsgBoxRepresentation;
+using SurgSim::Graphics::OsgCamera;
+using SurgSim::Graphics::OsgMaterial;
+using SurgSim::Graphics::OsgPlaneRepresentation;
+using SurgSim::Graphics::OsgShader;
+using SurgSim::Graphics::OsgUniform;
+using SurgSim::Graphics::OsgViewElement;
+using SurgSim::Graphics::ViewElement;
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4f;
+using SurgSim::Physics::FixedRepresentation;
+using SurgSim::Physics::Representation;
+using SurgSim::Physics::PhysicsManager;
+
+
+std::shared_ptr<SceneElement> createPlane(const std::string& name)
+{
+ std::shared_ptr<DoubleSidedPlaneShape> planeShape = std::make_shared<DoubleSidedPlaneShape>();
+
+ std::shared_ptr<FixedRepresentation> physicsRepresentation =
+ std::make_shared<FixedRepresentation>(name + " Physics");
+ physicsRepresentation->setShape(planeShape);
+
+ std::shared_ptr<OsgPlaneRepresentation> graphicsRepresentation =
+ std::make_shared<OsgPlaneRepresentation>(name + " Graphics");
+
+ std::shared_ptr<OsgMaterial> material = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<OsgShader> shader = std::make_shared<OsgShader>();
+
+ std::shared_ptr<OsgUniform<Vector4f>> uniform = std::make_shared<OsgUniform<Vector4f>>("color");
+ uniform->set(Vector4f(0.0f, 0.6f, 1.0f, 0.0f));
+ material->addUniform(uniform);
+
+ shader->setFragmentShaderSource(
+ "uniform vec4 color;\n"
+ "void main(void)\n"
+ "{\n"
+ " gl_FragColor = color;\n"
+ "}");
+ material->setShader(shader);
+ graphicsRepresentation->setMaterial(material);
+
+ std::shared_ptr<SceneElement> planeElement = std::make_shared<BasicSceneElement>(name);
+ planeElement->addComponent(physicsRepresentation);
+ planeElement->addComponent(graphicsRepresentation);
+ planeElement->addComponent(material);
+
+ auto rigidCollision = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Plane Collision");
+ physicsRepresentation->setCollisionRepresentation(rigidCollision);
+ planeElement->addComponent(rigidCollision);
+
+ return planeElement;
+}
+
+
+std::shared_ptr<SceneElement> createBox(const std::string& name)
+{
+ std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(0.2, 0.2, 0.2); // in m
+
+ std::shared_ptr<OsgBoxRepresentation> graphicsRepresentation =
+ std::make_shared<OsgBoxRepresentation>(name + "-Graphics");
+ graphicsRepresentation->setSizeXYZ(box->getSizeX(), box->getSizeY(), box->getSizeZ());
+
+ std::shared_ptr<SurgSim::Input::InputComponent> inputComponent =
+ std::make_shared<SurgSim::Input::InputComponent>("input");
+ inputComponent->setDeviceName("MultiAxisDevice");
+ std::shared_ptr<SceneElement> boxElement = std::make_shared<BasicSceneElement>(name);
+ boxElement->addComponent(graphicsRepresentation);
+ boxElement->addComponent(inputComponent);
+
+ std::shared_ptr<DriveElementFromInputBehavior> driver;
+ driver = std::make_shared<DriveElementFromInputBehavior>("Driver");
+ driver->setSource(inputComponent);
+ boxElement->addComponent(driver);
+
+ auto addSphere = std::make_shared<AddSphereFromInputBehavior>("SphereAdder");
+ addSphere->setInputComponent(inputComponent);
+ boxElement->addComponent(addSphere);
+
+ return boxElement;
+}
+
+
+int main(int argc, char* argv[])
+{
+ std::shared_ptr<SurgSim::Graphics::OsgManager> graphicsManager = std::make_shared<SurgSim::Graphics::OsgManager>();
+ std::shared_ptr<PhysicsManager> physicsManager = std::make_shared<PhysicsManager>();
+ std::shared_ptr<SurgSim::Framework::BehaviorManager> behaviorManager =
+ std::make_shared<SurgSim::Framework::BehaviorManager>();
+ std::shared_ptr<SurgSim::Input::InputManager> inputManager = std::make_shared<SurgSim::Input::InputManager>();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ std::shared_ptr<SurgSim::Device::MultiAxisDevice> toolDevice =
+ std::make_shared<SurgSim::Device::MultiAxisDevice>("MultiAxisDevice");
+ toolDevice->setPositionScale(toolDevice->getPositionScale() * 10.0);
+ toolDevice->setOrientationScale(toolDevice->getOrientationScale() * 3.0);
+ SURGSIM_ASSERT(toolDevice->initialize() == true) <<
+ "Could not initialize device '" << toolDevice->getName() << "' for the tool.";
+
+ inputManager->addDevice(toolDevice);
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime(new SurgSim::Framework::Runtime());
+ runtime->addManager(physicsManager);
+ runtime->addManager(graphicsManager);
+ runtime->addManager(behaviorManager);
+ runtime->addManager(inputManager);
+
+ std::shared_ptr<SurgSim::Framework::Scene> scene = runtime->getScene();
+ scene->addSceneElement(createBox("box"));
+
+ std::shared_ptr<SceneElement> plane = createPlane("plane");
+ plane->setPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, -1.0, 0.0)));
+ scene->addSceneElement(plane);
+
+ std::shared_ptr<ViewElement> viewElement = std::make_shared<OsgViewElement>("view");
+
+ viewElement->setPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.5, 5.0)));
+ scene->addSceneElement(viewElement);
+
+ runtime->execute();
+
+ return 0;
+}
diff --git a/Examples/AddSphereFromInput/CMakeLists.txt b/Examples/AddSphereFromInput/CMakeLists.txt
new file mode 100644
index 0000000..9bafb6a
--- /dev/null
+++ b/Examples/AddSphereFromInput/CMakeLists.txt
@@ -0,0 +1,52 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+find_package(OpenSceneGraph COMPONENTS osg osgViewer osgText osgUtil osgDB osgGA osgAnimation REQUIRED)
+find_package(OpenThreads)
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ ${CMAKE_CURRENT_SOURCE_DIR}
+ ${OSG_INCLUDE_DIR}
+ ${OPENTHREADS_INCLUDE_DIR}
+)
+
+set(SOURCES
+ AddSphereBehavior.cpp
+ AddSphereFromInput.cpp
+)
+
+set(HEADERS
+ AddSphereBehavior.h
+)
+
+add_executable(AddSphereFromInput ${SOURCES} ${HEADERS})
+surgsim_show_ide_folders("${SOURCES}" "${HEADERS}")
+
+set(LIBS
+ MultiAxisDevice
+ SurgSimBlocks
+ SurgSimGraphics
+ SurgSimMath
+ ${YAML_CPP_LIBRARIES}
+)
+
+target_link_libraries(AddSphereFromInput ${LIBS})
+
+# Put AddSphereFromInput into folder "Examples"
+set_target_properties(AddSphereFromInput PROPERTIES FOLDER "Examples")
diff --git a/Examples/CMakeLists.txt b/Examples/CMakeLists.txt
new file mode 100644
index 0000000..fb9dd05
--- /dev/null
+++ b/Examples/CMakeLists.txt
@@ -0,0 +1,27 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+if(BUILD_DEVICE_MULTIAXIS)
+ add_subdirectory(AddSphereFromInput)
+ add_subdirectory(Stapling)
+endif(BUILD_DEVICE_MULTIAXIS)
+
+if(BUILD_DEVICE_MULTIAXIS OR BUILD_DEVICE_NOVINT)
+ add_subdirectory(InputVtc)
+endif(BUILD_DEVICE_MULTIAXIS OR BUILD_DEVICE_NOVINT)
+
+add_subdirectory(DroppingBalls)
+
+add_subdirectory(GraphicsScene)
diff --git a/Examples/DroppingBalls/AddRandomSphereBehavior.cpp b/Examples/DroppingBalls/AddRandomSphereBehavior.cpp
new file mode 100644
index 0000000..3163e5f
--- /dev/null
+++ b/Examples/DroppingBalls/AddRandomSphereBehavior.cpp
@@ -0,0 +1,96 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <sstream>
+#include <stdlib.h>
+
+#include "Examples/DroppingBalls/AddRandomSphereBehavior.h"
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Blocks/SphereElement.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Blocks::SphereElement;
+using SurgSim::Framework::Behavior;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Math::Vector3d;
+
+/// \file
+/// A Behavior that creates randomly positioned SphereElements at a fixed rate.
+/// \sa SurgSim::Blocks::SphereElement
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+AddRandomSphereBehavior::AddRandomSphereBehavior(const std::string& name):
+ Behavior(name), m_totalTime(0.0), m_numElements(0),
+ m_distribution_xz(0.0, 1.0), m_distribution_y(1.0, 2.0)
+{
+}
+
+
+AddRandomSphereBehavior::~AddRandomSphereBehavior()
+{
+}
+
+bool AddRandomSphereBehavior::doInitialize()
+{
+ return true;
+}
+
+bool AddRandomSphereBehavior::doWakeUp()
+{
+ return true;
+}
+
+void AddRandomSphereBehavior::update(double dt)
+{
+ // Accumulate the time steps since the previous sphere was created.
+ m_totalTime += dt;
+
+ if (m_totalTime > 3.0)
+ {
+ m_totalTime = 0.0;
+
+ std::stringstream ss;
+ ss << ++ m_numElements;
+
+ // Generate a random position.
+ double m_x = m_distribution_xz(m_generator);
+ double m_y = m_distribution_y(m_generator);
+ double m_z = m_distribution_xz(m_generator);
+
+ std::string name = "sphereId_" + ss.str();
+ // Create the pose, with no rotation and the previously determined position.
+ SurgSim::Math::RigidTransform3d pose = SurgSim::Math::makeRigidTransform
+ (SurgSim::Math::Quaterniond::Identity(), Vector3d(m_x, m_y, m_z));
+
+ // Create the SphereElement.
+ std::shared_ptr<SceneElement> element = std::make_shared<SphereElement>(name);
+ element->setPose(pose);
+
+ // Add the SphereElement to the Scene.
+ getScene()->addSceneElement(element);
+ }
+}
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
diff --git a/Examples/DroppingBalls/AddRandomSphereBehavior.h b/Examples/DroppingBalls/AddRandomSphereBehavior.h
new file mode 100644
index 0000000..0b30e48
--- /dev/null
+++ b/Examples/DroppingBalls/AddRandomSphereBehavior.h
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef EXAMPLES_DROPPINGBALLS_ADDRANDOMSPHEREBEHAVIOR_H
+#define EXAMPLES_DROPPINGBALLS_ADDRANDOMSPHEREBEHAVIOR_H
+
+#include <random>
+
+#include "SurgSim/Framework/Behavior.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+/// A behavior to add sphere elements into scene dynamically.
+/// AddSphereBehavior will be processed by BehaviorManager in its update() call.
+class AddRandomSphereBehavior: public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name The name of add random sphere behavior.
+ explicit AddRandomSphereBehavior(const std::string& name = "AddRandomSphereBehavior");
+
+ /// Destructor
+ ~AddRandomSphereBehavior();
+
+ /// Update the behavior
+ /// \param dt The length of time (seconds) between update calls.
+ virtual void update(double dt) override;
+
+protected:
+ /// Initialize the behavior
+ virtual bool doInitialize() override;
+ /// Wakeup the behavior
+ virtual bool doWakeUp() override;
+
+private:
+ /// Control how often a sphere is added
+ double m_totalTime;
+ /// Record how many sphere have been added
+ int m_numElements;
+ /// Random number generator for randomized positions.
+ std::default_random_engine m_generator;
+ /// Distribution of random numbers for the x and z coordinates.
+ std::uniform_real_distribution<double> m_distribution_xz;
+ /// Distribution of random numbers for the y coordinate.
+ std::uniform_real_distribution<double> m_distribution_y;
+};
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
+
+#endif //EXAMPLES_DROPPINGBALLS_ADDRANDOMSPHEREBEHAVIOR_H
diff --git a/Examples/DroppingBalls/CMakeLists.txt b/Examples/DroppingBalls/CMakeLists.txt
new file mode 100644
index 0000000..57360fd
--- /dev/null
+++ b/Examples/DroppingBalls/CMakeLists.txt
@@ -0,0 +1,57 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ ${CMAKE_CURRENT_SOURCE_DIR}
+ ${OSG_INCLUDE_DIR}
+ ${OPENTHREADS_INCLUDE_DIR}
+)
+
+set(SOURCES
+ AddRandomSphereBehavior.cpp
+ DroppingBalls.cpp
+)
+
+set(HEADERS
+ AddRandomSphereBehavior.h
+)
+
+file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+add_executable(DroppingBalls ${SOURCES} ${HEADERS})
+surgsim_show_ide_folders("${SOURCES}" "${HEADERS}")
+
+SET(LIBS
+ SurgSimBlocks
+ SurgSimPhysics
+ SurgSimGraphics
+ SurgSimMath
+ ${YAML_CPP_LIBRARIES}
+)
+
+target_link_libraries(DroppingBalls ${LIBS})
+
+# Put DroppingBalls into folder "Examples"
+set_target_properties(DroppingBalls PROPERTIES FOLDER "Examples")
diff --git a/Examples/DroppingBalls/Data/Earth.png b/Examples/DroppingBalls/Data/Earth.png
new file mode 100644
index 0000000..76ee917
Binary files /dev/null and b/Examples/DroppingBalls/Data/Earth.png differ
diff --git a/Examples/DroppingBalls/DroppingBalls.cpp b/Examples/DroppingBalls/DroppingBalls.cpp
new file mode 100644
index 0000000..bb13f99
--- /dev/null
+++ b/Examples/DroppingBalls/DroppingBalls.cpp
@@ -0,0 +1,293 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <boost/thread.hpp>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgPlaneRepresentation.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/ViewElement.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+#include "Examples/DroppingBalls/AddRandomSphereBehavior.h"
+
+using SurgSim::Blocks::AddRandomSphereBehavior;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Framework::Logger;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Graphics::OsgMaterial;
+using SurgSim::Graphics::OsgPlaneRepresentation;
+using SurgSim::Graphics::OsgShader;
+using SurgSim::Graphics::OsgSphereRepresentation;
+using SurgSim::Graphics::OsgTexture2d;
+using SurgSim::Graphics::OsgUniform;
+using SurgSim::Graphics::OsgViewElement;
+using SurgSim::Graphics::ViewElement;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector4f;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::FixedRepresentation;
+using SurgSim::Physics::Representation;
+using SurgSim::Physics::RigidRepresentation;
+using SurgSim::Physics::PhysicsManager;
+
+/// \file
+/// Example of how to put together a very simple demo of balls colliding with each other.
+/// Discrete Collision Detection (dcd) is used to detect collisions between spheres.
+
+/// Simple behavior to show that the spheres are moving while we don't have graphics.
+/// \note A Behavior is a type of Component that causes changes or actions.
+class PrintoutBehavior : public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name The printout behavior name
+ explicit PrintoutBehavior(const std::string& name = "PrintOutBehavior") :
+ Behavior(name) {}
+ ~PrintoutBehavior() {}
+
+ /// Set representation for printout behavior
+ /// \param representation The representation information
+ void setRepresentation(std::shared_ptr<RigidRepresentation> representation)
+ {
+ m_representation = representation;
+ }
+
+ /// Perform per-period actions, i.e., what to do each "frame".
+ /// \note Behavior::update() is called by ComponentManager::processBehaviors(), which is called by
+ /// BehaviorManager::doUpdate() or Graphics::Manager::doUpdate() or PhysicsManager::doUpdate() depending on the
+ /// output of Behavior::getTargetManagerType(). Those manager's \c doUpdate() functions are called by
+ /// BasicThread() inside a \c while(running) loop.
+ /// Managers (e.g., ComponentManager) are threads and run their own update loops.
+ virtual void update(double dt)
+ {
+ // SURGSIM_LOG_DEBUG is a macro to ensure only messages of a certain threshold are output.
+ SURGSIM_LOG_DEBUG(m_logger) << m_representation->getName() << ": " <<
+ m_representation->getPose().translation().transpose();
+ }
+
+protected:
+ /// Allocate the internal structures.
+ /// \return Success?
+ /// \note Initialization is a two-step process. First the ComponentManager calls initialize() on each Component,
+ /// which calls doInitialize() to setup the internal structures.
+ virtual bool doInitialize()
+ {
+ // If the named logger does not exist, the LoggerManager creates a new logger that uses the default output
+ m_logger = Logger::getLogger("printout");
+ // Set the threshold for display of messages.
+ m_logger->setThreshold(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ return true;
+ }
+ /// Setup foreign references. After this the Component is ready to update().
+ /// \return Success?
+ /// \note The second step of initialization. This function acquires any foreign references, all of which were
+ /// allocated by the respective foreign objects' doInitialize().
+ virtual bool doWakeUp()
+ {
+ return true;
+ }
+
+private:
+ // A Representation is a type of Component that stores information.
+ // A RigidRepresentation stores 6 degree-of-freedom (DOF) pose, force, torque, inertia, and compliance.
+ std::shared_ptr<RigidRepresentation> m_representation;
+
+ // A Logger can output to file or stream.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+};
+
+/// Creates a planar SceneElement with graphics, physics, and collision.
+std::shared_ptr<SceneElement> createPlane(const std::string& name)
+{
+ std::shared_ptr<DoubleSidedPlaneShape> shape = std::make_shared<DoubleSidedPlaneShape>();
+
+ // A FixedRepresentation has no motion or compliance. It does not change.
+ std::shared_ptr<FixedRepresentation> physics = std::make_shared<FixedRepresentation>("Physics");
+ physics->setShape(shape);
+
+ // An OsgPlaneRepresentation is a Component containing the OSG-specific graphics information to display a plane.
+ std::shared_ptr<OsgPlaneRepresentation> graphics;
+ graphics = std::make_shared<OsgPlaneRepresentation>("Graphics");
+
+ // A OsgMaterial is an OSG implementation of a Material. Materials define visual appearance and contain Uniforms
+ // and a Shader. Uniforms represent values that are relatively constant, e.g., textures or position of a light.
+ std::shared_ptr<OsgMaterial> material = std::make_shared<OsgMaterial>("material");
+ // An OsgShader is an OSG implementation of a Shader. Shaders are programs that determine how to render a geometry.
+ std::shared_ptr<OsgShader> shader = std::make_shared<OsgShader>();
+
+ // Create a Uniform for the RGBA color of the plane.
+ std::shared_ptr<OsgUniform<Vector4f>> uniform = std::make_shared<OsgUniform<Vector4f>>("color");
+ uniform->set(Vector4f(0.0f, 0.6f, 1.0f, 0.0f));
+ material->addUniform(uniform);
+
+ // This Shader sets the fragment's color to the value of the "color" uniform.
+ shader->setFragmentShaderSource(
+ "uniform vec4 color;\n"
+ "void main(void)\n"
+ "{\n"
+ " gl_FragColor = color;\n"
+ "}");
+ material->setShader(shader);
+ graphics->setMaterial(material);
+
+ // RigidCollisionRepresentation will use provided physics representation to do collisions. Collision detection
+ // occurs in SurgSim::Physics::DcdCollision::doUpdate(), which uses the Shape. Then the physics representations
+ // (of the colliding pair) are used to generate constraints that the solver uses to calculate forces that will
+ // un-collide the pair. The entire process of collision detection, constraint generation, and solving is handled in
+ // SurgSim::PhysicsManager::doUpdate().
+ std::shared_ptr<SurgSim::Physics::RigidCollisionRepresentation> collision;
+ collision = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Collision");
+ physics->setCollisionRepresentation(collision);
+
+ // Here the SceneElement for the plane is created, to which the various Components that collectively define the
+ // plane are added.
+ std::shared_ptr<SceneElement> element = std::make_shared<BasicSceneElement>(name);
+ element->addComponent(physics);
+ element->addComponent(collision);
+ element->addComponent(graphics);
+ element->addComponent(material);
+
+ // This Behavior will add balls to the Scene at random locations every few seconds.
+ element->addComponent(std::make_shared<AddRandomSphereBehavior>());
+
+ return element;
+}
+
+
+/// Creates a SceneElement of a rigid sphere with a graphic texture loaded from an image file.
+std::shared_ptr<SceneElement> createEarth(const SurgSim::Framework::ApplicationData& data, const std::string& name)
+{
+ // A RigidRepresentation is for a non-deformable 6 degree-of-freedom (DOF) object with compliance and inertia.
+ std::shared_ptr<RigidRepresentation> physics = std::make_shared<RigidRepresentation>("Physics");
+
+ // Density determines inertia and compliance, which are used in the collision response.
+ physics->setDensity(5513.0);
+ // Damping generates a force that opposes the velocity.
+ physics->setLinearDamping(0.1);
+
+ // A SphereShape is a Shape for a sphere. It has functions to find shape properties such as the mass center,
+ // volume, and inertia. The constructor's argument is the radius in meters.
+ std::shared_ptr<SphereShape> shape = std::make_shared<SphereShape>(0.5);
+ physics->setShape(shape);
+
+ std::shared_ptr<OsgSphereRepresentation> graphics = std::make_shared<OsgSphereRepresentation>("Graphics");
+ graphics->setRadius(shape->getRadius());
+
+ std::shared_ptr<OsgMaterial> material = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<OsgTexture2d> texture = std::make_shared<OsgTexture2d>();
+
+ // findFile() will look in the folders specified by the ApplicationData.
+ std::string image = data.findFile("Earth.png");
+ SURGSIM_ASSERT(image != "") << "Could not find image file for sphere texture: " << image;
+ SURGSIM_ASSERT(texture->loadImage(image)) << "Could not load image file for sphere texture: " << image;
+
+ std::shared_ptr<OsgUniform<std::shared_ptr<OsgTexture2d>>> uniform =
+ std::make_shared<OsgUniform<std::shared_ptr<OsgTexture2d>>>("diffuseMap");
+ uniform->set(texture);
+ material->addUniform(uniform);
+ graphics->setMaterial(material);
+
+ // By adding the PrintoutBehavior, the BehaviorManager will output this SceneElement's position each update.
+ std::shared_ptr<PrintoutBehavior> printoutBehavior = std::make_shared<PrintoutBehavior>();
+ printoutBehavior->setRepresentation(physics);
+
+ // Now create the SceneElement based on the physics and graphics. Note there is no collision Component.
+ std::shared_ptr<SceneElement> element = std::make_shared<BasicSceneElement>(name);
+ element->addComponent(physics);
+ element->addComponent(graphics);
+ element->addComponent(material);
+ element->addComponent(printoutBehavior);
+
+ return element;
+}
+
+
+int main(int argc, char* argv[])
+{
+ // The config file contains a list of folder locations which will be used to find resources, such as images, shader
+ // code, physics descriptions, etc.
+ const SurgSim::Framework::ApplicationData data("config.txt");
+
+ // Here the various Managers are created. Managers contain and update their type of Components. A Manager is a
+ // sub-class of BasicThread, so each Manager runs in its own thread and can have its own target update rate.
+ // The three types of Managers are:
+ // a) Graphics::Manager to display the graphic scene from graphics representations (using graphics-shapes,
+ // materials, shaders, etc.),
+ // b) PhysicsManager to update the physics simulation based on physics representations (using physics-shapes,
+ // fixed/rigid/FEM models, compliance, inertia, collision type, etc.), and
+ // c) BehaviorManager for any actions not handled in the graphics or physics threads.
+ std::shared_ptr<SurgSim::Graphics::OsgManager> graphicsManager = std::make_shared<SurgSim::Graphics::OsgManager>();
+ std::shared_ptr<PhysicsManager> physicsManager = std::make_shared<PhysicsManager>();
+ std::shared_ptr<SurgSim::Framework::BehaviorManager> behaviorManager =
+ std::make_shared<SurgSim::Framework::BehaviorManager>();
+ // A Runtime is the top-level container for all of the Managers and a single Scene.
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime(new SurgSim::Framework::Runtime());
+
+ runtime->addManager(physicsManager);
+ runtime->addManager(graphicsManager);
+ runtime->addManager(behaviorManager);
+
+ // A Scene is a container for all of the SceneElements, which in turn contain their Components.
+ // Since the Scene contains all of the Elements, a Runtime therefore has access to all of the Components.
+ std::shared_ptr<SurgSim::Framework::Scene> scene = runtime->getScene();
+
+ std::shared_ptr<SceneElement> earth = createEarth(data, "earth");
+ earth->setPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 3.0, 0.0)));
+ scene->addSceneElement(earth);
+
+ std::shared_ptr<SceneElement> plane = createPlane("plane");
+ plane->setPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.0)));
+ scene->addSceneElement(plane);
+
+ std::shared_ptr<ViewElement> view = std::make_shared<OsgViewElement>("view");
+ view->setPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.5, 5.0)));
+ scene->addSceneElement(view);
+
+ // Run the simulation, starting with initialize/startup of Managers and Components. For each Component of each
+ // Element (Runtime::preprocessSceneElements) the Runtime tries to give access to the Component to each of the
+ // Managers (Runtime::addComponents). The Managers only accept the types of Components that they manage. Once the
+ // Managers are running, the Runtime can be used to stop/step the Managers.
+ // Runtime::execute() will block until one of the Managers quits.
+ runtime->execute();
+
+ return 0;
+}
diff --git a/Examples/DroppingBalls/config.txt.in b/Examples/DroppingBalls/config.txt.in
new file mode 100644
index 0000000..d05f004
--- /dev/null
+++ b/Examples/DroppingBalls/config.txt.in
@@ -0,0 +1 @@
+${CMAKE_CURRENT_BINARY_DIR}/Data/
diff --git a/Examples/GraphicsScene/CMakeLists.txt b/Examples/GraphicsScene/CMakeLists.txt
new file mode 100644
index 0000000..1de07d0
--- /dev/null
+++ b/Examples/GraphicsScene/CMakeLists.txt
@@ -0,0 +1,56 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+find_package(OpenSceneGraph COMPONENTS osg osgViewer osgText osgUtil osgDB osgGA osgAnimation REQUIRED)
+find_package(OpenThreads)
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${OSG_INCLUDE_DIR}"
+ "${OPENTHREADS_INCLUDE_DIR}"
+)
+
+set(SOURCES
+ GraphicsScene.cpp
+)
+
+set(HEADERS
+)
+
+file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+add_executable(GraphicsScene ${SOURCES} ${HEADERS})
+surgsim_show_ide_folders("${SOURCES}" "${HEADERS}")
+
+SET(LIBS
+ SurgSimBlocks
+ SurgSimGraphics
+ ${YAML_CPP_LIBRARIES}
+)
+
+target_link_libraries(GraphicsScene ${LIBS})
+
+# Put GraphicsScene into folder "Examples"
+set_target_properties(GraphicsScene PROPERTIES FOLDER "Examples")
diff --git a/Examples/GraphicsScene/Data/Textures/CheckerBoard.png b/Examples/GraphicsScene/Data/Textures/CheckerBoard.png
new file mode 100644
index 0000000..aafbea2
Binary files /dev/null and b/Examples/GraphicsScene/Data/Textures/CheckerBoard.png differ
diff --git a/Examples/GraphicsScene/GraphicsScene.cpp b/Examples/GraphicsScene/GraphicsScene.cpp
new file mode 100644
index 0000000..1b5ce5b
--- /dev/null
+++ b/Examples/GraphicsScene/GraphicsScene.cpp
@@ -0,0 +1,463 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <osg/Matrix>
+#include <osg/Camera>
+
+#include "SurgSim/Blocks/PoseInterpolator.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/TransferPropertiesBehavior.h"
+#include "SurgSim/Graphics/Light.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/RenderPass.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+
+using SurgSim::Framework::Logger;
+using SurgSim::Graphics::OsgTextureUniform;
+using SurgSim::Graphics::OsgTexture2d;
+using SurgSim::Graphics::OsgUniform;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Matrix44f;
+
+/// \file
+/// This example creates a simple graphics scene and use the RenderPass object to show
+/// a simple shadowing algorithm. For the algorithm see http://en.wikipedia.org/wiki/Shadow_mapping
+/// There are two preprocessing passes that use specific shaders, plus a main pass that renders the
+/// objects using a shaders that can process the output from the preprocessing steps.
+/// Each shaders output becomes the input for the next shader, some parameters from other scene elements
+/// is passed into the shaders as uniforms.
+/// All of the information is kept up to date using a \sa TransferPropertiesBehavior
+/// Both the light and the main camera are being moved through a \sa PoseInterpolator to demonstrate
+/// dynamic changes and how to handle them in the rendering pipeline
+
+namespace
+{
+
+std::unordered_map<std::string, std::shared_ptr<SurgSim::Graphics::OsgMaterial>> materials;
+
+std::shared_ptr<SurgSim::Graphics::ViewElement> createView(const std::string& name, int x, int y, int width, int height)
+{
+ using SurgSim::Graphics::OsgViewElement;
+
+ std::shared_ptr<OsgViewElement> viewElement = std::make_shared<OsgViewElement>(name);
+ std::array<int, 2> position = {x, y};
+ std::array<int, 2> dimensions = {width, height};
+ viewElement->getView()->setPosition(position);
+ viewElement->getView()->setDimensions(dimensions);
+
+ /// It's an OsgViewElement, we have an OsgView, turn on mapping of uniform and attribute values
+ std::dynamic_pointer_cast<SurgSim::Graphics::OsgView>(viewElement->getView())->setOsgMapsUniforms(true);
+
+ // Move the camera from left to right over along the scene
+ auto interpolator = std::make_shared<SurgSim::Blocks::PoseInterpolator>("Interpolator_2");
+ RigidTransform3d from = makeRigidTransform(
+ Vector3d(-4.0, 2.0, -4.0),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0));
+ RigidTransform3d to = makeRigidTransform(
+ Vector3d(4.0, 2.0, -4.0),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0));
+ interpolator->setTarget(viewElement);
+ interpolator->setStartingPose(from);
+ interpolator->setDuration(10.0);
+ interpolator->setEndingPose(to);
+ interpolator->setPingPong(true);
+
+ viewElement->setPose(from);
+
+ viewElement->addComponent(interpolator);
+
+ return viewElement;
+}
+
+std::shared_ptr<SurgSim::Graphics::OsgMaterial> createMaterialWithShaders(
+ const SurgSim::Framework::ApplicationData& data,
+ const std::string& name)
+{
+
+ auto shader = SurgSim::Graphics::loadShader(data, name);
+
+ std::shared_ptr<SurgSim::Graphics::OsgMaterial> material;
+ if (shader != nullptr)
+ {
+ material = std::make_shared<SurgSim::Graphics::OsgMaterial>(name);
+ material->setShader(shader);
+ }
+
+ return material;
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createLight()
+{
+ auto result = std::make_shared<SurgSim::Framework::BasicSceneElement>("Light");
+
+ auto light = std::make_shared<SurgSim::Graphics::OsgLight>("Light");
+ light->setDiffuseColor(Vector4d(0.5, 0.5, 0.5, 1.0));
+ light->setSpecularColor(Vector4d(0.8, 0.8, 0.8, 1.0));
+ light->setLightGroupReference(SurgSim::Graphics::Representation::DefaultGroupName);
+ result->addComponent(light);
+
+ // Move the light from left to right over along the scene
+ auto interpolator = std::make_shared<SurgSim::Blocks::PoseInterpolator>("Interpolator");
+ RigidTransform3d from = makeRigidTransform(Vector3d(5.0, 3.0, -5.0),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0));
+ RigidTransform3d to = makeRigidTransform(Vector3d(-5.0, 3.0, -5.0),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0));
+ interpolator->setTarget(result);
+ interpolator->setStartingPose(from);
+ interpolator->setDuration(10.0);
+ interpolator->setEndingPose(to);
+ interpolator->setPingPong(true);
+
+ result->setPose(from);
+
+ result->addComponent(interpolator);
+
+ return result;
+}
+
+/// Create the pass that renders the scene from the view of the light source
+/// the identifier 'shadowing' is used in all graphic objects to mark them as used
+/// in this pass
+std::shared_ptr<SurgSim::Graphics::RenderPass> createLightMapPass()
+{
+ auto pass = std::make_shared<SurgSim::Graphics::RenderPass>("shadowing");
+ auto renderTarget = std::make_shared<SurgSim::Graphics::OsgRenderTarget2d>(1024, 1024, 1.0, 1, false);
+ pass->setRenderTarget(renderTarget);
+ pass->setRenderOrder(SurgSim::Graphics::Camera::RENDER_ORDER_PRE_RENDER, 0);
+ materials["depthMap"]->getShader()->setGlobalScope(true);
+ pass->setMaterial(materials["depthMap"]);
+
+ return pass;
+}
+
+/// Create the pass that renders shadowed pixels into the scene,
+/// the identifier 'shadowed' can be used in all graphics objects to mark them
+/// as used in this pass
+std::shared_ptr<SurgSim::Graphics::RenderPass> createShadowMapPass()
+{
+ auto pass = std::make_shared<SurgSim::Graphics::RenderPass>("shadowed");
+ auto renderTarget = std::make_shared<SurgSim::Graphics::OsgRenderTarget2d>(1024, 1024, 1.0, 1, false);
+ pass->setRenderTarget(renderTarget);
+ pass->setRenderOrder(SurgSim::Graphics::Camera::RENDER_ORDER_PRE_RENDER, 1);
+ materials["shadowMap"]->getShader()->setGlobalScope(true);
+ pass->setMaterial(materials["shadowMap"]);
+ return pass;
+}
+
+void configureShinyMaterial()
+{
+ // This will change the shared material
+ auto material = materials["shiny"];
+
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", SurgSim::Math::Vector4f(0.8, 0.8, 0.1, 1.0));
+
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", SurgSim::Math::Vector4f(0.9, 0.9, 0.1, 1.0));
+
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 32.0f);
+}
+
+void configureTexturedMaterial(const std::string& filename)
+{
+ auto material = materials["texturedShadowed"];
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", SurgSim::Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", SurgSim::Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 32.0f);
+
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ texture->loadImage(filename);
+
+ material->addUniform("sampler2D", "diffuseMap");
+ material->setValue("diffuseMap", texture);
+}
+
+/// A simple box as a scenelement
+class SimpleBox : public SurgSim::Framework::BasicSceneElement
+{
+public:
+ explicit SimpleBox(const std::string& name) : BasicSceneElement(name)
+ {
+ m_box = std::make_shared<SurgSim::Graphics::OsgBoxRepresentation>(getName() + " Graphics");
+
+ // The material that this object uses
+ m_box->setMaterial(materials["basicShadowed"]);
+
+ // Assign this to the pass for shadowing objects
+ m_box->addGroupReference("shadowing");
+
+ // Assign this to the pass for shadowed objects
+ m_box->addGroupReference("shadowed");
+ }
+
+ virtual bool doInitialize() override
+ {
+ addComponent(m_box);
+ return true;
+ }
+
+ void setSize(double width, double height, double length)
+ {
+ m_box->setSizeXYZ(width, height, length);
+ }
+
+ void setMaterial(const std::shared_ptr<SurgSim::Graphics::Material> material)
+ {
+ m_box->setMaterial(material);
+ }
+
+private:
+ std::shared_ptr<SurgSim::Graphics::OsgBoxRepresentation> m_box;
+};
+
+
+class SimpleSphere : public SurgSim::Framework::BasicSceneElement
+{
+public:
+ explicit SimpleSphere(const std::string& name) : BasicSceneElement(name)
+ {
+ m_sphere = std::make_shared<SurgSim::Graphics::OsgSphereRepresentation>(getName() + " Graphics");
+ m_sphere->setMaterial(materials["basicShadowed"]);
+ m_sphere->addGroupReference("shadowing");
+ m_sphere->addGroupReference("shadowed");
+ }
+
+ virtual bool doInitialize() override
+ {
+ addComponent(m_sphere);
+ return true;
+ }
+
+ void setRadius(double radius)
+ {
+ m_sphere->setRadius(radius);
+ }
+
+ void setMaterial(const std::shared_ptr<SurgSim::Graphics::Material> material)
+ {
+ m_sphere->setMaterial(material);
+ }
+
+private:
+ std::shared_ptr<SurgSim::Graphics::OsgSphereRepresentation> m_sphere;
+};
+
+
+}
+
+// Create an array of spheres
+void addSpheres(std::shared_ptr<SurgSim::Framework::Scene> scene)
+{
+ double radius = 0.05;
+ Vector3d origin(-1.0, 0.0, -1.0);
+ Vector3d spacing(0.5, 0.5, 0.5);
+ for (int i = 0; i < 3; ++i)
+ {
+ for (int j = 0; j < 3; ++j)
+ {
+ auto sphere = std::make_shared<SimpleSphere>("Sphere_" + std::to_string(static_cast<int64_t>(i * 3 + j)));
+ sphere->setRadius(radius);
+ Vector3d position = origin + Vector3d(spacing.array() * Vector3d(static_cast<double>(i),
+ 1.0,
+ static_cast<double>(j)).array());
+ sphere->setPose(makeRigidTransform(Quaterniond::Identity(), position));
+ scene->addSceneElement(sphere);
+ }
+ }
+
+}
+
+std::shared_ptr<SurgSim::Graphics::ScreenSpaceQuadRepresentation> makeDebugQuad(
+ const std::string& name,
+ std::shared_ptr<SurgSim::Graphics::Texture> texture,
+ double x, double y, double width, double height)
+{
+ auto result = std::make_shared<SurgSim::Graphics::OsgScreenSpaceQuadRepresentation>(name);
+ result->setTexture(texture);
+ result->setSize(width, height);
+ result->setLocation(x, y);
+ return result;
+}
+
+
+void createScene(std::shared_ptr<SurgSim::Framework::Runtime> runtime)
+{
+ configureShinyMaterial();
+ configureTexturedMaterial(runtime->getApplicationData()->findFile("Textures/CheckerBoard.png"));
+
+ auto scene = runtime->getScene();
+ auto box = std::make_shared<SimpleBox>("Plane");
+ box->setSize(3.0, 0.01, 3.0);
+ box->setPose(RigidTransform3d::Identity());
+ box->setMaterial(materials["texturedShadowed"]);
+ scene->addSceneElement(box);
+
+ box = std::make_shared<SimpleBox>("Box 1");
+ box->setSize(0.25, 1.0, 0.25);
+ box->setPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(1.0, 0.5, -1.0)));
+ scene->addSceneElement(box);
+
+ addSpheres(scene);
+
+ auto sphere = std::make_shared<SimpleSphere>("Shiny Sphere");
+ sphere->setRadius(0.25);
+ sphere->setMaterial(materials["shiny"]);
+
+ scene->addSceneElement(sphere);
+
+ std::shared_ptr<SurgSim::Graphics::ViewElement> viewElement = createView("View", 40, 40, 1024, 768);
+ scene->addSceneElement(viewElement);
+ auto mainCamera = viewElement->getCamera();
+
+ // This behavior is responsible to keep all values updated, in this example most targets
+ // will be uniforms that are used in shaders
+ auto copier = std::make_shared<SurgSim::Framework::TransferPropertiesBehavior>("Copier");
+ copier->setTargetManagerType(SurgSim::Framework::MANAGER_TYPE_GRAPHICS);
+ viewElement->addComponent(copier);
+ viewElement->addComponent(materials["shiny"]);
+ viewElement->addComponent(materials["texturedShadowed"]);
+
+
+ auto lightElement = createLight();
+ scene->addSceneElement(lightElement);
+
+ auto lightMapPass = createLightMapPass();
+ scene->addSceneElement(lightMapPass);
+
+ // connect the light pose and the light map camera pose, so when the light moves,
+ // this camera will move as well
+ copier->connect(lightElement->getPoseComponent(), "Pose", lightMapPass->getPoseComponent(), "Pose");
+
+ auto shadowMapPass = createShadowMapPass();
+
+ // The following three uniforms in the shadowMapPass, carry the information from the
+ // lightMapPass. They are used to project the incoming point into the space of the lightMap
+ // The view matrix of the camera used to render the light map
+ shadowMapPass->getMaterial()->addUniform("mat4", "lightViewMatrix");
+ copier->connect(lightMapPass->getCamera(), "FloatViewMatrix",
+ shadowMapPass->getMaterial(), "lightViewMatrix");
+
+ // The projection matrix of the camera used to render the light map
+ shadowMapPass->getMaterial()->addUniform("mat4", "lightProjectionMatrix");
+ copier->connect(lightMapPass->getCamera(), "FloatProjectionMatrix",
+ shadowMapPass->getMaterial(), "lightProjectionMatrix");
+
+// The inverse view matrix of the camera used to render the light map
+// HS-2016-jun-04 Leave commented for now, this exposes a bug in terms of timing with the
+// copy behaviors, while the inverseViewMatrix uniform is now available from the camera, using this
+// form causes rendering artifacts, possibly due to a mismatch between the viewMatrix and the inverse
+// auto inverseViewMatrix = std::make_shared<OsgUniform<Matrix44f>>("inverseViewMatrix");
+// shadowMapPass->getMaterial()->addUniform(inverseViewMatrix);
+// copier->connect(shadowMapPass->getCamera(), "FloatInverseViewMatrix",
+// shadowMapPass->getMaterial() , "inverseViewMatrix");
+
+ // Get the result of the lightMapPass and pass it on to the shadowMapPass, because it is used
+ // in a pass we ask the system to use a higher than normal texture unit (in this case 8) for
+ // this texture, this prevents the texture from being overwritten by other textures
+ std::shared_ptr<SurgSim::Graphics::OsgMaterial> material;
+ material = std::dynamic_pointer_cast<SurgSim::Graphics::OsgMaterial>(shadowMapPass->getMaterial());
+
+ material->addUniform("sampler2D", "encodedLightDepthMap");
+ material->setValue("encodedLightDepthMap", lightMapPass->getRenderTarget()->getColorTarget(0));
+ material->getUniform("encodedLightDepthMap")->setValue("MinimumTextureUnit", static_cast<size_t>(8));
+
+ // Make the camera in the shadowMapPass follow the main camera that is being used to render the
+ // whole scene
+ copier->connect(viewElement->getPoseComponent(), "Pose", shadowMapPass->getPoseComponent(), "Pose");
+ copier->connect(mainCamera, "ProjectionMatrix", shadowMapPass->getCamera() , "ProjectionMatrix");
+ scene->addSceneElement(shadowMapPass);
+
+
+ // Put the result of the last pass into the main camera to make it accessible
+ material = std::make_shared<SurgSim::Graphics::OsgMaterial>("material");
+
+ material->addUniform("sampler2D", "shadowMap");
+ material->setValue("shadowMap", shadowMapPass->getRenderTarget()->getColorTarget(0));
+ material->getUniform("shadowMap")->setValue("MinimumTextureUnit", static_cast<size_t>(8));
+ mainCamera->setMaterial(material);
+ viewElement->addComponent(material);
+
+ auto debug = std::make_shared<SurgSim::Framework::BasicSceneElement>("debug");
+ debug->addComponent(makeDebugQuad("light",
+ lightMapPass->getRenderTarget()->getColorTarget(0), 0, 0, 256, 256));
+ debug->addComponent(makeDebugQuad("shadow",
+ shadowMapPass->getRenderTarget()->getColorTarget(0), 1024 - 256, 0, 256, 256));
+ scene->addSceneElement(debug);
+}
+
+
+int main(int argc, char* argv[])
+{
+ auto runtime(std::make_shared<SurgSim::Framework::Runtime>("config.txt"));
+ auto data = runtime->getApplicationData();
+
+ materials["basicLit"] = createMaterialWithShaders(*data, "Shaders/basic_lit");
+ materials["basicUnlit"] = createMaterialWithShaders(*data, "Shaders/basic_unlit");
+ materials["basicShadowed"] = createMaterialWithShaders(*data, "Shaders/s_mapping");
+ materials["texturedShadowed"] = createMaterialWithShaders(*data, "Shaders/ds_mapping_material");
+ materials["shiny"] = createMaterialWithShaders(*data, "Shaders/material");
+ materials["depthMap"] = createMaterialWithShaders(*data, "Shaders/depth_map");
+ materials["shadowMap"] = createMaterialWithShaders(*data, "Shaders/shadow_map");
+ materials["default"] = materials["basic_lit"];
+
+ runtime->addManager(std::make_shared<SurgSim::Graphics::OsgManager>());
+ runtime->addManager(std::make_shared<SurgSim::Framework::BehaviorManager>());
+
+ createScene(runtime);
+
+ runtime->execute();
+
+ return 0;
+}
diff --git a/Examples/GraphicsScene/config.txt.in b/Examples/GraphicsScene/config.txt.in
new file mode 100644
index 0000000..4686595
--- /dev/null
+++ b/Examples/GraphicsScene/config.txt.in
@@ -0,0 +1,2 @@
+${SURGSIM_SOURCE_DIR}/Data/
+${CMAKE_CURRENT_BINARY_DIR}/Data/
diff --git a/Examples/InputVtc/CMakeLists.txt b/Examples/InputVtc/CMakeLists.txt
new file mode 100644
index 0000000..4a197c7
--- /dev/null
+++ b/Examples/InputVtc/CMakeLists.txt
@@ -0,0 +1,58 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ ${CMAKE_CURRENT_SOURCE_DIR}
+ ${OSG_INCLUDE_DIR}
+ ${OPENTHREADS_INCLUDE_DIR}
+)
+
+set(SOURCES
+ DeviceFactory.cpp
+ InputVtc.cpp
+)
+
+set(HEADERS
+ DeviceFactory.h
+)
+
+add_executable(InputVtc ${SOURCES} ${HEADERS})
+
+set(LIBS
+ IdentityPoseDevice
+ SurgSimBlocks
+ SurgSimGraphics
+ SurgSimMath
+ ${YAML_CPP_LIBRARIES}
+)
+
+if(BUILD_DEVICE_MULTIAXIS)
+ list(APPEND LIBS MultiAxisDevice)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMULTIAXISDEVICE_LIBRARY_AVAILABLE")
+endif(BUILD_DEVICE_MULTIAXIS)
+
+if(BUILD_DEVICE_NOVINT)
+ list(APPEND LIBS NovintDevice)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DNOVINT_LIBRARY_AVAILABLE")
+endif(BUILD_DEVICE_NOVINT)
+
+target_link_libraries(InputVtc ${LIBS})
+
+# Put InputVtc into folder "Examples"
+set_target_properties(InputVtc PROPERTIES FOLDER "Examples")
diff --git a/Examples/InputVtc/DeviceFactory.cpp b/Examples/InputVtc/DeviceFactory.cpp
new file mode 100644
index 0000000..2b48d1c
--- /dev/null
+++ b/Examples/InputVtc/DeviceFactory.cpp
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "Examples/InputVtc/DeviceFactory.h"
+
+#include "SurgSim/Framework/Log.h"
+
+#ifdef MULTIAXISDEVICE_LIBRARY_AVAILABLE
+#include "SurgSim/Devices/MultiAxis/MultiAxisDevice.h"
+#endif // MULTIAXISDEVICE_LIBRARY_AVAILABLE
+
+#ifdef NOVINT_LIBRARY_AVAILABLE
+#include "SurgSim/Devices/Novint/NovintDevice.h"
+#endif // NOVINT_LIBRARY_AVAILABLE
+
+using SurgSim::Framework::Logger;
+
+
+DeviceFactory::DeviceFactory()
+{
+}
+DeviceFactory::~DeviceFactory()
+{
+}
+
+std::shared_ptr<SurgSim::Input::DeviceInterface> DeviceFactory::getDevice(const std::string& name)
+{
+ std::shared_ptr<Logger> logger = Logger::getDefaultLogger();
+
+ // First check for a Falcon. Did we build NovintDevice, and will the device initialize?
+#ifdef NOVINT_LIBRARY_AVAILABLE
+ SURGSIM_LOG_INFO(logger) << "DeviceFactory is going to try using a NovintDevice, a default Falcon.";
+ std::shared_ptr<SurgSim::Device::NovintDevice> novintDevice =
+ std::make_shared<SurgSim::Device::NovintDevice>(name, "");
+ novintDevice->setPositionScale(novintDevice->getPositionScale() * 10.0);
+
+ if (novintDevice->initialize())
+ {
+ return novintDevice;
+ }
+ SURGSIM_LOG_WARNING(logger) << "Could not initialize the NovintDevice.";
+#endif // NOVINT_LIBRARY_AVAILABLE
+
+ // Then try MultiAxisDevice.
+#ifdef MULTIAXISDEVICE_LIBRARY_AVAILABLE
+ SURGSIM_LOG_INFO(logger) << "DeviceFactory is going to try using a MultiAxisDevice.";
+ std::shared_ptr<SurgSim::Device::MultiAxisDevice> multiAxisDevice =
+ std::make_shared<SurgSim::Device::MultiAxisDevice>(name);
+ multiAxisDevice->setPositionScale(multiAxisDevice->getPositionScale() * 10.0);
+ multiAxisDevice->setOrientationScale(multiAxisDevice->getOrientationScale() * 3.0);
+
+ if (multiAxisDevice->initialize())
+ {
+ return multiAxisDevice;
+ }
+ SURGSIM_LOG_WARNING(logger) << "Could not initialize the MultiAxisDevice.";
+#endif // MULTIAXISDEVICE_LIBRARY_AVAILABLE
+
+ // failed to instantiate a device
+ return nullptr;
+}
diff --git a/Examples/InputVtc/DeviceFactory.h b/Examples/InputVtc/DeviceFactory.h
new file mode 100644
index 0000000..e58bac3
--- /dev/null
+++ b/Examples/InputVtc/DeviceFactory.h
@@ -0,0 +1,45 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef EXAMPLES_INPUTVTC_DEVICEFACTORY_H
+#define EXAMPLES_INPUTVTC_DEVICEFACTORY_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/DeviceInterface.h"
+
+/// A class that creates an instance of a suitable subclass of DeviceInterface.
+///
+/// The device created depends on the compiler flags (from CMake) and success/failure of device initialization attempts.
+class DeviceFactory
+{
+public:
+ /// Constructor.
+ DeviceFactory();
+
+ /// Destructor.
+ ~DeviceFactory();
+
+ /// Creates a device.
+ /// This class will try to create a NovintDevice. If the NovintDevice library is not available, or the device does
+ /// not initialize, this class tries to create a MultiAxisDevice.
+ /// \param name The name passed to the device.
+ /// \return A shared pointer to an instance of a subclass of DeviceInterface, or nullptr on failure.
+ std::shared_ptr<SurgSim::Input::DeviceInterface> getDevice(const std::string& name);
+};
+
+
+#endif // EXAMPLES_INPUTVTC_DEVICEFACTORY_H
diff --git a/Examples/InputVtc/InputVtc.cpp b/Examples/InputVtc/InputVtc.cpp
new file mode 100644
index 0000000..881df9c
--- /dev/null
+++ b/Examples/InputVtc/InputVtc.cpp
@@ -0,0 +1,255 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Blocks/DriveElementFromInputBehavior.h"
+#include "SurgSim/Devices/MultiAxis/MultiAxisDevice.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgPlaneRepresentation.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Input/InputManager.h"
+#include "SurgSim/Input/OutputComponent.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/VirtualToolCoupler.h"
+
+#include "Examples/InputVtc/DeviceFactory.h"
+
+using SurgSim::Blocks::DriveElementFromInputBehavior;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Framework::Logger;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Graphics::OsgBoxRepresentation;
+using SurgSim::Graphics::OsgMaterial;
+using SurgSim::Graphics::OsgPlaneRepresentation;
+using SurgSim::Graphics::OsgShader;
+using SurgSim::Graphics::OsgUniform;
+using SurgSim::Graphics::OsgViewElement;
+using SurgSim::Graphics::ViewElement;
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Vector4f;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::FixedRepresentation;
+using SurgSim::Physics::Representation;
+using SurgSim::Physics::RigidRepresentation;
+using SurgSim::Physics::PhysicsManager;
+using SurgSim::Physics::VirtualToolCoupler;
+
+std::shared_ptr<SceneElement> createPlane(const std::string& name)
+{
+ std::shared_ptr<DoubleSidedPlaneShape> planeShape = std::make_shared<DoubleSidedPlaneShape>();
+
+ std::shared_ptr<FixedRepresentation> physicsRepresentation =
+ std::make_shared<FixedRepresentation>(name + " Physics");
+ physicsRepresentation->setShape(planeShape);
+
+ std::shared_ptr<OsgPlaneRepresentation> graphicsRepresentation =
+ std::make_shared<OsgPlaneRepresentation>(name + " Graphics");
+
+ std::shared_ptr<OsgMaterial> material = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<OsgShader> shader = std::make_shared<OsgShader>();
+
+ std::shared_ptr<OsgUniform<Vector4f>> uniform = std::make_shared<OsgUniform<Vector4f>>("color");
+ uniform->set(Vector4f(0.0f, 0.6f, 1.0f, 0.0f));
+ material->addUniform(uniform);
+
+ shader->setFragmentShaderSource(
+ "uniform vec4 color;\n"
+ "void main(void)\n"
+ "{\n"
+ " gl_FragColor = color;\n"
+ "}");
+ material->setShader(shader);
+ graphicsRepresentation->setMaterial(material);
+
+ std::shared_ptr<SceneElement> planeElement = std::make_shared<BasicSceneElement>(name);
+ planeElement->addComponent(physicsRepresentation);
+ planeElement->addComponent(graphicsRepresentation);
+ planeElement->addComponent(material);
+
+ std::shared_ptr<SurgSim::Physics::RigidCollisionRepresentation> collisionRepresentation;
+ collisionRepresentation = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>(name + " Collision");
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+ planeElement->addComponent(collisionRepresentation);
+ return planeElement;
+}
+
+
+std::shared_ptr<SceneElement> createBox(const std::string& name, const std::string& toolDeviceName)
+{
+ std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(0.8, 2.0, 0.2); // in meter
+
+ // Physics Components
+ std::shared_ptr<RigidRepresentation> physicsRepresentation =
+ std::make_shared<RigidRepresentation>(name + " Physics");
+ physicsRepresentation->setIsGravityEnabled(false);
+ physicsRepresentation->setDensity(0.1);
+ physicsRepresentation->setShape(box);
+
+ // Collision Components
+ auto collisionRepresentation =
+ std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Box Collision Representation");
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+
+ // Graphics Components
+ std::shared_ptr<SurgSim::Graphics::BoxRepresentation> graphicsRepresentation =
+ std::make_shared<OsgBoxRepresentation>(name + " Graphics");
+ graphicsRepresentation->setSizeXYZ(box->getSizeX(), box->getSizeY(), box->getSizeZ());
+
+ // Input Components
+ std::shared_ptr<SurgSim::Input::InputComponent> inputComponent =
+ std::make_shared<SurgSim::Input::InputComponent>(name + " Input");
+ inputComponent->setDeviceName(toolDeviceName);
+
+ // Output Components
+ std::shared_ptr<SurgSim::Input::OutputComponent> outputComponent = nullptr;
+ outputComponent = std::make_shared<SurgSim::Input::OutputComponent>(name + " Output");
+ outputComponent->setDeviceName(toolDeviceName);
+
+ // A VTC (virtual tool coupler, aka "god object") is used to couple an input/output thread and a physics thread,
+ // running at different rates. Picture a user holding a haptic device (e.g., Falcon or Omni). The
+ // device's pose is used to position a simulated tool, but that pose may cause collisions and the resulting forces
+ // are to be displayed to the user via the device. If the collisions and physics response can be determined in the
+ // callback from the device's API, the appropriate forces can be calculated there. Unfortunately, typically physics
+ // threads update much slower than the ~1000 Hz used for threads controlling haptic devices. For example, if the
+ // physics thread updates at 100 Hz, there will be ~10 haptic callbacks that each receive the same force, which
+ // tends to create an unstable response in the haptic device (delays in feedback loops often cause limit cycles and
+ // other instabilities), and reduces the fidelity of the haptic "feel".
+ //
+ // Instead, we couple the pose coming from the haptic device to a "virtual tool". The virtual tool follows the
+ // input pose exactly as long as it is not colliding. As soon as the virtual tool collides, it interacts with the
+ // scene normally (through collisions and physics), plus the virtual tool and haptic device are connected via spring
+ // & damper forces.
+ //
+ // The spring forces attempt to pull the haptic device and the virtual tool together (pulling against the user on
+ // one side and the physics scene on the other). The damping forces remove energy from the system to increase
+ // stability. Note that the forces applied to the haptic device come solely from the spring & damper connected to
+ // the virtual tool, should be zero when the tool is not colliding, and should be calculated in a high update rate
+ // thread. We pass the device forces&torques and the derivatives (Jacobians) of forces&torques with respect to
+ // position and velocity, so that the device's Scaffold can make those calculations. The forces on the device can
+ // be scaled up or down from the forces on the virtual tool.
+ std::shared_ptr<VirtualToolCoupler> inputCoupler = std::make_shared<VirtualToolCoupler>(name + " Input Coupler");
+ inputCoupler->setInput(inputComponent);
+ inputCoupler->setOutput(outputComponent);
+ inputCoupler->setRepresentation(physicsRepresentation);
+
+ // The SceneElement
+ std::shared_ptr<BasicSceneElement> boxElement = std::make_shared<BasicSceneElement>(name);
+ boxElement->addComponent(physicsRepresentation);
+ boxElement->addComponent(collisionRepresentation);
+ boxElement->addComponent(graphicsRepresentation);
+ boxElement->addComponent(inputComponent);
+ boxElement->addComponent(outputComponent);
+ boxElement->addComponent(inputCoupler);
+
+ return boxElement;
+}
+
+std::shared_ptr<SceneElement> createBoxForRawInput(const std::string& name, const std::string& toolDeviceName)
+{
+ std::shared_ptr<SurgSim::Graphics::BoxRepresentation> graphicsRepresentation;
+ graphicsRepresentation = std::make_shared<OsgBoxRepresentation>(name + " Graphics");
+ graphicsRepresentation->setSizeXYZ(0.8, 2.0, 0.2);
+ std::shared_ptr<OsgMaterial> material = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<OsgShader> shader = std::make_shared<OsgShader>();
+ shader->setVertexShaderSource(
+ "void main(void)\n"
+ "{\n"
+ " gl_Position = ftransform();\n"
+ "}");
+ shader->setFragmentShaderSource(
+ "void main(void)\n"
+ "{\n"
+ " gl_FragColor = vec4(0.2, 0.2, 0.2, 1.0);\n"
+ "}");
+ material->setShader(shader);
+ graphicsRepresentation->setMaterial(material);
+
+ std::shared_ptr<SurgSim::Input::InputComponent> inputComponent;
+ inputComponent = std::make_shared<SurgSim::Input::InputComponent>(name + " Input");
+ inputComponent->setDeviceName(toolDeviceName);
+
+ std::shared_ptr<DriveElementFromInputBehavior> driver;
+ driver = std::make_shared<DriveElementFromInputBehavior>(name + " Driver");
+ driver->setSource(inputComponent);
+
+ std::shared_ptr<BasicSceneElement> element = std::make_shared<BasicSceneElement>(name);
+ element->addComponent(graphicsRepresentation);
+ element->addComponent(inputComponent);
+ element->addComponent(driver);
+ element->addComponent(material);
+
+ return element;
+}
+
+int main(int argc, char* argv[])
+{
+ static const char* const toolDeviceName = "Tool Device";
+ std::shared_ptr<SurgSim::Graphics::OsgManager> graphicsManager = std::make_shared<SurgSim::Graphics::OsgManager>();
+ std::shared_ptr<PhysicsManager> physicsManager = std::make_shared<PhysicsManager>();
+ std::shared_ptr<SurgSim::Framework::BehaviorManager> behaviorManager =
+ std::make_shared<SurgSim::Framework::BehaviorManager>();
+ std::shared_ptr<SurgSim::Input::InputManager> inputManager = std::make_shared<SurgSim::Input::InputManager>();
+
+ DeviceFactory deviceFactory;
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device = deviceFactory.getDevice(toolDeviceName);
+ SURGSIM_ASSERT(device != nullptr) << "Unable to get a device, is one connected?";
+ inputManager->addDevice(device);
+
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime(new SurgSim::Framework::Runtime());
+ runtime->addManager(physicsManager);
+ runtime->addManager(graphicsManager);
+ runtime->addManager(behaviorManager);
+ runtime->addManager(inputManager);
+
+ std::shared_ptr<SurgSim::Framework::Scene> scene = runtime->getScene();
+ scene->addSceneElement(createBox("VTC Box", toolDeviceName));
+ scene->addSceneElement(createBoxForRawInput("Raw Input", toolDeviceName));
+
+ std::shared_ptr<SceneElement> plane = createPlane("Plane");
+ plane->setPose(makeRigidTransform(SurgSim::Math::Quaterniond::Identity(), Vector3d(0.0, -1.0, 0.0)));
+ scene->addSceneElement(plane);
+
+ std::shared_ptr<ViewElement> viewElement = std::make_shared<OsgViewElement>("view");
+ viewElement->setPose(makeRigidTransform(SurgSim::Math::Quaterniond::Identity(), Vector3d(0.0, 0.5, 5.0)));
+ scene->addSceneElement(viewElement);
+
+ runtime->execute();
+
+ return 0;
+}
diff --git a/Examples/Stapling/CMakeLists.txt b/Examples/Stapling/CMakeLists.txt
new file mode 100644
index 0000000..5823ab3
--- /dev/null
+++ b/Examples/Stapling/CMakeLists.txt
@@ -0,0 +1,67 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ ${CMAKE_CURRENT_SOURCE_DIR}
+ ${OSG_INCLUDE_DIR}
+)
+
+set(SOURCES
+ StapleElement.cpp
+ StaplerBehavior.cpp
+)
+
+set(HEADERS
+ StapleElement.h
+ StaplerBehavior.h
+)
+
+add_executable(Stapling
+ Stapling.cpp
+ ${SOURCES}
+ ${HEADERS}
+)
+
+add_executable(SerializedStapling
+ SerializedStapling.cpp
+ ${SOURCES}
+ ${HEADERS}
+)
+
+add_dependencies(SerializedStapling yaml-cpp)
+
+surgsim_show_ide_folders("Main.cpp SerializedMain.cpp ${SOURCES}" "${HEADERS}")
+
+set(LIBS
+ IdentityPoseDevice
+ MultiAxisDevice
+ SurgSimBlocks
+ SurgSimFramework
+ SurgSimGraphics
+ ${YAML_CPP_LIBRARIES}
+)
+target_link_libraries(Stapling ${LIBS})
+target_link_libraries(SerializedStapling ${LIBS})
+
+file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+set_target_properties(Stapling PROPERTIES FOLDER "Examples")
+set_target_properties(SerializedStapling PROPERTIES FOLDER "Examples")
diff --git a/Examples/Stapling/Data/Geometry/arm_collision.ply b/Examples/Stapling/Data/Geometry/arm_collision.ply
new file mode 100644
index 0000000..bc55c96
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/arm_collision.ply
@@ -0,0 +1,2125 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 1582
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+property float s
+property float t
+element face 528
+property list uchar uint vertex_indices
+end_header
+-0.034406 -0.085027 -0.138728 0.463145 -0.829541 -0.312024 0.135340 0.509730
+-0.095526 -0.117518 -0.143070 0.463145 -0.829541 -0.312024 0.282049 0.406200
+-0.073474 -0.097069 -0.164703 0.463145 -0.829541 -0.312024 0.259420 0.472470
+-0.095526 -0.117518 -0.143070 -0.477746 -0.729957 0.488795 0.282049 0.406200
+-0.089139 -0.098688 -0.108707 -0.477746 -0.729957 0.488795 0.188060 0.358130
+-0.129650 -0.099186 -0.149046 -0.477746 -0.729957 0.488795 0.350790 0.366940
+-0.129650 -0.099186 -0.149046 -0.496761 -0.707815 0.502222 0.350790 0.366940
+-0.133343 -0.079854 -0.125453 -0.496761 -0.707815 0.502222 0.293109 0.318900
+-0.158610 -0.087944 -0.161847 -0.496761 -0.707815 0.502222 0.443599 0.357970
+-0.133343 -0.079854 -0.125453 -0.732850 -0.346174 0.585742 0.293109 0.318900
+-0.147506 -0.066233 -0.135123 -0.732850 -0.346174 0.585742 0.342009 0.305170
+-0.158610 -0.087944 -0.161847 -0.732850 -0.346174 0.585742 0.443599 0.357970
+-0.140091 -0.047694 -0.134212 -0.313873 0.817137 0.483498 0.319526 0.270466
+-0.112058 -0.031450 -0.143467 -0.313873 0.817137 0.483498 0.278074 0.212619
+-0.137006 -0.024205 -0.171907 -0.313873 0.817137 0.483498 0.365560 0.214520
+-0.147506 -0.066233 -0.135123 -0.903873 0.349389 0.246860 0.342009 0.305170
+-0.127485 -0.041471 -0.096863 -0.903873 0.349389 0.246860 0.207157 0.259402
+-0.140091 -0.047694 -0.134212 -0.903873 0.349389 0.246860 0.319526 0.270466
+-0.127485 -0.041471 -0.096863 -0.715701 0.686737 0.127140 0.207157 0.259402
+-0.107972 -0.020968 -0.097765 -0.715701 0.686737 0.127140 0.188810 0.220410
+-0.140091 -0.047694 -0.134212 -0.715701 0.686737 0.127140 0.319526 0.270466
+-0.140091 -0.047694 -0.134212 -0.531301 0.834859 -0.143978 0.319526 0.270466
+-0.107972 -0.020968 -0.097765 -0.531301 0.834859 -0.143978 0.188810 0.220410
+-0.112058 -0.031450 -0.143467 -0.531301 0.834859 -0.143978 0.278074 0.212619
+-0.127485 -0.041471 -0.096863 -0.718029 0.687994 0.105349 0.207157 0.259402
+-0.100711 -0.017457 -0.071205 -0.718029 0.687994 0.105349 0.153949 0.220629
+-0.107972 -0.020968 -0.097765 -0.718029 0.687994 0.105349 0.188810 0.220410
+-0.100711 -0.017457 -0.071205 -0.795001 0.438312 0.419352 0.153949 0.220629
+-0.127485 -0.041471 -0.096863 -0.795001 0.438312 0.419352 0.207157 0.259402
+-0.094869 -0.027848 -0.049269 -0.795001 0.438312 0.419352 0.131870 0.234050
+-0.094869 -0.027848 -0.049269 -0.834640 0.121010 0.537339 0.131870 0.234050
+-0.127485 -0.041471 -0.096863 -0.834640 0.121010 0.537339 0.207157 0.259402
+-0.117000 -0.054747 -0.077587 -0.834640 0.121010 0.537339 0.163450 0.270300
+-0.117000 -0.054747 -0.077587 -0.650931 -0.557859 0.514861 0.163450 0.270300
+-0.133343 -0.079854 -0.125453 -0.650931 -0.557859 0.514861 0.293109 0.318900
+-0.095276 -0.068078 -0.064566 -0.650931 -0.557859 0.514861 0.130968 0.286588
+-0.127485 -0.041471 -0.096863 -0.882285 -0.013640 0.470517 0.207157 0.259402
+-0.147506 -0.066233 -0.135123 -0.882285 -0.013640 0.470517 0.342009 0.305170
+-0.117000 -0.054747 -0.077587 -0.882285 -0.013640 0.470517 0.163450 0.270300
+-0.147506 -0.066233 -0.135123 -0.754814 -0.438572 0.487761 0.342009 0.305170
+-0.133343 -0.079854 -0.125453 -0.754814 -0.438572 0.487761 0.293109 0.318900
+-0.117000 -0.054747 -0.077587 -0.754814 -0.438572 0.487761 0.163450 0.270300
+-0.113704 -0.014641 -0.177831 0.106434 0.895194 0.432782 0.354810 0.871980
+-0.112058 -0.031450 -0.143467 0.106434 0.895194 0.432782 0.296340 0.934129
+-0.089934 -0.029127 -0.153713 0.106434 0.895194 0.432782 0.266570 0.800189
+-0.089934 -0.029127 -0.153713 -0.118744 0.992428 -0.031396 0.266570 0.800189
+-0.112058 -0.031450 -0.143467 -0.118744 0.992428 -0.031396 0.296340 0.934129
+-0.079289 -0.027650 -0.147286 -0.118744 0.992428 -0.031396 0.236930 0.764110
+-0.137006 -0.024205 -0.171907 -0.244844 0.866271 0.435461 0.366090 0.965530
+-0.112058 -0.031450 -0.143467 -0.244844 0.866271 0.435461 0.296340 0.934129
+-0.113704 -0.014641 -0.177831 -0.244844 0.866271 0.435461 0.354810 0.871980
+-0.079289 -0.027650 -0.147286 0.201966 0.942022 -0.267964 0.236930 0.764110
+-0.082414 -0.008469 -0.082211 0.201966 0.942022 -0.267964 0.147348 0.905596
+-0.051717 -0.030921 -0.138004 0.201966 0.942022 -0.267964 0.172420 0.668253
+-0.082414 -0.008469 -0.082211 0.000804 0.927855 -0.372941 0.147348 0.905596
+-0.025528 -0.015002 -0.098342 0.000804 0.927855 -0.372941 0.106645 0.656621
+-0.051717 -0.030921 -0.138004 0.000804 0.927855 -0.372941 0.172420 0.668253
+-0.112058 -0.031450 -0.143467 -0.136721 0.968133 -0.209823 0.296340 0.934129
+-0.107972 -0.020968 -0.097765 -0.136721 0.968133 -0.209823 0.209280 0.974420
+-0.079289 -0.027650 -0.147286 -0.136721 0.968133 -0.209823 0.236930 0.764110
+-0.107972 -0.020968 -0.097765 -0.276259 0.918193 -0.283906 0.209280 0.974420
+-0.082414 -0.008469 -0.082211 -0.276259 0.918193 -0.283906 0.147348 0.905596
+-0.079289 -0.027650 -0.147286 -0.276259 0.918193 -0.283906 0.236930 0.764110
+-0.142783 -0.123768 -0.254342 0.665454 -0.706442 -0.241061 0.656300 0.541330
+-0.101305 -0.091627 -0.234032 0.665454 -0.706442 -0.241061 0.553391 0.588060
+-0.102843 -0.103309 -0.204043 0.665454 -0.706442 -0.241061 0.405861 0.510860
+-0.102843 -0.103309 -0.204043 0.696247 -0.680205 -0.229262 0.405861 0.510860
+-0.101305 -0.091627 -0.234032 0.696247 -0.680205 -0.229262 0.553391 0.588060
+-0.072018 -0.074296 -0.196510 0.696247 -0.680205 -0.229262 0.356783 0.579623
+-0.142783 -0.123768 -0.254342 0.627018 -0.777378 -0.050307 0.656300 0.541330
+-0.142754 -0.121434 -0.290047 0.627018 -0.777378 -0.050307 0.750953 0.583825
+-0.101305 -0.091627 -0.234032 0.627018 -0.777378 -0.050307 0.553391 0.588060
+-0.118145 -0.075826 -0.291408 0.815307 -0.450766 -0.363433 0.807090 0.695700
+-0.101305 -0.091627 -0.234032 0.815307 -0.450766 -0.363433 0.553391 0.588060
+-0.142754 -0.121434 -0.290047 0.815307 -0.450766 -0.363433 0.750953 0.583825
+-0.094587 -0.070190 -0.237734 0.811379 -0.336776 -0.477751 0.571026 0.654340
+-0.072018 -0.074296 -0.196510 0.811379 -0.336776 -0.477751 0.356783 0.579623
+-0.101305 -0.091627 -0.234032 0.811379 -0.336776 -0.477751 0.553391 0.588060
+-0.101305 -0.091627 -0.234032 0.875332 -0.334594 -0.349057 0.553391 0.588060
+-0.118145 -0.075826 -0.291408 0.875332 -0.334594 -0.349057 0.807090 0.695700
+-0.094587 -0.070190 -0.237734 0.875332 -0.334594 -0.349057 0.571026 0.654340
+-0.118145 -0.075826 -0.291408 0.910991 0.067262 -0.406905 0.807090 0.695700
+-0.107834 -0.057088 -0.265226 0.910991 0.067262 -0.406905 0.726160 0.731651
+-0.094587 -0.070190 -0.237734 0.910991 0.067262 -0.406905 0.571026 0.654340
+-0.100382 -0.018451 -0.205285 0.630584 0.461830 -0.623760 0.411672 0.797526
+-0.071727 -0.047559 -0.197868 0.630584 0.461830 -0.623760 0.379059 0.666993
+-0.094587 -0.070190 -0.237734 0.630584 0.461830 -0.623760 0.571026 0.654340
+-0.107834 -0.057088 -0.265226 0.908900 0.289840 -0.299822 0.726160 0.731651
+-0.100382 -0.018451 -0.205285 0.908900 0.289840 -0.299822 0.411672 0.797526
+-0.094587 -0.070190 -0.237734 0.908900 0.289840 -0.299822 0.571026 0.654340
+-0.100382 -0.018451 -0.205285 0.872117 0.354694 -0.337053 0.411672 0.797526
+-0.107834 -0.057088 -0.265226 0.872117 0.354694 -0.337053 0.726160 0.731651
+-0.117261 -0.018451 -0.248959 0.872117 0.354694 -0.337053 0.595351 0.822090
+-0.100382 -0.018451 -0.205285 0.712515 0.701656 0.000902 0.411672 0.797526
+-0.089934 -0.029127 -0.153713 0.712515 0.701656 0.000902 0.266570 0.800189
+-0.071727 -0.047559 -0.197868 0.712515 0.701656 0.000902 0.379059 0.666993
+-0.089934 -0.029127 -0.153713 0.084482 0.931430 -0.353979 0.266570 0.800189
+-0.079289 -0.027650 -0.147286 0.084482 0.931430 -0.353979 0.236930 0.764110
+-0.071727 -0.047559 -0.197868 0.084482 0.931430 -0.353979 0.379059 0.666993
+-0.137698 -0.007571 -0.242054 0.278313 0.960489 0.001757 0.534770 0.882390
+-0.113704 -0.014641 -0.177831 0.278313 0.960489 0.001757 0.354810 0.871980
+-0.100382 -0.018451 -0.205285 0.278313 0.960489 0.001757 0.411672 0.797526
+-0.113704 -0.014641 -0.177831 0.447186 0.889536 0.093549 0.354810 0.871980
+-0.089934 -0.029127 -0.153713 0.447186 0.889536 0.093549 0.266570 0.800189
+-0.100382 -0.018451 -0.205285 0.447186 0.889536 0.093549 0.411672 0.797526
+-0.137698 -0.007571 -0.242054 0.420355 0.892698 -0.162457 0.534770 0.882390
+-0.100382 -0.018451 -0.205285 0.420355 0.892698 -0.162457 0.411672 0.797526
+-0.117261 -0.018451 -0.248959 0.420355 0.892698 -0.162457 0.595351 0.822090
+-0.169827 -0.013664 -0.222033 -0.096689 0.984765 0.144531 0.462741 0.950520
+-0.113704 -0.014641 -0.177831 -0.096689 0.984765 0.144531 0.354810 0.871980
+-0.137698 -0.007571 -0.242054 -0.096689 0.984765 0.144531 0.534770 0.882390
+-0.072018 -0.074296 -0.196510 0.276017 -0.787433 -0.551147 0.356783 0.579623
+-0.054913 -0.089177 -0.166683 0.276017 -0.787433 -0.551147 0.228820 0.514750
+-0.073474 -0.097069 -0.164703 0.276017 -0.787433 -0.551147 0.255890 0.486160
+-0.072018 -0.074296 -0.196510 0.708039 -0.379637 -0.595447 0.356783 0.579623
+-0.053999 -0.068243 -0.178943 0.708039 -0.379637 -0.595447 0.256240 0.570673
+-0.054913 -0.089177 -0.166683 0.708039 -0.379637 -0.595447 0.228820 0.514750
+-0.094587 -0.070190 -0.237734 0.875211 -0.034034 -0.482543 0.571026 0.654340
+-0.071727 -0.047559 -0.197868 0.875211 -0.034034 -0.482543 0.379059 0.666993
+-0.072018 -0.074296 -0.196510 0.875211 -0.034034 -0.482543 0.356783 0.579623
+-0.071727 -0.047559 -0.197868 0.769087 -0.040767 -0.637842 0.379059 0.666993
+-0.056141 -0.053097 -0.178721 0.769087 -0.040767 -0.637842 0.263661 0.612951
+-0.072018 -0.074296 -0.196510 0.769087 -0.040767 -0.637842 0.356783 0.579623
+-0.072018 -0.074296 -0.196510 0.675592 0.106238 -0.729581 0.356783 0.579623
+-0.056141 -0.053097 -0.178721 0.675592 0.106238 -0.729581 0.263661 0.612951
+-0.053999 -0.068243 -0.178943 0.675592 0.106238 -0.729581 0.256240 0.570673
+-0.056141 -0.053097 -0.178721 0.744189 0.114890 -0.658015 0.263661 0.612951
+-0.040966 -0.049422 -0.160917 0.744189 0.114890 -0.658015 0.191410 0.590020
+-0.053999 -0.068243 -0.178943 0.744189 0.114890 -0.658015 0.256240 0.570673
+-0.056141 -0.053097 -0.178721 0.318008 0.837714 -0.443966 0.263661 0.612951
+-0.079289 -0.027650 -0.147286 0.318008 0.837714 -0.443966 0.236930 0.764110
+-0.040966 -0.049422 -0.160917 0.318008 0.837714 -0.443966 0.191410 0.590020
+-0.040966 -0.049422 -0.160917 0.272144 0.807082 -0.523982 0.191410 0.590020
+-0.079289 -0.027650 -0.147286 0.272144 0.807082 -0.523982 0.236930 0.764110
+-0.051717 -0.030921 -0.138004 0.272144 0.807082 -0.523982 0.172420 0.668253
+-0.051717 -0.030921 -0.138004 0.476749 0.660592 -0.579939 0.172420 0.668253
+-0.025528 -0.015002 -0.098342 0.476749 0.660592 -0.579939 0.106645 0.656621
+-0.024684 -0.048566 -0.135880 0.476749 0.660592 -0.579939 0.122524 0.566469
+-0.071727 -0.047559 -0.197868 0.562916 0.794306 -0.228482 0.379059 0.666993
+-0.079289 -0.027650 -0.147286 0.562916 0.794306 -0.228482 0.236930 0.764110
+-0.056141 -0.053097 -0.178721 0.562916 0.794306 -0.228482 0.263661 0.612951
+-0.053999 -0.068243 -0.178943 0.785684 0.046741 -0.616860 0.256240 0.570673
+-0.040966 -0.049422 -0.160917 0.785684 0.046741 -0.616860 0.191410 0.590020
+-0.032947 -0.077119 -0.152802 0.785684 0.046741 -0.616860 0.169058 0.522833
+-0.040966 -0.049422 -0.160917 0.834235 0.081761 -0.545313 0.191410 0.590020
+-0.024684 -0.048566 -0.135880 0.834235 0.081761 -0.545313 0.122524 0.566469
+-0.032947 -0.077119 -0.152802 0.834235 0.081761 -0.545313 0.169058 0.522833
+-0.040966 -0.049422 -0.160917 0.528082 0.764562 -0.369561 0.191410 0.590020
+-0.051717 -0.030921 -0.138004 0.528082 0.764562 -0.369561 0.172420 0.668253
+-0.024684 -0.048566 -0.135880 0.528082 0.764562 -0.369561 0.122524 0.566469
+-0.034406 -0.085027 -0.138728 0.608441 -0.717136 -0.339875 0.133570 0.514180
+-0.054913 -0.089177 -0.166683 0.608441 -0.717136 -0.339875 0.228820 0.514750
+-0.032947 -0.077119 -0.152802 0.608441 -0.717136 -0.339875 0.169058 0.522833
+-0.054913 -0.089177 -0.166683 0.637355 -0.409948 -0.652473 0.228820 0.514750
+-0.053999 -0.068243 -0.178943 0.637355 -0.409948 -0.652473 0.256240 0.570673
+-0.032947 -0.077119 -0.152802 0.637355 -0.409948 -0.652473 0.169058 0.522833
+-0.034406 -0.085027 -0.138728 0.374905 -0.916591 -0.138950 0.133570 0.514180
+-0.073474 -0.097069 -0.164703 0.374905 -0.916591 -0.138950 0.255890 0.486160
+-0.054913 -0.089177 -0.166683 0.374905 -0.916591 -0.138950 0.228820 0.514750
+-0.095526 -0.117518 -0.143070 0.512856 -0.820429 -0.252735 0.282049 0.406200
+-0.102843 -0.103309 -0.204043 0.512856 -0.820429 -0.252735 0.415881 0.497320
+-0.073474 -0.097069 -0.164703 0.512856 -0.820429 -0.252735 0.259420 0.472470
+-0.073474 -0.097069 -0.164703 0.676644 -0.612992 -0.407913 0.259420 0.472470
+-0.102843 -0.103309 -0.204043 0.676644 -0.612992 -0.407913 0.415881 0.497320
+-0.072018 -0.074296 -0.196510 0.676644 -0.612992 -0.407913 0.361480 0.499110
+-0.095526 -0.117518 -0.143070 0.236883 -0.939518 -0.247370 0.282049 0.406200
+-0.124310 -0.115529 -0.178188 0.236883 -0.939518 -0.247370 0.421920 0.440600
+-0.102843 -0.103309 -0.204043 0.236883 -0.939518 -0.247370 0.415881 0.497320
+-0.102843 -0.103309 -0.204043 0.424966 -0.904694 0.030537 0.415881 0.497320
+-0.151491 -0.126917 -0.226451 0.424966 -0.904694 0.030537 0.608100 0.477680
+-0.142783 -0.123768 -0.254342 0.424966 -0.904694 0.030537 0.645660 0.519570
+-0.168455 -0.131316 -0.250414 0.272706 -0.960063 -0.062537 0.689931 0.491150
+-0.142754 -0.121434 -0.290047 0.272706 -0.960063 -0.062537 0.754360 0.554610
+-0.142783 -0.123768 -0.254342 0.272706 -0.960063 -0.062537 0.645660 0.519570
+-0.162641 -0.121335 -0.214932 -0.169057 -0.941215 0.292463 0.600580 0.452870
+-0.168455 -0.131316 -0.250414 -0.169057 -0.941215 0.292463 0.689931 0.491150
+-0.151491 -0.126917 -0.226451 -0.169057 -0.941215 0.292463 0.608100 0.477680
+-0.168455 -0.131316 -0.250414 -0.250108 -0.920587 0.299941 0.689931 0.491150
+-0.162641 -0.121335 -0.214932 -0.250108 -0.920587 0.299941 0.600580 0.452870
+-0.192548 -0.119802 -0.235165 -0.250108 -0.920587 0.299941 0.712841 0.454750
+-0.192548 -0.119802 -0.235165 -0.575113 -0.594797 0.561660 0.712841 0.454750
+-0.175871 -0.092860 -0.189557 -0.575113 -0.594797 0.561660 0.559241 0.386500
+-0.205504 -0.102593 -0.230207 -0.575113 -0.594797 0.561660 0.730051 0.431070
+-0.205504 -0.102593 -0.230207 -0.813566 -0.099523 0.572892 0.730051 0.431070
+-0.180305 -0.073818 -0.189423 -0.813566 -0.099523 0.572892 0.576059 0.350370
+-0.204769 -0.059822 -0.221733 -0.813566 -0.099523 0.572892 0.717807 0.342718
+-0.162641 -0.121335 -0.214932 -0.386754 -0.765893 0.513643 0.600580 0.452870
+-0.161023 -0.105518 -0.190129 -0.386754 -0.765893 0.513643 0.528620 0.411331
+-0.192548 -0.119802 -0.235165 -0.386754 -0.765893 0.513643 0.712841 0.454750
+-0.161023 -0.105518 -0.190129 -0.521526 -0.637389 0.567226 0.528620 0.411331
+-0.175871 -0.092860 -0.189557 -0.521526 -0.637389 0.567226 0.559241 0.386500
+-0.192548 -0.119802 -0.235165 -0.521526 -0.637389 0.567226 0.712841 0.454750
+-0.175871 -0.092860 -0.189557 -0.772734 -0.184209 0.607413 0.559241 0.386500
+-0.180305 -0.073818 -0.189423 -0.772734 -0.184209 0.607413 0.576059 0.350370
+-0.205504 -0.102593 -0.230207 -0.772734 -0.184209 0.607413 0.730051 0.431070
+-0.180305 -0.073818 -0.189423 -0.797283 -0.000158 0.603606 0.576059 0.350370
+-0.164097 -0.053186 -0.168009 -0.797283 -0.000158 0.603606 0.473341 0.291050
+-0.204769 -0.059822 -0.221733 -0.797283 -0.000158 0.603606 0.717807 0.342718
+-0.161023 -0.105518 -0.190129 -0.561833 -0.680306 0.470667 0.528620 0.411331
+-0.158610 -0.087944 -0.161847 -0.561833 -0.680306 0.470667 0.443599 0.357970
+-0.175871 -0.092860 -0.189557 -0.561833 -0.680306 0.470667 0.559241 0.386500
+-0.175871 -0.092860 -0.189557 -0.816845 -0.194029 0.543248 0.559241 0.386500
+-0.158610 -0.087944 -0.161847 -0.816845 -0.194029 0.543248 0.443599 0.357970
+-0.180305 -0.073818 -0.189423 -0.816845 -0.194029 0.543248 0.576059 0.350370
+-0.161023 -0.105518 -0.190129 -0.494924 -0.718494 0.488688 0.528620 0.411331
+-0.129650 -0.099186 -0.149046 -0.494924 -0.718494 0.488688 0.350790 0.366940
+-0.158610 -0.087944 -0.161847 -0.494924 -0.718494 0.488688 0.443599 0.357970
+-0.158610 -0.087944 -0.161847 -0.818624 -0.229412 0.526522 0.443599 0.357970
+-0.147506 -0.066233 -0.135123 -0.818624 -0.229412 0.526522 0.342009 0.305170
+-0.180305 -0.073818 -0.189423 -0.818624 -0.229412 0.526522 0.576059 0.350370
+-0.180305 -0.073818 -0.189423 -0.854780 0.158605 0.494161 0.576059 0.350370
+-0.147506 -0.066233 -0.135123 -0.854780 0.158605 0.494161 0.342009 0.305170
+-0.164097 -0.053186 -0.168009 -0.854780 0.158605 0.494161 0.473341 0.291050
+-0.147506 -0.066233 -0.135123 -0.801007 0.294772 0.521054 0.342009 0.305170
+-0.140091 -0.047694 -0.134212 -0.801007 0.294772 0.521054 0.319526 0.270466
+-0.164097 -0.053186 -0.168009 -0.801007 0.294772 0.521054 0.473341 0.291050
+-0.140091 -0.047694 -0.134212 -0.624110 0.709123 0.328073 0.319526 0.270466
+-0.147443 -0.045298 -0.153377 -0.624110 0.709123 0.328073 0.381480 0.262520
+-0.164097 -0.053186 -0.168009 -0.624110 0.709123 0.328073 0.473341 0.291050
+-0.175397 -0.029894 -0.208187 -0.687990 0.526958 0.498984 0.596889 0.253480
+-0.164097 -0.053186 -0.168009 -0.687990 0.526958 0.498984 0.473341 0.291050
+-0.147443 -0.045298 -0.153377 -0.687990 0.526958 0.498984 0.381480 0.262520
+-0.164097 -0.053186 -0.168009 -0.723318 0.489391 0.487142 0.473341 0.291050
+-0.175397 -0.029894 -0.208187 -0.723318 0.489391 0.487142 0.596889 0.253480
+-0.204769 -0.059822 -0.221733 -0.723318 0.489391 0.487142 0.717807 0.342718
+-0.175397 -0.029894 -0.208187 -0.741902 0.563983 0.362636 0.596889 0.253480
+-0.169827 -0.013664 -0.222033 -0.741902 0.563983 0.362636 0.587758 0.225680
+-0.204769 -0.059822 -0.221733 -0.741902 0.563983 0.362636 0.717807 0.342718
+-0.102843 -0.103309 -0.204043 0.519801 -0.853827 0.028033 0.415881 0.497320
+-0.124310 -0.115529 -0.178188 0.519801 -0.853827 0.028033 0.421920 0.440600
+-0.123395 -0.115491 -0.193997 0.519801 -0.853827 0.028033 0.457371 0.460770
+-0.102843 -0.103309 -0.204043 0.471050 -0.876510 -0.099206 0.415881 0.497320
+-0.123395 -0.115491 -0.193997 0.471050 -0.876510 -0.099206 0.457371 0.460770
+-0.151491 -0.126917 -0.226451 0.471050 -0.876510 -0.099206 0.608100 0.477680
+-0.124310 -0.115529 -0.178188 -0.059584 -0.998206 -0.005848 0.421920 0.440600
+-0.139576 -0.114521 -0.194705 -0.059584 -0.998206 -0.005848 0.499065 0.446365
+-0.123395 -0.115491 -0.193997 -0.059584 -0.998206 -0.005848 0.457371 0.460770
+-0.123395 -0.115491 -0.193997 -0.072024 -0.919631 0.386125 0.457371 0.460770
+-0.139576 -0.114521 -0.194705 -0.072024 -0.919631 0.386125 0.499065 0.446365
+-0.151491 -0.126917 -0.226451 -0.072024 -0.919631 0.386125 0.608100 0.477680
+-0.139576 -0.114521 -0.194705 -0.064537 -0.921118 0.383896 0.499065 0.446365
+-0.162641 -0.121335 -0.214932 -0.064537 -0.921118 0.383896 0.600580 0.452870
+-0.151491 -0.126917 -0.226451 -0.064537 -0.921118 0.383896 0.608100 0.477680
+-0.139576 -0.114521 -0.194705 -0.227822 -0.814171 0.534063 0.499065 0.446365
+-0.161023 -0.105518 -0.190129 -0.227822 -0.814171 0.534063 0.528620 0.411331
+-0.162641 -0.121335 -0.214932 -0.227822 -0.814171 0.534063 0.600580 0.452870
+-0.129650 -0.099186 -0.149046 -0.294368 -0.884819 0.361169 0.350790 0.366940
+-0.161023 -0.105518 -0.190129 -0.294368 -0.884819 0.361169 0.528620 0.411331
+-0.139576 -0.114521 -0.194705 -0.294368 -0.884819 0.361169 0.499065 0.446365
+-0.095526 -0.117518 -0.143070 -0.489842 -0.795621 0.356430 0.282049 0.406200
+-0.129650 -0.099186 -0.149046 -0.489842 -0.795621 0.356430 0.350790 0.366940
+-0.124310 -0.115529 -0.178188 -0.489842 -0.795621 0.356430 0.421920 0.440600
+-0.168455 -0.131316 -0.250414 0.279015 -0.960051 -0.021281 0.689931 0.491150
+-0.142783 -0.123768 -0.254342 0.279015 -0.960051 -0.021281 0.645660 0.519570
+-0.151491 -0.126917 -0.226451 0.279015 -0.960051 -0.021281 0.608100 0.477680
+-0.175397 -0.029894 -0.208187 -0.578082 0.635273 0.512102 0.596889 0.253480
+-0.137006 -0.024205 -0.171907 -0.578082 0.635273 0.512102 0.365560 0.214520
+-0.169827 -0.013664 -0.222033 -0.578082 0.635273 0.512102 0.587758 0.225680
+-0.169827 -0.013664 -0.222033 -0.272387 0.890056 0.365521 0.587758 0.225680
+-0.137006 -0.024205 -0.171907 -0.272387 0.890056 0.365521 0.365560 0.214520
+-0.113704 -0.014641 -0.177831 -0.272387 0.890056 0.365521 0.380571 0.208017
+-0.175397 -0.029894 -0.208187 -0.549633 0.688114 0.473712 0.596889 0.253480
+-0.147443 -0.045298 -0.153377 -0.549633 0.688114 0.473712 0.381480 0.262520
+-0.137006 -0.024205 -0.171907 -0.549633 0.688114 0.473712 0.365560 0.214520
+-0.147443 -0.045298 -0.153377 -0.686218 0.641229 0.343410 0.381480 0.262520
+-0.140091 -0.047694 -0.134212 -0.686218 0.641229 0.343410 0.319526 0.270466
+-0.137006 -0.024205 -0.171907 -0.686218 0.641229 0.343410 0.365560 0.214520
+-0.095276 -0.068078 -0.064566 -0.488904 -0.747201 0.450181 0.130968 0.286588
+-0.133343 -0.079854 -0.125453 -0.488904 -0.747201 0.450181 0.293109 0.318900
+-0.089139 -0.098688 -0.108707 -0.488904 -0.747201 0.450181 0.188060 0.358130
+-0.133343 -0.079854 -0.125453 -0.493067 -0.709186 0.503924 0.293109 0.318900
+-0.129650 -0.099186 -0.149046 -0.493067 -0.709186 0.503924 0.350790 0.366940
+-0.089139 -0.098688 -0.108707 -0.493067 -0.709186 0.503924 0.188060 0.358130
+-0.029661 -0.072947 -0.026017 -0.402720 -0.693022 0.597944 0.040560 0.331810
+-0.016507 -0.053189 0.005742 -0.402720 -0.693022 0.597944 0.082710 0.266979
+-0.095276 -0.068078 -0.064566 -0.402720 -0.693022 0.597944 0.130968 0.286588
+-0.095276 -0.068078 -0.064566 -0.529318 -0.485446 0.695819 0.130968 0.286588
+-0.016507 -0.053189 0.005742 -0.529318 -0.485446 0.695819 0.082710 0.266979
+-0.077178 -0.040515 -0.031569 -0.529318 -0.485446 0.695819 0.101726 0.254639
+-0.095276 -0.068078 -0.064566 -0.636433 -0.383377 0.669309 0.130968 0.286588
+-0.077178 -0.040515 -0.031569 -0.636433 -0.383377 0.669309 0.101726 0.254639
+-0.117000 -0.054747 -0.077587 -0.636433 -0.383377 0.669309 0.163450 0.270300
+-0.117000 -0.054747 -0.077587 -0.737679 -0.096543 0.668213 0.163450 0.270300
+-0.077178 -0.040515 -0.031569 -0.737679 -0.096543 0.668213 0.101726 0.254639
+-0.094869 -0.027848 -0.049269 -0.737679 -0.096543 0.668213 0.131870 0.234050
+-0.029661 -0.072947 -0.026017 0.012096 -0.951483 0.307463 0.040560 0.331810
+-0.069241 -0.093206 -0.087154 0.012096 -0.951483 0.307463 0.131630 0.354850
+-0.012845 -0.075192 -0.033626 0.012096 -0.951483 0.307463 0.064239 0.351802
+-0.069241 -0.093206 -0.087154 -0.000226 -0.917027 0.398825 0.131630 0.354850
+-0.095526 -0.117518 -0.143070 -0.000226 -0.917027 0.398825 0.282049 0.406200
+-0.047403 -0.100711 -0.104398 -0.000226 -0.917027 0.398825 0.159656 0.410770
+-0.095276 -0.068078 -0.064566 -0.329351 -0.796728 0.506708 0.130968 0.286588
+-0.089139 -0.098688 -0.108707 -0.329351 -0.796728 0.506708 0.188060 0.358130
+-0.069241 -0.093206 -0.087154 -0.329351 -0.796728 0.506708 0.131630 0.354850
+-0.069241 -0.093206 -0.087154 -0.320327 -0.804401 0.500329 0.131630 0.354850
+-0.089139 -0.098688 -0.108707 -0.320327 -0.804401 0.500329 0.188060 0.358130
+-0.095526 -0.117518 -0.143070 -0.320327 -0.804401 0.500329 0.282049 0.406200
+-0.029661 -0.072947 -0.026017 -0.347068 -0.799819 0.489728 0.040560 0.331810
+-0.095276 -0.068078 -0.064566 -0.347068 -0.799819 0.489728 0.130968 0.286588
+-0.069241 -0.093206 -0.087154 -0.347068 -0.799819 0.489728 0.131630 0.354850
+-0.129650 -0.099186 -0.149046 -0.454677 -0.809791 0.370820 0.350790 0.366940
+-0.139576 -0.114521 -0.194705 -0.454677 -0.809791 0.370820 0.499065 0.446365
+-0.124310 -0.115529 -0.178188 -0.454677 -0.809791 0.370820 0.421920 0.440600
+-0.100711 -0.017457 -0.071205 -0.440118 0.897938 0.001620 0.143802 0.982094
+-0.082414 -0.008469 -0.082211 -0.440118 0.897938 0.001620 0.147348 0.905596
+-0.107972 -0.020968 -0.097765 -0.440118 0.897938 0.001620 0.209280 0.974420
+-0.045772 -0.006289 -0.019566 -0.515516 0.712087 0.476629 0.000000 0.000000
+-0.070686 -0.013774 -0.035330 -0.515516 0.712087 0.476629 1.000000 1.000000
+-0.060966 -0.016531 -0.020698 -0.515516 0.712087 0.476629 1.000000 0.000000
+-0.045772 -0.006289 -0.019566 -0.515529 0.712052 0.476667 0.000000 0.000000
+-0.080407 -0.011017 -0.049962 -0.515529 0.712052 0.476667 1.000000 0.000000
+-0.070686 -0.013774 -0.035330 -0.515529 0.712052 0.476667 1.000000 1.000000
+-0.042958 -0.003989 -0.023216 -0.365347 0.888354 0.278117 0.000000 0.000000
+-0.080407 -0.011017 -0.049962 -0.365347 0.888354 0.278117 1.000000 1.000000
+-0.045772 -0.006289 -0.019566 -0.365347 0.888354 0.278117 1.000000 0.000000
+-0.042958 -0.003989 -0.023216 -0.264898 0.956846 0.119474 0.000000 0.000000
+-0.064116 -0.005870 -0.055063 -0.264898 0.956846 0.119474 1.000000 1.000000
+-0.080407 -0.011017 -0.049962 -0.264898 0.956846 0.119474 1.000000 0.000000
+-0.045772 -0.006289 -0.019566 -0.536799 0.754638 0.377317 0.000000 0.000000
+-0.060966 -0.016531 -0.020698 -0.536799 0.754638 0.377317 1.000000 1.000000
+-0.041213 -0.007368 -0.010922 -0.536799 0.754638 0.377317 1.000000 0.000000
+-0.041213 -0.007368 -0.010922 -0.561355 0.562765 0.606775 0.000000 0.000000
+-0.060966 -0.016531 -0.020698 -0.561355 0.562765 0.606775 1.000000 0.000000
+-0.045510 -0.018062 -0.004979 -0.561355 0.562765 0.606775 1.000000 1.000000
+-0.038985 -0.007852 -0.007374 -0.673320 0.546990 0.497435 0.000000 0.000000
+-0.041213 -0.007368 -0.010922 -0.673320 0.546990 0.497435 1.000000 0.000000
+-0.045510 -0.018062 -0.004979 -0.673320 0.546990 0.497435 1.000000 1.000000
+-0.038985 -0.007852 -0.007374 -0.640237 0.537824 0.548491 0.000000 0.000000
+-0.045510 -0.018062 -0.004979 -0.640237 0.537824 0.548491 1.000000 1.000000
+-0.031328 -0.009383 0.003065 -0.640237 0.537824 0.548491 1.000000 0.000000
+-0.031328 -0.009383 0.003065 -0.627779 0.416148 0.657810 0.000000 0.000000
+-0.045510 -0.018062 -0.004979 -0.627779 0.416148 0.657810 1.000000 1.000000
+-0.030055 -0.019593 0.010739 -0.627779 0.416148 0.657810 1.000000 0.000000
+-0.031328 -0.009383 0.003065 -0.541170 0.461086 0.703232 0.000000 0.000000
+-0.030055 -0.019593 0.010739 -0.541170 0.461086 0.703232 1.000000 1.000000
+-0.024969 -0.006524 0.006084 -0.541170 0.461086 0.703232 1.000000 0.000000
+0.005457 0.003922 0.021488 -0.480042 0.796582 0.367447 0.000000 0.000000
+-0.003050 0.001094 0.016505 -0.480042 0.796582 0.367447 1.000000 0.000000
+-0.003552 -0.007667 0.034842 -0.480042 0.796582 0.367447 1.000000 1.000000
+0.005457 0.003922 0.021488 -0.615744 0.751523 0.236795 0.000000 0.000000
+-0.003552 -0.007667 0.034842 -0.615744 0.751523 0.236795 1.000000 0.000000
+0.012243 0.001958 0.045367 -0.615744 0.751523 0.236795 1.000000 1.000000
+0.005457 0.003922 0.021488 -0.318409 0.933086 0.167231 0.000000 0.000000
+0.012243 0.001958 0.045367 -0.318409 0.933086 0.167231 1.000000 1.000000
+0.017288 0.006507 0.029591 -0.318409 0.933086 0.167231 1.000000 0.000000
+0.025400 0.008882 0.035581 -0.374675 0.915863 0.144271 0.000000 0.000000
+0.017288 0.006507 0.029591 -0.374675 0.915863 0.144271 1.000000 0.000000
+0.012243 0.001958 0.045367 -0.374675 0.915863 0.144271 1.000000 1.000000
+0.025400 0.008882 0.035581 -0.494629 0.867597 -0.051154 0.000000 0.000000
+0.012243 0.001958 0.045367 -0.494629 0.867597 -0.051154 1.000000 1.000000
+0.028037 0.011583 0.055893 -0.494629 0.867597 -0.051154 1.000000 0.000000
+0.025400 0.008882 0.035581 -0.265878 0.959503 -0.093073 0.000000 0.000000
+0.028037 0.011583 0.055893 -0.265878 0.959503 -0.093073 1.000000 1.000000
+0.028559 0.009976 0.037835 -0.265878 0.959503 -0.093073 1.000000 0.000000
+0.028559 0.009976 0.037835 -0.116648 0.988960 -0.091381 0.000000 0.000000
+0.028037 0.011583 0.055893 -0.116648 0.988960 -0.091381 1.000000 0.000000
+0.036877 0.011536 0.044100 -0.116648 0.988960 -0.091381 1.000000 1.000000
+0.028559 0.009976 0.037835 -0.087429 0.987671 -0.129854 0.000000 0.000000
+0.036877 0.011536 0.044100 -0.087429 0.987671 -0.129854 1.000000 1.000000
+0.031086 0.009975 0.036126 -0.087429 0.987671 -0.129854 1.000000 0.000000
+0.037350 0.010384 0.030379 -0.110824 0.990030 -0.086942 0.000000 0.000000
+0.036877 0.011536 0.044100 -0.110824 0.990030 -0.086942 1.000000 1.000000
+0.045717 0.011490 0.032308 -0.110824 0.990030 -0.086942 1.000000 0.000000
+0.031086 0.009975 0.036126 -0.144849 0.985556 -0.087740 0.000000 0.000000
+0.036877 0.011536 0.044100 -0.144849 0.985556 -0.087740 1.000000 1.000000
+0.037350 0.010384 0.030379 -0.144849 0.985556 -0.087740 1.000000 0.000000
+0.040411 0.009454 0.028108 -0.026287 0.912167 -0.408975 0.000000 0.000000
+0.037350 0.010384 0.030379 -0.026287 0.912167 -0.408975 1.000000 0.000000
+0.045717 0.011490 0.032308 -0.026287 0.912167 -0.408975 1.000000 1.000000
+0.048486 0.003959 0.022397 0.143885 0.806883 -0.572920 0.000000 0.000000
+0.040411 0.009454 0.028108 0.143885 0.806883 -0.572920 1.000000 0.000000
+0.045717 0.011490 0.032308 0.143885 0.806883 -0.572920 1.000000 1.000000
+0.055057 -0.002370 0.018595 0.420243 0.711610 -0.563034 0.000000 0.000000
+0.052980 -0.000209 0.019776 0.420243 0.711610 -0.563034 1.000000 0.000000
+0.063030 -0.001161 0.026074 0.420243 0.711610 -0.563034 1.000000 1.000000
+0.055057 -0.002370 0.018595 0.368501 0.772161 -0.517663 0.000000 0.000000
+0.063030 -0.001161 0.026074 0.368501 0.772161 -0.517663 1.000000 0.000000
+0.050502 -0.009120 0.005284 0.368501 0.772161 -0.517663 1.000000 1.000000
+0.055057 -0.002370 0.018595 0.709834 0.499780 -0.496342 0.000000 0.000000
+0.050502 -0.009120 0.005284 0.709834 0.499780 -0.496342 1.000000 1.000000
+0.050963 -0.002376 0.012734 0.709834 0.499780 -0.496342 1.000000 0.000000
+0.050963 -0.002376 0.012734 0.698510 0.508482 -0.503519 0.000000 0.000000
+0.050502 -0.009120 0.005284 0.698510 0.508482 -0.503519 1.000000 1.000000
+0.042826 -0.002162 0.001662 0.698510 0.508482 -0.503519 1.000000 0.000000
+0.042826 -0.002162 0.001662 0.700957 0.518210 -0.490018 0.000000 0.000000
+0.050502 -0.009120 0.005284 0.700957 0.518210 -0.490018 1.000000 0.000000
+0.040124 -0.009825 -0.010307 0.700957 0.518210 -0.490018 1.000000 1.000000
+0.037686 -0.002161 -0.005301 0.676497 0.541338 -0.499304 0.000000 0.000000
+0.042826 -0.002162 0.001662 0.676497 0.541338 -0.499304 1.000000 0.000000
+0.040124 -0.009825 -0.010307 0.676497 0.541338 -0.499304 1.000000 1.000000
+0.037686 -0.002161 -0.005301 0.691229 0.536271 -0.484372 0.000000 0.000000
+0.040124 -0.009825 -0.010307 0.691229 0.536271 -0.484372 1.000000 1.000000
+0.029745 -0.010530 -0.025899 0.691229 0.536271 -0.484372 1.000000 0.000000
+0.037686 -0.002161 -0.005301 0.544411 0.682763 -0.487290 0.000000 0.000000
+0.029745 -0.010530 -0.025899 0.544411 0.682763 -0.487290 1.000000 1.000000
+0.026633 -0.001955 -0.017361 0.544411 0.682763 -0.487290 1.000000 0.000000
+0.026633 -0.001955 -0.017361 0.612689 0.658097 -0.437631 0.000000 0.000000
+0.029745 -0.010530 -0.025899 0.612689 0.658097 -0.437631 1.000000 0.000000
+0.019367 -0.011236 -0.041490 0.612689 0.658097 -0.437631 1.000000 1.000000
+0.026633 -0.001955 -0.017361 0.419944 0.797484 -0.433204 0.000000 0.000000
+0.019367 -0.011236 -0.041490 0.419944 0.797484 -0.433204 1.000000 1.000000
+0.018301 -0.002420 -0.026294 0.419944 0.797484 -0.433204 1.000000 0.000000
+0.018301 -0.002420 -0.026294 0.310508 0.831512 -0.460622 0.000000 0.000000
+0.019367 -0.011236 -0.041490 0.310508 0.831512 -0.460622 1.000000 1.000000
+0.010551 -0.002935 -0.032448 0.310508 0.831512 -0.460622 1.000000 0.000000
+0.010551 -0.002935 -0.032448 0.442964 0.832609 -0.332483 0.000000 0.000000
+0.019367 -0.011236 -0.041490 0.442964 0.832609 -0.332483 1.000000 1.000000
+0.008989 -0.011941 -0.057082 0.442964 0.832609 -0.332483 1.000000 0.000000
+0.010551 -0.002935 -0.032448 0.230950 0.908997 -0.346967 0.000000 0.000000
+0.008989 -0.011941 -0.057082 0.230950 0.908997 -0.346967 1.000000 1.000000
+0.002911 -0.002986 -0.037667 0.230950 0.908997 -0.346967 1.000000 0.000000
+0.002911 -0.002986 -0.037667 0.251867 0.906368 -0.339205 0.000000 0.000000
+0.008989 -0.011941 -0.057082 0.251867 0.906368 -0.339205 1.000000 1.000000
+-0.004764 -0.002641 -0.042444 0.251867 0.906368 -0.339205 1.000000 0.000000
+-0.004764 -0.002641 -0.042444 0.286681 0.907444 -0.307180 0.000000 0.000000
+0.008989 -0.011941 -0.057082 0.286681 0.907444 -0.307180 1.000000 0.000000
+-0.008270 -0.013472 -0.077712 0.286681 0.907444 -0.307180 1.000000 1.000000
+-0.014017 -0.001829 -0.047775 0.257267 0.916287 -0.306972 0.000000 0.000000
+-0.004764 -0.002641 -0.042444 0.257267 0.916287 -0.306972 1.000000 0.000000
+-0.008270 -0.013472 -0.077712 0.257267 0.916287 -0.306972 1.000000 1.000000
+-0.014017 -0.001829 -0.047775 0.279052 0.911860 -0.301068 0.000000 0.000000
+-0.008270 -0.013472 -0.077712 0.279052 0.911860 -0.301068 1.000000 1.000000
+-0.025528 -0.015002 -0.098342 0.279052 0.911860 -0.301068 1.000000 0.000000
+-0.014017 -0.001829 -0.047775 0.106410 0.959588 -0.260514 0.000000 0.000000
+-0.036677 -0.007862 -0.079253 0.106410 0.959588 -0.260514 1.000000 1.000000
+-0.020227 0.000071 -0.043313 0.106410 0.959588 -0.260514 1.000000 0.000000
+-0.014017 -0.001829 -0.047775 0.133057 0.951290 -0.278106 0.000000 0.000000
+-0.025528 -0.015002 -0.098342 0.133057 0.951290 -0.278106 1.000000 0.000000
+-0.036677 -0.007862 -0.079253 0.133057 0.951290 -0.278106 1.000000 1.000000
+-0.020227 0.000071 -0.043313 0.140005 0.951457 -0.274096 0.000000 0.000000
+-0.036677 -0.007862 -0.079253 0.140005 0.951457 -0.274096 1.000000 1.000000
+-0.047826 -0.000722 -0.060163 0.140005 0.951457 -0.274096 1.000000 0.000000
+-0.026591 0.001059 -0.038391 0.051904 0.989943 -0.131603 0.000000 0.000000
+-0.020227 0.000071 -0.043313 0.051904 0.989943 -0.131603 1.000000 0.000000
+-0.047826 -0.000722 -0.060163 0.051904 0.989943 -0.131603 1.000000 1.000000
+-0.026591 0.001059 -0.038391 -0.038404 0.998284 -0.044205 0.000000 0.000000
+-0.047826 -0.000722 -0.060163 -0.038404 0.998284 -0.044205 1.000000 1.000000
+-0.030314 0.001072 -0.034863 -0.038404 0.998284 -0.044205 1.000000 0.000000
+-0.034613 -0.000049 -0.031344 -0.212069 0.975881 0.051798 0.000000 0.000000
+-0.030314 0.001072 -0.034863 -0.212069 0.975881 0.051798 1.000000 0.000000
+-0.055971 -0.003296 -0.057613 -0.212069 0.975881 0.051798 1.000000 1.000000
+-0.030314 0.001072 -0.034863 -0.266010 0.956932 0.116270 0.000000 0.000000
+-0.047826 -0.000722 -0.060163 -0.266010 0.956932 0.116270 1.000000 0.000000
+-0.055971 -0.003296 -0.057613 -0.266010 0.956932 0.116270 1.000000 1.000000
+-0.038250 -0.001447 -0.027644 -0.267589 0.958424 0.099096 0.000000 0.000000
+-0.034613 -0.000049 -0.031344 -0.267589 0.958424 0.099096 1.000000 0.000000
+-0.055971 -0.003296 -0.057613 -0.267589 0.958424 0.099096 1.000000 1.000000
+-0.038250 -0.001447 -0.027644 -0.270871 0.957291 0.101107 0.000000 0.000000
+-0.055971 -0.003296 -0.057613 -0.270871 0.957291 0.101107 1.000000 1.000000
+-0.064116 -0.005870 -0.055063 -0.270871 0.957291 0.101107 1.000000 0.000000
+-0.038250 -0.001447 -0.027644 -0.339058 0.925165 0.170615 0.000000 0.000000
+-0.064116 -0.005870 -0.055063 -0.339058 0.925165 0.170615 1.000000 1.000000
+-0.042958 -0.003989 -0.023216 -0.339058 0.925165 0.170615 1.000000 0.000000
+-0.024969 -0.006524 0.006084 -0.700964 0.465309 0.540497 0.000000 0.000000
+-0.030055 -0.019593 0.010739 -0.700964 0.465309 0.540497 1.000000 0.000000
+-0.023429 -0.016611 0.016765 -0.700964 0.465309 0.540497 1.000000 1.000000
+-0.019299 -0.004036 0.008727 -0.539148 0.572132 0.618049 0.000000 0.000000
+-0.024969 -0.006524 0.006084 -0.539148 0.572132 0.618049 1.000000 0.000000
+-0.023429 -0.016611 0.016765 -0.539148 0.572132 0.618049 1.000000 1.000000
+-0.012031 -0.001713 0.012042 -0.484270 0.578566 0.656311 0.000000 0.000000
+-0.019299 -0.004036 0.008727 -0.484270 0.578566 0.656311 1.000000 0.000000
+-0.023429 -0.016611 0.016765 -0.484270 0.578566 0.656311 1.000000 1.000000
+-0.012031 -0.001713 0.012042 -0.657173 0.632668 0.409701 0.000000 0.000000
+-0.023429 -0.016611 0.016765 -0.657173 0.632668 0.409701 1.000000 1.000000
+-0.016803 -0.013630 0.022790 -0.657173 0.632668 0.409701 1.000000 0.000000
+-0.006243 0.000018 0.014676 -0.460670 0.689048 0.559460 0.000000 0.000000
+-0.012031 -0.001713 0.012042 -0.460670 0.689048 0.559460 1.000000 0.000000
+-0.016803 -0.013630 0.022790 -0.460670 0.689048 0.559460 1.000000 1.000000
+-0.006243 0.000018 0.014676 -0.629542 0.694363 0.348620 0.000000 0.000000
+-0.016803 -0.013630 0.022790 -0.629542 0.694363 0.348620 1.000000 1.000000
+-0.003552 -0.007667 0.034842 -0.629542 0.694363 0.348620 1.000000 0.000000
+-0.006243 0.000018 0.014676 -0.479183 0.796996 0.367668 0.000000 0.000000
+-0.003552 -0.007667 0.034842 -0.479183 0.796996 0.367668 1.000000 1.000000
+-0.003050 0.001094 0.016505 -0.479183 0.796996 0.367668 1.000000 0.000000
+0.052980 -0.000209 0.019776 0.444707 0.768639 -0.459815 0.000000 0.000000
+0.048486 0.003959 0.022397 0.444707 0.768639 -0.459815 1.000000 0.000000
+0.045717 0.011490 0.032308 0.444707 0.768639 -0.459815 1.000000 1.000000
+0.052980 -0.000209 0.019776 0.386545 0.775200 -0.499648 0.000000 0.000000
+0.045717 0.011490 0.032308 0.386545 0.775200 -0.499648 1.000000 0.000000
+0.063030 -0.001161 0.026074 0.386545 0.775200 -0.499648 1.000000 1.000000
+0.053360 0.088072 0.187479 -0.745121 -0.286945 0.602044 0.322451 0.094662
+0.048824 0.075798 0.176015 -0.745121 -0.286945 0.602044 0.338641 0.110258
+0.060302 0.081476 0.192927 -0.745121 -0.286945 0.602044 0.310308 0.088773
+0.048824 0.075798 0.176015 -0.916966 0.394495 -0.059550 0.338641 0.110258
+0.053360 0.088072 0.187479 -0.916966 0.394495 -0.059550 0.322451 0.094662
+0.053192 0.085858 0.175399 -0.916966 0.394495 -0.059550 0.335220 0.101700
+-0.100711 -0.017457 -0.071205 -0.665292 0.590463 0.456881 0.966166 0.212554
+-0.094869 -0.027848 -0.049269 -0.665292 0.590463 0.456881 0.952723 0.231841
+-0.080407 -0.011017 -0.049962 -0.665292 0.590463 0.456881 0.934786 0.219472
+-0.080407 -0.011017 -0.049962 -0.629837 0.563209 0.534884 0.934786 0.219472
+-0.094869 -0.027848 -0.049269 -0.629837 0.563209 0.534884 0.952723 0.231841
+-0.071377 -0.019477 -0.030421 -0.629837 0.563209 0.534884 0.889548 0.226645
+-0.094869 -0.027848 -0.049269 -0.649287 0.138224 0.747877 0.952723 0.231841
+-0.077178 -0.040515 -0.031569 -0.649287 0.138224 0.747877 0.899208 0.251069
+-0.071377 -0.019477 -0.030421 -0.649287 0.138224 0.747877 0.889548 0.226645
+-0.071377 -0.019477 -0.030421 -0.697333 0.154085 0.699989 0.889548 0.226645
+-0.077178 -0.040515 -0.031569 -0.697333 0.154085 0.699989 0.899208 0.251069
+-0.060966 -0.016531 -0.020698 -0.697333 0.154085 0.699989 0.847640 0.236770
+-0.077178 -0.040515 -0.031569 -0.673515 0.125054 0.728518 0.899208 0.251069
+-0.030500 -0.033141 0.010319 -0.673515 0.125054 0.728518 0.740681 0.264800
+-0.060966 -0.016531 -0.020698 -0.673515 0.125054 0.728518 0.847640 0.236770
+-0.030500 -0.033141 0.010319 -0.613359 -0.556141 0.560800 0.740681 0.264800
+-0.016507 -0.053189 0.005742 -0.613359 -0.556141 0.560800 0.718071 0.309760
+-0.001950 -0.040273 0.034472 -0.613359 -0.556141 0.560800 0.643033 0.298300
+-0.077178 -0.040515 -0.031569 -0.523697 -0.519646 0.675062 0.899208 0.251069
+-0.016507 -0.053189 0.005742 -0.523697 -0.519646 0.675062 0.718071 0.309760
+-0.030500 -0.033141 0.010319 -0.523697 -0.519646 0.675062 0.740681 0.264800
+0.009464 -0.061640 0.000422 -0.181614 -0.859215 0.478294 0.670646 0.372138
+-0.001950 -0.040273 0.034472 -0.181614 -0.859215 0.478294 0.643033 0.298300
+-0.016507 -0.053189 0.005742 -0.181614 -0.859215 0.478294 0.718071 0.309760
+0.009464 -0.061640 0.000422 -0.227359 -0.857354 0.461792 0.670646 0.372138
+0.031230 -0.028204 0.073215 -0.227359 -0.857354 0.461792 0.513823 0.299301
+-0.001950 -0.040273 0.034472 -0.227359 -0.857354 0.461792 0.643033 0.298300
+-0.001950 -0.040273 0.034472 -0.691545 -0.419585 0.587975 0.643033 0.298300
+-0.001327 -0.021202 0.048814 -0.691545 -0.419585 0.587975 0.618682 0.273535
+-0.023862 -0.025303 0.019383 -0.691545 -0.419585 0.587975 0.712310 0.264110
+-0.001327 -0.021202 0.048814 -0.750619 0.410812 0.517497 0.618682 0.273535
+-0.003552 -0.007667 0.034842 -0.750619 0.410812 0.517497 0.647650 0.255020
+-0.023862 -0.025303 0.019383 -0.750619 0.410812 0.517497 0.712310 0.264110
+-0.001327 -0.021202 0.048814 -0.839834 0.316908 0.440737 0.618682 0.273535
+0.016955 -0.004583 0.071701 -0.839834 0.316908 0.440737 0.548534 0.263306
+-0.003552 -0.007667 0.034842 -0.839834 0.316908 0.440737 0.647650 0.255020
+0.016955 -0.004583 0.071701 -0.726514 0.460848 0.509702 0.548534 0.263306
+0.029876 -0.001883 0.087677 -0.726514 0.460848 0.509702 0.498770 0.268030
+0.034158 0.013303 0.080050 -0.726514 0.460848 0.509702 0.513733 0.250058
+0.043933 -0.002318 0.113530 -0.668360 -0.655072 0.352384 0.426570 0.255980
+0.034355 0.014456 0.126546 -0.668360 -0.655072 0.352384 0.427820 0.215940
+0.029876 -0.001883 0.087677 -0.668360 -0.655072 0.352384 0.498770 0.268030
+0.029876 -0.001883 0.087677 -0.838913 -0.460432 0.290218 0.498770 0.268030
+0.034355 0.014456 0.126546 -0.838913 -0.460432 0.290218 0.427820 0.215940
+0.027702 0.029101 0.130549 -0.838913 -0.460432 0.290218 0.434330 0.198350
+0.027702 0.029101 0.130549 -0.991845 -0.023218 -0.125317 0.434330 0.198350
+0.027156 0.044410 0.132034 -0.991845 -0.023218 -0.125317 0.437699 0.188555
+0.030087 0.039882 0.109675 -0.991845 -0.023218 -0.125317 0.470850 0.222740
+0.034355 0.014456 0.126546 -0.785511 -0.467636 0.405326 0.427820 0.215940
+0.033561 0.031491 0.144661 -0.785511 -0.467636 0.405326 0.402106 0.187773
+0.027702 0.029101 0.130549 -0.785511 -0.467636 0.405326 0.434330 0.198350
+0.033561 0.031491 0.144661 -0.916944 -0.070794 0.392685 0.402106 0.187773
+0.027156 0.044410 0.132034 -0.916944 -0.070794 0.392685 0.437699 0.188555
+0.027702 0.029101 0.130549 -0.916944 -0.070794 0.392685 0.434330 0.198350
+0.033561 0.031491 0.144661 -0.940232 -0.189813 0.282727 0.402106 0.187773
+0.031436 0.049052 0.149384 -0.940232 -0.189813 0.282727 0.399565 0.167196
+0.027156 0.044410 0.132034 -0.940232 -0.189813 0.282727 0.437699 0.188555
+0.027156 0.044410 0.132034 -0.864752 0.495672 0.080705 0.437699 0.188555
+0.031436 0.049052 0.149384 -0.864752 0.495672 0.080705 0.399565 0.167196
+0.042691 0.068252 0.152059 -0.864752 0.495672 0.080705 0.431340 0.169670
+0.031436 0.049052 0.149384 -0.828294 0.515973 -0.218405 0.399565 0.167196
+0.036733 0.065820 0.168909 -0.828294 0.515973 -0.218405 0.369148 0.127941
+0.042691 0.068252 0.152059 -0.828294 0.515973 -0.218405 0.395810 0.143320
+0.031436 0.049052 0.149384 -0.720002 -0.417785 0.554124 0.399565 0.167196
+0.041507 0.041081 0.156460 -0.720002 -0.417785 0.554124 0.365580 0.162280
+0.036733 0.065820 0.168909 -0.720002 -0.417785 0.554124 0.369148 0.127941
+0.036733 0.065820 0.168909 -0.470592 -0.467407 0.748380 0.369148 0.127941
+0.041507 0.041081 0.156460 -0.470592 -0.467407 0.748380 0.365580 0.162280
+0.053331 0.064539 0.178546 -0.470592 -0.467407 0.748380 0.324280 0.113570
+0.054500 0.046985 0.157414 0.334482 -0.813087 0.476457 0.327890 0.162200
+0.057856 0.057044 0.172224 0.334482 -0.813087 0.476457 0.313070 0.128910
+0.041507 0.041081 0.156460 0.334482 -0.813087 0.476457 0.365580 0.162280
+0.041507 0.041081 0.156460 -0.065725 -0.666251 0.742826 0.365580 0.162280
+0.057856 0.057044 0.172224 -0.065725 -0.666251 0.742826 0.313070 0.128910
+0.053331 0.064539 0.178546 -0.065725 -0.666251 0.742826 0.324280 0.113570
+0.057856 0.057044 0.172224 -0.004204 -0.646233 0.763129 0.313070 0.128910
+0.060302 0.081476 0.192927 -0.004204 -0.646233 0.763129 0.299460 0.087742
+0.053331 0.064539 0.178546 -0.004204 -0.646233 0.763129 0.309980 0.099810
+0.057856 0.057044 0.172224 0.038133 -0.648233 0.760487 0.313070 0.128910
+0.072808 0.072316 0.184492 0.038133 -0.648233 0.760487 0.284679 0.109697
+0.060302 0.081476 0.192927 0.038133 -0.648233 0.760487 0.299460 0.087742
+0.053331 0.064539 0.178546 -0.502495 -0.006800 0.864553 0.324280 0.113570
+0.048824 0.075798 0.176015 -0.502495 -0.006800 0.864553 0.344190 0.112360
+0.036733 0.065820 0.168909 -0.502495 -0.006800 0.864553 0.369148 0.127941
+0.048824 0.075798 0.176015 -0.598940 0.794892 -0.097051 0.378837 0.128303
+0.042691 0.068252 0.152059 -0.598940 0.794892 -0.097051 0.395810 0.143320
+0.036733 0.065820 0.168909 -0.598940 0.794892 -0.097051 0.369148 0.127941
+0.042691 0.068252 0.152059 -0.909661 0.401486 0.106418 0.384310 0.126990
+0.048824 0.075798 0.176015 -0.909661 0.401486 0.106418 0.344190 0.112360
+0.053192 0.085858 0.175399 -0.909661 0.401486 0.106418 0.347560 0.104090
+-0.001950 -0.040273 0.034472 -0.660818 -0.256176 0.705474 0.643033 0.298300
+-0.023862 -0.025303 0.019383 -0.660818 -0.256176 0.705474 0.712310 0.264110
+-0.030500 -0.033141 0.010319 -0.660818 -0.256176 0.705474 0.740681 0.264800
+-0.023862 -0.025303 0.019383 -0.810222 0.008444 0.586062 0.712310 0.264110
+-0.030055 -0.019593 0.010739 -0.810222 0.008444 0.586062 0.733560 0.250650
+-0.030500 -0.033141 0.010319 -0.810222 0.008444 0.586062 0.740681 0.264800
+0.053331 0.064539 0.178546 -0.784552 -0.180721 0.593142 0.325550 0.108270
+0.060302 0.081476 0.192927 -0.784552 -0.180721 0.593142 0.310308 0.088773
+0.048824 0.075798 0.176015 -0.784552 -0.180721 0.593142 0.338641 0.110258
+0.057856 0.057044 0.172224 0.804863 -0.559570 0.197677 0.313070 0.128910
+0.054500 0.046985 0.157414 0.804863 -0.559570 0.197677 0.327890 0.162200
+0.062351 0.052413 0.140813 0.804863 -0.559570 0.197677 0.316680 0.182090
+0.030087 0.039882 0.109675 -0.956082 0.140357 -0.257308 0.470850 0.222740
+0.034158 0.013303 0.080050 -0.956082 0.140357 -0.257308 0.513733 0.250058
+0.029876 -0.001883 0.087677 -0.956082 0.140357 -0.257308 0.498770 0.268030
+0.030087 0.039882 0.109675 -0.994873 0.051019 -0.087321 0.470850 0.222740
+0.029876 -0.001883 0.087677 -0.994873 0.051019 -0.087321 0.498770 0.268030
+0.027702 0.029101 0.130549 -0.994873 0.051019 -0.087321 0.434330 0.198350
+0.054500 0.046985 0.157414 0.801033 -0.566444 0.193618 0.327890 0.162200
+0.044423 0.031511 0.153834 0.801033 -0.566444 0.193618 0.364836 0.180676
+0.062351 0.052413 0.140813 0.801033 -0.566444 0.193618 0.316680 0.182090
+0.044423 0.031511 0.153834 -0.492545 -0.644665 0.584642 0.364836 0.180676
+0.033561 0.031491 0.144661 -0.492545 -0.644665 0.584642 0.402106 0.187773
+0.034355 0.014456 0.126546 -0.492545 -0.644665 0.584642 0.427820 0.215940
+0.041507 0.041081 0.156460 -0.597367 -0.376343 0.708179 0.365580 0.162280
+0.033561 0.031491 0.144661 -0.597367 -0.376343 0.708179 0.402106 0.187773
+0.044423 0.031511 0.153834 -0.597367 -0.376343 0.708179 0.364836 0.180676
+0.041507 0.041081 0.156460 -0.685984 -0.265235 0.677552 0.365580 0.162280
+0.031436 0.049052 0.149384 -0.685984 -0.265235 0.677552 0.399565 0.167196
+0.033561 0.031491 0.144661 -0.685984 -0.265235 0.677552 0.402106 0.187773
+0.044423 0.031511 0.153834 0.043531 -0.252010 0.966745 0.364836 0.180676
+0.054500 0.046985 0.157414 0.043531 -0.252010 0.966745 0.327890 0.162200
+0.041507 0.041081 0.156460 0.043531 -0.252010 0.966745 0.365580 0.162280
+0.044423 0.031511 0.153834 -0.616245 -0.362089 0.699380 0.364836 0.180676
+0.077645 0.019022 0.176641 -0.616245 -0.362089 0.699380 0.260121 0.223980
+0.062326 0.031884 0.169802 -0.616245 -0.362089 0.699380 0.301326 0.204647
+0.077645 0.019022 0.176641 -0.093862 -0.853624 0.512363 0.260121 0.223980
+0.089982 -0.007425 0.134839 -0.093862 -0.853624 0.512363 0.295650 0.315700
+0.090978 0.012292 0.167871 -0.093862 -0.853624 0.512363 0.243450 0.264540
+0.089982 -0.007425 0.134839 -0.198069 -0.839011 0.506783 0.295650 0.315700
+0.104416 0.007223 0.164731 -0.198069 -0.839011 0.506783 0.220810 0.296310
+0.090978 0.012292 0.167871 -0.198069 -0.839011 0.506783 0.243450 0.264540
+0.089982 -0.007425 0.134839 0.157287 -0.945881 0.283847 0.295650 0.315700
+0.071473 -0.024618 0.087802 0.157287 -0.945881 0.283847 0.405190 0.354640
+0.099112 -0.012738 0.112075 0.157287 -0.945881 0.283847 0.310110 0.370450
+0.089982 -0.007425 0.134839 -0.003403 -0.938786 0.344485 0.295650 0.315700
+0.056200 -0.017740 0.106395 -0.003403 -0.938786 0.344485 0.410687 0.294059
+0.071473 -0.024618 0.087802 -0.003403 -0.938786 0.344485 0.405190 0.354640
+0.071473 -0.024618 0.087802 -0.048886 -0.930285 0.363565 0.405190 0.354640
+0.031230 -0.028204 0.073215 -0.048886 -0.930285 0.363565 0.513823 0.299301
+0.048578 -0.040054 0.045226 -0.048886 -0.930285 0.363565 0.518630 0.377680
+0.031230 -0.028204 0.073215 -0.842235 -0.292242 0.453029 0.513823 0.299301
+0.043933 -0.002318 0.113530 -0.842235 -0.292242 0.453029 0.426570 0.255980
+0.029876 -0.001883 0.087677 -0.842235 -0.292242 0.453029 0.498770 0.268030
+0.029876 -0.001883 0.087677 -0.688296 -0.376206 0.620257 0.498770 0.268030
+0.016955 -0.004583 0.071701 -0.688296 -0.376206 0.620257 0.548534 0.263306
+0.031230 -0.028204 0.073215 -0.688296 -0.376206 0.620257 0.513823 0.299301
+0.031230 -0.028204 0.073215 -0.611092 -0.322985 0.722667 0.513823 0.299301
+0.016955 -0.004583 0.071701 -0.611092 -0.322985 0.722667 0.548534 0.263306
+-0.001327 -0.021202 0.048814 -0.611092 -0.322985 0.722667 0.618682 0.273535
+0.056200 -0.017740 0.106395 -0.539644 -0.666945 0.513779 0.410687 0.294059
+0.077645 0.019022 0.176641 -0.539644 -0.666945 0.513779 0.260121 0.223980
+0.043933 -0.002318 0.113530 -0.539644 -0.666945 0.513779 0.426570 0.255980
+0.043933 -0.002318 0.113530 -0.489524 -0.655452 0.575108 0.426570 0.255980
+0.031230 -0.028204 0.073215 -0.489524 -0.655452 0.575108 0.513823 0.299301
+0.056200 -0.017740 0.106395 -0.489524 -0.655452 0.575108 0.410687 0.294059
+0.056200 -0.017740 0.106395 -0.032782 -0.945844 0.322962 0.410687 0.294059
+0.031230 -0.028204 0.073215 -0.032782 -0.945844 0.322962 0.513823 0.299301
+0.071473 -0.024618 0.087802 -0.032782 -0.945844 0.322962 0.405190 0.354640
+-0.001327 -0.021202 0.048814 -0.591435 -0.472219 0.653616 0.618682 0.273535
+-0.001950 -0.040273 0.034472 -0.591435 -0.472219 0.653616 0.643033 0.298300
+0.031230 -0.028204 0.073215 -0.591435 -0.472219 0.653616 0.513823 0.299301
+0.048578 -0.040054 0.045226 0.522461 -0.851548 0.043590 0.518630 0.377680
+0.038577 -0.048319 0.003636 0.522461 -0.851548 0.043590 0.601120 0.425850
+0.069661 -0.026788 0.051686 0.522461 -0.851548 0.043590 0.462860 0.409610
+0.031230 -0.028204 0.073215 0.035688 -0.912147 0.408305 0.513823 0.299301
+0.009464 -0.061640 0.000422 0.035688 -0.912147 0.408305 0.670646 0.372138
+0.048578 -0.040054 0.045226 0.035688 -0.912147 0.408305 0.518630 0.377680
+0.048578 -0.040054 0.045226 0.407063 -0.909632 0.082882 0.518630 0.377680
+0.009464 -0.061640 0.000422 0.407063 -0.909632 0.082882 0.670646 0.372138
+0.038577 -0.048319 0.003636 0.407063 -0.909632 0.082882 0.601120 0.425850
+0.009464 -0.061640 0.000422 0.405337 -0.909086 0.096254 0.670646 0.372138
+-0.012845 -0.075192 -0.033626 0.405337 -0.909086 0.096254 0.768420 0.382030
+0.038577 -0.048319 0.003636 0.405337 -0.909086 0.096254 0.601120 0.425850
+0.038577 -0.048319 0.003636 0.511382 -0.854700 -0.089312 0.601120 0.425850
+-0.012845 -0.075192 -0.033626 0.511382 -0.854700 -0.089312 0.768420 0.382030
+0.022281 -0.052876 -0.046062 0.511382 -0.854700 -0.089312 0.714108 0.468367
+-0.012845 -0.075192 -0.033626 0.567700 -0.809188 0.151432 0.768420 0.382030
+-0.012438 -0.080839 -0.065327 0.567700 -0.809188 0.151432 0.815400 0.434250
+0.022281 -0.052876 -0.046062 0.567700 -0.809188 0.151432 0.714108 0.468367
+-0.012438 -0.080839 -0.065327 0.645768 -0.735881 -0.203625 0.815400 0.434250
+-0.047403 -0.100711 -0.104398 0.645768 -0.735881 -0.203625 0.951641 0.425704
+0.007882 -0.060295 -0.075129 0.645768 -0.735881 -0.203625 0.786970 0.489360
+-0.095526 -0.117518 -0.143070 0.470368 -0.856341 -0.213151 0.981023 0.456908
+-0.034406 -0.085027 -0.138728 0.470368 -0.856341 -0.213151 0.961750 0.509730
+-0.047403 -0.100711 -0.104398 0.470368 -0.856341 -0.213151 0.951641 0.425704
+-0.047403 -0.100711 -0.104398 0.625410 -0.771661 -0.115767 0.951641 0.425704
+-0.034406 -0.085027 -0.138728 0.625410 -0.771661 -0.115767 0.961750 0.509730
+0.007882 -0.060295 -0.075129 0.625410 -0.771661 -0.115767 0.786970 0.489360
+0.044423 0.031511 0.153834 -0.601876 -0.558302 0.571003 0.364836 0.180676
+0.034355 0.014456 0.126546 -0.601876 -0.558302 0.571003 0.427820 0.215940
+0.077645 0.019022 0.176641 -0.601876 -0.558302 0.571003 0.260121 0.223980
+0.034355 0.014456 0.126546 -0.515910 -0.689282 0.508653 0.427820 0.215940
+0.043933 -0.002318 0.113530 -0.515910 -0.689282 0.508653 0.426570 0.255980
+0.077645 0.019022 0.176641 -0.515910 -0.689282 0.508653 0.260121 0.223980
+0.089982 -0.007425 0.134839 -0.155682 -0.854882 0.494914 0.295650 0.315700
+0.077645 0.019022 0.176641 -0.155682 -0.854882 0.494914 0.260121 0.223980
+0.056200 -0.017740 0.106395 -0.155682 -0.854882 0.494914 0.410687 0.294059
+0.086777 -0.007718 0.051774 0.768201 -0.481981 -0.421381 0.428198 0.444416
+0.099099 0.007341 0.057013 0.768201 -0.481981 -0.421381 0.395210 0.454600
+0.116055 -0.004211 0.101138 0.768201 -0.481981 -0.421381 0.289090 0.422220
+0.099099 0.007341 0.057013 0.499735 -0.771389 -0.393985 0.395210 0.454600
+0.117596 0.011385 0.072557 0.499735 -0.771389 -0.393985 0.326990 0.473156
+0.116055 -0.004211 0.101138 0.499735 -0.771389 -0.393985 0.289090 0.422220
+0.117596 0.011385 0.072557 0.726269 -0.619128 -0.298686 0.326990 0.473156
+0.135593 0.023447 0.091315 0.726269 -0.619128 -0.298686 0.260220 0.478520
+0.116055 -0.004211 0.101138 0.726269 -0.619128 -0.298686 0.289090 0.422220
+0.153810 0.036576 0.117798 0.769282 -0.594302 -0.234544 0.196867 0.467708
+0.140320 0.014932 0.128395 0.769282 -0.594302 -0.234544 0.195960 0.427720
+0.135593 0.023447 0.091315 0.769282 -0.594302 -0.234544 0.260220 0.478520
+0.140320 0.014932 0.128395 0.872224 -0.425780 0.240700 0.195960 0.427720
+0.153810 0.036576 0.117798 0.872224 -0.425780 0.240700 0.196867 0.467708
+0.142930 0.026574 0.139531 0.872224 -0.425780 0.240700 0.174691 0.414974
+0.135350 0.021139 0.160543 0.247142 -0.851050 0.463287 0.158440 0.365650
+0.115943 0.014365 0.158452 0.247142 -0.851050 0.463287 0.203120 0.329410
+0.111779 0.000945 0.136021 0.247142 -0.851050 0.463287 0.245470 0.357710
+0.119415 0.029098 0.161260 -0.044159 -0.176988 0.983222 0.188520 0.331390
+0.115943 0.014365 0.158452 -0.044159 -0.176988 0.983222 0.203120 0.329410
+0.135350 0.021139 0.160543 -0.044159 -0.176988 0.983222 0.158440 0.365650
+0.119415 0.029098 0.161260 0.969037 -0.201043 -0.143350 0.188520 0.331390
+0.120143 0.026107 0.170376 0.969037 -0.201043 -0.143350 0.178018 0.316824
+0.115943 0.014365 0.158452 0.969037 -0.201043 -0.143350 0.203120 0.329410
+0.104416 0.007223 0.164731 0.618089 -0.719833 0.315921 0.220810 0.296310
+0.111779 0.000945 0.136021 0.618089 -0.719833 0.315921 0.245470 0.357710
+0.115943 0.014365 0.158452 0.618089 -0.719833 0.315921 0.203120 0.329410
+0.089982 -0.007425 0.134839 0.321758 -0.885320 0.335677 0.295650 0.315700
+0.099112 -0.012738 0.112075 0.321758 -0.885320 0.335677 0.310110 0.370450
+0.111779 0.000945 0.136021 0.321758 -0.885320 0.335677 0.245470 0.357710
+0.099112 -0.012738 0.112075 0.535414 -0.823546 0.187359 0.310110 0.370450
+0.116055 -0.004211 0.101138 0.535414 -0.823546 0.187359 0.289090 0.422220
+0.111779 0.000945 0.136021 0.535414 -0.823546 0.187359 0.245470 0.357710
+0.099112 -0.012738 0.112075 0.445046 -0.895465 -0.008706 0.310110 0.370450
+0.069661 -0.026788 0.051686 0.445046 -0.895465 -0.008706 0.462860 0.409610
+0.116055 -0.004211 0.101138 0.445046 -0.895465 -0.008706 0.289090 0.422220
+0.135593 0.023447 0.091315 0.751786 -0.615293 -0.237134 0.260220 0.478520
+0.140320 0.014932 0.128395 0.751786 -0.615293 -0.237134 0.195960 0.427720
+0.116055 -0.004211 0.101138 0.751786 -0.615293 -0.237134 0.289090 0.422220
+0.071473 -0.024618 0.087802 0.526997 -0.849511 0.024602 0.405190 0.354640
+0.048578 -0.040054 0.045226 0.526997 -0.849511 0.024602 0.518630 0.377680
+0.069661 -0.026788 0.051686 0.526997 -0.849511 0.024602 0.462860 0.409610
+0.143992 0.050780 0.185383 -0.796384 0.373403 0.475754 0.102570 0.343770
+0.124638 0.045934 0.156789 -0.796384 0.373403 0.475754 0.172400 0.336930
+0.119415 0.029098 0.161260 -0.796384 0.373403 0.475754 0.188520 0.331390
+0.143992 0.050780 0.185383 -0.892768 0.217083 0.394766 0.102570 0.343770
+0.149787 0.066389 0.189905 -0.892768 0.217083 0.394766 0.078462 0.341776
+0.142136 0.060030 0.176099 -0.892768 0.217083 0.394766 0.112540 0.336640
+0.158948 0.073733 0.201971 0.079730 -0.617483 0.782533 0.082315 0.372172
+0.143992 0.050780 0.185383 0.079730 -0.617483 0.782533 0.102570 0.343770
+0.156843 0.044757 0.179321 0.079730 -0.617483 0.782533 0.082094 0.377823
+0.143992 0.050780 0.185383 0.052633 -0.650386 0.757779 0.102570 0.343770
+0.135350 0.021139 0.160543 0.052633 -0.650386 0.757779 0.158440 0.365650
+0.156843 0.044757 0.179321 0.052633 -0.650386 0.757779 0.087780 0.372400
+0.111779 0.000945 0.136021 0.519729 -0.833631 0.186927 0.245470 0.357710
+0.116055 -0.004211 0.101138 0.519729 -0.833631 0.186927 0.289090 0.422220
+0.135350 0.021139 0.160543 0.519729 -0.833631 0.186927 0.158440 0.365650
+0.116055 -0.004211 0.101138 0.425900 -0.873837 0.234560 0.289090 0.422220
+0.140320 0.014932 0.128395 0.425900 -0.873837 0.234560 0.195960 0.427720
+0.135350 0.021139 0.160543 0.425900 -0.873837 0.234560 0.158440 0.365650
+0.143992 0.050780 0.185383 -0.829621 0.152396 0.537126 0.102570 0.343770
+0.158948 0.073733 0.201971 -0.829621 0.152396 0.537126 0.045070 0.346890
+0.149787 0.066389 0.189905 -0.829621 0.152396 0.537126 0.078462 0.341776
+0.164352 0.057553 0.175030 0.869006 -0.491847 0.053994 0.073840 0.398740
+0.174433 0.077625 0.195623 0.869006 -0.491847 0.053994 0.020480 0.387770
+0.156843 0.044757 0.179321 0.869006 -0.491847 0.053994 0.078100 0.382990
+0.156843 0.044757 0.179321 0.429872 -0.575227 0.695934 0.078100 0.382990
+0.174433 0.077625 0.195623 0.429872 -0.575227 0.695934 0.020480 0.387770
+0.158948 0.073733 0.201971 0.429872 -0.575227 0.695934 0.045070 0.346890
+0.164352 0.057553 0.175030 0.654188 -0.676188 0.338833 0.073840 0.398740
+0.173943 0.072128 0.185599 0.654188 -0.676188 0.338833 0.037193 0.399844
+0.174433 0.077625 0.195623 0.654188 -0.676188 0.338833 0.020480 0.387770
+0.069661 -0.026788 0.051686 0.709077 -0.635008 -0.306553 0.462860 0.409610
+0.068895 -0.018779 0.033324 0.709077 -0.635008 -0.306553 0.491740 0.437240
+0.086777 -0.007718 0.051774 0.709077 -0.635008 -0.306553 0.428198 0.444416
+0.069661 -0.026788 0.051686 0.366670 -0.929597 0.037458 0.462860 0.409610
+0.099112 -0.012738 0.112075 0.366670 -0.929597 0.037458 0.310110 0.370450
+0.071473 -0.024618 0.087802 0.366670 -0.929597 0.037458 0.405190 0.354640
+0.038577 -0.048319 0.003636 0.800629 -0.529312 -0.280752 0.601120 0.425850
+0.053625 -0.028163 0.008548 0.800629 -0.529312 -0.280752 0.561610 0.440210
+0.069661 -0.026788 0.051686 0.800629 -0.529312 -0.280752 0.462860 0.409610
+0.069661 -0.026788 0.051686 0.785880 -0.554125 -0.274478 0.462860 0.409610
+0.053625 -0.028163 0.008548 0.785880 -0.554125 -0.274478 0.561610 0.440210
+0.068895 -0.018779 0.033324 0.785880 -0.554125 -0.274478 0.491740 0.437240
+0.053625 -0.028163 0.008548 0.807024 -0.550314 -0.214163 0.561610 0.440210
+0.038577 -0.048319 0.003636 0.807024 -0.550314 -0.214163 0.601120 0.425850
+0.022281 -0.052876 -0.046062 0.807024 -0.550314 -0.214163 0.714108 0.468367
+0.022281 -0.052876 -0.046062 0.668114 -0.729858 -0.144678 0.714108 0.468367
+-0.012438 -0.080839 -0.065327 0.668114 -0.729858 -0.144678 0.815400 0.434250
+0.007882 -0.060295 -0.075129 0.668114 -0.729858 -0.144678 0.786970 0.489360
+0.069661 -0.026788 0.051686 0.693113 -0.620400 -0.367013 0.462860 0.409610
+0.086777 -0.007718 0.051774 0.693113 -0.620400 -0.367013 0.428198 0.444416
+0.116055 -0.004211 0.101138 0.693113 -0.620400 -0.367013 0.289090 0.422220
+0.143992 0.050780 0.185383 -0.253221 -0.576939 0.776544 0.102570 0.343770
+0.119415 0.029098 0.161260 -0.253221 -0.576939 0.776544 0.188520 0.331390
+0.135350 0.021139 0.160543 -0.253221 -0.576939 0.776544 0.158440 0.365650
+0.077645 0.019022 0.176641 0.042084 -0.760703 0.647735 0.260121 0.223980
+0.090978 0.012292 0.167871 0.042084 -0.760703 0.647735 0.243450 0.264540
+0.085054 0.021891 0.179529 0.042084 -0.760703 0.647735 0.240270 0.232350
+0.085054 0.021891 0.179529 -0.531019 -0.632109 0.564320 0.240270 0.232350
+0.087669 0.033440 0.194926 -0.531019 -0.632109 0.564320 0.211122 0.213362
+0.075763 0.037689 0.188482 -0.531019 -0.632109 0.564320 0.246170 0.197800
+0.075763 0.037689 0.188482 -0.879984 0.084000 0.467517 0.246170 0.197800
+0.086585 0.059950 0.204852 -0.879984 0.084000 0.467517 0.187156 0.190790
+0.071425 0.047528 0.178549 -0.879984 0.084000 0.467517 0.267450 0.197950
+0.075763 0.037689 0.188482 -0.948637 -0.124740 0.290736 0.246170 0.197800
+0.071425 0.047528 0.178549 -0.948637 -0.124740 0.290736 0.267450 0.197950
+0.073688 0.029712 0.178289 -0.948637 -0.124740 0.290736 0.265743 0.210273
+0.071425 0.047528 0.178549 -0.606906 -0.088616 0.789818 0.267450 0.197950
+0.062326 0.031884 0.169802 -0.606906 -0.088616 0.789818 0.301326 0.204647
+0.073688 0.029712 0.178289 -0.606906 -0.088616 0.789818 0.265743 0.210273
+0.073688 0.029712 0.178289 -0.604123 -0.335083 0.723017 0.265743 0.210273
+0.062326 0.031884 0.169802 -0.604123 -0.335083 0.723017 0.301326 0.204647
+0.077645 0.019022 0.176641 -0.604123 -0.335083 0.723017 0.260121 0.223980
+0.085054 0.021891 0.179529 -0.269053 -0.243256 0.931899 0.240270 0.232350
+0.073688 0.029712 0.178289 -0.269053 -0.243256 0.931899 0.265743 0.210273
+0.077645 0.019022 0.176641 -0.269053 -0.243256 0.931899 0.260121 0.223980
+0.085054 0.021891 0.179529 -0.498692 -0.630416 0.594880 0.240270 0.232350
+0.075763 0.037689 0.188482 -0.498692 -0.630416 0.594880 0.246170 0.197800
+0.073688 0.029712 0.178289 -0.498692 -0.630416 0.594880 0.265743 0.210273
+0.062326 0.031884 0.169802 -0.849376 0.227831 0.476081 0.301326 0.204647
+0.071425 0.047528 0.178549 -0.849376 0.227831 0.476081 0.267450 0.197950
+0.061518 0.042216 0.163416 -0.849376 0.227831 0.476081 0.310410 0.197450
+0.062326 0.031884 0.169802 -0.621038 0.376365 0.687504 0.301326 0.204647
+0.061518 0.042216 0.163416 -0.621038 0.376365 0.687504 0.310410 0.197450
+0.044423 0.031511 0.153834 -0.621038 0.376365 0.687504 0.364836 0.180676
+0.090978 0.012292 0.167871 0.033077 -0.461478 0.886535 0.243450 0.264540
+0.104416 0.007223 0.164731 0.033077 -0.461478 0.886535 0.220810 0.296310
+0.095701 0.027930 0.175835 0.033077 -0.461478 0.886535 0.220030 0.259690
+0.095701 0.027930 0.175835 -0.373817 -0.555589 0.742685 0.220030 0.259690
+0.104416 0.007223 0.164731 -0.373817 -0.555589 0.742685 0.220810 0.296310
+0.109910 0.024071 0.180100 -0.373817 -0.555589 0.742685 0.186271 0.283171
+0.104416 0.007223 0.164731 0.592484 -0.639773 0.489543 0.220810 0.296310
+0.120143 0.026107 0.170376 0.592484 -0.639773 0.489543 0.178018 0.316824
+0.109910 0.024071 0.180100 0.592484 -0.639773 0.489543 0.186271 0.283171
+0.109910 0.024071 0.180100 0.472250 -0.819184 0.325450 0.186271 0.283171
+0.120143 0.026107 0.170376 0.472250 -0.819184 0.325450 0.178018 0.316824
+0.141667 0.052178 0.204766 0.472250 -0.819184 0.325450 0.098020 0.295780
+0.120143 0.026107 0.170376 0.568638 -0.786656 0.240464 0.178018 0.316824
+0.141560 0.048534 0.193098 0.568638 -0.786656 0.240464 0.098520 0.321330
+0.141667 0.052178 0.204766 0.568638 -0.786656 0.240464 0.098020 0.295780
+0.123196 0.044287 0.202612 -0.491871 -0.233698 0.838718 0.092590 0.280640
+0.141494 0.076623 0.222353 -0.491871 -0.233698 0.838718 0.059790 0.277920
+0.123289 0.059764 0.206979 -0.491871 -0.233698 0.838718 0.112692 0.264495
+0.141667 0.052178 0.204766 0.999135 -0.041404 0.003769 0.081990 0.304590
+0.141560 0.048534 0.193098 0.999135 -0.041404 0.003769 0.098520 0.321330
+0.142189 0.063498 0.190741 0.999135 -0.041404 0.003769 0.087940 0.327060
+0.141560 0.048534 0.193098 0.828272 -0.120991 -0.547108 0.098520 0.321330
+0.135284 0.055424 0.182073 0.828272 -0.120991 -0.547108 0.107730 0.328780
+0.142189 0.063498 0.190741 0.828272 -0.120991 -0.547108 0.087940 0.327060
+0.141560 0.048534 0.193098 0.799768 -0.185219 -0.571021 0.098520 0.321330
+0.120143 0.026107 0.170376 0.799768 -0.185219 -0.571021 0.178018 0.316824
+0.135284 0.055424 0.182073 0.799768 -0.185219 -0.571021 0.107730 0.328780
+0.135284 0.055424 0.182073 0.867247 -0.285024 -0.408221 0.107730 0.328780
+0.120143 0.026107 0.170376 0.867247 -0.285024 -0.408221 0.178018 0.316824
+0.119302 0.038419 0.159993 0.867247 -0.285024 -0.408221 0.180790 0.332860
+0.120143 0.026107 0.170376 0.996860 0.001323 -0.079174 0.178018 0.316824
+0.119415 0.029098 0.161260 0.996860 0.001323 -0.079174 0.188520 0.331390
+0.119302 0.038419 0.159993 0.996860 0.001323 -0.079174 0.180790 0.332860
+0.090978 0.012292 0.167871 0.524756 -0.506802 0.683946 0.243450 0.264540
+0.095701 0.027930 0.175835 0.524756 -0.506802 0.683946 0.220030 0.259690
+0.085054 0.021891 0.179529 0.524756 -0.506802 0.683946 0.240270 0.232350
+0.100002 0.036926 0.190618 0.964968 -0.215553 -0.149578 0.190200 0.243860
+0.095701 0.027930 0.175835 0.964968 -0.215553 -0.149578 0.220030 0.259690
+0.097660 0.038978 0.172552 0.964968 -0.215553 -0.149578 0.211300 0.262970
+0.100002 0.036926 0.190618 0.548370 -0.775661 0.312474 0.190200 0.243860
+0.085054 0.021891 0.179529 0.548370 -0.775661 0.312474 0.240270 0.232350
+0.095701 0.027930 0.175835 0.548370 -0.775661 0.312474 0.220030 0.259690
+0.107389 0.046282 0.210566 0.668609 -0.737112 0.098125 0.147990 0.226080
+0.085054 0.021891 0.179529 0.668609 -0.737112 0.098125 0.240270 0.232350
+0.100002 0.036926 0.190618 0.668609 -0.737112 0.098125 0.190200 0.243860
+0.107389 0.046282 0.210566 0.052277 -0.803117 0.593524 0.147990 0.226080
+0.087669 0.033440 0.194926 0.052277 -0.803117 0.593524 0.211122 0.213362
+0.085054 0.021891 0.179529 0.052277 -0.803117 0.593524 0.240270 0.232350
+0.108825 0.043741 0.199336 0.924372 -0.026921 -0.380540 0.159984 0.245722
+0.097660 0.038978 0.172552 0.924372 -0.026921 -0.380540 0.211300 0.262970
+0.108636 0.055260 0.198062 0.924372 -0.026921 -0.380540 0.155020 0.252040
+0.108636 0.055260 0.198062 0.727876 0.391428 -0.563011 0.155020 0.252040
+0.097660 0.038978 0.172552 0.727876 0.391428 -0.563011 0.211300 0.262970
+0.088707 0.052685 0.170507 0.727876 0.391428 -0.563011 0.200790 0.261530
+0.131489 0.078973 0.229080 0.754704 -0.639453 0.146699 0.069787 0.240614
+0.118713 0.062923 0.224846 0.754704 -0.639453 0.146699 0.104782 0.225075
+0.107389 0.046282 0.210566 0.754704 -0.639453 0.146699 0.147990 0.226080
+0.118713 0.062923 0.224846 0.059877 -0.673205 0.737028 0.104782 0.225075
+0.107296 0.067456 0.229914 0.059877 -0.673205 0.737028 0.116859 0.196548
+0.107389 0.046282 0.210566 0.059877 -0.673205 0.737028 0.147990 0.226080
+0.107389 0.046282 0.210566 -0.085449 -0.672296 0.735334 0.147990 0.226080
+0.107296 0.067456 0.229914 -0.085449 -0.672296 0.735334 0.116859 0.196548
+0.093507 0.047680 0.210231 -0.085449 -0.672296 0.735334 0.175260 0.199050
+0.107296 0.067456 0.229914 -0.744129 -0.132778 0.654708 0.116859 0.196548
+0.086585 0.059950 0.204852 -0.744129 -0.132778 0.654708 0.187156 0.190790
+0.093507 0.047680 0.210231 -0.744129 -0.132778 0.654708 0.175260 0.199050
+0.108825 0.043741 0.199336 0.821736 -0.049308 -0.567731 0.159984 0.245722
+0.108636 0.055260 0.198062 0.821736 -0.049308 -0.567731 0.155020 0.252040
+0.131489 0.078973 0.229080 0.821736 -0.049308 -0.567731 0.069787 0.240614
+0.142987 0.029221 0.148360 0.853671 -0.470722 0.222860 0.161963 0.397901
+0.135350 0.021139 0.160543 0.853671 -0.470722 0.222860 0.158440 0.365650
+0.140320 0.014932 0.128395 0.853671 -0.470722 0.222860 0.195960 0.427720
+0.150255 0.033474 0.161588 0.605924 -0.753726 0.254467 0.126154 0.391636
+0.156843 0.044757 0.179321 0.605924 -0.753726 0.254467 0.089596 0.378162
+0.135350 0.021139 0.160543 0.605924 -0.753726 0.254467 0.158440 0.365650
+0.142987 0.029221 0.148360 0.885699 -0.170473 -0.431829 0.161963 0.397901
+0.150595 0.051764 0.155065 0.885699 -0.170473 -0.431829 0.121890 0.403040
+0.150255 0.033474 0.161588 0.885699 -0.170473 -0.431829 0.126154 0.391636
+0.150595 0.051764 0.155065 0.832961 -0.199547 -0.516097 0.121890 0.403040
+0.164352 0.057553 0.175030 0.832961 -0.199547 -0.516097 0.073840 0.398740
+0.150255 0.033474 0.161588 0.832961 -0.199547 -0.516097 0.126154 0.391636
+0.150255 0.033474 0.161588 0.862659 -0.505784 0.001328 0.126154 0.391636
+0.164352 0.057553 0.175030 0.862659 -0.505784 0.001328 0.073840 0.398740
+0.156843 0.044757 0.179321 0.862659 -0.505784 0.001328 0.089596 0.378162
+0.100002 0.036926 0.190618 0.424000 -0.870155 0.251107 0.190200 0.243860
+0.108825 0.043741 0.199336 0.424000 -0.870155 0.251107 0.159984 0.245722
+0.107389 0.046282 0.210566 0.424000 -0.870155 0.251107 0.147990 0.226080
+0.108825 0.043741 0.199336 0.701902 -0.691795 -0.169568 0.159984 0.245722
+0.100002 0.036926 0.190618 0.701902 -0.691795 -0.169568 0.190200 0.243860
+0.097660 0.038978 0.172552 0.701902 -0.691795 -0.169568 0.211300 0.262970
+0.089982 -0.007425 0.134839 0.330601 -0.900733 0.281749 0.295650 0.315700
+0.111779 0.000945 0.136021 0.330601 -0.900733 0.281749 0.245470 0.357710
+0.104416 0.007223 0.164731 0.330601 -0.900733 0.281749 0.220810 0.296310
+0.104416 0.007223 0.164731 0.632117 -0.651732 0.419134 0.220810 0.296310
+0.115943 0.014365 0.158452 0.632117 -0.651732 0.419134 0.203120 0.329410
+0.120143 0.026107 0.170376 0.632117 -0.651732 0.419134 0.178018 0.316824
+-0.047403 -0.100711 -0.104398 0.030618 -0.901724 0.431227 0.951641 0.425704
+-0.012438 -0.080839 -0.065327 0.030618 -0.901724 0.431227 0.815400 0.434250
+-0.069241 -0.093206 -0.087154 0.030618 -0.901724 0.431227 0.914152 0.365852
+-0.069241 -0.093206 -0.087154 0.144648 -0.973826 0.175328 0.914152 0.365852
+-0.012438 -0.080839 -0.065327 0.144648 -0.973826 0.175328 0.815400 0.434250
+-0.012845 -0.075192 -0.033626 0.144648 -0.973826 0.175328 0.768420 0.382030
+-0.012845 -0.075192 -0.033626 0.033640 -0.935960 0.350495 0.768420 0.382030
+0.009464 -0.061640 0.000422 0.033640 -0.935960 0.350495 0.670646 0.372138
+-0.029661 -0.072947 -0.026017 0.033640 -0.935960 0.350495 0.793950 0.336740
+0.009464 -0.061640 0.000422 -0.148008 -0.811083 0.565896 0.670646 0.372138
+-0.016507 -0.053189 0.005742 -0.148008 -0.811083 0.565896 0.718071 0.309760
+-0.029661 -0.072947 -0.026017 -0.148008 -0.811083 0.565896 0.790105 0.328337
+0.174312 0.080673 0.176682 -0.036027 -0.475138 0.879174 0.042770 0.427560
+0.168610 0.067224 0.169180 -0.036027 -0.475138 0.879174 0.058830 0.426820
+0.182614 0.078900 0.176064 -0.036027 -0.475138 0.879174 0.042790 0.429381
+0.168610 0.067224 0.169180 0.299846 -0.725121 0.619913 0.062340 0.427740
+0.179213 0.068282 0.165289 0.299846 -0.725121 0.619913 0.055790 0.445457
+0.182614 0.078900 0.176064 0.299846 -0.725121 0.619913 0.042790 0.429381
+-0.034406 -0.085027 -0.138728 0.750296 -0.606529 -0.263020 0.959968 0.514181
+-0.032947 -0.077119 -0.152802 0.750296 -0.606529 -0.263020 0.958829 0.529430
+0.007882 -0.060295 -0.075129 0.750296 -0.606529 -0.263020 0.932195 0.522238
+-0.032947 -0.077119 -0.152802 0.888469 -0.143330 -0.435981 0.958829 0.529430
+0.003153 -0.046601 -0.089268 0.888469 -0.143330 -0.435981 0.824771 0.564740
+0.007882 -0.060295 -0.075129 0.888469 -0.143330 -0.435981 0.787970 0.551140
+0.007882 -0.060295 -0.075129 0.901465 -0.118530 -0.416308 0.787970 0.551140
+0.003153 -0.046601 -0.089268 0.901465 -0.118530 -0.416308 0.824771 0.564740
+0.022281 -0.052876 -0.046062 0.901465 -0.118530 -0.416308 0.707390 0.565900
+0.003153 -0.046601 -0.089268 0.889847 -0.178533 -0.419879 0.824771 0.564740
+0.029309 -0.029773 -0.040991 0.889847 -0.178533 -0.419879 0.694998 0.593245
+0.022281 -0.052876 -0.046062 0.889847 -0.178533 -0.419879 0.707390 0.565900
+0.003153 -0.046601 -0.089268 0.831577 0.197122 -0.519251 0.824771 0.564740
+-0.004285 -0.027983 -0.094112 0.831577 0.197122 -0.519251 0.848720 0.590070
+0.029309 -0.029773 -0.040991 0.831577 0.197122 -0.519251 0.694998 0.593245
+-0.004285 -0.027983 -0.094112 0.762594 0.447416 -0.467192 0.848720 0.590070
+0.008989 -0.011941 -0.057082 0.762594 0.447416 -0.467192 0.757109 0.626270
+0.029309 -0.029773 -0.040991 0.762594 0.447416 -0.467192 0.694998 0.593245
+-0.025528 -0.015002 -0.098342 0.508655 0.647474 -0.567492 0.894100 0.654540
+-0.004285 -0.027983 -0.094112 0.508655 0.647474 -0.567492 0.848720 0.590070
+-0.024684 -0.048566 -0.135880 0.508655 0.647474 -0.567492 0.936076 0.565704
+-0.004285 -0.027983 -0.094112 0.837053 0.202129 -0.508415 0.848720 0.590070
+0.003153 -0.046601 -0.089268 0.837053 0.202129 -0.508415 0.824771 0.564740
+-0.024684 -0.048566 -0.135880 0.837053 0.202129 -0.508415 0.936076 0.565704
+0.087669 0.033440 0.194926 -0.521755 -0.206783 0.827655 0.211122 0.213362
+0.107389 0.046282 0.210566 -0.521755 -0.206783 0.827655 0.147990 0.226080
+0.075763 0.037689 0.188482 -0.521755 -0.206783 0.827655 0.246170 0.197800
+0.075763 0.037689 0.188482 -0.099325 -0.870994 0.481149 0.246170 0.197800
+0.107389 0.046282 0.210566 -0.099325 -0.870994 0.481149 0.147990 0.226080
+0.093507 0.047680 0.210231 -0.099325 -0.870994 0.481149 0.175260 0.199050
+-0.082414 -0.008469 -0.082211 -0.391394 0.915132 0.096663 0.963945 0.892963
+-0.100711 -0.017457 -0.071205 -0.391394 0.915132 0.096663 0.970203 0.982094
+-0.080407 -0.011017 -0.049962 -0.391394 0.915132 0.096663 0.928039 0.969549
+0.029309 -0.029773 -0.040991 0.894507 0.071045 -0.441373 0.694998 0.593245
+0.050502 -0.009120 0.005284 0.894507 0.071045 -0.441373 0.573141 0.624110
+0.053625 -0.028163 0.008548 0.894507 0.071045 -0.441373 0.563770 0.587680
+0.086777 -0.007718 0.051774 0.681626 0.542006 -0.491543 0.435511 0.601870
+0.063030 -0.001161 0.026074 0.681626 0.542006 -0.491543 0.516921 0.625450
+0.073622 0.006154 0.048828 0.681626 0.542006 -0.491543 0.455920 0.622257
+0.063030 -0.001161 0.026074 0.451598 0.766511 -0.456639 0.516921 0.625450
+0.062588 0.018440 0.058539 0.451598 0.766511 -0.456639 0.462180 0.663670
+0.073622 0.006154 0.048828 0.451598 0.766511 -0.456639 0.455920 0.622257
+0.063030 -0.001161 0.026074 0.405018 0.785142 -0.468522 0.516921 0.625450
+0.045717 0.011490 0.032308 0.405018 0.785142 -0.468522 0.534848 0.690702
+0.062588 0.018440 0.058539 0.405018 0.785142 -0.468522 0.462180 0.663670
+0.062588 0.018440 0.058539 -0.173223 0.959148 -0.223672 0.462180 0.663670
+0.028037 0.011583 0.055893 -0.173223 0.959148 -0.223672 0.537360 0.785940
+0.046779 0.017207 0.065495 -0.173223 0.959148 -0.223672 0.484460 0.711190
+0.063030 -0.001161 0.026074 0.844207 0.061935 -0.532427 0.516921 0.625450
+0.068895 -0.018779 0.033324 0.844207 0.061935 -0.532427 0.497300 0.593550
+0.050502 -0.009120 0.005284 0.844207 0.061935 -0.532427 0.573141 0.624110
+0.068895 -0.018779 0.033324 0.842489 0.046169 -0.536731 0.497300 0.593550
+0.053625 -0.028163 0.008548 0.842489 0.046169 -0.536731 0.563770 0.587680
+0.050502 -0.009120 0.005284 0.842489 0.046169 -0.536731 0.573141 0.624110
+0.068895 -0.018779 0.033324 0.728993 -0.038560 -0.683434 0.497300 0.593550
+0.063030 -0.001161 0.026074 0.728993 -0.038560 -0.683434 0.516921 0.625450
+0.086777 -0.007718 0.051774 0.728993 -0.038560 -0.683434 0.435511 0.601870
+0.046779 0.017207 0.065495 -0.289907 0.957040 0.005316 0.484460 0.711190
+0.028037 0.011583 0.055893 -0.289907 0.957040 0.005316 0.537360 0.785940
+0.034158 0.013303 0.080050 -0.289907 0.957040 0.005316 0.491340 0.772430
+0.028037 0.011583 0.055893 -0.745791 0.650730 0.142639 0.537360 0.785940
+0.016955 -0.004583 0.071701 -0.745791 0.650730 0.142639 0.536530 0.818012
+0.034158 0.013303 0.080050 -0.745791 0.650730 0.142639 0.491340 0.772430
+0.030087 0.039882 0.109675 -0.467734 0.624982 -0.624997 0.442490 0.810560
+0.052997 0.025909 0.078557 -0.467734 0.624982 -0.624997 0.450460 0.714650
+0.034158 0.013303 0.080050 -0.467734 0.624982 -0.624997 0.491340 0.772430
+0.052997 0.025909 0.078557 -0.335140 0.742402 -0.580103 0.450460 0.714650
+0.030087 0.039882 0.109675 -0.335140 0.742402 -0.580103 0.442490 0.810560
+0.037569 0.053938 0.123341 -0.335140 0.742402 -0.580103 0.410010 0.816830
+0.037569 0.053938 0.123341 0.007768 0.894407 -0.447187 0.410010 0.816830
+0.042691 0.068252 0.152059 0.007768 0.894407 -0.447187 0.349380 0.855820
+0.047565 0.058833 0.133305 0.007768 0.894407 -0.447187 0.373890 0.809190
+0.047565 0.058833 0.133305 0.498963 0.819491 -0.281905 0.373890 0.809190
+0.042691 0.068252 0.152059 0.498963 0.819491 -0.281905 0.349380 0.855820
+0.062351 0.052413 0.140813 0.498963 0.819491 -0.281905 0.331610 0.786970
+0.058327 0.063550 0.158191 0.977882 0.175297 0.114093 0.307721 0.837801
+0.057856 0.057044 0.172224 0.977882 0.175297 0.114093 0.297906 0.831523
+0.062351 0.052413 0.140813 0.977882 0.175297 0.114093 0.331610 0.786970
+0.042691 0.068252 0.152059 0.409099 0.808356 -0.423319 0.349380 0.855820
+0.058327 0.063550 0.158191 0.409099 0.808356 -0.423319 0.307721 0.837801
+0.062351 0.052413 0.140813 0.409099 0.808356 -0.423319 0.331610 0.786970
+0.042691 0.068252 0.152059 0.405169 0.177834 -0.896780 0.319714 0.849451
+0.056886 0.077181 0.160243 0.405169 0.177834 -0.896780 0.302102 0.842047
+0.058327 0.063550 0.158191 0.405169 0.177834 -0.896780 0.307721 0.837801
+0.058327 0.063550 0.158191 0.782993 -0.573945 -0.239813 0.286889 0.843306
+0.072808 0.072316 0.184492 0.782993 -0.573945 -0.239813 0.257003 0.877169
+0.057856 0.057044 0.172224 0.782993 -0.573945 -0.239813 0.297906 0.831523
+0.058327 0.063550 0.158191 0.839232 0.166641 -0.517611 0.286889 0.843306
+0.056886 0.077181 0.160243 0.839232 0.166641 -0.517611 0.305477 0.856494
+0.072808 0.072316 0.184492 0.839232 0.166641 -0.517611 0.257003 0.877169
+0.056886 0.077181 0.160243 0.808889 0.370093 -0.456869 0.305477 0.856494
+0.063223 0.090410 0.182179 0.808889 0.370093 -0.456869 0.270710 0.887400
+0.072808 0.072316 0.184492 0.808889 0.370093 -0.456869 0.257003 0.877169
+0.056886 0.077181 0.160243 -0.049432 0.861534 -0.505288 0.305477 0.856494
+0.053192 0.085858 0.175399 -0.049432 0.861534 -0.505288 0.298639 0.885778
+0.063223 0.090410 0.182179 -0.049432 0.861534 -0.505288 0.270710 0.887400
+0.042691 0.068252 0.152059 -0.215916 0.823748 -0.524232 0.336759 0.863565
+0.053192 0.085858 0.175399 -0.215916 0.823748 -0.524232 0.298639 0.885778
+0.056886 0.077181 0.160243 -0.215916 0.823748 -0.524232 0.305477 0.856494
+0.042691 0.068252 0.152059 -0.743626 0.641830 -0.187280 0.349380 0.855820
+0.037569 0.053938 0.123341 -0.743626 0.641830 -0.187280 0.410010 0.816830
+0.027156 0.044410 0.132034 -0.743626 0.641830 -0.187280 0.415793 0.851809
+0.053360 0.088072 0.187479 0.179671 0.731949 0.657243 0.282632 0.904939
+0.060302 0.081476 0.192927 0.179671 0.731949 0.657243 0.268112 0.906565
+0.063223 0.090410 0.182179 0.179671 0.731949 0.657243 0.270710 0.887400
+0.063223 0.090410 0.182179 0.701219 0.442880 0.558703 0.270710 0.887400
+0.060302 0.081476 0.192927 0.701219 0.442880 0.558703 0.268112 0.906565
+0.072808 0.072316 0.184492 0.701219 0.442880 0.558703 0.257003 0.877169
+0.047565 0.058833 0.133305 0.347701 0.930974 0.111313 0.373890 0.809190
+0.062351 0.052413 0.140813 0.347701 0.930974 0.111313 0.329190 0.784830
+0.068440 0.051968 0.125515 0.347701 0.930974 0.111313 0.344030 0.750640
+0.062351 0.052413 0.140813 0.438217 0.886495 0.148635 0.329190 0.784830
+0.084341 0.041774 0.139434 0.438217 0.886495 0.148635 0.288930 0.734470
+0.068440 0.051968 0.125515 0.438217 0.886495 0.148635 0.344030 0.750640
+0.062351 0.052413 0.140813 0.421293 0.820772 0.385805 0.329190 0.784830
+0.061518 0.042216 0.163416 0.421293 0.820772 0.385805 0.295840 0.820350
+0.084341 0.041774 0.139434 0.421293 0.820772 0.385805 0.288930 0.734470
+0.061518 0.042216 0.163416 -0.485820 0.873986 0.011261 0.295840 0.820350
+0.071425 0.047528 0.178549 -0.485820 0.873986 0.011261 0.250100 0.818029
+0.075682 0.050078 0.164293 -0.485820 0.873986 0.011261 0.266450 0.791830
+0.075682 0.050078 0.164293 -0.421508 0.805782 -0.415988 0.273470 0.776000
+0.084341 0.041774 0.139434 -0.421508 0.805782 -0.415988 0.288930 0.734470
+0.061518 0.042216 0.163416 -0.421508 0.805782 -0.415988 0.295840 0.820350
+0.084341 0.041774 0.139434 0.594443 0.798600 -0.094210 0.288930 0.734470
+0.071762 0.046670 0.101566 0.594443 0.798600 -0.094210 0.374767 0.709924
+0.068440 0.051968 0.125515 0.594443 0.798600 -0.094210 0.344030 0.750640
+0.084341 0.041774 0.139434 0.428511 0.903175 -0.025570 0.288930 0.734470
+0.092476 0.037228 0.115191 0.428511 0.903175 -0.025570 0.311310 0.679780
+0.071762 0.046670 0.101566 0.428511 0.903175 -0.025570 0.374767 0.709924
+0.092476 0.037228 0.115191 0.585187 0.704593 -0.401380 0.311310 0.679780
+0.081658 0.033262 0.092457 0.585187 0.704593 -0.401380 0.369880 0.669890
+0.071762 0.046670 0.101566 0.585187 0.704593 -0.401380 0.374767 0.709924
+0.081658 0.033262 0.092457 -0.056815 0.926175 -0.372790 0.369880 0.669890
+0.062588 0.018440 0.058539 -0.056815 0.926175 -0.372790 0.462180 0.663670
+0.052997 0.025909 0.078557 -0.056815 0.926175 -0.372790 0.450460 0.714650
+0.111719 0.044626 0.093231 -0.336433 0.907171 -0.252691 0.306530 0.603540
+0.101997 0.035315 0.072748 -0.336433 0.907171 -0.252691 0.357290 0.597774
+0.081658 0.033262 0.092457 -0.336433 0.907171 -0.252691 0.369880 0.669890
+0.081658 0.033262 0.092457 -0.313049 0.922211 -0.226993 0.369880 0.669890
+0.101997 0.035315 0.072748 -0.313049 0.922211 -0.226993 0.357290 0.597774
+0.062588 0.018440 0.058539 -0.313049 0.922211 -0.226993 0.462180 0.663670
+0.062588 0.018440 0.058539 -0.070025 0.733132 -0.676471 0.462180 0.663670
+0.101997 0.035315 0.072748 -0.070025 0.733132 -0.676471 0.357290 0.597774
+0.082305 0.017903 0.055916 -0.070025 0.733132 -0.676471 0.424935 0.616345
+0.101997 0.035315 0.072748 0.329360 0.436749 -0.837121 0.357290 0.597774
+0.099099 0.007341 0.057013 0.329360 0.436749 -0.837121 0.392743 0.582936
+0.082305 0.017903 0.055916 0.329360 0.436749 -0.837121 0.424935 0.616345
+0.111719 0.044626 0.093231 0.077988 0.893121 -0.443004 0.306530 0.603540
+0.121015 0.040796 0.087146 0.077988 0.893121 -0.443004 0.297580 0.574370
+0.101997 0.035315 0.072748 0.077988 0.893121 -0.443004 0.357290 0.597774
+0.121015 0.040796 0.087146 0.425958 0.501006 -0.753361 0.297580 0.574370
+0.123985 0.028751 0.080815 0.425958 0.501006 -0.753361 0.301840 0.556970
+0.101997 0.035315 0.072748 0.425958 0.501006 -0.753361 0.357290 0.597774
+0.123985 0.028751 0.080815 0.419674 0.411575 -0.809000 0.301840 0.556970
+0.099099 0.007341 0.057013 0.419674 0.411575 -0.809000 0.392743 0.582936
+0.101997 0.035315 0.072748 0.419674 0.411575 -0.809000 0.357290 0.597774
+0.123985 0.028751 0.080815 0.618935 0.139825 -0.772896 0.301840 0.556970
+0.117596 0.011385 0.072557 0.618935 0.139825 -0.772896 0.329168 0.559598
+0.099099 0.007341 0.057013 0.618935 0.139825 -0.772896 0.392743 0.582936
+0.117596 0.011385 0.072557 0.690001 0.087795 -0.718464 0.329168 0.559598
+0.123985 0.028751 0.080815 0.690001 0.087795 -0.718464 0.301840 0.556970
+0.135593 0.023447 0.091315 0.690001 0.087795 -0.718464 0.261680 0.547960
+0.123985 0.028751 0.080815 0.711784 0.322511 -0.623980 0.301840 0.556970
+0.138591 0.044894 0.105820 0.711784 0.322511 -0.623980 0.231540 0.563650
+0.135593 0.023447 0.091315 0.711784 0.322511 -0.623980 0.261680 0.547960
+0.123985 0.028751 0.080815 0.580459 0.486386 -0.653066 0.301840 0.556970
+0.121015 0.040796 0.087146 0.580459 0.486386 -0.653066 0.297580 0.574370
+0.138591 0.044894 0.105820 0.580459 0.486386 -0.653066 0.231540 0.563650
+0.111719 0.044626 0.093231 0.183978 0.933850 -0.306719 0.306530 0.603540
+0.127937 0.047717 0.112370 0.183978 0.933850 -0.306719 0.243160 0.595480
+0.121015 0.040796 0.087146 0.183978 0.933850 -0.306719 0.297580 0.574370
+0.092476 0.037228 0.115191 -0.353719 0.935337 0.005146 0.311310 0.679780
+0.111719 0.044626 0.093231 -0.353719 0.935337 0.005146 0.306530 0.603540
+0.081658 0.033262 0.092457 -0.353719 0.935337 0.005146 0.369880 0.669890
+0.037569 0.053938 0.123341 -0.056134 0.917262 -0.394308 0.410010 0.816830
+0.047565 0.058833 0.133305 -0.056134 0.917262 -0.394308 0.373890 0.809190
+0.071762 0.046670 0.101566 -0.056134 0.917262 -0.394308 0.374767 0.709924
+0.047565 0.058833 0.133305 0.247467 0.952698 -0.176429 0.373890 0.809190
+0.068440 0.051968 0.125515 0.247467 0.952698 -0.176429 0.344030 0.750640
+0.071762 0.046670 0.101566 0.247467 0.952698 -0.176429 0.374767 0.709924
+0.052997 0.025909 0.078557 -0.550643 0.791501 -0.265177 0.450460 0.714650
+0.046779 0.017207 0.065495 -0.550643 0.791501 -0.265177 0.484460 0.711190
+0.034158 0.013303 0.080050 -0.550643 0.791501 -0.265177 0.491340 0.772430
+0.052997 0.025909 0.078557 -0.262672 0.856016 -0.445242 0.450460 0.714650
+0.062588 0.018440 0.058539 -0.262672 0.856016 -0.445242 0.462180 0.663670
+0.046779 0.017207 0.065495 -0.262672 0.856016 -0.445242 0.484460 0.711190
+0.062588 0.018440 0.058539 -0.093763 0.564177 -0.820313 0.462180 0.663670
+0.082305 0.017903 0.055916 -0.093763 0.564177 -0.820313 0.424935 0.616345
+0.073622 0.006154 0.048828 -0.093763 0.564177 -0.820313 0.455920 0.622257
+0.082305 0.017903 0.055916 0.424912 0.216272 -0.879020 0.424935 0.616345
+0.086777 -0.007718 0.051774 0.424912 0.216272 -0.879020 0.435511 0.601870
+0.073622 0.006154 0.048828 0.424912 0.216272 -0.879020 0.455920 0.622257
+0.082305 0.017903 0.055916 0.181094 0.187678 -0.965392 0.424935 0.616345
+0.099099 0.007341 0.057013 0.181094 0.187678 -0.965392 0.392743 0.582936
+0.086777 -0.007718 0.051774 0.181094 0.187678 -0.965392 0.435511 0.601870
+0.105390 0.048647 0.164927 0.052283 0.952515 -0.299970 0.259600 0.732880
+0.084341 0.041774 0.139434 0.052283 0.952515 -0.299970 0.288930 0.734470
+0.075682 0.050078 0.164293 0.052283 0.952515 -0.299970 0.266450 0.791830
+0.037569 0.053938 0.123341 -0.753064 0.618642 -0.224001 0.410010 0.816830
+0.030087 0.039882 0.109675 -0.753064 0.618642 -0.224001 0.442490 0.810560
+0.027156 0.044410 0.132034 -0.753064 0.618642 -0.224001 0.415793 0.851809
+0.037569 0.053938 0.123341 -0.190908 0.801157 -0.567188 0.410010 0.816830
+0.071762 0.046670 0.101566 -0.190908 0.801157 -0.567188 0.374767 0.709924
+0.052997 0.025909 0.078557 -0.190908 0.801157 -0.567188 0.450460 0.714650
+0.062351 0.052413 0.140813 -0.621950 0.722121 0.302853 0.318930 0.812548
+0.044423 0.031511 0.153834 -0.621950 0.722121 0.302853 0.301720 0.824680
+0.061518 0.042216 0.163416 -0.621950 0.722121 0.302853 0.316160 0.818176
+0.135593 0.023447 0.091315 0.689132 0.337391 -0.641299 0.261680 0.547960
+0.138591 0.044894 0.105820 0.689132 0.337391 -0.641299 0.231540 0.563650
+0.153810 0.036576 0.117798 0.689132 0.337391 -0.641299 0.204756 0.548310
+0.121015 0.040796 0.087146 0.078730 0.955641 -0.283816 0.297580 0.574370
+0.127937 0.047717 0.112370 0.078730 0.955641 -0.283816 0.243160 0.595480
+0.138591 0.044894 0.105820 0.078730 0.955641 -0.283816 0.231540 0.563650
+0.127937 0.047717 0.112370 -0.079966 0.992494 -0.092529 0.243160 0.595480
+0.111719 0.044626 0.093231 -0.079966 0.992494 -0.092529 0.306530 0.603540
+0.124840 0.048888 0.127607 -0.079966 0.992494 -0.092529 0.225220 0.625490
+0.111719 0.044626 0.093231 -0.343962 0.938866 0.014885 0.306530 0.603540
+0.092476 0.037228 0.115191 -0.343962 0.938866 0.014885 0.262670 0.665250
+0.124840 0.048888 0.127607 -0.343962 0.938866 0.014885 0.225220 0.625490
+0.124840 0.048888 0.127607 -0.287104 0.947413 -0.141349 0.225220 0.625490
+0.092476 0.037228 0.115191 -0.287104 0.947413 -0.141349 0.262670 0.665250
+0.117201 0.050385 0.153157 -0.287104 0.947413 -0.141349 0.200900 0.681330
+0.153810 0.036576 0.117798 0.682750 0.273587 -0.677498 0.204756 0.548310
+0.138591 0.044894 0.105820 0.682750 0.273587 -0.677498 0.231540 0.563650
+0.161981 0.058897 0.135046 0.682750 0.273587 -0.677498 0.205510 0.554570
+0.138591 0.044894 0.105820 -0.026246 0.901799 -0.431359 0.231540 0.563650
+0.127937 0.047717 0.112370 -0.026246 0.901799 -0.431359 0.243160 0.595480
+0.143106 0.052573 0.121599 -0.026246 0.901799 -0.431359 0.196590 0.574980
+0.127937 0.047717 0.112370 -0.234628 0.964423 -0.121808 0.243160 0.595480
+0.124840 0.048888 0.127607 -0.234628 0.964423 -0.121808 0.225220 0.625490
+0.143106 0.052573 0.121599 -0.234628 0.964423 -0.121808 0.196590 0.574980
+0.124840 0.048888 0.127607 0.072311 0.996704 -0.036778 0.225220 0.625490
+0.117201 0.050385 0.153157 0.072311 0.996704 -0.036778 0.200900 0.681330
+0.130922 0.049236 0.148996 0.072311 0.996704 -0.036778 0.179280 0.644260
+0.161981 0.058897 0.135046 0.013148 0.897611 -0.440593 0.205510 0.554570
+0.138591 0.044894 0.105820 0.013148 0.897611 -0.440593 0.231540 0.563650
+0.143106 0.052573 0.121599 0.013148 0.897611 -0.440593 0.196590 0.574980
+0.124840 0.048888 0.127607 0.163404 0.984578 -0.062483 0.225220 0.625490
+0.130922 0.049236 0.148996 0.163404 0.984578 -0.062483 0.179280 0.644260
+0.136199 0.047431 0.134354 0.163404 0.984578 -0.062483 0.190281 0.610355
+0.143992 0.050780 0.185383 -0.806753 0.330047 0.490120 0.151250 0.671000
+0.142136 0.060030 0.176099 -0.806753 0.330047 0.490120 0.111570 0.658070
+0.124638 0.045934 0.156789 -0.806753 0.330047 0.490120 0.178405 0.669185
+0.124638 0.045934 0.156789 -0.555997 0.825311 -0.098641 0.178405 0.669185
+0.142136 0.060030 0.176099 -0.555997 0.825311 -0.098641 0.111570 0.658070
+0.130922 0.049236 0.148996 -0.555997 0.825311 -0.098641 0.179280 0.644260
+0.142136 0.060030 0.176099 -0.445050 0.879929 -0.166298 0.111570 0.658070
+0.158775 0.066912 0.167984 -0.445050 0.879929 -0.166298 0.089401 0.614255
+0.130922 0.049236 0.148996 -0.445050 0.879929 -0.166298 0.179280 0.644260
+0.158775 0.066912 0.167984 -0.538539 0.565618 -0.624542 0.089401 0.614255
+0.142136 0.060030 0.176099 -0.538539 0.565618 -0.624542 0.111570 0.658070
+0.159528 0.087068 0.185589 -0.538539 0.565618 -0.624542 0.063021 0.631922
+0.142136 0.060030 0.176099 -0.850902 0.455459 0.261769 0.111570 0.658070
+0.149787 0.066389 0.189905 -0.850902 0.455459 0.261769 0.077689 0.659309
+0.159528 0.087068 0.185589 -0.850902 0.455459 0.261769 0.063021 0.631922
+0.149787 0.066389 0.189905 -0.903421 0.428521 0.014169 0.077689 0.659309
+0.159469 0.086296 0.205175 -0.903421 0.428521 0.014169 0.032164 0.653374
+0.159528 0.087068 0.185589 -0.903421 0.428521 0.014169 0.063021 0.631922
+0.169176 0.083665 0.181754 0.897506 0.249696 -0.363503 0.047225 0.608980
+0.176435 0.092959 0.206061 0.897506 0.249696 -0.363503 0.007966 0.628355
+0.173943 0.072128 0.185599 0.897506 0.249696 -0.363503 0.042490 0.600000
+0.169176 0.083665 0.181754 0.953574 0.036913 -0.298888 0.047225 0.608980
+0.172181 0.094044 0.192623 0.953574 0.036913 -0.298888 0.024866 0.619390
+0.176435 0.092959 0.206061 0.953574 0.036913 -0.298888 0.007966 0.628355
+0.159528 0.087068 0.185589 -0.017556 0.725530 -0.687967 0.058333 0.632730
+0.172181 0.094044 0.192623 -0.017556 0.725530 -0.687967 0.024866 0.619390
+0.169176 0.083665 0.181754 -0.017556 0.725530 -0.687967 0.047225 0.608980
+0.172181 0.094044 0.192623 0.283248 0.958969 -0.012238 0.024866 0.619390
+0.165030 0.096221 0.197703 0.283248 0.958969 -0.012238 0.028538 0.636841
+0.176435 0.092959 0.206061 0.283248 0.958969 -0.012238 0.007966 0.628355
+0.159528 0.087068 0.185589 -0.144013 0.819906 -0.554089 0.058333 0.632730
+0.165030 0.096221 0.197703 -0.144013 0.819906 -0.554089 0.028538 0.636841
+0.172181 0.094044 0.192623 -0.144013 0.819906 -0.554089 0.024866 0.619390
+0.165030 0.096221 0.197703 -0.866743 0.498464 0.017036 0.028538 0.636841
+0.159528 0.087068 0.185589 -0.866743 0.498464 0.017036 0.063021 0.631922
+0.159469 0.086296 0.205175 -0.866743 0.498464 0.017036 0.032164 0.653374
+0.159469 0.086296 0.205175 -0.300410 0.675374 0.673516 0.032164 0.653374
+0.176435 0.092959 0.206061 -0.300410 0.675374 0.673516 0.007966 0.628355
+0.165030 0.096221 0.197703 -0.300410 0.675374 0.673516 0.028538 0.636841
+0.130922 0.049236 0.148996 0.229247 0.816022 0.530616 0.179280 0.644260
+0.117201 0.050385 0.153157 0.229247 0.816022 0.530616 0.200900 0.681330
+0.124638 0.045934 0.156789 0.229247 0.816022 0.530616 0.181247 0.669140
+0.124638 0.045934 0.156789 -0.142384 0.472025 0.870011 0.178405 0.669185
+0.117201 0.050385 0.153157 -0.142384 0.472025 0.870011 0.200900 0.681330
+0.119302 0.038419 0.159993 -0.142384 0.472025 0.870011 0.182900 0.687000
+0.117201 0.050385 0.153157 0.791086 0.402199 0.460890 0.200900 0.681330
+0.113399 0.048360 0.161450 0.791086 0.402199 0.460890 0.194766 0.703056
+0.119302 0.038419 0.159993 0.791086 0.402199 0.460890 0.182900 0.687000
+0.105390 0.048647 0.164927 0.052436 0.876901 -0.477802 0.205070 0.725670
+0.075682 0.050078 0.164293 0.052436 0.876901 -0.477802 0.266450 0.791830
+0.088707 0.052685 0.170507 0.052436 0.876901 -0.477802 0.230999 0.772807
+0.071425 0.047528 0.178549 -0.241205 0.965243 0.100629 0.250100 0.818029
+0.088707 0.052685 0.170507 -0.241205 0.965243 0.100629 0.230999 0.772807
+0.075682 0.050078 0.164293 -0.241205 0.965243 0.100629 0.266450 0.791830
+0.105390 0.048647 0.164927 -0.221711 0.971910 -0.078968 0.205070 0.725670
+0.117201 0.050385 0.153157 -0.221711 0.971910 -0.078968 0.200900 0.681330
+0.084341 0.041774 0.139434 -0.221711 0.971910 -0.078968 0.260430 0.698770
+0.117201 0.050385 0.153157 -0.154762 0.960323 -0.232010 0.200900 0.681330
+0.092476 0.037228 0.115191 -0.154762 0.960323 -0.232010 0.262670 0.665250
+0.084341 0.041774 0.139434 -0.154762 0.960323 -0.232010 0.260430 0.698770
+0.088707 0.052685 0.170507 0.637007 0.574152 -0.514366 0.230999 0.772807
+0.098148 0.065280 0.196258 0.637007 0.574152 -0.514366 0.169760 0.791210
+0.108636 0.055260 0.198062 0.637007 0.574152 -0.514366 0.148075 0.768006
+0.088707 0.052685 0.170507 -0.532034 0.821139 -0.206567 0.230999 0.772807
+0.086585 0.059950 0.204852 -0.532034 0.821139 -0.206567 0.176753 0.823046
+0.098148 0.065280 0.196258 -0.532034 0.821139 -0.206567 0.169760 0.791210
+0.098148 0.065280 0.196258 0.484165 0.363483 -0.795905 0.169760 0.791210
+0.119937 0.077291 0.214998 0.484165 0.363483 -0.795905 0.094384 0.772592
+0.108636 0.055260 0.198062 0.484165 0.363483 -0.795905 0.148075 0.768006
+0.086585 0.059950 0.204852 -0.569170 0.770049 -0.288219 0.172969 0.822403
+0.104757 0.078397 0.218252 -0.569170 0.770049 -0.288219 0.119475 0.806425
+0.098148 0.065280 0.196258 -0.569170 0.770049 -0.288219 0.167530 0.791000
+0.104757 0.078397 0.218252 -0.044623 0.863844 -0.501779 0.119475 0.806425
+0.119937 0.077291 0.214998 -0.044623 0.863844 -0.501779 0.094384 0.772592
+0.098148 0.065280 0.196258 -0.044623 0.863844 -0.501779 0.167530 0.791000
+0.118628 0.093343 0.233243 0.366299 0.919435 0.143053 0.068000 0.797370
+0.128282 0.088243 0.241302 0.366299 0.919435 0.143053 0.043946 0.784224
+0.128568 0.090583 0.225530 0.366299 0.919435 0.143053 0.062010 0.771769
+0.128568 0.090583 0.225530 0.964069 0.259680 0.056009 0.062010 0.771769
+0.128282 0.088243 0.241302 0.964069 0.259680 0.056009 0.043946 0.784224
+0.131489 0.078973 0.229080 0.964069 0.259680 0.056009 0.057647 0.763215
+0.128282 0.088243 0.241302 -0.172738 0.725585 0.666099 0.043946 0.784224
+0.118628 0.093343 0.233243 -0.172738 0.725585 0.666099 0.068000 0.797370
+0.113825 0.086887 0.239030 -0.172738 0.725585 0.666099 0.071740 0.814150
+0.086585 0.059950 0.204852 -0.369460 0.904245 -0.214102 0.176753 0.823046
+0.088707 0.052685 0.170507 -0.369460 0.904245 -0.214102 0.230999 0.772807
+0.071425 0.047528 0.178549 -0.369460 0.904245 -0.214102 0.250100 0.818029
+0.104757 0.078397 0.218252 -0.767031 0.637287 0.074351 0.113396 0.806480
+0.113825 0.086887 0.239030 -0.767031 0.637287 0.074351 0.071740 0.814150
+0.118628 0.093343 0.233243 -0.767031 0.637287 0.074351 0.068000 0.797370
+0.104757 0.078397 0.218252 -0.724534 0.688353 0.034938 0.113396 0.806480
+0.086585 0.059950 0.204852 -0.724534 0.688353 0.034938 0.172969 0.822403
+0.113825 0.086887 0.239030 -0.724534 0.688353 0.034938 0.071740 0.814150
+0.108636 0.055260 0.198062 0.763522 0.098921 -0.638161 0.148075 0.768006
+0.119937 0.077291 0.214998 0.763522 0.098921 -0.638161 0.094384 0.772592
+0.131489 0.078973 0.229080 0.763522 0.098921 -0.638161 0.057647 0.763215
+0.128568 0.090583 0.225530 0.773107 0.000565 -0.634276 0.062010 0.771769
+0.131489 0.078973 0.229080 0.773107 0.000565 -0.634276 0.057647 0.763215
+0.119937 0.077291 0.214998 0.773107 0.000565 -0.634276 0.094384 0.772592
+0.117201 0.050385 0.153157 0.166062 0.937727 0.305108 0.200900 0.681330
+0.105390 0.048647 0.164927 0.166062 0.937727 0.305108 0.205070 0.725670
+0.113399 0.048360 0.161450 0.166062 0.937727 0.305108 0.194766 0.703056
+0.105390 0.048647 0.164927 -0.154431 0.889921 -0.429176 0.205070 0.725670
+0.126783 0.063450 0.187924 -0.154431 0.889921 -0.429176 0.123640 0.711560
+0.113399 0.048360 0.161450 -0.154431 0.889921 -0.429176 0.194766 0.703056
+0.097660 0.038978 0.172552 -0.748673 0.658556 0.076109 0.207600 0.751920
+0.114482 0.056148 0.189459 -0.748673 0.658556 0.076109 0.147190 0.741800
+0.105390 0.048647 0.164927 -0.748673 0.658556 0.076109 0.205070 0.725670
+0.105390 0.048647 0.164927 -0.515642 0.853940 -0.069998 0.205070 0.725670
+0.114482 0.056148 0.189459 -0.515642 0.853940 -0.069998 0.147190 0.741800
+0.126783 0.063450 0.187924 -0.515642 0.853940 -0.069998 0.123640 0.711560
+0.097660 0.038978 0.172552 -0.819569 0.385936 0.423509 0.207600 0.751920
+0.107971 0.041337 0.190356 -0.819569 0.385936 0.423509 0.161290 0.754120
+0.114482 0.056148 0.189459 -0.819569 0.385936 0.423509 0.147190 0.741800
+0.107971 0.041337 0.190356 -0.850928 0.395041 0.346213 0.161290 0.754120
+0.123289 0.059764 0.206979 -0.850928 0.395041 0.346213 0.100160 0.745330
+0.114482 0.056148 0.189459 -0.850928 0.395041 0.346213 0.147190 0.741800
+0.114482 0.056148 0.189459 -0.843830 0.416628 0.338189 0.147190 0.741800
+0.123289 0.059764 0.206979 -0.843830 0.416628 0.338189 0.100160 0.745330
+0.127109 0.070575 0.203192 -0.843830 0.416628 0.338189 0.099430 0.733770
+0.114482 0.056148 0.189459 -0.509573 0.783824 -0.354901 0.147190 0.741800
+0.127109 0.070575 0.203192 -0.509573 0.783824 -0.354901 0.099430 0.733770
+0.126783 0.063450 0.187924 -0.509573 0.783824 -0.354901 0.123640 0.711560
+0.142189 0.063498 0.190741 0.131333 0.671051 -0.729686 0.092348 0.685197
+0.135284 0.055424 0.182073 0.131333 0.671051 -0.729686 0.119930 0.686806
+0.126783 0.063450 0.187924 0.131333 0.671051 -0.729686 0.123640 0.711560
+0.126783 0.063450 0.187924 0.307594 0.751384 -0.583788 0.123640 0.711560
+0.135284 0.055424 0.182073 0.307594 0.751384 -0.583788 0.119930 0.686806
+0.113399 0.048360 0.161450 0.307594 0.751384 -0.583788 0.194766 0.703056
+0.135284 0.055424 0.182073 0.542714 0.428171 -0.722586 0.119930 0.686806
+0.119302 0.038419 0.159993 0.542714 0.428171 -0.722586 0.182900 0.687000
+0.113399 0.048360 0.161450 0.542714 0.428171 -0.722586 0.194766 0.703056
+0.127109 0.070575 0.203192 -0.875533 0.402820 0.266792 0.099430 0.733770
+0.123289 0.059764 0.206979 -0.875533 0.402820 0.266792 0.100160 0.745330
+0.148253 0.104663 0.221112 -0.875533 0.402820 0.266792 0.025313 0.709884
+0.123289 0.059764 0.206979 -0.877269 0.408353 0.252284 0.100160 0.745330
+0.144382 0.092417 0.227473 -0.877269 0.408353 0.252284 0.026020 0.727900
+0.148253 0.104663 0.221112 -0.877269 0.408353 0.252284 0.025313 0.709884
+0.148253 0.104663 0.221112 -0.750055 0.605322 -0.266465 0.025313 0.709884
+0.126783 0.063450 0.187924 -0.750055 0.605322 -0.266465 0.123640 0.711560
+0.127109 0.070575 0.203192 -0.750055 0.605322 -0.266465 0.099430 0.733770
+0.160456 0.093784 0.213636 0.150116 0.536996 -0.830121 0.020110 0.678290
+0.142189 0.063498 0.190741 0.150116 0.536996 -0.830121 0.092348 0.685197
+0.126783 0.063450 0.187924 0.150116 0.536996 -0.830121 0.123640 0.711560
+0.154832 0.073607 0.208689 0.839615 -0.103280 -0.533273 0.052320 0.675130
+0.142189 0.063498 0.190741 0.839615 -0.103280 -0.533273 0.092348 0.685197
+0.160456 0.093784 0.213636 0.839615 -0.103280 -0.533273 0.020110 0.678290
+0.126783 0.063450 0.187924 0.056925 0.608025 -0.791874 0.123640 0.711560
+0.148253 0.104663 0.221112 0.056925 0.608025 -0.791874 0.025313 0.709884
+0.160456 0.093784 0.213636 0.056925 0.608025 -0.791874 0.020110 0.678290
+0.144382 0.092417 0.227473 -0.238748 0.505996 0.828835 0.026020 0.727900
+0.156812 0.096983 0.228266 -0.238748 0.505996 0.828835 0.004768 0.704040
+0.148253 0.104663 0.221112 -0.238748 0.505996 0.828835 0.025313 0.709884
+0.160456 0.093784 0.213636 0.666468 0.745528 0.002985 0.020110 0.678290
+0.148253 0.104663 0.221112 0.666468 0.745528 0.002985 0.025313 0.709884
+0.156812 0.096983 0.228266 0.666468 0.745528 0.002985 0.004768 0.704040
+0.053625 -0.028163 0.008548 0.886028 -0.175324 -0.429205 0.563770 0.587680
+0.022281 -0.052876 -0.046062 0.886028 -0.175324 -0.429205 0.707390 0.565900
+0.029309 -0.029773 -0.040991 0.886028 -0.175324 -0.429205 0.694998 0.593245
+0.142987 0.029221 0.148360 0.933907 -0.344071 0.097126 0.162840 0.613160
+0.142930 0.026574 0.139531 0.933907 -0.344071 0.097126 0.168030 0.609880
+0.150595 0.051764 0.155065 0.933907 -0.344071 0.097126 0.128281 0.611740
+0.142930 0.026574 0.139531 0.205114 0.467758 -0.859728 0.168030 0.609880
+0.130922 0.049236 0.148996 0.205114 0.467758 -0.859728 0.177100 0.643890
+0.150595 0.051764 0.155065 0.205114 0.467758 -0.859728 0.128281 0.611740
+0.136199 0.047431 0.134354 -0.626252 0.779220 -0.024993 0.190281 0.610355
+0.151269 0.059879 0.144841 -0.626252 0.779220 -0.024993 0.146100 0.591670
+0.143106 0.052573 0.121599 -0.626252 0.779220 -0.024993 0.196590 0.574980
+0.142930 0.026574 0.139531 -0.722055 -0.061990 0.689053 0.168030 0.609880
+0.159537 0.060161 0.159955 -0.722055 -0.061990 0.689053 0.117383 0.597700
+0.136199 0.047431 0.134354 -0.722055 -0.061990 0.689053 0.190281 0.610355
+0.136199 0.047431 0.134354 -0.732416 0.557915 0.390253 0.190281 0.610355
+0.159537 0.060161 0.159955 -0.732416 0.557915 0.390253 0.117383 0.597700
+0.151269 0.059879 0.144841 -0.732416 0.557915 0.390253 0.146100 0.591670
+0.143106 0.052573 0.121599 -0.141877 0.957493 -0.251153 0.196590 0.574980
+0.151269 0.059879 0.144841 -0.141877 0.957493 -0.251153 0.146100 0.591670
+0.161981 0.058897 0.135046 -0.141877 0.957493 -0.251153 0.137991 0.550913
+0.130922 0.049236 0.148996 0.892027 0.356758 0.277508 0.177100 0.643890
+0.142930 0.026574 0.139531 0.892027 0.356758 0.277508 0.168030 0.609880
+0.136199 0.047431 0.134354 0.892027 0.356758 0.277508 0.170267 0.618432
+0.150595 0.051764 0.155065 0.815290 0.051619 -0.576748 0.128281 0.611740
+0.158775 0.066912 0.167984 0.815290 0.051619 -0.576748 0.089401 0.614255
+0.164352 0.057553 0.175030 0.815290 0.051619 -0.576748 0.075400 0.602490
+0.150595 0.051764 0.155065 0.169529 0.584917 -0.793178 0.128281 0.611740
+0.130922 0.049236 0.148996 0.169529 0.584917 -0.793178 0.177100 0.643890
+0.158775 0.066912 0.167984 0.169529 0.584917 -0.793178 0.089401 0.614255
+0.159537 0.060161 0.159955 -0.596094 -0.180948 0.782259 0.117383 0.597700
+0.142930 0.026574 0.139531 -0.596094 -0.180948 0.782259 0.168030 0.609880
+0.156379 0.044803 0.153996 -0.596094 -0.180948 0.782259 0.124200 0.603440
+0.164352 0.057553 0.175030 0.762134 -0.032642 -0.646596 0.075400 0.602490
+0.158775 0.066912 0.167984 0.762134 -0.032642 -0.646596 0.089401 0.614255
+0.173943 0.072128 0.185599 0.762134 -0.032642 -0.646596 0.042490 0.600000
+0.158775 0.066912 0.167984 0.742392 0.085284 -0.664515 0.089401 0.614255
+0.169176 0.083665 0.181754 0.742392 0.085284 -0.664515 0.055650 0.607740
+0.173943 0.072128 0.185599 0.742392 0.085284 -0.664515 0.042490 0.600000
+0.158775 0.066912 0.167984 -0.066216 0.657796 -0.750280 0.089401 0.614255
+0.159528 0.087068 0.185589 -0.066216 0.657796 -0.750280 0.063021 0.631922
+0.169176 0.083665 0.181754 -0.066216 0.657796 -0.750280 0.055650 0.607740
+0.136199 0.047431 0.134354 -0.078967 0.938671 0.335651 0.190281 0.610355
+0.143106 0.052573 0.121599 -0.078967 0.938671 0.335651 0.196590 0.574980
+0.124840 0.048888 0.127607 -0.078967 0.938671 0.335651 0.225220 0.625490
+0.156379 0.044803 0.153996 -0.639687 -0.160153 0.751766 0.116410 0.602830
+0.168610 0.067224 0.169180 -0.639687 -0.160153 0.751766 0.071290 0.596710
+0.159537 0.060161 0.159955 -0.639687 -0.160153 0.751766 0.117383 0.597700
+0.159537 0.060161 0.159955 -0.649562 -0.141322 0.747060 0.117383 0.597700
+0.168610 0.067224 0.169180 -0.649562 -0.141322 0.747060 0.071290 0.596710
+0.174312 0.080673 0.176682 -0.649562 -0.141322 0.747060 0.046580 0.584910
+0.159537 0.060161 0.159955 -0.854005 0.237795 0.462740 0.102401 0.594698
+0.174312 0.080673 0.176682 -0.854005 0.237795 0.462740 0.046580 0.584910
+0.151269 0.059879 0.144841 -0.854005 0.237795 0.462740 0.134250 0.589370
+0.151269 0.059879 0.144841 -0.855175 0.294363 0.426645 0.134250 0.589370
+0.174312 0.080673 0.176682 -0.855175 0.294363 0.426645 0.046580 0.584910
+0.172340 0.088145 0.167574 -0.855175 0.294363 0.426645 0.061920 0.574880
+0.161018 0.070392 0.147527 -0.684546 0.692765 -0.226877 0.119270 0.573250
+0.151269 0.059879 0.144841 -0.684546 0.692765 -0.226877 0.134250 0.589370
+0.172340 0.088145 0.167574 -0.684546 0.692765 -0.226877 0.061920 0.574880
+0.172340 0.088145 0.167574 -0.217964 0.788476 -0.575150 0.061920 0.574880
+0.177914 0.078618 0.152401 -0.217964 0.788476 -0.575150 0.076558 0.550459
+0.161018 0.070392 0.147527 -0.217964 0.788476 -0.575150 0.119270 0.573250
+0.161981 0.058897 0.135046 -0.155900 0.720547 -0.675653 0.137991 0.550913
+0.161018 0.070392 0.147527 -0.155900 0.720547 -0.675653 0.119270 0.573250
+0.177914 0.078618 0.152401 -0.155900 0.720547 -0.675653 0.076558 0.550459
+0.161981 0.058897 0.135046 -0.498954 0.617936 -0.607618 0.137991 0.550913
+0.151269 0.059879 0.144841 -0.498954 0.617936 -0.607618 0.134250 0.589370
+0.161018 0.070392 0.147527 -0.498954 0.617936 -0.607618 0.119270 0.573250
+0.177914 0.078618 0.152401 0.491377 0.807443 -0.326473 0.076558 0.550459
+0.172340 0.088145 0.167574 0.491377 0.807443 -0.326473 0.061920 0.574880
+0.184018 0.079932 0.164838 0.491377 0.807443 -0.326473 0.046470 0.549800
+0.104757 0.078397 0.218252 -0.087544 0.744781 -0.661541 0.119475 0.806425
+0.118628 0.093343 0.233243 -0.087544 0.744781 -0.661541 0.068000 0.797370
+0.119937 0.077291 0.214998 -0.087544 0.744781 -0.661541 0.094384 0.772592
+0.119937 0.077291 0.214998 -0.302290 0.704802 -0.641775 0.094384 0.772592
+0.118628 0.093343 0.233243 -0.302290 0.704802 -0.641775 0.068000 0.797370
+0.128568 0.090583 0.225530 -0.302290 0.704802 -0.641775 0.062010 0.771769
+0.086585 0.059950 0.204852 -0.739178 -0.127042 0.661420 0.187156 0.190790
+0.075763 0.037689 0.188482 -0.739178 -0.127042 0.661420 0.246170 0.197800
+0.093507 0.047680 0.210231 -0.739178 -0.127042 0.661420 0.175260 0.199050
+0.097660 0.038978 0.172552 -0.194770 0.310967 0.930250 0.210540 0.264840
+0.095701 0.027930 0.175835 -0.194770 0.310967 0.930250 0.220030 0.259690
+0.109910 0.024071 0.180100 -0.194770 0.310967 0.930250 0.186271 0.283171
+0.109910 0.024071 0.180100 -0.489258 -0.485360 0.724605 0.186271 0.283171
+0.123196 0.044287 0.202612 -0.489258 -0.485360 0.724605 0.122720 0.274200
+0.107971 0.041337 0.190356 -0.489258 -0.485360 0.724605 0.166593 0.267539
+0.123289 0.059764 0.206979 -0.586337 -0.216715 0.780540 0.112692 0.264495
+0.107971 0.041337 0.190356 -0.586337 -0.216715 0.780540 0.166593 0.267539
+0.123196 0.044287 0.202612 -0.586337 -0.216715 0.780540 0.122720 0.274200
+0.109910 0.024071 0.180100 0.273703 -0.790311 0.548174 0.186271 0.283171
+0.141667 0.052178 0.204766 0.273703 -0.790311 0.548174 0.098020 0.295780
+0.123196 0.044287 0.202612 0.273703 -0.790311 0.548174 0.122720 0.274200
+0.109910 0.024071 0.180100 -0.775908 -0.384301 0.500279 0.186271 0.283171
+0.107971 0.041337 0.190356 -0.775908 -0.384301 0.500279 0.166593 0.267539
+0.097660 0.038978 0.172552 -0.775908 -0.384301 0.500279 0.210540 0.264840
+0.123289 0.059764 0.206979 -0.540231 -0.168491 0.824476 0.112692 0.264495
+0.141494 0.076623 0.222353 -0.540231 -0.168491 0.824476 0.059790 0.277920
+0.144382 0.092417 0.227473 -0.540231 -0.168491 0.824476 0.036651 0.275151
+0.141494 0.076623 0.222353 0.056107 -0.317159 0.946711 0.053758 0.278100
+0.156812 0.096983 0.228266 0.056107 -0.317159 0.946711 0.033925 0.290432
+0.144382 0.092417 0.227473 0.056107 -0.317159 0.946711 0.036651 0.275151
+0.141667 0.052178 0.204766 0.417494 -0.528731 0.739015 0.043468 0.298260
+0.156812 0.096983 0.228266 0.417494 -0.528731 0.739015 0.033925 0.290432
+0.141494 0.076623 0.222353 0.417494 -0.528731 0.739015 0.053758 0.278100
+0.141667 0.052178 0.204766 0.668676 -0.509881 0.541197 0.043468 0.298260
+0.154832 0.073607 0.208689 0.668676 -0.509881 0.541197 0.049561 0.321938
+0.156812 0.096983 0.228266 0.668676 -0.509881 0.541197 0.033925 0.290432
+0.154832 0.073607 0.208689 0.899394 -0.322928 0.294630 0.049561 0.321938
+0.160456 0.093784 0.213636 0.899394 -0.322928 0.294630 0.025280 0.326291
+0.156812 0.096983 0.228266 0.899394 -0.322928 0.294630 0.010440 0.305130
+0.154832 0.073607 0.208689 0.829718 -0.449044 -0.331555 0.048226 0.322009
+0.141667 0.052178 0.204766 0.829718 -0.449044 -0.331555 0.081990 0.304590
+0.142189 0.063498 0.190741 0.829718 -0.449044 -0.331555 0.087940 0.327060
+0.184018 0.079932 0.164838 0.591821 0.792575 0.146878 0.044750 0.458050
+0.172340 0.088145 0.167574 0.591821 0.792575 0.146878 0.035060 0.447515
+0.182614 0.078900 0.176064 0.591821 0.792575 0.146878 0.036999 0.436733
+0.141494 0.076623 0.222353 0.152655 -0.576454 0.802743 0.059790 0.277920
+0.123196 0.044287 0.202612 0.152655 -0.576454 0.802743 0.092590 0.280640
+0.141667 0.052178 0.204766 0.152655 -0.576454 0.802743 0.081990 0.304590
+0.172340 0.088145 0.167574 0.210187 0.777679 0.592483 0.036470 0.432090
+0.174312 0.080673 0.176682 0.210187 0.777679 0.592483 0.043260 0.424780
+0.182614 0.078900 0.176064 0.210187 0.777679 0.592483 0.036999 0.436733
+0.168610 0.067224 0.169180 0.319246 -0.644602 0.694673 0.067350 0.424040
+0.156379 0.044803 0.153996 0.319246 -0.644602 0.694673 0.116650 0.420010
+0.179213 0.068282 0.165289 0.319246 -0.644602 0.694673 0.085250 0.429680
+0.177914 0.078618 0.152401 0.739921 -0.005832 -0.672668 0.075080 0.466000
+0.172189 0.056197 0.146298 0.739921 -0.005832 -0.672668 0.098560 0.464290
+0.161981 0.058897 0.135046 0.739921 -0.005832 -0.672668 0.116410 0.468430
+0.172189 0.056197 0.146298 0.748595 0.211528 -0.628380 0.098560 0.464290
+0.153810 0.036576 0.117798 0.748595 0.211528 -0.628380 0.173706 0.466822
+0.161981 0.058897 0.135046 0.748595 0.211528 -0.628380 0.116410 0.468430
+0.156379 0.044803 0.153996 0.620950 -0.772732 0.131555 0.128690 0.429070
+0.153810 0.036576 0.117798 0.620950 -0.772732 0.131555 0.173706 0.466822
+0.172189 0.056197 0.146298 0.620950 -0.772732 0.131555 0.098560 0.464290
+0.142930 0.026574 0.139531 0.765804 -0.636695 0.090357 0.174691 0.414974
+0.153810 0.036576 0.117798 0.765804 -0.636695 0.090357 0.173706 0.466822
+0.156379 0.044803 0.153996 0.765804 -0.636695 0.090357 0.128690 0.429070
+0.172189 0.056197 0.146298 0.918839 -0.382700 -0.096309 0.098560 0.464290
+0.184018 0.079932 0.164838 0.918839 -0.382700 -0.096309 0.044750 0.458050
+0.179213 0.068282 0.165289 0.918839 -0.382700 -0.096309 0.058440 0.452240
+0.172189 0.056197 0.146298 0.896665 -0.112398 -0.428203 0.098560 0.464290
+0.177914 0.078618 0.152401 0.896665 -0.112398 -0.428203 0.075080 0.466000
+0.184018 0.079932 0.164838 0.896665 -0.112398 -0.428203 0.044750 0.458050
+0.179213 0.068282 0.165289 0.640173 -0.733080 0.229724 0.085250 0.429680
+0.156379 0.044803 0.153996 0.640173 -0.733080 0.229724 0.116650 0.420010
+0.172189 0.056197 0.146298 0.640173 -0.733080 0.229724 0.098560 0.464290
+0.003153 -0.046601 -0.089268 0.856116 0.056674 -0.513667 0.824771 0.564740
+-0.032947 -0.077119 -0.152802 0.856116 0.056674 -0.513667 0.958829 0.529430
+-0.024684 -0.048566 -0.135880 0.856116 0.056674 -0.513667 0.936076 0.565704
+0.097660 0.038978 0.172552 0.374096 0.371146 0.849884 0.207600 0.751920
+0.105390 0.048647 0.164927 0.374096 0.371146 0.849884 0.205070 0.725670
+0.088707 0.052685 0.170507 0.374096 0.371146 0.849884 0.230999 0.772807
+0.179213 0.068282 0.165289 0.922539 -0.377374 0.080688 0.055790 0.445457
+0.184018 0.079932 0.164838 0.922539 -0.377374 0.080688 0.049275 0.450166
+0.182614 0.078900 0.176064 0.922539 -0.377374 0.080688 0.039630 0.437958
+0.118713 0.062923 0.224846 0.130581 -0.574499 0.808022 0.104782 0.225075
+0.128282 0.088243 0.241302 0.130581 -0.574499 0.808022 0.086230 0.212680
+0.107296 0.067456 0.229914 0.130581 -0.574499 0.808022 0.116859 0.196548
+0.118713 0.062923 0.224846 0.546977 -0.591640 0.592265 0.102323 0.225356
+0.131489 0.078973 0.229080 0.546977 -0.591640 0.592265 0.069787 0.240614
+0.128282 0.088243 0.241302 0.546977 -0.591640 0.592265 0.086230 0.212680
+0.128282 0.088243 0.241302 -0.106723 -0.392687 0.913459 0.086230 0.212680
+0.113825 0.086887 0.239030 -0.106723 -0.392687 0.913459 0.085630 0.194932
+0.107296 0.067456 0.229914 -0.106723 -0.392687 0.913459 0.116859 0.196548
+0.158948 0.073733 0.201971 -0.747981 -0.134729 0.649902 0.045070 0.346890
+0.159469 0.086296 0.205175 -0.747981 -0.134729 0.649902 0.036382 0.346078
+0.149787 0.066389 0.189905 -0.747981 -0.134729 0.649902 0.078462 0.341776
+0.173943 0.072128 0.185599 0.987532 -0.153296 0.035792 0.037193 0.399844
+0.176435 0.092959 0.206061 0.987532 -0.153296 0.035792 0.016240 0.393960
+0.174433 0.077625 0.195623 0.987532 -0.153296 0.035792 0.020480 0.387770
+0.176435 0.092959 0.206061 0.047143 -0.248685 0.967436 0.007915 0.371467
+0.159469 0.086296 0.205175 0.047143 -0.248685 0.967436 0.021672 0.354337
+0.158948 0.073733 0.201971 0.047143 -0.248685 0.967436 0.030040 0.353640
+0.158948 0.073733 0.201971 0.431624 -0.545427 0.718477 0.030040 0.353640
+0.174433 0.077625 0.195623 0.431624 -0.545427 0.718477 0.019210 0.387510
+0.176435 0.092959 0.206061 0.431624 -0.545427 0.718477 0.007915 0.371467
+0.113825 0.086887 0.239030 -0.763241 -0.045901 0.644482 0.085630 0.194932
+0.086585 0.059950 0.204852 -0.763241 -0.045901 0.644482 0.187156 0.190790
+0.107296 0.067456 0.229914 -0.763241 -0.045901 0.644482 0.116859 0.196548
+0.142987 0.029221 0.148360 0.638381 -0.762440 -0.105617 0.161963 0.397901
+0.150255 0.033474 0.161588 0.638381 -0.762440 -0.105617 0.126154 0.391636
+0.135350 0.021139 0.160543 0.638381 -0.762440 -0.105617 0.158440 0.365650
+0.119302 0.038419 0.159993 0.369961 0.129537 0.919972 0.182860 0.335000
+0.119415 0.029098 0.161260 0.369961 0.129537 0.919972 0.188520 0.331390
+0.124638 0.045934 0.156789 0.369961 0.129537 0.919972 0.172400 0.336930
+0.142930 0.026574 0.139531 0.953163 -0.291356 0.081197 0.174691 0.414974
+0.142987 0.029221 0.148360 0.953163 -0.291356 0.081197 0.161963 0.397901
+0.140320 0.014932 0.128395 0.953163 -0.291356 0.081197 0.195960 0.427720
+0.131489 0.078973 0.229080 0.711285 -0.660514 0.240407 0.069787 0.240614
+0.107389 0.046282 0.210566 0.711285 -0.660514 0.240407 0.147990 0.226080
+0.108825 0.043741 0.199336 0.711285 -0.660514 0.240407 0.159984 0.245722
+0.053360 0.088072 0.187479 -0.311539 0.935424 -0.167110 0.282632 0.904939
+0.063223 0.090410 0.182179 -0.311539 0.935424 -0.167110 0.270710 0.887400
+0.053192 0.085858 0.175399 -0.311539 0.935424 -0.167110 0.298639 0.885778
+0.052997 0.025909 0.078557 0.193159 0.644918 -0.739440 0.450460 0.714650
+0.071762 0.046670 0.101566 0.193159 0.644918 -0.739440 0.374767 0.709924
+0.081658 0.033262 0.092457 0.193159 0.644918 -0.739440 0.369880 0.669890
+-0.060966 -0.016531 -0.020698 -0.651389 0.538782 0.534235 0.841232 0.965642
+-0.070686 -0.013774 -0.035330 -0.651389 0.538782 0.534235 0.884636 0.967596
+-0.071377 -0.019477 -0.030421 -0.651389 0.538782 0.534235 0.879123 0.988764
+-0.070686 -0.013774 -0.035330 -0.651354 0.538799 0.534260 0.884636 0.967596
+-0.080407 -0.011017 -0.049962 -0.651354 0.538799 0.534260 0.928039 0.969549
+-0.071377 -0.019477 -0.030421 -0.651354 0.538799 0.534260 0.879123 0.988764
+-0.030055 -0.019593 0.010739 -0.712964 0.001680 0.701198 0.733560 0.250650
+-0.045510 -0.018062 -0.004979 -0.712964 0.001680 0.701198 0.790600 0.243710
+-0.030500 -0.033141 0.010319 -0.712964 0.001680 0.701198 0.740681 0.264800
+-0.045510 -0.018062 -0.004979 -0.712964 0.001681 0.701199 0.790600 0.243710
+-0.060966 -0.016531 -0.020698 -0.712964 0.001681 0.701199 0.847640 0.236770
+-0.030500 -0.033141 0.010319 -0.712964 0.001681 0.701199 0.740681 0.264800
+-0.003552 -0.007667 0.034842 -0.709785 0.235454 0.663902 0.647650 0.255020
+-0.016803 -0.013630 0.022790 -0.709785 0.235454 0.663902 0.690605 0.252835
+-0.023862 -0.025303 0.019383 -0.709785 0.235454 0.663902 0.712310 0.264110
+-0.025528 -0.015002 -0.098342 0.523553 0.697209 -0.489685 0.894100 0.654540
+-0.008270 -0.013472 -0.077712 0.523553 0.697209 -0.489685 0.825605 0.640405
+-0.004285 -0.027983 -0.094112 0.523553 0.697209 -0.489685 0.848720 0.590070
+-0.008270 -0.013472 -0.077712 0.523510 0.697224 -0.489709 0.825605 0.640405
+0.008989 -0.011941 -0.057082 0.523510 0.697224 -0.489709 0.757109 0.626270
+-0.004285 -0.027983 -0.094112 0.523510 0.697224 -0.489709 0.848720 0.590070
+-0.047826 -0.000722 -0.060163 0.009873 0.938464 -0.345236 0.877380 0.822860
+-0.036677 -0.007862 -0.079253 0.009873 0.938464 -0.345236 0.885739 0.738700
+-0.082414 -0.008469 -0.082211 0.009873 0.938464 -0.345236 0.963945 0.892963
+-0.036677 -0.007862 -0.079253 0.009874 0.938458 -0.345252 0.885739 0.738700
+-0.025528 -0.015002 -0.098342 0.009874 0.938458 -0.345252 0.894100 0.654540
+-0.082414 -0.008469 -0.082211 0.009874 0.938458 -0.345252 0.963945 0.892963
+-0.080407 -0.011017 -0.049962 -0.273470 0.957407 0.092664 0.928039 0.969549
+-0.064116 -0.005870 -0.055063 -0.273470 0.957407 0.092664 0.902709 0.896205
+-0.082414 -0.008469 -0.082211 -0.273470 0.957407 0.092664 0.963945 0.892963
+0.045717 0.011490 0.032308 -0.182390 0.973132 -0.140527 0.534848 0.690702
+0.036877 0.011536 0.044100 -0.182390 0.973132 -0.140527 0.536104 0.738321
+0.062588 0.018440 0.058539 -0.182390 0.973132 -0.140527 0.462180 0.663670
+0.036877 0.011536 0.044100 -0.182362 0.973130 -0.140576 0.536104 0.738321
+0.028037 0.011583 0.055893 -0.182362 0.973130 -0.140576 0.537360 0.785940
+0.062588 0.018440 0.058539 -0.182362 0.973130 -0.140576 0.462180 0.663670
+0.028037 0.011583 0.055893 -0.631896 0.718168 0.291449 0.537360 0.785940
+0.012243 0.001958 0.045367 -0.631896 0.718168 0.291449 0.575925 0.816265
+0.016955 -0.004583 0.071701 -0.631896 0.718168 0.291449 0.536530 0.818012
+0.012243 0.001958 0.045367 -0.631858 0.718200 0.291451 0.575925 0.816265
+-0.003552 -0.007667 0.034842 -0.631858 0.718200 0.291451 0.614490 0.846590
+0.016955 -0.004583 0.071701 -0.631858 0.718200 0.291451 0.536530 0.818012
+0.029745 -0.010530 -0.025899 0.757465 0.392194 -0.521949 0.708175 0.630170
+0.040124 -0.009825 -0.010307 0.757465 0.392194 -0.521949 0.683707 0.632120
+0.029309 -0.029773 -0.040991 0.757465 0.392194 -0.521949 0.694998 0.593245
+0.040124 -0.009825 -0.010307 0.757479 0.392177 -0.521942 0.683707 0.632120
+0.050502 -0.009120 0.005284 0.757479 0.392177 -0.521942 0.659240 0.634070
+0.029309 -0.029773 -0.040991 0.757479 0.392177 -0.521942 0.694998 0.593245
+0.008989 -0.011941 -0.057082 0.757481 0.392213 -0.521912 0.757109 0.626270
+0.019367 -0.011236 -0.041490 0.757481 0.392213 -0.521912 0.732642 0.628220
+0.029309 -0.029773 -0.040991 0.757481 0.392213 -0.521912 0.694998 0.593245
+0.019367 -0.011236 -0.041490 0.757458 0.392199 -0.521955 0.732642 0.628220
+0.029745 -0.010530 -0.025899 0.757458 0.392199 -0.521955 0.708175 0.630170
+0.029309 -0.029773 -0.040991 0.757458 0.392199 -0.521955 0.694998 0.593245
+-0.064116 -0.005870 -0.055063 -0.273531 0.957385 0.092707 0.902709 0.896205
+-0.055971 -0.003296 -0.057613 -0.273531 0.957385 0.092707 0.890044 0.859532
+-0.082414 -0.008469 -0.082211 -0.273531 0.957385 0.092707 0.963945 0.892963
+-0.047826 -0.000722 -0.060163 -0.273531 0.957385 0.092707 0.877380 0.822860
+-0.016803 -0.013630 0.022790 -0.709692 0.235359 0.664036 0.690605 0.252835
+-0.023429 -0.016611 0.016765 -0.709692 0.235359 0.664036 0.712083 0.251742
+-0.023862 -0.025303 0.019383 -0.709692 0.235359 0.664036 0.712310 0.264110
+-0.023429 -0.016611 0.016765 -0.709760 0.235342 0.663969 0.712083 0.251742
+-0.030055 -0.019593 0.010739 -0.709760 0.235342 0.663969 0.733560 0.250650
+-0.023862 -0.025303 0.019383 -0.709760 0.235342 0.663969 0.712310 0.264110
+3 0 1 2
+3 3 4 5
+3 6 7 8
+3 9 10 11
+3 12 13 14
+3 15 16 17
+3 18 19 20
+3 21 22 23
+3 24 25 26
+3 27 28 29
+3 30 31 32
+3 33 34 35
+3 36 37 38
+3 39 40 41
+3 42 43 44
+3 45 46 47
+3 48 49 50
+3 51 52 53
+3 54 55 56
+3 57 58 59
+3 60 61 62
+3 63 64 65
+3 66 67 68
+3 69 70 71
+3 72 73 74
+3 75 76 77
+3 78 79 80
+3 81 82 83
+3 84 85 86
+3 87 88 89
+3 90 91 92
+3 93 94 95
+3 96 97 98
+3 99 100 101
+3 102 103 104
+3 105 106 107
+3 108 109 110
+3 111 112 113
+3 114 115 116
+3 117 118 119
+3 120 121 122
+3 123 124 125
+3 126 127 128
+3 129 130 131
+3 132 133 134
+3 135 136 137
+3 138 139 140
+3 141 142 143
+3 144 145 146
+3 147 148 149
+3 150 151 152
+3 153 154 155
+3 156 157 158
+3 159 160 161
+3 162 163 164
+3 165 166 167
+3 168 169 170
+3 171 172 173
+3 174 175 176
+3 177 178 179
+3 180 181 182
+3 183 184 185
+3 186 187 188
+3 189 190 191
+3 192 193 194
+3 195 196 197
+3 198 199 200
+3 201 202 203
+3 204 205 206
+3 207 208 209
+3 210 211 212
+3 213 214 215
+3 216 217 218
+3 219 220 221
+3 222 223 224
+3 225 226 227
+3 228 229 230
+3 231 232 233
+3 234 235 236
+3 237 238 239
+3 240 241 242
+3 243 244 245
+3 246 247 248
+3 249 250 251
+3 252 253 254
+3 255 256 257
+3 258 259 260
+3 261 262 263
+3 264 265 266
+3 267 268 269
+3 270 271 272
+3 273 274 275
+3 276 277 278
+3 279 280 281
+3 282 283 284
+3 285 286 287
+3 288 289 290
+3 291 292 293
+3 294 295 296
+3 297 298 299
+3 300 301 302
+3 303 304 305
+3 306 307 308
+3 309 310 311
+3 312 313 314
+3 315 316 317
+3 318 319 320
+3 321 322 323
+3 324 325 326
+3 327 328 329
+3 330 331 332
+3 333 334 335
+3 336 337 338
+3 339 340 341
+3 342 343 344
+3 345 346 347
+3 348 349 350
+3 351 352 353
+3 354 355 356
+3 357 358 359
+3 360 361 362
+3 363 364 365
+3 366 367 368
+3 369 370 371
+3 372 373 374
+3 375 376 377
+3 378 379 380
+3 381 382 383
+3 384 385 386
+3 387 388 389
+3 390 391 392
+3 393 394 395
+3 396 397 398
+3 399 400 401
+3 402 403 404
+3 405 406 407
+3 408 409 410
+3 411 412 413
+3 414 415 416
+3 417 418 419
+3 420 421 422
+3 423 424 425
+3 426 427 428
+3 429 430 431
+3 432 433 434
+3 435 436 437
+3 438 439 440
+3 441 442 443
+3 444 445 446
+3 447 448 449
+3 450 451 452
+3 453 454 455
+3 456 457 458
+3 459 460 461
+3 462 463 464
+3 465 466 467
+3 468 469 470
+3 471 472 473
+3 474 475 476
+3 477 478 479
+3 480 481 482
+3 483 484 485
+3 486 487 488
+3 489 490 491
+3 492 493 494
+3 495 496 497
+3 498 499 500
+3 501 502 503
+3 504 505 506
+3 507 508 509
+3 510 511 512
+3 513 514 515
+3 516 517 518
+3 519 520 521
+3 522 523 524
+3 525 526 527
+3 528 529 530
+3 531 532 533
+3 534 535 536
+3 537 538 539
+3 540 541 542
+3 543 544 545
+3 546 547 548
+3 549 550 551
+3 552 553 554
+3 555 556 557
+3 558 559 560
+3 561 562 563
+3 564 565 566
+3 567 568 569
+3 570 571 572
+3 573 574 575
+3 576 577 578
+3 579 580 581
+3 582 583 584
+3 585 586 587
+3 588 589 590
+3 591 592 593
+3 594 595 596
+3 597 598 599
+3 600 601 602
+3 603 604 605
+3 606 607 608
+3 609 610 611
+3 612 613 614
+3 615 616 617
+3 618 619 620
+3 621 622 623
+3 624 625 626
+3 627 628 629
+3 630 631 632
+3 633 634 635
+3 636 637 638
+3 639 640 641
+3 642 643 644
+3 645 646 647
+3 648 649 650
+3 651 652 653
+3 654 655 656
+3 657 658 659
+3 660 661 662
+3 663 664 665
+3 666 667 668
+3 669 670 671
+3 672 673 674
+3 675 676 677
+3 678 679 680
+3 681 682 683
+3 684 685 686
+3 687 688 689
+3 690 691 692
+3 693 694 695
+3 696 697 698
+3 699 700 701
+3 702 703 704
+3 705 706 707
+3 708 709 710
+3 711 712 713
+3 714 715 716
+3 717 718 719
+3 720 721 722
+3 723 724 725
+3 726 727 728
+3 729 730 731
+3 732 733 734
+3 735 736 737
+3 738 739 740
+3 741 742 743
+3 744 745 746
+3 747 748 749
+3 750 751 752
+3 753 754 755
+3 756 757 758
+3 759 760 761
+3 762 763 764
+3 765 766 767
+3 768 769 770
+3 771 772 773
+3 774 775 776
+3 777 778 779
+3 780 781 782
+3 783 784 785
+3 786 787 788
+3 789 790 791
+3 792 793 794
+3 795 796 797
+3 798 799 800
+3 801 802 803
+3 804 805 806
+3 807 808 809
+3 810 811 812
+3 813 814 815
+3 816 817 818
+3 819 820 821
+3 822 823 824
+3 825 826 827
+3 828 829 830
+3 831 832 833
+3 834 835 836
+3 837 838 839
+3 840 841 842
+3 843 844 845
+3 846 847 848
+3 849 850 851
+3 852 853 854
+3 855 856 857
+3 858 859 860
+3 861 862 863
+3 864 865 866
+3 867 868 869
+3 870 871 872
+3 873 874 875
+3 876 877 878
+3 879 880 881
+3 882 883 884
+3 885 886 887
+3 888 889 890
+3 891 892 893
+3 894 895 896
+3 897 898 899
+3 900 901 902
+3 903 904 905
+3 906 907 908
+3 909 910 911
+3 912 913 914
+3 915 916 917
+3 918 919 920
+3 921 922 923
+3 924 925 926
+3 927 928 929
+3 930 931 932
+3 933 934 935
+3 936 937 938
+3 939 940 941
+3 942 943 944
+3 945 946 947
+3 948 949 950
+3 951 952 953
+3 954 955 956
+3 957 958 959
+3 960 961 962
+3 963 964 965
+3 966 967 968
+3 969 970 971
+3 972 973 974
+3 975 976 977
+3 978 979 980
+3 981 982 983
+3 984 985 986
+3 987 988 989
+3 990 991 992
+3 993 994 995
+3 996 997 998
+3 999 1000 1001
+3 1002 1003 1004
+3 1005 1006 1007
+3 1008 1009 1010
+3 1011 1012 1013
+3 1014 1015 1016
+3 1017 1018 1019
+3 1020 1021 1022
+3 1023 1024 1025
+3 1026 1027 1028
+3 1029 1030 1031
+3 1032 1033 1034
+3 1035 1036 1037
+3 1038 1039 1040
+3 1041 1042 1043
+3 1044 1045 1046
+3 1047 1048 1049
+3 1050 1051 1052
+3 1053 1054 1055
+3 1056 1057 1058
+3 1059 1060 1061
+3 1062 1063 1064
+3 1065 1066 1067
+3 1068 1069 1070
+3 1071 1072 1073
+3 1074 1075 1076
+3 1077 1078 1079
+3 1080 1081 1082
+3 1083 1084 1085
+3 1086 1087 1088
+3 1089 1090 1091
+3 1092 1093 1094
+3 1095 1096 1097
+3 1098 1099 1100
+3 1101 1102 1103
+3 1104 1105 1106
+3 1107 1108 1109
+3 1110 1111 1112
+3 1113 1114 1115
+3 1116 1117 1118
+3 1119 1120 1121
+3 1122 1123 1124
+3 1125 1126 1127
+3 1128 1129 1130
+3 1131 1132 1133
+3 1134 1135 1136
+3 1137 1138 1139
+3 1140 1141 1142
+3 1143 1144 1145
+3 1146 1147 1148
+3 1149 1150 1151
+3 1152 1153 1154
+3 1155 1156 1157
+3 1158 1159 1160
+3 1161 1162 1163
+3 1164 1165 1166
+3 1167 1168 1169
+3 1170 1171 1172
+3 1173 1174 1175
+3 1176 1177 1178
+3 1179 1180 1181
+3 1182 1183 1184
+3 1185 1186 1187
+3 1188 1189 1190
+3 1191 1192 1193
+3 1194 1195 1196
+3 1197 1198 1199
+3 1200 1201 1202
+3 1203 1204 1205
+3 1206 1207 1208
+3 1209 1210 1211
+3 1212 1213 1214
+3 1215 1216 1217
+3 1218 1219 1220
+3 1221 1222 1223
+3 1224 1225 1226
+3 1227 1228 1229
+3 1230 1231 1232
+3 1233 1234 1235
+3 1236 1237 1238
+3 1239 1240 1241
+3 1242 1243 1244
+3 1245 1246 1247
+3 1248 1249 1250
+3 1251 1252 1253
+3 1254 1255 1256
+3 1257 1258 1259
+3 1260 1261 1262
+3 1263 1264 1265
+3 1266 1267 1268
+3 1269 1270 1271
+3 1272 1273 1274
+3 1275 1276 1277
+3 1278 1279 1280
+3 1281 1282 1283
+3 1284 1285 1286
+3 1287 1288 1289
+3 1290 1291 1292
+3 1293 1294 1295
+3 1296 1297 1298
+3 1299 1300 1301
+3 1302 1303 1304
+3 1305 1306 1307
+3 1308 1309 1310
+3 1311 1312 1313
+3 1314 1315 1316
+3 1317 1318 1319
+3 1320 1321 1322
+3 1323 1324 1325
+3 1326 1327 1328
+3 1329 1330 1331
+3 1332 1333 1334
+3 1335 1336 1337
+3 1338 1339 1340
+3 1341 1342 1343
+3 1344 1345 1346
+3 1347 1348 1349
+3 1350 1351 1352
+3 1353 1354 1355
+3 1356 1357 1358
+3 1359 1360 1361
+3 1362 1363 1364
+3 1365 1366 1367
+3 1368 1369 1370
+3 1371 1372 1373
+3 1374 1375 1376
+3 1377 1378 1379
+3 1380 1381 1382
+3 1383 1384 1385
+3 1386 1387 1388
+3 1389 1390 1391
+3 1392 1393 1394
+3 1395 1396 1397
+3 1398 1399 1400
+3 1401 1402 1403
+3 1404 1405 1406
+3 1407 1408 1409
+3 1410 1411 1412
+3 1413 1414 1415
+3 1416 1417 1418
+3 1419 1420 1421
+3 1422 1423 1424
+3 1425 1426 1427
+3 1428 1429 1430
+3 1431 1432 1433
+3 1434 1435 1436
+3 1437 1438 1439
+3 1440 1441 1442
+3 1443 1444 1445
+3 1446 1447 1448
+3 1449 1450 1451
+3 1452 1453 1454
+3 1455 1456 1457
+3 1458 1459 1460
+3 1461 1462 1463
+3 1464 1465 1466
+3 1467 1468 1469
+3 1470 1471 1472
+3 1473 1474 1475
+3 1476 1477 1478
+3 1479 1480 1481
+3 1482 1483 1484
+3 1485 1486 1487
+3 1488 1489 1490
+3 1491 1492 1493
+3 1494 1495 1496
+3 1497 1498 1499
+3 1500 1501 1502
+3 1503 1504 1505
+3 1506 1507 1508
+3 1509 1510 1511
+3 1512 1513 1514
+3 1515 1516 1517
+3 1518 1519 1520
+3 1521 1522 1523
+3 1524 1525 1526
+3 1527 1528 1529
+3 1530 1531 1532
+3 1533 1534 1535
+3 1536 1537 1538
+3 1539 1540 1541
+3 1542 1543 1544
+3 1545 1546 1547
+3 1548 1549 1550
+3 1551 1552 1553
+3 1554 1555 1556
+3 1557 1558 1559
+3 1560 1561 1562
+3 1563 1564 1565
+3 1566 1567 1568
+3 1569 1570 1571
+3 1572 1573 1574
+3 1573 1575 1574
+3 1576 1577 1578
+3 1579 1580 1581
diff --git a/Examples/Stapling/Data/Geometry/forearm.osgb b/Examples/Stapling/Data/Geometry/forearm.osgb
new file mode 100644
index 0000000..11f06cc
Binary files /dev/null and b/Examples/Stapling/Data/Geometry/forearm.osgb differ
diff --git a/Examples/Stapling/Data/Geometry/forearm.png b/Examples/Stapling/Data/Geometry/forearm.png
new file mode 100644
index 0000000..471328e
Binary files /dev/null and b/Examples/Stapling/Data/Geometry/forearm.png differ
diff --git a/Examples/Stapling/Data/Geometry/forearm_normal.png b/Examples/Stapling/Data/Geometry/forearm_normal.png
new file mode 100644
index 0000000..59d463a
Binary files /dev/null and b/Examples/Stapling/Data/Geometry/forearm_normal.png differ
diff --git a/Examples/Stapling/Data/Geometry/staple.mtl b/Examples/Stapling/Data/Geometry/staple.mtl
new file mode 100644
index 0000000..70d3ba1
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/staple.mtl
@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2
diff --git a/Examples/Stapling/Data/Geometry/staple.obj b/Examples/Stapling/Data/Geometry/staple.obj
new file mode 100644
index 0000000..3545daa
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/staple.obj
@@ -0,0 +1,236 @@
+# Blender v2.69 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib staple.mtl
+o staple.001
+v 0.004801 0.002520 -0.000394
+v 0.004801 0.002520 0.000394
+v 0.003882 0.000105 -0.000000
+v 0.003892 0.003619 -0.000391
+v 0.003892 0.003619 0.000391
+v 0.003596 0.004065 0.000404
+v 0.003596 0.004065 -0.000404
+v 0.004153 0.004621 -0.000404
+v 0.004153 0.004621 0.000404
+v 0.004592 0.003976 0.000391
+v 0.004592 0.003976 -0.000391
+v 0.003154 0.004379 0.000391
+v 0.003154 0.004379 -0.000392
+v 0.004002 0.003100 -0.000391
+v 0.004002 0.003100 0.000391
+v 0.002671 0.004518 0.000394
+v 0.002671 0.004518 -0.000394
+v 0.003468 0.005101 -0.000392
+v 0.003468 0.005101 0.000391
+v 0.002757 0.005300 -0.000394
+v 0.002757 0.005300 0.000394
+v 0.004783 0.003198 -0.000391
+v 0.004783 0.003198 0.000391
+v 0.004013 0.002520 -0.000394
+v 0.004013 0.002520 0.000394
+v -0.001127 -0.003682 -0.000391
+v -0.000813 -0.004125 -0.000404
+v -0.001369 -0.004681 -0.000404
+v -0.001850 -0.003996 -0.000391
+v -0.000367 -0.004420 -0.000391
+v -0.000725 -0.005121 -0.000391
+v 0.000053 -0.005311 -0.000391
+v 0.000152 -0.004530 -0.000391
+v -0.000813 -0.004125 0.000404
+v -0.000367 -0.004420 0.000391
+v -0.000725 -0.005121 0.000391
+v -0.001369 -0.004681 0.000404
+v -0.001127 -0.003682 0.000392
+v 0.000152 -0.004530 0.000391
+v -0.001266 -0.003200 -0.000394
+v -0.001266 -0.003200 0.000394
+v -0.001850 -0.003996 0.000392
+v -0.002048 -0.003285 -0.000394
+v -0.002048 -0.003285 0.000394
+v 0.000053 -0.005311 0.000391
+v -0.002077 0.002660 -0.000394
+v -0.002077 0.002660 0.000394
+v -0.002048 0.003285 0.000394
+v -0.002048 0.003285 -0.000394
+v 0.000053 0.005311 -0.000391
+v -0.000725 0.005121 -0.000391
+v -0.000725 0.005121 0.000391
+v 0.000053 0.005311 0.000391
+v -0.001850 0.003996 0.000391
+v -0.001850 0.003996 -0.000392
+v -0.001369 0.004681 0.000404
+v -0.001369 0.004681 -0.000404
+v -0.001127 0.003682 -0.000392
+v -0.001127 0.003682 0.000391
+v -0.001266 0.003200 0.000394
+v -0.001266 0.003200 -0.000394
+v -0.001289 -0.002660 -0.000394
+v -0.001289 0.002660 -0.000394
+v -0.001289 0.002660 0.000394
+v -0.001289 -0.002660 0.000394
+v -0.002077 -0.002660 -0.000394
+v -0.002077 -0.002660 0.000394
+v 0.000152 0.004530 -0.000391
+v 0.000152 0.004530 0.000391
+v -0.000367 0.004420 0.000391
+v -0.000367 0.004420 -0.000391
+v -0.000813 0.004125 -0.000404
+v -0.000813 0.004125 0.000404
+v 0.003892 -0.003619 -0.000391
+v 0.003596 -0.004065 -0.000404
+v 0.003596 -0.004065 0.000404
+v 0.003892 -0.003619 0.000391
+v 0.004153 -0.004621 -0.000404
+v 0.004592 -0.003976 -0.000391
+v 0.004592 -0.003976 0.000391
+v 0.004153 -0.004621 0.000404
+v 0.003154 -0.004379 -0.000391
+v 0.003154 -0.004379 0.000392
+v 0.004002 -0.003100 -0.000391
+v 0.004002 -0.003100 0.000391
+v 0.002671 -0.004518 -0.000394
+v 0.002671 -0.004518 0.000394
+v 0.003468 -0.005101 -0.000391
+v 0.003468 -0.005101 0.000392
+v 0.002757 -0.005300 -0.000394
+v 0.002757 -0.005300 0.000394
+v 0.004783 -0.003198 -0.000391
+v 0.004783 -0.003198 0.000391
+v 0.000732 -0.004541 -0.000394
+v 0.002132 -0.004541 -0.000394
+v 0.002132 -0.005329 -0.000394
+v 0.000732 -0.005329 -0.000394
+v 0.000732 -0.004541 0.000394
+v 0.002132 -0.004541 0.000394
+v 0.000732 -0.005329 0.000394
+v 0.002132 -0.005329 0.000394
+v 0.002132 0.004541 0.000394
+v 0.002132 0.004541 -0.000394
+v 0.002132 0.005329 -0.000394
+v 0.002132 0.005329 0.000394
+v 0.000732 0.004541 -0.000394
+v 0.000732 0.005329 -0.000394
+v 0.000732 0.004541 0.000394
+v 0.000732 0.005329 0.000394
+v 0.004801 -0.002520 -0.000394
+v 0.004801 -0.002520 0.000394
+v 0.004013 -0.002520 -0.000394
+v 0.003882 -0.000105 0.000000
+v 0.004013 -0.002520 0.000394
+usemtl None
+s 1
+f 1 2 3
+f 4 5 6 7
+f 8 9 10 11
+f 7 6 12 13
+f 14 15 5 4
+f 13 12 16 17
+f 18 19 9 8
+f 20 21 19 18
+f 22 11 10 23
+f 24 25 15 14
+f 22 14 4 11
+f 7 8 11 4
+f 13 18 8 7
+f 17 20 18 13
+f 21 16 12 19
+f 6 9 19 12
+f 5 10 9 6
+f 15 23 10 5
+f 26 27 28 29
+f 27 30 31 28
+f 32 31 30 33
+f 30 27 34 35
+f 28 31 36 37
+f 27 26 38 34
+f 33 30 35 39
+f 26 40 41 38
+f 29 28 37 42
+f 43 29 42 44
+f 32 45 36 31
+f 40 26 29 43
+f 44 42 38 41
+f 34 38 42 37
+f 35 34 37 36
+f 39 35 36 45
+f 46 47 48 49
+f 50 51 52 53
+f 49 48 54 55
+f 55 54 56 57
+f 58 59 60 61
+f 62 63 64 65
+f 46 66 67 47
+f 65 64 47 67
+f 66 46 63 62
+f 68 69 70 71
+f 72 73 59 58
+f 57 56 52 51
+f 71 70 73 72
+f 61 60 64 63
+f 50 68 71 51
+f 72 57 51 71
+f 58 55 57 72
+f 61 49 55 58
+f 61 63 46 49
+f 48 47 64 60
+f 48 60 59 54
+f 73 56 54 59
+f 70 52 56 73
+f 69 53 52 70
+f 40 62 65 41
+f 40 43 66 62
+f 44 41 65 67
+f 66 43 44 67
+f 74 75 76 77
+f 78 79 80 81
+f 75 82 83 76
+f 84 74 77 85
+f 82 86 87 83
+f 88 78 81 89
+f 90 88 89 91
+f 92 93 80 79
+f 92 79 74 84
+f 75 74 79 78
+f 82 75 78 88
+f 86 82 88 90
+f 91 89 83 87
+f 76 83 89 81
+f 77 76 81 80
+f 85 77 80 93
+f 94 95 96 97
+f 95 94 98 99
+f 99 98 100 101
+f 97 96 101 100
+f 24 1 3
+f 3 25 24
+f 3 2 25
+f 17 16 102 103
+f 1 22 23 2
+f 104 105 21 20
+f 1 24 14 22
+f 17 103 104 20
+f 21 105 102 16
+f 25 2 23 15
+f 106 107 104 103
+f 103 102 108 106
+f 102 105 109 108
+f 107 109 105 104
+f 94 33 39 98
+f 33 94 97 32
+f 97 100 45 32
+f 45 100 98 39
+f 106 108 69 68
+f 107 50 53 109
+f 68 50 107 106
+f 53 69 108 109
+f 86 95 99 87
+f 110 111 93 92
+f 96 90 91 101
+f 84 112 110 92
+f 86 90 96 95
+f 91 87 99 101
+f 113 110 112
+f 113 112 114
+f 114 111 113
+f 111 110 113
+f 112 84 85 114
+f 93 111 114 85
diff --git a/Examples/Stapling/Data/Geometry/staple_collision.ply b/Examples/Stapling/Data/Geometry/staple_collision.ply
new file mode 100644
index 0000000..554e737
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/staple_collision.ply
@@ -0,0 +1,741 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 504
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+element face 224
+property list uchar uint vertex_indices
+end_header
+0.004801 0.000394 0.002520 0.934616 0.000000 -0.355657
+0.004801 -0.000394 0.002520 0.934616 0.000000 -0.355657
+0.003882 0.000000 0.000105 0.934616 0.000000 -0.355657
+0.003892 0.000391 0.003619 -0.833198 0.000000 -0.552975
+0.003892 -0.000391 0.003619 -0.833198 0.000000 -0.552975
+0.003596 -0.000404 0.004065 -0.833198 0.000000 -0.552975
+0.003596 0.000404 0.004065 -0.833198 -0.000000 -0.552975
+0.004153 0.000404 0.004621 0.826688 -0.000000 0.562660
+0.004153 -0.000404 0.004621 0.826688 -0.000000 0.562660
+0.004592 -0.000391 0.003976 0.826688 -0.000000 0.562660
+0.004592 0.000391 0.003976 0.826688 0.000000 0.562660
+0.003596 0.000404 0.004065 -0.579143 0.000000 -0.815226
+0.003596 -0.000404 0.004065 -0.579143 0.000000 -0.815226
+0.003154 -0.000391 0.004379 -0.579143 0.000000 -0.815226
+0.003154 0.000391 0.004379 -0.579143 -0.000000 -0.815226
+0.004002 0.000391 0.003100 -0.978269 0.000000 -0.207340
+0.004002 -0.000391 0.003100 -0.978269 0.000000 -0.207340
+0.003892 -0.000391 0.003619 -0.978269 0.000000 -0.207340
+0.003892 0.000391 0.003619 -0.978269 -0.000000 -0.207340
+0.003154 0.000391 0.004379 -0.276560 0.000000 -0.960997
+0.003154 -0.000391 0.004379 -0.276560 0.000000 -0.960997
+0.002671 -0.000394 0.004518 -0.276560 0.000000 -0.960997
+0.002671 0.000394 0.004518 -0.276560 -0.000000 -0.960997
+0.003468 0.000391 0.005101 0.573863 -0.000000 0.818951
+0.003468 -0.000391 0.005101 0.573863 -0.000000 0.818951
+0.004153 -0.000404 0.004621 0.573863 -0.000000 0.818951
+0.004153 0.000404 0.004621 0.573863 0.000000 0.818951
+0.002757 0.000394 0.005300 0.269529 -0.000000 0.962992
+0.002757 -0.000394 0.005300 0.269529 -0.000000 0.962992
+0.003468 -0.000391 0.005101 0.269529 -0.000000 0.962992
+0.003468 0.000391 0.005101 0.269529 0.000000 0.962992
+0.004783 0.000391 0.003198 0.971162 -0.000000 0.238421
+0.004592 0.000391 0.003976 0.971162 -0.000000 0.238421
+0.004592 -0.000391 0.003976 0.971162 -0.000000 0.238421
+0.004783 -0.000391 0.003198 0.971162 0.000000 0.238421
+0.004013 0.000394 0.002520 -0.999820 0.000000 -0.018962
+0.004013 -0.000394 0.002520 -0.999820 0.000000 -0.018962
+0.004002 -0.000391 0.003100 -0.999820 0.000000 -0.018962
+0.004002 0.000391 0.003100 -0.999820 -0.000000 -0.018962
+0.004783 0.000391 0.003198 -0.000000 1.000000 0.000000
+0.004002 0.000391 0.003100 -0.000000 1.000000 0.000000
+0.003892 0.000391 0.003619 -0.000000 1.000000 0.000000
+0.004592 0.000391 0.003976 0.000000 1.000000 0.000000
+0.003596 0.000404 0.004065 0.011978 0.999856 -0.012000
+0.004153 0.000404 0.004621 0.011978 0.999856 -0.012000
+0.004592 0.000391 0.003976 0.011978 0.999856 -0.012000
+0.003596 0.000404 0.004065 0.011103 0.999701 -0.021770
+0.004592 0.000391 0.003976 0.011103 0.999701 -0.021770
+0.003892 0.000391 0.003619 0.011103 0.999701 -0.021770
+0.003154 0.000391 0.004379 -0.014544 0.999874 0.006325
+0.003468 0.000391 0.005101 -0.014544 0.999874 0.006325
+0.004153 0.000404 0.004621 -0.014544 0.999874 0.006325
+0.003154 0.000391 0.004379 -0.017178 0.999704 0.017209
+0.004153 0.000404 0.004621 -0.017178 0.999704 0.017209
+0.003596 0.000404 0.004065 -0.017178 0.999704 0.017209
+0.002671 0.000394 0.004518 0.004093 0.999992 -0.000450
+0.002757 0.000394 0.005300 0.004093 0.999992 -0.000450
+0.003468 0.000391 0.005101 0.004093 0.999992 -0.000450
+0.002671 0.000394 0.004518 0.005520 0.999982 -0.002401
+0.003468 0.000391 0.005101 0.005520 0.999982 -0.002401
+0.003154 0.000391 0.004379 0.005520 0.999982 -0.002401
+0.002757 -0.000394 0.005300 0.006021 -0.999982 -0.000662
+0.002671 -0.000394 0.004518 0.006021 -0.999982 -0.000662
+0.003154 -0.000391 0.004379 0.006021 -0.999982 -0.000662
+0.002757 -0.000394 0.005300 0.003762 -0.999992 -0.001636
+0.003154 -0.000391 0.004379 0.003762 -0.999992 -0.001636
+0.003468 -0.000391 0.005101 0.003762 -0.999992 -0.001636
+0.003596 -0.000404 0.004065 -0.011149 -0.999875 0.011169
+0.004153 -0.000404 0.004621 -0.011149 -0.999875 0.011169
+0.003468 -0.000391 0.005101 -0.011149 -0.999875 0.011169
+0.003596 -0.000404 0.004065 -0.022463 -0.999700 0.009769
+0.003468 -0.000391 0.005101 -0.022463 -0.999700 0.009769
+0.003154 -0.000391 0.004379 -0.022463 -0.999700 0.009769
+0.003892 -0.000391 0.003619 0.007629 -0.999859 -0.014959
+0.004592 -0.000391 0.003976 0.007629 -0.999859 -0.014959
+0.004153 -0.000404 0.004621 0.007629 -0.999859 -0.014959
+0.003892 -0.000391 0.003619 0.017496 -0.999693 -0.017527
+0.004153 -0.000404 0.004621 0.017496 -0.999693 -0.017527
+0.003596 -0.000404 0.004065 0.017496 -0.999693 -0.017527
+0.004002 -0.000391 0.003100 0.000000 -1.000000 -0.000000
+0.004783 -0.000391 0.003198 0.000000 -1.000000 -0.000000
+0.004592 -0.000391 0.003976 0.000000 -1.000000 -0.000000
+0.003892 -0.000391 0.003619 0.000000 -1.000000 -0.000000
+-0.001127 0.000391 -0.003682 -0.017168 0.999705 0.017168
+-0.000813 0.000404 -0.004125 -0.017168 0.999705 0.017168
+-0.001369 0.000404 -0.004681 -0.017168 0.999705 0.017168
+-0.001127 0.000391 -0.003682 -0.006315 0.999874 0.014541
+-0.001369 0.000404 -0.004681 -0.006315 0.999874 0.014541
+-0.001850 0.000391 -0.003996 -0.006315 0.999874 0.014541
+-0.000813 0.000404 -0.004125 0.021782 0.999701 -0.011124
+-0.000367 0.000391 -0.004420 0.021782 0.999701 -0.011124
+-0.000725 0.000391 -0.005121 0.021782 0.999701 -0.011124
+-0.000813 0.000404 -0.004125 0.011991 0.999856 -0.011991
+-0.000725 0.000391 -0.005121 0.011991 0.999856 -0.011991
+-0.001369 0.000404 -0.004681 0.011991 0.999856 -0.011991
+0.000053 0.000391 -0.005311 0.000000 1.000000 0.000000
+-0.000725 0.000391 -0.005121 0.000000 1.000000 0.000000
+-0.000367 0.000391 -0.004420 0.000000 1.000000 0.000000
+0.000152 0.000391 -0.004530 0.000000 1.000000 0.000000
+-0.000367 0.000391 -0.004420 0.551676 -0.000000 0.834059
+-0.000813 0.000404 -0.004125 0.551676 -0.000000 0.834059
+-0.000813 -0.000404 -0.004125 0.551676 -0.000000 0.834059
+-0.000367 -0.000391 -0.004420 0.551676 0.000000 0.834059
+-0.001369 0.000404 -0.004681 -0.564132 0.000000 -0.825684
+-0.000725 0.000391 -0.005121 -0.564132 0.000000 -0.825684
+-0.000725 -0.000391 -0.005121 -0.564132 0.000000 -0.825684
+-0.001369 -0.000404 -0.004681 -0.564132 0.000000 -0.825684
+-0.000813 0.000404 -0.004125 0.815843 -0.000000 0.578273
+-0.001127 0.000391 -0.003682 0.815843 -0.000000 0.578273
+-0.001127 -0.000391 -0.003682 0.815843 -0.000000 0.578273
+-0.000813 -0.000404 -0.004125 0.815843 0.000000 0.578273
+0.000152 0.000391 -0.004530 0.207340 -0.000000 0.978269
+-0.000367 0.000391 -0.004420 0.207340 -0.000000 0.978269
+-0.000367 -0.000391 -0.004420 0.207340 -0.000000 0.978269
+0.000152 -0.000391 -0.004530 0.207340 0.000000 0.978269
+-0.001127 0.000391 -0.003682 0.960844 -0.000000 0.277090
+-0.001266 0.000394 -0.003200 0.960844 -0.000000 0.277090
+-0.001266 -0.000394 -0.003200 0.960844 -0.000000 0.277090
+-0.001127 -0.000391 -0.003682 0.960844 0.000000 0.277090
+-0.001850 0.000391 -0.003996 -0.818389 0.000000 -0.574665
+-0.001369 0.000404 -0.004681 -0.818389 0.000000 -0.574665
+-0.001369 -0.000404 -0.004681 -0.818389 0.000000 -0.574665
+-0.001850 -0.000391 -0.003996 -0.818389 0.000000 -0.574665
+-0.002048 0.000394 -0.003285 -0.963343 0.000000 -0.268273
+-0.001850 0.000391 -0.003996 -0.963343 0.000000 -0.268273
+-0.001850 -0.000391 -0.003996 -0.963343 0.000000 -0.268273
+-0.002048 -0.000394 -0.003285 -0.963343 0.000000 -0.268273
+0.000053 0.000391 -0.005311 -0.237244 0.000000 -0.971450
+0.000053 -0.000391 -0.005311 -0.237244 0.000000 -0.971450
+-0.000725 -0.000391 -0.005121 -0.237244 0.000000 -0.971450
+-0.000725 0.000391 -0.005121 -0.237244 -0.000000 -0.971450
+-0.001266 0.000394 -0.003200 0.002402 0.999982 -0.005531
+-0.001127 0.000391 -0.003682 0.002402 0.999982 -0.005531
+-0.001850 0.000391 -0.003996 0.002402 0.999982 -0.005531
+-0.001266 0.000394 -0.003200 0.000445 0.999992 -0.004095
+-0.001850 0.000391 -0.003996 0.000445 0.999992 -0.004095
+-0.002048 0.000394 -0.003285 0.000445 0.999992 -0.004095
+-0.002048 -0.000394 -0.003285 0.001635 -0.999992 -0.003764
+-0.001850 -0.000391 -0.003996 0.001635 -0.999992 -0.003764
+-0.001127 -0.000391 -0.003682 0.001635 -0.999992 -0.003764
+-0.002048 -0.000394 -0.003285 0.000656 -0.999982 -0.006035
+-0.001127 -0.000391 -0.003682 0.000656 -0.999982 -0.006035
+-0.001266 -0.000394 -0.003200 0.000656 -0.999982 -0.006035
+-0.000813 -0.000404 -0.004125 -0.009742 -0.999701 0.022431
+-0.001127 -0.000391 -0.003682 -0.009742 -0.999701 0.022431
+-0.001850 -0.000391 -0.003996 -0.009742 -0.999701 0.022431
+-0.000813 -0.000404 -0.004125 -0.011148 -0.999876 0.011148
+-0.001850 -0.000391 -0.003996 -0.011148 -0.999876 0.011148
+-0.001369 -0.000404 -0.004681 -0.011148 -0.999876 0.011148
+-0.000367 -0.000391 -0.004420 0.017538 -0.999692 -0.017538
+-0.000813 -0.000404 -0.004125 0.017538 -0.999692 -0.017538
+-0.001369 -0.000404 -0.004681 0.017538 -0.999692 -0.017538
+-0.000367 -0.000391 -0.004420 0.014963 -0.999859 -0.007641
+-0.001369 -0.000404 -0.004681 0.014963 -0.999859 -0.007641
+-0.000725 -0.000391 -0.005121 0.014963 -0.999859 -0.007641
+0.000152 -0.000391 -0.004530 0.000000 -1.000000 0.000000
+-0.000367 -0.000391 -0.004420 0.000000 -1.000000 0.000000
+-0.000725 -0.000391 -0.005121 0.000000 -1.000000 0.000000
+0.000053 -0.000391 -0.005311 0.000000 -1.000000 0.000000
+-0.002077 0.000394 0.002660 -0.998925 0.000000 0.046350
+-0.002077 -0.000394 0.002660 -0.998925 0.000000 0.046350
+-0.002048 -0.000394 0.003285 -0.998925 0.000000 0.046350
+-0.002048 0.000394 0.003285 -0.998925 0.000000 0.046350
+0.000053 0.000391 0.005311 -0.237244 0.000000 0.971450
+-0.000725 0.000391 0.005121 -0.237244 0.000000 0.971450
+-0.000725 -0.000391 0.005121 -0.237244 0.000000 0.971450
+0.000053 -0.000391 0.005311 -0.237244 0.000000 0.971450
+-0.002048 0.000394 0.003285 -0.963343 0.000000 0.268273
+-0.002048 -0.000394 0.003285 -0.963343 0.000000 0.268273
+-0.001850 -0.000391 0.003996 -0.963343 0.000000 0.268273
+-0.001850 0.000391 0.003996 -0.963343 0.000000 0.268273
+-0.001850 0.000391 0.003996 -0.818389 0.000000 0.574665
+-0.001850 -0.000391 0.003996 -0.818389 0.000000 0.574665
+-0.001369 -0.000404 0.004681 -0.818389 0.000000 0.574665
+-0.001369 0.000404 0.004681 -0.818389 0.000000 0.574665
+-0.001127 0.000391 0.003682 0.960844 0.000000 -0.277090
+-0.001127 -0.000391 0.003682 0.960844 0.000000 -0.277090
+-0.001266 -0.000394 0.003200 0.960844 0.000000 -0.277090
+-0.001266 0.000394 0.003200 0.960844 0.000000 -0.277090
+-0.001289 0.000394 -0.002660 1.000000 -0.000000 0.000000
+-0.001289 0.000394 0.002660 1.000000 -0.000000 0.000000
+-0.001289 -0.000394 0.002660 1.000000 -0.000000 0.000000
+-0.001289 -0.000394 -0.002660 1.000000 -0.000000 0.000000
+-0.002077 0.000394 0.002660 -1.000000 0.000000 0.000000
+-0.002077 0.000394 -0.002660 -1.000000 0.000000 0.000000
+-0.002077 -0.000394 -0.002660 -1.000000 0.000000 0.000000
+-0.002077 -0.000394 0.002660 -1.000000 0.000000 0.000000
+-0.001289 -0.000394 -0.002660 0.000000 -1.000000 0.000000
+-0.001289 -0.000394 0.002660 0.000000 -1.000000 0.000000
+-0.002077 -0.000394 0.002660 0.000000 -1.000000 0.000000
+-0.002077 -0.000394 -0.002660 0.000000 -1.000000 0.000000
+-0.002077 0.000394 -0.002660 0.000000 1.000000 0.000000
+-0.002077 0.000394 0.002660 0.000000 1.000000 0.000000
+-0.001289 0.000394 0.002660 0.000000 1.000000 0.000000
+-0.001289 0.000394 -0.002660 0.000000 1.000000 -0.000000
+0.000152 0.000391 0.004530 0.207340 0.000000 -0.978269
+0.000152 -0.000391 0.004530 0.207340 0.000000 -0.978269
+-0.000367 -0.000391 0.004420 0.207340 0.000000 -0.978269
+-0.000367 0.000391 0.004420 0.207340 0.000000 -0.978269
+-0.000813 0.000404 0.004125 0.815843 0.000000 -0.578273
+-0.000813 -0.000404 0.004125 0.815843 0.000000 -0.578273
+-0.001127 -0.000391 0.003682 0.815843 0.000000 -0.578273
+-0.001127 0.000391 0.003682 0.815843 0.000000 -0.578273
+-0.001369 0.000404 0.004681 -0.564132 0.000000 0.825684
+-0.001369 -0.000404 0.004681 -0.564132 0.000000 0.825684
+-0.000725 -0.000391 0.005121 -0.564132 0.000000 0.825684
+-0.000725 0.000391 0.005121 -0.564132 0.000000 0.825684
+-0.000367 0.000391 0.004420 0.551676 0.000000 -0.834059
+-0.000367 -0.000391 0.004420 0.551676 0.000000 -0.834059
+-0.000813 -0.000404 0.004125 0.551676 0.000000 -0.834059
+-0.000813 0.000404 0.004125 0.551676 0.000000 -0.834059
+-0.001266 0.000394 0.003200 0.999094 0.000000 -0.042554
+-0.001266 -0.000394 0.003200 0.999094 0.000000 -0.042554
+-0.001289 -0.000394 0.002660 0.999094 0.000000 -0.042554
+-0.001289 0.000394 0.002660 0.999094 0.000000 -0.042554
+0.000053 0.000391 0.005311 0.000000 1.000000 -0.000000
+0.000152 0.000391 0.004530 0.000000 1.000000 -0.000000
+-0.000367 0.000391 0.004420 0.000000 1.000000 -0.000000
+-0.000725 0.000391 0.005121 -0.000000 1.000000 0.000000
+-0.000813 0.000404 0.004125 0.011991 0.999856 0.011991
+-0.001369 0.000404 0.004681 0.011991 0.999856 0.011991
+-0.000725 0.000391 0.005121 0.011991 0.999856 0.011991
+-0.000813 0.000404 0.004125 0.021782 0.999701 0.011124
+-0.000725 0.000391 0.005121 0.021782 0.999701 0.011124
+-0.000367 0.000391 0.004420 0.021782 0.999701 0.011124
+-0.001127 0.000391 0.003682 -0.006315 0.999874 -0.014541
+-0.001850 0.000391 0.003996 -0.006315 0.999874 -0.014541
+-0.001369 0.000404 0.004681 -0.006315 0.999874 -0.014541
+-0.001127 0.000391 0.003682 -0.017168 0.999705 -0.017168
+-0.001369 0.000404 0.004681 -0.017168 0.999705 -0.017168
+-0.000813 0.000404 0.004125 -0.017168 0.999705 -0.017168
+-0.001266 0.000394 0.003200 0.000445 0.999992 0.004095
+-0.002048 0.000394 0.003285 0.000445 0.999992 0.004095
+-0.001850 0.000391 0.003996 0.000445 0.999992 0.004095
+-0.001266 0.000394 0.003200 0.002402 0.999982 0.005531
+-0.001850 0.000391 0.003996 0.002402 0.999982 0.005531
+-0.001127 0.000391 0.003682 0.002402 0.999982 0.005531
+-0.001266 0.000394 0.003200 0.000000 1.000000 0.000000
+-0.002048 0.000394 0.003285 -0.000000 1.000000 0.000000
+-0.002048 -0.000394 0.003285 0.000000 -1.000000 0.000000
+-0.001266 -0.000394 0.003200 -0.000000 -1.000000 0.000000
+-0.002048 -0.000394 0.003285 0.000656 -0.999982 0.006035
+-0.001266 -0.000394 0.003200 0.000656 -0.999982 0.006035
+-0.001127 -0.000391 0.003682 0.000656 -0.999982 0.006035
+-0.002048 -0.000394 0.003285 0.001635 -0.999992 0.003764
+-0.001127 -0.000391 0.003682 0.001635 -0.999992 0.003764
+-0.001850 -0.000391 0.003996 0.001635 -0.999992 0.003764
+-0.000813 -0.000404 0.004125 -0.011148 -0.999876 -0.011148
+-0.001369 -0.000404 0.004681 -0.011148 -0.999876 -0.011148
+-0.001850 -0.000391 0.003996 -0.011148 -0.999876 -0.011148
+-0.000813 -0.000404 0.004125 -0.009742 -0.999701 -0.022431
+-0.001850 -0.000391 0.003996 -0.009742 -0.999701 -0.022431
+-0.001127 -0.000391 0.003682 -0.009742 -0.999701 -0.022431
+-0.000367 -0.000391 0.004420 0.014963 -0.999859 0.007641
+-0.000725 -0.000391 0.005121 0.014963 -0.999859 0.007641
+-0.001369 -0.000404 0.004681 0.014963 -0.999859 0.007641
+-0.000367 -0.000391 0.004420 0.017538 -0.999692 0.017538
+-0.001369 -0.000404 0.004681 0.017538 -0.999692 0.017538
+-0.000813 -0.000404 0.004125 0.017538 -0.999692 0.017538
+0.000152 -0.000391 0.004530 0.000000 -1.000000 0.000000
+0.000053 -0.000391 0.005311 0.000000 -1.000000 0.000000
+-0.000725 -0.000391 0.005121 0.000000 -1.000000 0.000000
+-0.000367 -0.000391 0.004420 0.000000 -1.000000 0.000000
+-0.001266 0.000394 -0.003200 0.999094 -0.000000 0.042554
+-0.001289 0.000394 -0.002660 0.999094 -0.000000 0.042554
+-0.001289 -0.000394 -0.002660 0.999094 -0.000000 0.042554
+-0.001266 -0.000394 -0.003200 0.999094 0.000000 0.042554
+-0.001266 0.000394 -0.003200 -0.000000 1.000000 0.000000
+-0.002048 0.000394 -0.003285 -0.000000 1.000000 0.000000
+-0.002048 -0.000394 -0.003285 0.000000 -1.000000 -0.000000
+-0.001266 -0.000394 -0.003200 0.000000 -1.000000 -0.000000
+-0.002077 0.000394 -0.002660 -0.998925 0.000000 -0.046350
+-0.002048 0.000394 -0.003285 -0.998925 0.000000 -0.046350
+-0.002048 -0.000394 -0.003285 -0.998925 0.000000 -0.046350
+-0.002077 -0.000394 -0.002660 -0.998925 0.000000 -0.046350
+0.003892 0.000391 -0.003619 -0.833198 0.000000 0.552975
+0.003596 0.000404 -0.004065 -0.833198 0.000000 0.552975
+0.003596 -0.000404 -0.004065 -0.833198 0.000000 0.552975
+0.003892 -0.000391 -0.003619 -0.833198 0.000000 0.552975
+0.004153 0.000404 -0.004621 0.826688 0.000000 -0.562660
+0.004592 0.000391 -0.003976 0.826688 0.000000 -0.562660
+0.004592 -0.000391 -0.003976 0.826688 0.000000 -0.562660
+0.004153 -0.000404 -0.004621 0.826688 0.000000 -0.562660
+0.003596 0.000404 -0.004065 -0.579143 0.000000 0.815226
+0.003154 0.000391 -0.004379 -0.579143 0.000000 0.815226
+0.003154 -0.000391 -0.004379 -0.579143 0.000000 0.815226
+0.003596 -0.000404 -0.004065 -0.579143 0.000000 0.815226
+0.004002 0.000391 -0.003100 -0.978269 0.000000 0.207340
+0.003892 0.000391 -0.003619 -0.978269 0.000000 0.207340
+0.003892 -0.000391 -0.003619 -0.978269 0.000000 0.207340
+0.004002 -0.000391 -0.003100 -0.978269 0.000000 0.207340
+0.003154 0.000391 -0.004379 -0.276560 0.000000 0.960997
+0.002671 0.000394 -0.004518 -0.276560 0.000000 0.960997
+0.002671 -0.000394 -0.004518 -0.276560 0.000000 0.960997
+0.003154 -0.000391 -0.004379 -0.276560 0.000000 0.960997
+0.003468 0.000391 -0.005101 0.573863 0.000000 -0.818951
+0.004153 0.000404 -0.004621 0.573863 0.000000 -0.818951
+0.004153 -0.000404 -0.004621 0.573863 0.000000 -0.818951
+0.003468 -0.000391 -0.005101 0.573863 0.000000 -0.818951
+0.002757 0.000394 -0.005300 0.269529 0.000000 -0.962992
+0.003468 0.000391 -0.005101 0.269529 0.000000 -0.962992
+0.003468 -0.000391 -0.005101 0.269529 0.000000 -0.962992
+0.002757 -0.000394 -0.005300 0.269529 0.000000 -0.962992
+0.004783 0.000391 -0.003198 0.971162 0.000000 -0.238421
+0.004783 -0.000391 -0.003198 0.971162 0.000000 -0.238421
+0.004592 -0.000391 -0.003976 0.971162 0.000000 -0.238421
+0.004592 0.000391 -0.003976 0.971162 0.000000 -0.238421
+0.004783 0.000391 -0.003198 -0.000000 1.000000 0.000000
+0.004592 0.000391 -0.003976 -0.000000 1.000000 0.000000
+0.003892 0.000391 -0.003619 -0.000000 1.000000 0.000000
+0.004002 0.000391 -0.003100 -0.000000 1.000000 0.000000
+0.003596 0.000404 -0.004065 0.011103 0.999701 0.021770
+0.003892 0.000391 -0.003619 0.011103 0.999701 0.021770
+0.004592 0.000391 -0.003976 0.011103 0.999701 0.021770
+0.003596 0.000404 -0.004065 0.011978 0.999856 0.012000
+0.004592 0.000391 -0.003976 0.011978 0.999856 0.012000
+0.004153 0.000404 -0.004621 0.011978 0.999856 0.012000
+0.003154 0.000391 -0.004379 -0.017178 0.999704 -0.017209
+0.003596 0.000404 -0.004065 -0.017178 0.999704 -0.017209
+0.004153 0.000404 -0.004621 -0.017178 0.999704 -0.017209
+0.003154 0.000391 -0.004379 -0.014544 0.999874 -0.006325
+0.004153 0.000404 -0.004621 -0.014544 0.999874 -0.006325
+0.003468 0.000391 -0.005101 -0.014544 0.999874 -0.006325
+0.002671 0.000394 -0.004518 0.005520 0.999982 0.002401
+0.003154 0.000391 -0.004379 0.005520 0.999982 0.002401
+0.003468 0.000391 -0.005101 0.005520 0.999982 0.002401
+0.002671 0.000394 -0.004518 0.004093 0.999992 0.000450
+0.003468 0.000391 -0.005101 0.004093 0.999992 0.000450
+0.002757 0.000394 -0.005300 0.004093 0.999992 0.000450
+0.002757 -0.000394 -0.005300 0.003761 -0.999992 0.001636
+0.003468 -0.000391 -0.005101 0.003761 -0.999992 0.001636
+0.003154 -0.000391 -0.004379 0.003761 -0.999992 0.001636
+0.002757 -0.000394 -0.005300 0.006021 -0.999982 0.000662
+0.003154 -0.000391 -0.004379 0.006021 -0.999982 0.000662
+0.002671 -0.000394 -0.004518 0.006021 -0.999982 0.000662
+0.003596 -0.000404 -0.004065 -0.022463 -0.999700 -0.009769
+0.003154 -0.000391 -0.004379 -0.022463 -0.999700 -0.009769
+0.003468 -0.000391 -0.005101 -0.022463 -0.999700 -0.009769
+0.003596 -0.000404 -0.004065 -0.011149 -0.999875 -0.011169
+0.003468 -0.000391 -0.005101 -0.011149 -0.999875 -0.011169
+0.004153 -0.000404 -0.004621 -0.011149 -0.999875 -0.011169
+0.003892 -0.000391 -0.003619 0.017496 -0.999693 0.017527
+0.003596 -0.000404 -0.004065 0.017496 -0.999693 0.017527
+0.004153 -0.000404 -0.004621 0.017496 -0.999693 0.017527
+0.003892 -0.000391 -0.003619 0.007629 -0.999859 0.014959
+0.004153 -0.000404 -0.004621 0.007629 -0.999859 0.014959
+0.004592 -0.000391 -0.003976 0.007629 -0.999859 0.014959
+0.004002 -0.000391 -0.003100 0.000000 -1.000000 0.000000
+0.003892 -0.000391 -0.003619 0.000000 -1.000000 0.000000
+0.004592 -0.000391 -0.003976 0.000000 -1.000000 0.000000
+0.004783 -0.000391 -0.003198 -0.000000 -1.000000 0.000000
+0.000732 0.000394 -0.004541 0.000000 1.000000 -0.000000
+0.002132 0.000394 -0.004541 0.000000 1.000000 -0.000000
+0.002132 0.000394 -0.005329 0.000000 1.000000 -0.000000
+0.000732 0.000394 -0.005329 0.000000 1.000000 -0.000000
+0.002132 0.000394 -0.004541 0.000000 0.000000 1.000000
+0.000732 0.000394 -0.004541 0.000000 0.000000 1.000000
+0.000732 -0.000394 -0.004541 0.000000 0.000000 1.000000
+0.002132 -0.000394 -0.004541 0.000000 -0.000000 1.000000
+0.002132 -0.000394 -0.004541 0.000000 -1.000000 0.000000
+0.000732 -0.000394 -0.004541 0.000000 -1.000000 0.000000
+0.000732 -0.000394 -0.005329 0.000000 -1.000000 0.000000
+0.002132 -0.000394 -0.005329 0.000000 -1.000000 0.000000
+0.000732 0.000394 -0.005329 0.000000 0.000000 -1.000000
+0.002132 0.000394 -0.005329 0.000000 0.000000 -1.000000
+0.002132 -0.000394 -0.005329 0.000000 0.000000 -1.000000
+0.000732 -0.000394 -0.005329 0.000000 0.000000 -1.000000
+0.004013 0.000394 0.002520 0.000000 0.986951 -0.161018
+0.004801 0.000394 0.002520 0.000000 0.986951 -0.161018
+0.003882 0.000000 0.000105 0.000000 0.986951 -0.161018
+0.003882 0.000000 0.000105 -0.998532 0.000000 0.054165
+0.004013 -0.000394 0.002520 -0.998532 0.000000 0.054165
+0.004013 0.000394 0.002520 -0.998532 0.000000 0.054165
+0.003882 0.000000 0.000105 0.000000 -0.986951 -0.161018
+0.004801 -0.000394 0.002520 0.000000 -0.986951 -0.161018
+0.004013 -0.000394 0.002520 0.000000 -0.986951 -0.161018
+0.002671 0.000394 0.004518 -0.042633 0.000000 -0.999091
+0.002671 -0.000394 0.004518 -0.042633 0.000000 -0.999091
+0.002132 -0.000394 0.004541 -0.042633 0.000000 -0.999091
+0.002132 0.000394 0.004541 -0.042633 -0.000000 -0.999091
+0.004801 0.000394 0.002520 0.999648 -0.000000 0.026540
+0.004783 0.000391 0.003198 0.999648 -0.000000 0.026540
+0.004783 -0.000391 0.003198 0.999648 -0.000000 0.026540
+0.004801 -0.000394 0.002520 0.999648 0.000000 0.026540
+0.002132 0.000394 0.005329 0.046350 -0.000000 0.998925
+0.002132 -0.000394 0.005329 0.046350 -0.000000 0.998925
+0.002757 -0.000394 0.005300 0.046350 -0.000000 0.998925
+0.002757 0.000394 0.005300 0.046350 0.000000 0.998925
+0.004801 0.000394 0.002520 -0.000000 0.999987 0.005172
+0.004013 0.000394 0.002520 -0.000000 0.999987 0.005172
+0.004002 0.000391 0.003100 -0.000000 0.999987 0.005172
+0.004801 0.000394 0.002520 -0.000553 0.999990 0.004410
+0.004002 0.000391 0.003100 -0.000553 0.999990 0.004410
+0.004783 0.000391 0.003198 -0.000553 0.999990 0.004410
+0.002671 0.000394 0.004518 0.000000 1.000000 0.000000
+0.002132 0.000394 0.004541 0.000000 1.000000 0.000000
+0.002132 0.000394 0.005329 0.000000 1.000000 0.000000
+0.002757 0.000394 0.005300 0.000000 1.000000 0.000000
+0.002757 -0.000394 0.005300 0.000000 -1.000000 0.000000
+0.002132 -0.000394 0.005329 0.000000 -1.000000 0.000000
+0.002132 -0.000394 0.004541 0.000000 -1.000000 0.000000
+0.002671 -0.000394 0.004518 0.000000 -1.000000 0.000000
+0.004013 -0.000394 0.002520 0.000000 -0.999990 0.004425
+0.004801 -0.000394 0.002520 0.000000 -0.999990 0.004425
+0.004783 -0.000391 0.003198 0.000000 -0.999990 0.004425
+0.004013 -0.000394 0.002520 -0.000647 -0.999986 0.005160
+0.004783 -0.000391 0.003198 -0.000647 -0.999986 0.005160
+0.004002 -0.000391 0.003100 -0.000647 -0.999986 0.005160
+0.000732 0.000394 0.004541 0.000000 1.000000 0.000000
+0.000732 0.000394 0.005329 0.000000 1.000000 0.000000
+0.002132 0.000394 0.004541 0.000000 0.000000 -1.000000
+0.002132 -0.000394 0.004541 0.000000 0.000000 -1.000000
+0.000732 -0.000394 0.004541 0.000000 0.000000 -1.000000
+0.000732 0.000394 0.004541 0.000000 0.000000 -1.000000
+0.000732 -0.000394 0.005329 0.000000 -1.000000 0.000000
+0.000732 -0.000394 0.004541 0.000000 -1.000000 0.000000
+0.000732 0.000394 0.005329 0.000000 -0.000000 1.000000
+0.000732 -0.000394 0.005329 0.000000 -0.000000 1.000000
+0.002132 -0.000394 0.005329 0.000000 -0.000000 1.000000
+0.002132 0.000394 0.005329 0.000000 0.000000 1.000000
+0.000732 0.000394 -0.004541 0.018962 -0.000000 0.999820
+0.000152 0.000391 -0.004530 0.018962 -0.000000 0.999820
+0.000152 -0.000391 -0.004530 0.018962 -0.000000 0.999820
+0.000732 -0.000394 -0.004541 0.018962 0.000000 0.999820
+0.000152 0.000391 -0.004530 -0.005172 0.999987 0.000000
+0.000732 0.000394 -0.004541 -0.005172 0.999987 0.000000
+0.000732 0.000394 -0.005329 -0.005172 0.999987 0.000000
+0.000152 0.000391 -0.004530 -0.004403 0.999990 0.000558
+0.000732 0.000394 -0.005329 -0.004403 0.999990 0.000558
+0.000053 0.000391 -0.005311 -0.004403 0.999990 0.000558
+0.000732 0.000394 -0.005329 -0.026500 0.000000 -0.999649
+0.000732 -0.000394 -0.005329 -0.026500 0.000000 -0.999649
+0.000053 -0.000391 -0.005311 -0.026500 0.000000 -0.999649
+0.000053 0.000391 -0.005311 -0.026500 -0.000000 -0.999649
+0.000053 -0.000391 -0.005311 -0.004418 -0.999990 -0.000000
+0.000732 -0.000394 -0.005329 -0.004418 -0.999990 -0.000000
+0.000732 -0.000394 -0.004541 -0.004418 -0.999990 -0.000000
+0.000053 -0.000391 -0.005311 -0.005160 -0.999986 0.000654
+0.000732 -0.000394 -0.004541 -0.005160 -0.999986 0.000654
+0.000152 -0.000391 -0.004530 -0.005160 -0.999986 0.000654
+0.000732 0.000394 0.004541 0.018962 0.000000 -0.999820
+0.000732 -0.000394 0.004541 0.018962 0.000000 -0.999820
+0.000152 -0.000391 0.004530 0.018962 0.000000 -0.999820
+0.000152 0.000391 0.004530 0.018962 0.000000 -0.999820
+0.000732 0.000394 0.005329 -0.026500 0.000000 0.999649
+0.000053 0.000391 0.005311 -0.026500 0.000000 0.999649
+0.000053 -0.000391 0.005311 -0.026500 0.000000 0.999649
+0.000732 -0.000394 0.005329 -0.026500 0.000000 0.999649
+0.000152 0.000391 0.004530 -0.004403 0.999990 -0.000558
+0.000053 0.000391 0.005311 -0.004403 0.999990 -0.000558
+0.000732 0.000394 0.005329 -0.004403 0.999990 -0.000558
+0.000152 0.000391 0.004530 -0.005172 0.999987 0.000000
+0.000732 0.000394 0.005329 -0.005172 0.999987 0.000000
+0.000732 0.000394 0.004541 -0.005172 0.999987 0.000000
+0.000053 -0.000391 0.005311 -0.005160 -0.999986 -0.000654
+0.000152 -0.000391 0.004530 -0.005160 -0.999986 -0.000654
+0.000732 -0.000394 0.004541 -0.005160 -0.999986 -0.000654
+0.000053 -0.000391 0.005311 -0.004418 -0.999990 -0.000000
+0.000732 -0.000394 0.004541 -0.004418 -0.999990 -0.000000
+0.000732 -0.000394 0.005329 -0.004418 -0.999990 -0.000000
+0.002671 0.000394 -0.004518 -0.042633 0.000000 0.999091
+0.002132 0.000394 -0.004541 -0.042633 0.000000 0.999091
+0.002132 -0.000394 -0.004541 -0.042633 0.000000 0.999091
+0.002671 -0.000394 -0.004518 -0.042633 0.000000 0.999091
+0.004801 0.000394 -0.002520 0.999648 0.000000 -0.026540
+0.004801 -0.000394 -0.002520 0.999648 0.000000 -0.026540
+0.004783 -0.000391 -0.003198 0.999648 0.000000 -0.026540
+0.004783 0.000391 -0.003198 0.999648 0.000000 -0.026540
+0.002132 0.000394 -0.005329 0.046350 0.000000 -0.998925
+0.002757 0.000394 -0.005300 0.046350 0.000000 -0.998925
+0.002757 -0.000394 -0.005300 0.046350 0.000000 -0.998925
+0.002132 -0.000394 -0.005329 0.046350 0.000000 -0.998925
+0.004002 0.000391 -0.003100 0.000000 0.999987 -0.005172
+0.004013 0.000394 -0.002520 0.000000 0.999987 -0.005172
+0.004801 0.000394 -0.002520 0.000000 0.999987 -0.005172
+0.004002 0.000391 -0.003100 -0.000553 0.999990 -0.004410
+0.004801 0.000394 -0.002520 -0.000553 0.999990 -0.004410
+0.004783 0.000391 -0.003198 -0.000553 0.999990 -0.004410
+0.002671 0.000394 -0.004518 0.000000 1.000000 -0.000000
+0.002757 0.000394 -0.005300 0.000000 1.000000 -0.000000
+0.002757 -0.000394 -0.005300 0.000000 -1.000000 0.000000
+0.002671 -0.000394 -0.004518 0.000000 -1.000000 0.000000
+0.003882 0.000000 -0.000105 -0.000000 0.986951 0.161018
+0.004801 0.000394 -0.002520 -0.000000 0.986951 0.161018
+0.004013 0.000394 -0.002520 -0.000000 0.986951 0.161018
+0.003882 0.000000 -0.000105 -0.998532 0.000000 -0.054165
+0.004013 0.000394 -0.002520 -0.998532 0.000000 -0.054165
+0.004013 -0.000394 -0.002520 -0.998532 0.000000 -0.054165
+0.004013 -0.000394 -0.002520 0.000000 -0.986951 0.161018
+0.004801 -0.000394 -0.002520 0.000000 -0.986951 0.161018
+0.003882 0.000000 -0.000105 0.000000 -0.986951 0.161018
+0.004801 -0.000394 -0.002520 0.934616 0.000000 0.355657
+0.004801 0.000394 -0.002520 0.934616 0.000000 0.355657
+0.003882 0.000000 -0.000105 0.934616 0.000000 0.355657
+0.004013 0.000394 -0.002520 -0.999820 0.000000 0.018962
+0.004002 0.000391 -0.003100 -0.999820 0.000000 0.018962
+0.004002 -0.000391 -0.003100 -0.999820 0.000000 0.018962
+0.004013 -0.000394 -0.002520 -0.999820 0.000000 0.018962
+0.004783 -0.000391 -0.003198 0.000000 -0.999990 -0.004425
+0.004801 -0.000394 -0.002520 0.000000 -0.999990 -0.004425
+0.004013 -0.000394 -0.002520 0.000000 -0.999990 -0.004425
+0.004783 -0.000391 -0.003198 -0.000647 -0.999986 -0.005160
+0.004013 -0.000394 -0.002520 -0.000647 -0.999986 -0.005160
+0.004002 -0.000391 -0.003100 -0.000647 -0.999986 -0.005160
+3 0 1 2
+3 3 4 5
+3 3 5 6
+3 7 8 9
+3 7 9 10
+3 11 12 13
+3 11 13 14
+3 15 16 17
+3 15 17 18
+3 19 20 21
+3 19 21 22
+3 23 24 25
+3 23 25 26
+3 27 28 29
+3 27 29 30
+3 31 32 33
+3 31 33 34
+3 35 36 37
+3 35 37 38
+3 39 40 41
+3 39 41 42
+3 43 44 45
+3 46 47 48
+3 49 50 51
+3 52 53 54
+3 55 56 57
+3 58 59 60
+3 61 62 63
+3 64 65 66
+3 67 68 69
+3 70 71 72
+3 73 74 75
+3 76 77 78
+3 79 80 81
+3 79 81 82
+3 83 84 85
+3 86 87 88
+3 89 90 91
+3 92 93 94
+3 95 96 97
+3 95 97 98
+3 99 100 101
+3 99 101 102
+3 103 104 105
+3 103 105 106
+3 107 108 109
+3 107 109 110
+3 111 112 113
+3 111 113 114
+3 115 116 117
+3 115 117 118
+3 119 120 121
+3 119 121 122
+3 123 124 125
+3 123 125 126
+3 127 128 129
+3 127 129 130
+3 131 132 133
+3 134 135 136
+3 137 138 139
+3 140 141 142
+3 143 144 145
+3 146 147 148
+3 149 150 151
+3 152 153 154
+3 155 156 157
+3 155 157 158
+3 159 160 161
+3 159 161 162
+3 163 164 165
+3 163 165 166
+3 167 168 169
+3 167 169 170
+3 171 172 173
+3 171 173 174
+3 175 176 177
+3 175 177 178
+3 179 180 181
+3 179 181 182
+3 183 184 185
+3 183 185 186
+3 187 188 189
+3 187 189 190
+3 191 192 193
+3 191 193 194
+3 195 196 197
+3 195 197 198
+3 199 200 201
+3 199 201 202
+3 203 204 205
+3 203 205 206
+3 207 208 209
+3 207 209 210
+3 211 212 213
+3 211 213 214
+3 215 216 217
+3 215 217 218
+3 219 220 221
+3 222 223 224
+3 225 226 227
+3 228 229 230
+3 231 232 233
+3 234 235 236
+3 237 193 192
+3 237 192 238
+3 239 189 188
+3 239 188 240
+3 241 242 243
+3 244 245 246
+3 247 248 249
+3 250 251 252
+3 253 254 255
+3 256 257 258
+3 259 260 261
+3 259 261 262
+3 263 264 265
+3 263 265 266
+3 267 268 191
+3 267 191 194
+3 269 270 187
+3 269 187 190
+3 271 272 273
+3 271 273 274
+3 275 276 277
+3 275 277 278
+3 279 280 281
+3 279 281 282
+3 283 284 285
+3 283 285 286
+3 287 288 289
+3 287 289 290
+3 291 292 293
+3 291 293 294
+3 295 296 297
+3 295 297 298
+3 299 300 301
+3 299 301 302
+3 303 304 305
+3 303 305 306
+3 307 308 309
+3 307 309 310
+3 311 312 313
+3 314 315 316
+3 317 318 319
+3 320 321 322
+3 323 324 325
+3 326 327 328
+3 329 330 331
+3 332 333 334
+3 335 336 337
+3 338 339 340
+3 341 342 343
+3 344 345 346
+3 347 348 349
+3 347 349 350
+3 351 352 353
+3 351 353 354
+3 355 356 357
+3 355 357 358
+3 359 360 361
+3 359 361 362
+3 363 364 365
+3 363 365 366
+3 367 368 369
+3 370 371 372
+3 373 374 375
+3 376 377 378
+3 376 378 379
+3 380 381 382
+3 380 382 383
+3 384 385 386
+3 384 386 387
+3 388 389 390
+3 391 392 393
+3 394 395 396
+3 394 396 397
+3 398 399 400
+3 398 400 401
+3 402 403 404
+3 405 406 407
+3 408 409 396
+3 408 396 395
+3 410 411 412
+3 410 412 413
+3 400 399 414
+3 400 414 415
+3 416 417 418
+3 416 418 419
+3 420 421 422
+3 420 422 423
+3 424 425 426
+3 427 428 429
+3 430 431 432
+3 430 432 433
+3 434 435 436
+3 437 438 439
+3 440 441 442
+3 440 442 443
+3 444 445 446
+3 444 446 447
+3 448 449 450
+3 451 452 453
+3 454 455 456
+3 457 458 459
+3 460 461 462
+3 460 462 463
+3 464 465 466
+3 464 466 467
+3 468 469 470
+3 468 470 471
+3 472 473 474
+3 475 476 477
+3 478 479 353
+3 478 353 352
+3 480 481 359
+3 480 359 362
+3 482 483 484
+3 485 486 487
+3 488 489 490
+3 491 492 493
+3 494 495 496
+3 494 496 497
+3 498 499 500
+3 501 502 503
diff --git a/Examples/Stapling/Data/Geometry/stapler_collision.ply b/Examples/Stapling/Data/Geometry/stapler_collision.ply
new file mode 100644
index 0000000..09ecfbc
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/stapler_collision.ply
@@ -0,0 +1,679 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 490
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+element face 176
+property list uchar uint vertex_indices
+end_header
+-0.047435 0.068225 -0.013055 -0.006463 0.004409 -0.999969
+-0.084014 0.069053 -0.012815 -0.006463 0.004409 -0.999969
+-0.051680 0.081963 -0.012967 -0.006463 0.004409 -0.999969
+-0.090172 0.076305 -0.011796 -0.633821 0.000961 -0.773479
+-0.093381 0.075029 -0.009168 -0.633821 0.000961 -0.773479
+-0.093242 0.208561 -0.009116 -0.633821 0.000961 -0.773479
+-0.037628 0.055414 -0.005132 0.819461 0.573135 -0.000001
+-0.000021 0.001644 -0.005132 0.819461 0.573135 -0.000001
+-0.000021 0.001644 -0.012973 0.819461 0.573135 -0.000001
+-0.096996 0.074745 -0.002479 -0.985645 -0.168814 -0.002360
+-0.096686 0.072935 -0.002479 -0.985645 -0.168814 -0.002360
+-0.097007 0.074745 0.002114 -0.985645 -0.168814 -0.002360
+-0.095624 0.072915 0.005665 -0.944198 -0.166653 0.284106
+-0.095947 0.074745 0.005665 -0.944198 -0.166653 0.284106
+-0.096695 0.072931 0.002115 -0.944198 -0.166653 0.284106
+-0.000021 -0.001946 -0.012973 -0.000001 0.000000 -1.000000
+-0.000906 -0.002426 -0.012973 -0.000001 0.000000 -1.000000
+-0.001383 -0.001003 -0.012973 -0.000001 0.000000 -1.000000
+-0.053247 0.095488 -0.000274 0.999942 0.008032 0.007209
+-0.053158 0.095489 -0.012618 0.999942 0.008032 0.007209
+-0.053770 0.171623 -0.012553 0.999942 0.008032 0.007209
+-0.096317 0.208811 0.002055 0.000680 1.000000 -0.000203
+-0.095269 0.208811 0.005558 0.000680 1.000000 -0.000203
+-0.092877 0.208810 0.008646 0.000680 1.000000 -0.000203
+-0.000021 0.001644 0.004864 1.000000 0.000000 -0.000001
+-0.000021 0.001644 0.012973 1.000000 0.000000 -0.000001
+-0.000021 -0.001946 0.012973 1.000000 0.000000 -0.000001
+-0.041745 0.055505 0.006777 0.019748 0.999804 0.001563
+-0.041857 0.055498 0.012671 0.019748 0.999804 0.001563
+-0.037628 0.055414 0.012973 0.019748 0.999804 0.001563
+-0.040193 0.003943 0.002075 -0.118445 -0.992961 -0.000095
+-0.040181 0.003942 -0.002438 -0.118445 -0.992961 -0.000095
+-0.031077 0.002857 -0.012547 -0.118445 -0.992961 -0.000095
+-0.089963 0.208561 -0.011666 -0.392806 0.001038 -0.919621
+-0.087273 0.208561 -0.012815 -0.392806 0.001038 -0.919621
+-0.087618 0.078007 -0.012815 -0.392806 0.001038 -0.919621
+-0.000021 -0.001946 0.012973 0.000001 -0.000000 1.000000
+-0.001383 -0.001003 0.012973 0.000001 -0.000000 1.000000
+-0.000906 -0.002426 0.012973 0.000001 -0.000000 1.000000
+-0.000021 -0.001946 -0.005132 0.476763 -0.879032 -0.000001
+-0.000906 -0.002426 -0.005132 0.476763 -0.879032 -0.000001
+-0.000906 -0.002426 -0.012973 0.476763 -0.879032 -0.000001
+-0.002554 0.000370 -0.005132 0.843460 0.537193 -0.000001
+-0.002554 0.000370 0.004864 0.843460 0.537193 -0.000001
+-0.001677 -0.001007 0.004864 0.843460 0.537193 -0.000001
+-0.047435 0.068225 -0.013055 -0.006462 0.004444 -0.999969
+-0.042150 0.055658 -0.013145 -0.006462 0.004444 -0.999969
+-0.084014 0.069053 -0.012815 -0.006462 0.004444 -0.999969
+-0.041773 0.200249 0.012553 0.920621 -0.390458 -0.000001
+-0.053914 0.171623 -0.000315 0.920621 -0.390458 -0.000001
+-0.041773 0.200249 -0.012553 0.920621 -0.390458 -0.000001
+-0.000021 0.001644 0.004864 0.819461 0.573135 -0.000001
+-0.037628 0.055414 0.004864 0.819461 0.573135 -0.000001
+-0.037628 0.055414 0.012973 0.819461 0.573135 -0.000001
+-0.000021 -0.001946 0.004864 -0.000001 -0.000000 -1.000000
+-0.000906 -0.002426 0.004864 -0.000001 -0.000000 -1.000000
+-0.001677 -0.001007 0.004864 -0.000001 -0.000000 -1.000000
+-0.000021 -0.001946 -0.005132 0.000001 0.000000 1.000000
+-0.001677 -0.001007 -0.005132 0.000001 0.000000 1.000000
+-0.000906 -0.002426 -0.005132 0.000001 0.000000 1.000000
+-0.000021 -0.001946 0.012973 0.476763 -0.879032 -0.000001
+-0.000906 -0.002426 0.012973 0.476763 -0.879032 -0.000001
+-0.000906 -0.002426 0.004864 0.476763 -0.879032 -0.000001
+-0.041745 0.055505 0.006777 0.022098 0.999756 -0.000000
+-0.037628 0.055414 0.004864 0.022098 0.999756 -0.000000
+-0.041745 0.055505 -0.007044 0.022098 0.999756 -0.000000
+-0.031072 0.002857 0.012547 -0.115579 -0.993289 -0.004287
+-0.033488 0.003143 0.011418 -0.115579 -0.993289 -0.004287
+-0.036794 0.003539 0.008797 -0.115579 -0.993289 -0.004287
+-0.040193 0.003943 0.002075 -0.119150 -0.992876 0.000576
+-0.036794 0.003539 0.008797 -0.119150 -0.992876 0.000576
+-0.039168 0.003822 0.005534 -0.119150 -0.992876 0.000576
+-0.092877 0.208810 0.008646 -0.000436 0.999998 0.001671
+-0.089739 0.208807 0.011257 -0.000436 0.999998 0.001671
+-0.087126 0.208806 0.012545 -0.000436 0.999998 0.001671
+-0.039181 0.003824 -0.005801 -0.119055 -0.992887 -0.000563
+-0.036639 0.003521 -0.008986 -0.119055 -0.992887 -0.000563
+-0.040181 0.003942 -0.002438 -0.119055 -0.992887 -0.000563
+-0.093381 0.075029 -0.009168 -0.782036 0.001951 -0.623230
+-0.095959 0.074745 -0.005934 -0.782036 0.001951 -0.623230
+-0.095606 0.208561 -0.005958 -0.782036 0.001951 -0.623230
+-0.090172 0.076305 -0.011796 -0.617603 -0.061639 -0.784071
+-0.089830 0.072891 -0.011797 -0.617603 -0.061639 -0.784071
+-0.093381 0.075029 -0.009168 -0.617603 -0.061639 -0.784071
+-0.093225 0.072915 0.008919 -0.802974 -0.069129 0.591992
+-0.093407 0.075029 0.008919 -0.802974 -0.069129 0.591992
+-0.095624 0.072915 0.005665 -0.802974 -0.069129 0.591992
+-0.092240 0.071213 0.008919 -0.728036 -0.421338 0.540775
+-0.093225 0.072915 0.008919 -0.728036 -0.421338 0.540775
+-0.094701 0.071289 0.005665 -0.728036 -0.421338 0.540775
+-0.094701 0.071289 0.005665 -0.841292 -0.477560 0.253306
+-0.095624 0.072915 0.005665 -0.841292 -0.477560 0.253306
+-0.095792 0.071328 0.002115 -0.841292 -0.477560 0.253306
+-0.096686 0.072935 -0.002479 -0.871226 -0.490877 -0.002170
+-0.095780 0.071327 -0.002479 -0.871226 -0.490877 -0.002170
+-0.095792 0.071328 0.002115 -0.871226 -0.490877 -0.002170
+-0.089830 0.072891 -0.011797 -0.577254 -0.341760 -0.741605
+-0.088766 0.071096 -0.011798 -0.577254 -0.341760 -0.741605
+-0.092211 0.071210 -0.009169 -0.577254 -0.341760 -0.741605
+-0.087259 0.072864 0.012815 -0.415129 -0.028977 0.909301
+-0.087618 0.078007 0.012815 -0.415129 -0.028977 0.909301
+-0.090235 0.076305 0.011566 -0.415129 -0.028977 0.909301
+-0.084028 0.069070 0.012815 -0.358101 -0.304962 0.882475
+-0.087259 0.072864 0.012815 -0.358101 -0.304962 0.882475
+-0.088845 0.071115 0.011567 -0.358101 -0.304962 0.882475
+-0.088766 0.071096 -0.011798 -0.361377 -0.213704 -0.907600
+-0.089830 0.072891 -0.011797 -0.361377 -0.213704 -0.907600
+-0.087252 0.072855 -0.012815 -0.361377 -0.213704 -0.907600
+-0.089830 0.072891 -0.011797 -0.367478 -0.036540 -0.929314
+-0.090172 0.076305 -0.011796 -0.367478 -0.036540 -0.929314
+-0.087252 0.072855 -0.012815 -0.367478 -0.036540 -0.929314
+-0.095647 0.072931 -0.005934 -0.769381 -0.132330 -0.624934
+-0.095959 0.074745 -0.005934 -0.769381 -0.132330 -0.624934
+-0.093381 0.075029 -0.009168 -0.769381 -0.132330 -0.624934
+-0.094712 0.071288 -0.005934 -0.726665 -0.413532 -0.548588
+-0.095647 0.072931 -0.005934 -0.726665 -0.413532 -0.548588
+-0.093182 0.072891 -0.009169 -0.726665 -0.413532 -0.548588
+-0.095959 0.074745 -0.005934 -0.944955 -0.162528 -0.283979
+-0.095647 0.072931 -0.005934 -0.944955 -0.162528 -0.283979
+-0.096686 0.072935 -0.002479 -0.944955 -0.162528 -0.283979
+-0.095647 0.072931 -0.005934 -0.840521 -0.478325 -0.254419
+-0.094712 0.071288 -0.005934 -0.840521 -0.478325 -0.254419
+-0.095780 0.071327 -0.002479 -0.840521 -0.478325 -0.254419
+-0.093225 0.072915 0.008919 -0.586744 -0.339568 0.735136
+-0.092240 0.071213 0.008919 -0.586744 -0.339568 0.735136
+-0.089894 0.072892 0.011567 -0.586744 -0.339568 0.735136
+-0.093407 0.075029 0.008919 -0.621618 -0.053516 0.781490
+-0.093225 0.072915 0.008919 -0.621618 -0.053516 0.781490
+-0.089894 0.072892 0.011567 -0.621618 -0.053516 0.781490
+-0.000021 0.001644 -0.005132 1.000000 0.000000 -0.000001
+-0.000021 -0.001946 -0.005132 1.000000 0.000000 -0.000001
+-0.000021 -0.001946 -0.012973 1.000000 0.000000 -0.000001
+-0.000021 0.001644 0.004864 -0.000001 0.000000 -1.000000
+-0.002554 0.000370 -0.005132 0.000001 0.000000 1.000000
+-0.000021 0.001644 -0.005132 0.000001 0.000000 1.000000
+-0.000021 0.001644 0.012973 0.000001 0.000000 1.000000
+-0.047135 0.068208 -0.000183 0.954222 0.298299 -0.021847
+-0.047435 0.068225 -0.013055 0.954222 0.298299 -0.021847
+-0.051432 0.081950 -0.000232 0.954222 0.298299 -0.021847
+-0.088845 0.071115 0.011567 -0.353471 -0.289821 0.889417
+-0.033488 0.003143 0.011418 -0.353471 -0.289821 0.889417
+-0.084028 0.069070 0.012815 -0.353471 -0.289821 0.889417
+-0.087259 0.072864 0.012815 0.003594 0.000251 0.999994
+-0.051441 0.082047 0.012684 0.003594 0.000251 0.999994
+-0.087618 0.078007 0.012815 0.003594 0.000251 0.999994
+-0.087273 0.208561 -0.012815 0.007796 -0.000021 -0.999970
+-0.053770 0.171623 -0.012553 0.007796 -0.000021 -0.999970
+-0.087618 0.078007 -0.012815 0.007796 -0.000021 -0.999970
+-0.087126 0.208806 0.012545 0.002054 0.002056 0.999996
+-0.087618 0.078007 0.012815 0.002054 0.002056 0.999996
+-0.053770 0.171623 0.012553 0.002054 0.002056 0.999996
+-0.089739 0.208807 0.011257 -0.432689 0.003722 0.901535
+-0.090235 0.076305 0.011566 -0.432689 0.003722 0.901535
+-0.087618 0.078007 0.012815 -0.432689 0.003722 0.901535
+-0.087273 0.208561 -0.012815 0.005347 -0.002242 -0.999983
+-0.041773 0.200249 -0.012553 0.005347 -0.002242 -0.999983
+-0.053770 0.171623 -0.012553 0.005347 -0.002242 -0.999983
+-0.097007 0.074745 0.002114 -0.958206 0.005057 0.286034
+-0.095947 0.074745 0.005665 -0.958206 0.005057 0.286034
+-0.096317 0.208811 0.002055 -0.958206 0.005057 0.286034
+-0.047135 0.068208 -0.000183 0.954420 0.298450 0.003133
+-0.051432 0.081950 -0.000232 0.954420 0.298450 0.003133
+-0.047180 0.068216 0.012760 0.954420 0.298450 0.003133
+-0.088766 0.071096 -0.011798 -0.302794 -0.245934 -0.920778
+-0.084014 0.069053 -0.012815 -0.302794 -0.245934 -0.920778
+-0.033735 0.003342 -0.011798 -0.302794 -0.245934 -0.920778
+-0.040193 0.003943 0.002075 -0.755943 -0.622619 0.202228
+-0.039168 0.003822 0.005534 -0.755943 -0.622619 0.202228
+-0.094701 0.071289 0.005665 -0.755943 -0.622619 0.202228
+-0.031077 0.002857 -0.012547 -0.130308 -0.991474 0.000000
+-0.001677 -0.001007 -0.005132 -0.130308 -0.991474 0.000000
+-0.001677 -0.001007 0.004864 -0.130308 -0.991474 0.000000
+-0.095959 0.074745 -0.005934 -0.957786 0.002475 -0.287473
+-0.096996 0.074745 -0.002479 -0.957786 0.002475 -0.287473
+-0.095606 0.208561 -0.005958 -0.957786 0.002475 -0.287473
+-0.001677 -0.001007 0.004864 -0.878223 -0.477174 0.032077
+-0.000906 -0.002426 0.004864 -0.878223 -0.477174 0.032077
+-0.001383 -0.001003 0.012973 -0.878223 -0.477174 0.032077
+-0.001383 -0.001003 -0.012973 -0.948149 -0.317826 0.000001
+-0.000906 -0.002426 -0.012973 -0.948149 -0.317826 0.000001
+-0.000906 -0.002426 -0.005132 -0.948149 -0.317826 0.000001
+-0.095780 0.071327 -0.002479 -0.751442 -0.620148 -0.225282
+-0.094712 0.071288 -0.005934 -0.751442 -0.620148 -0.225282
+-0.040181 0.003942 -0.002438 -0.751442 -0.620148 -0.225282
+-0.096996 0.074745 -0.002479 -0.999984 0.005146 -0.002394
+-0.097007 0.074745 0.002114 -0.999984 0.005146 -0.002394
+-0.096317 0.208811 0.002055 -0.999984 0.005146 -0.002394
+-0.036794 0.003539 0.008797 -0.581021 -0.474638 0.661161
+-0.033488 0.003143 0.011418 -0.581021 -0.474638 0.661161
+-0.088845 0.071115 0.011567 -0.581021 -0.474638 0.661161
+-0.036639 0.003521 -0.008986 -0.222806 -0.960106 -0.168978
+-0.033735 0.003342 -0.011798 -0.222806 -0.960106 -0.168978
+-0.031077 0.002857 -0.012547 -0.222806 -0.960106 -0.168978
+-0.039168 0.003822 0.005534 -0.689399 -0.565648 0.452516
+-0.036794 0.003539 0.008797 -0.689399 -0.565648 0.452516
+-0.092240 0.071213 0.008919 -0.689399 -0.565648 0.452516
+-0.087273 0.208561 -0.012815 0.027754 0.999615 0.000074
+-0.096305 0.208811 -0.002418 0.027754 0.999615 0.000074
+-0.096317 0.208811 0.002055 0.027754 0.999615 0.000074
+-0.093242 0.208561 -0.009116 -0.126530 0.987430 -0.094717
+-0.095606 0.208561 -0.005958 -0.126530 0.987430 -0.094717
+-0.096305 0.208811 -0.002418 -0.126530 0.987430 -0.094717
+-0.095792 0.071328 0.002115 -0.771337 -0.636425 -0.001877
+-0.095780 0.071327 -0.002479 -0.771337 -0.636425 -0.001877
+-0.040193 0.003943 0.002075 -0.771337 -0.636425 -0.001877
+-0.087259 0.072864 0.012815 0.003003 0.002556 0.999992
+-0.084028 0.069070 0.012815 0.003003 0.002556 0.999992
+-0.051441 0.082047 0.012684 0.003003 0.002556 0.999992
+-0.041773 0.200249 0.012553 0.922231 -0.386502 -0.010322
+-0.053770 0.171623 0.012553 0.922231 -0.386502 -0.010322
+-0.053914 0.171623 -0.000315 0.922231 -0.386502 -0.010322
+-0.053247 0.095488 -0.000274 0.991104 0.132897 0.007155
+-0.051432 0.081950 -0.000232 0.991104 0.132897 0.007155
+-0.053158 0.095489 -0.012618 0.991104 0.132897 0.007155
+-0.087252 0.072855 -0.012815 -0.004198 -0.000298 -0.999991
+-0.087618 0.078007 -0.012815 -0.004198 -0.000298 -0.999991
+-0.051680 0.081963 -0.012967 -0.004198 -0.000298 -0.999991
+-0.087273 0.208561 -0.012815 0.179756 0.983655 -0.010545
+-0.087126 0.208806 0.012545 0.179756 0.983655 -0.010545
+-0.041773 0.200249 -0.012553 0.179756 0.983655 -0.010545
+-0.041745 0.055505 -0.007044 0.022098 0.999756 -0.000000
+-0.037628 0.055414 -0.005132 0.022098 0.999756 -0.000000
+-0.037628 0.055414 -0.012973 0.022098 0.999756 -0.000000
+-0.047135 0.068208 -0.000183 0.920560 0.390602 -0.000001
+-0.041745 0.055505 0.006777 0.920560 0.390602 -0.000001
+-0.041745 0.055505 -0.007044 0.920560 0.390602 -0.000001
+-0.087126 0.208806 0.012545 -0.000162 0.000069 1.000000
+-0.053770 0.171623 0.012553 -0.000162 0.000069 1.000000
+-0.041773 0.200249 0.012553 -0.000162 0.000069 1.000000
+-0.051432 0.081950 -0.000232 0.991112 0.132854 -0.006854
+-0.053247 0.095488 -0.000274 0.991112 0.132854 -0.006854
+-0.053158 0.095489 0.012618 0.991112 0.132854 -0.006854
+-0.033735 0.003342 -0.011798 -0.617557 -0.501590 -0.605831
+-0.036639 0.003521 -0.008986 -0.617557 -0.501590 -0.605831
+-0.088766 0.071096 -0.011798 -0.617557 -0.501590 -0.605831
+-0.042150 0.055658 -0.013145 0.039357 0.025286 -0.998905
+-0.037628 0.055414 -0.012973 0.039357 0.025286 -0.998905
+-0.001383 -0.001003 -0.012973 0.039357 0.025286 -0.998905
+-0.000021 0.001644 -0.012973 -0.000001 0.000000 -1.000000
+-0.002554 0.000370 -0.005132 0.843342 0.537377 -0.000001
+-0.037628 0.055414 -0.005132 0.843342 0.537377 -0.000001
+-0.037628 0.055414 0.004864 0.843342 0.537377 -0.000001
+-0.002554 0.000370 0.004864 -0.000001 0.000000 -1.000000
+-0.037628 0.055414 0.004864 -0.000001 0.000000 -1.000000
+-0.037628 0.055414 -0.005132 0.000001 -0.000000 1.000000
+-0.047135 0.068208 -0.000183 0.919972 0.391972 0.002955
+-0.047180 0.068216 0.012760 0.919972 0.391972 0.002955
+-0.041745 0.055505 0.006777 0.919972 0.391972 0.002955
+-0.053914 0.171623 -0.000315 0.999899 0.008754 -0.011191
+-0.053770 0.171623 0.012553 0.999899 0.008754 -0.011191
+-0.053247 0.095488 -0.000274 0.999899 0.008754 -0.011191
+-0.095947 0.074745 0.005665 -0.788462 0.004478 0.615067
+-0.093407 0.075029 0.008919 -0.788462 0.004478 0.615067
+-0.095269 0.208811 0.005558 -0.788462 0.004478 0.615067
+-0.053914 0.171623 -0.000315 0.922226 -0.386500 0.010850
+-0.053770 0.171623 -0.012553 0.922226 -0.386500 0.010850
+-0.041773 0.200249 -0.012553 0.922226 -0.386500 0.010850
+-0.084028 0.069070 0.012815 0.001345 -0.006436 0.999978
+-0.041857 0.055498 0.012671 0.001345 -0.006436 0.999978
+-0.047180 0.068216 0.012760 0.001345 -0.006436 0.999978
+-0.087273 0.208561 -0.012815 0.000000 1.000000 0.000000
+-0.089963 0.208561 -0.011666 0.000000 1.000000 0.000000
+-0.093242 0.208561 -0.009116 0.000000 1.000000 0.000000
+-0.031072 0.002857 0.012547 -0.129001 -0.991631 0.005166
+-0.001677 -0.001007 0.004864 -0.129001 -0.991631 0.005166
+-0.001383 -0.001003 0.012973 -0.129001 -0.991631 0.005166
+-0.031077 0.002857 -0.012547 -0.128982 -0.991633 -0.005342
+-0.001383 -0.001003 -0.012973 -0.128982 -0.991633 -0.005342
+-0.001677 -0.001007 -0.005132 -0.128982 -0.991633 -0.005342
+-0.093407 0.075029 0.008919 -0.641676 0.004107 0.766965
+-0.090235 0.076305 0.011566 -0.641676 0.004107 0.766965
+-0.092877 0.208810 0.008646 -0.641676 0.004107 0.766965
+-0.036639 0.003521 -0.008986 -0.674910 -0.555408 -0.485818
+-0.039181 0.003824 -0.005801 -0.674910 -0.555408 -0.485818
+-0.092211 0.071210 -0.009169 -0.674910 -0.555408 -0.485818
+-0.084028 0.069070 0.012815 0.002845 -0.001773 0.999994
+-0.031072 0.002857 0.012547 0.002845 -0.001773 0.999994
+-0.041857 0.055498 0.012671 0.002845 -0.001773 0.999994
+-0.001383 -0.001003 -0.012973 -0.016259 -0.014732 -0.999759
+-0.031077 0.002857 -0.012547 -0.016259 -0.014732 -0.999759
+-0.042150 0.055658 -0.013145 -0.016259 -0.014732 -0.999759
+-0.041745 0.055505 -0.007044 0.916266 0.400029 -0.020828
+-0.047435 0.068225 -0.013055 0.916266 0.400029 -0.020828
+-0.047135 0.068208 -0.000183 0.916266 0.400029 -0.020828
+-0.037628 0.055414 0.012973 0.000001 0.000000 1.000000
+-0.084014 0.069053 -0.012815 -0.003509 -0.002988 -0.999989
+-0.087252 0.072855 -0.012815 -0.003509 -0.002988 -0.999989
+-0.051680 0.081963 -0.012967 -0.003509 -0.002988 -0.999989
+-0.089963 0.208561 -0.011666 -0.613890 0.001746 -0.789389
+-0.090172 0.076305 -0.011796 -0.613890 0.001746 -0.789389
+-0.093242 0.208561 -0.009116 -0.613890 0.001746 -0.789389
+-0.037628 0.055414 -0.012973 0.819461 0.573135 -0.000001
+-0.096686 0.072935 -0.002479 -0.985527 -0.169508 -0.002078
+-0.096695 0.072931 0.002115 -0.985527 -0.169508 -0.002078
+-0.097007 0.074745 0.002114 -0.985527 -0.169508 -0.002078
+-0.095947 0.074745 0.005665 -0.945488 -0.162464 0.282238
+-0.097007 0.074745 0.002114 -0.945488 -0.162464 0.282238
+-0.096695 0.072931 0.002115 -0.945488 -0.162464 0.282238
+-0.053914 0.171623 -0.000315 0.999892 0.008766 0.011764
+-0.053247 0.095488 -0.000274 0.999892 0.008766 0.011764
+-0.053770 0.171623 -0.012553 0.999892 0.008766 0.011764
+-0.000021 -0.001946 0.004864 1.000000 0.000000 -0.000001
+-0.037628 0.055414 0.012973 0.022098 0.999756 -0.000000
+-0.031072 0.002857 0.012547 -0.118257 -0.992983 0.000024
+-0.040193 0.003943 0.002075 -0.118257 -0.992983 0.000024
+-0.031077 0.002857 -0.012547 -0.118257 -0.992983 0.000024
+-0.090172 0.076305 -0.011796 -0.371438 0.001500 -0.928457
+-0.089963 0.208561 -0.011666 -0.371438 0.001500 -0.928457
+-0.087618 0.078007 -0.012815 -0.371438 0.001500 -0.928457
+-0.000021 -0.001946 -0.012973 0.476763 -0.879032 -0.000001
+-0.001677 -0.001007 -0.005132 0.843460 0.537193 -0.000001
+-0.000021 0.001644 0.012973 0.819461 0.573135 -0.000001
+-0.000021 -0.001946 0.004864 0.476763 -0.879032 -0.000001
+-0.037628 0.055414 0.004864 0.022098 0.999756 -0.000000
+-0.040193 0.003943 0.002075 -0.118512 -0.992953 0.000248
+-0.031072 0.002857 0.012547 -0.118512 -0.992953 0.000248
+-0.036794 0.003539 0.008797 -0.118512 -0.992953 0.000248
+-0.096317 0.208811 0.002055 0.000920 1.000000 -0.000329
+-0.092877 0.208810 0.008646 0.000920 1.000000 -0.000329
+-0.087126 0.208806 0.012545 0.000920 1.000000 -0.000329
+-0.093242 0.208561 -0.009116 -0.800548 0.001067 -0.599268
+-0.093381 0.075029 -0.009168 -0.800548 0.001067 -0.599268
+-0.095606 0.208561 -0.005958 -0.800548 0.001067 -0.599268
+-0.089830 0.072891 -0.011797 -0.615991 -0.056968 -0.785691
+-0.093182 0.072891 -0.009169 -0.615991 -0.056968 -0.785691
+-0.093381 0.075029 -0.009168 -0.615991 -0.056968 -0.785691
+-0.093407 0.075029 0.008919 -0.775043 -0.136797 0.616923
+-0.095947 0.074745 0.005665 -0.775043 -0.136797 0.616923
+-0.095624 0.072915 0.005665 -0.775043 -0.136797 0.616923
+-0.093225 0.072915 0.008919 -0.732102 -0.415579 0.539741
+-0.095624 0.072915 0.005665 -0.732102 -0.415579 0.539741
+-0.094701 0.071289 0.005665 -0.732102 -0.415579 0.539741
+-0.095624 0.072915 0.005665 -0.843104 -0.474934 0.252218
+-0.096695 0.072931 0.002115 -0.843104 -0.474934 0.252218
+-0.095792 0.071328 0.002115 -0.843104 -0.474934 0.252218
+-0.096695 0.072931 0.002115 -0.871270 -0.490800 -0.002134
+-0.096686 0.072935 -0.002479 -0.871270 -0.490800 -0.002134
+-0.095792 0.071328 0.002115 -0.871270 -0.490800 -0.002134
+-0.093182 0.072891 -0.009169 -0.581186 -0.335710 -0.741297
+-0.089830 0.072891 -0.011797 -0.581186 -0.335710 -0.741297
+-0.092211 0.071210 -0.009169 -0.581186 -0.335710 -0.741297
+-0.089894 0.072892 0.011567 -0.428024 -0.042501 0.902768
+-0.087259 0.072864 0.012815 -0.428024 -0.042501 0.902768
+-0.090235 0.076305 0.011566 -0.428024 -0.042501 0.902768
+-0.087259 0.072864 0.012815 -0.417005 -0.246167 0.874934
+-0.089894 0.072892 0.011567 -0.417005 -0.246167 0.874934
+-0.088845 0.071115 0.011567 -0.417005 -0.246167 0.874934
+-0.084014 0.069053 -0.012815 -0.308650 -0.262863 -0.914132
+-0.088766 0.071096 -0.011798 -0.308650 -0.262863 -0.914132
+-0.087252 0.072855 -0.012815 -0.308650 -0.262863 -0.914132
+-0.090172 0.076305 -0.011796 -0.355878 -0.025282 -0.934190
+-0.087618 0.078007 -0.012815 -0.355878 -0.025282 -0.934190
+-0.087252 0.072855 -0.012815 -0.355878 -0.025282 -0.934190
+-0.093182 0.072891 -0.009169 -0.793686 -0.073593 -0.603860
+-0.095647 0.072931 -0.005934 -0.793686 -0.073593 -0.603860
+-0.093381 0.075029 -0.009168 -0.793686 -0.073593 -0.603860
+-0.092211 0.071210 -0.009169 -0.723574 -0.417958 -0.549319
+-0.094712 0.071288 -0.005934 -0.723574 -0.417958 -0.549319
+-0.093182 0.072891 -0.009169 -0.723574 -0.417958 -0.549319
+-0.096996 0.074745 -0.002479 -0.945156 -0.161879 -0.283682
+-0.095959 0.074745 -0.005934 -0.945156 -0.161879 -0.283682
+-0.096686 0.072935 -0.002479 -0.945156 -0.161879 -0.283682
+-0.096686 0.072935 -0.002479 -0.842901 -0.474917 -0.252928
+-0.095647 0.072931 -0.005934 -0.842901 -0.474917 -0.252928
+-0.095780 0.071327 -0.002479 -0.842901 -0.474917 -0.252928
+-0.092240 0.071213 0.008919 -0.583538 -0.344474 0.735406
+-0.088845 0.071115 0.011567 -0.583538 -0.344474 0.735406
+-0.089894 0.072892 0.011567 -0.583538 -0.344474 0.735406
+-0.090235 0.076305 0.011566 -0.624605 -0.062178 0.778462
+-0.093407 0.075029 0.008919 -0.624605 -0.062178 0.778462
+-0.089894 0.072892 0.011567 -0.624605 -0.062178 0.778462
+-0.000021 0.001644 -0.012973 1.000000 0.000000 -0.000001
+-0.051432 0.081950 -0.000232 0.955235 0.295282 -0.018302
+-0.047435 0.068225 -0.013055 0.955235 0.295282 -0.018302
+-0.051680 0.081963 -0.012967 0.955235 0.295282 -0.018302
+-0.033488 0.003143 0.011418 -0.430415 -0.347611 0.833012
+-0.031072 0.002857 0.012547 -0.430415 -0.347611 0.833012
+-0.084028 0.069070 0.012815 -0.430415 -0.347611 0.833012
+-0.051441 0.082047 0.012684 0.003031 0.005297 0.999981
+-0.053158 0.095489 0.012618 0.003031 0.005297 0.999981
+-0.087618 0.078007 0.012815 0.003031 0.005297 0.999981
+-0.053770 0.171623 -0.012553 0.005261 0.000896 -0.999986
+-0.053158 0.095489 -0.012618 0.005261 0.000896 -0.999986
+-0.087618 0.078007 -0.012815 0.005261 0.000896 -0.999986
+-0.087618 0.078007 0.012815 0.005263 0.000896 0.999986
+-0.053158 0.095489 0.012618 0.005263 0.000896 0.999986
+-0.053770 0.171623 0.012553 0.005263 0.000896 0.999986
+-0.087126 0.208806 0.012545 -0.442121 0.003515 0.896948
+-0.089739 0.208807 0.011257 -0.442121 0.003515 0.896948
+-0.087618 0.078007 0.012815 -0.442121 0.003515 0.896948
+-0.095947 0.074745 0.005665 -0.958031 0.005074 0.286618
+-0.095269 0.208811 0.005558 -0.958031 0.005074 0.286618
+-0.096317 0.208811 0.002055 -0.958031 0.005074 0.286618
+-0.051432 0.081950 -0.000232 0.955677 0.294413 -0.001546
+-0.051441 0.082047 0.012684 0.955677 0.294413 -0.001546
+-0.047180 0.068216 0.012760 0.955677 0.294413 -0.001546
+-0.084014 0.069053 -0.012815 -0.304325 -0.247093 -0.919963
+-0.031077 0.002857 -0.012547 -0.304325 -0.247093 -0.919963
+-0.033735 0.003342 -0.011798 -0.304325 -0.247093 -0.919963
+-0.095792 0.071328 0.002115 -0.751639 -0.620306 0.224186
+-0.040193 0.003943 0.002075 -0.751639 -0.620306 0.224186
+-0.094701 0.071289 0.005665 -0.751639 -0.620306 0.224186
+-0.031072 0.002857 0.012547 -0.130323 -0.991472 0.000026
+-0.031077 0.002857 -0.012547 -0.130323 -0.991472 0.000026
+-0.001677 -0.001007 0.004864 -0.130323 -0.991472 0.000026
+-0.096996 0.074745 -0.002479 -0.980976 0.005144 -0.194062
+-0.096305 0.208811 -0.002418 -0.980976 0.005144 -0.194062
+-0.095606 0.208561 -0.005958 -0.980976 0.005144 -0.194062
+-0.000906 -0.002426 0.004864 -0.948149 -0.317826 0.000001
+-0.000906 -0.002426 0.012973 -0.948149 -0.317826 0.000001
+-0.001383 -0.001003 0.012973 -0.948149 -0.317826 0.000001
+-0.001677 -0.001007 -0.005132 -0.878192 -0.477157 -0.033170
+-0.001383 -0.001003 -0.012973 -0.878192 -0.477157 -0.033170
+-0.000906 -0.002426 -0.005132 -0.878192 -0.477157 -0.033170
+-0.094712 0.071288 -0.005934 -0.755829 -0.622538 -0.202904
+-0.039181 0.003824 -0.005801 -0.755829 -0.622538 -0.202904
+-0.040181 0.003942 -0.002438 -0.755829 -0.622538 -0.202904
+-0.096305 0.208811 -0.002418 -0.999983 0.005155 -0.002683
+-0.096996 0.074745 -0.002479 -0.999983 0.005155 -0.002683
+-0.096317 0.208811 0.002055 -0.999983 0.005155 -0.002683
+-0.092240 0.071213 0.008919 -0.555408 -0.456306 0.695203
+-0.036794 0.003539 0.008797 -0.555408 -0.456306 0.695203
+-0.088845 0.071115 0.011567 -0.555408 -0.456306 0.695203
+-0.040181 0.003942 -0.002438 -0.118811 -0.992917 -0.000429
+-0.036639 0.003521 -0.008986 -0.118811 -0.992917 -0.000429
+-0.031077 0.002857 -0.012547 -0.118811 -0.992917 -0.000429
+-0.094701 0.071289 0.005665 -0.670687 -0.553011 0.494326
+-0.039168 0.003822 0.005534 -0.670687 -0.553011 0.494326
+-0.092240 0.071213 0.008919 -0.670687 -0.553011 0.494326
+-0.087126 0.208806 0.012545 0.011646 0.999885 -0.009727
+-0.087273 0.208561 -0.012815 0.011646 0.999885 -0.009727
+-0.096317 0.208811 0.002055 0.011646 0.999885 -0.009727
+-0.095780 0.071327 -0.002479 -0.771336 -0.636426 -0.001909
+-0.040181 0.003942 -0.002438 -0.771336 -0.636426 -0.001909
+-0.040193 0.003943 0.002075 -0.771336 -0.636426 -0.001909
+-0.084028 0.069070 0.012815 0.001633 0.005997 0.999981
+-0.047180 0.068216 0.012760 0.001633 0.005997 0.999981
+-0.051441 0.082047 0.012684 0.001633 0.005997 0.999981
+-0.051432 0.081950 -0.000232 0.993845 0.109095 -0.019244
+-0.051680 0.081963 -0.012967 0.993845 0.109095 -0.019244
+-0.053158 0.095489 -0.012618 0.993845 0.109095 -0.019244
+-0.087618 0.078007 -0.012815 -0.006985 0.025030 -0.999662
+-0.053158 0.095489 -0.012618 -0.006985 0.025030 -0.999662
+-0.051680 0.081963 -0.012967 -0.006985 0.025030 -0.999662
+-0.087126 0.208806 0.012545 0.185404 0.982662 -0.000000
+-0.041773 0.200249 0.012553 0.185404 0.982662 -0.000000
+-0.041773 0.200249 -0.012553 0.185404 0.982662 -0.000000
+-0.042150 0.055658 -0.013145 0.053052 0.998360 0.021515
+-0.041745 0.055505 -0.007044 0.053052 0.998360 0.021515
+-0.037628 0.055414 -0.012973 0.053052 0.998360 0.021515
+-0.051441 0.082047 0.012684 0.991941 0.126703 -0.000262
+-0.051432 0.081950 -0.000232 0.991941 0.126703 -0.000262
+-0.053158 0.095489 0.012618 0.991941 0.126703 -0.000262
+-0.036639 0.003521 -0.008986 -0.550116 -0.453535 -0.701197
+-0.092211 0.071210 -0.009169 -0.550116 -0.453535 -0.701197
+-0.088766 0.071096 -0.011798 -0.550116 -0.453535 -0.701197
+-0.037628 0.055414 -0.012973 -0.000001 -0.000000 -1.000000
+-0.002554 0.000370 0.004864 0.843342 0.537377 -0.000001
+-0.002554 0.000370 -0.005132 0.843342 0.537377 -0.000001
+-0.037628 0.055414 0.004864 0.843342 0.537377 -0.000001
+-0.047180 0.068216 0.012760 0.922358 0.385918 0.017984
+-0.041857 0.055498 0.012671 0.922358 0.385918 0.017984
+-0.041745 0.055505 0.006777 0.922358 0.385918 0.017984
+-0.053770 0.171623 0.012553 0.999944 0.008032 -0.006905
+-0.053158 0.095489 0.012618 0.999944 0.008032 -0.006905
+-0.053247 0.095488 -0.000274 0.999944 0.008032 -0.006905
+-0.093407 0.075029 0.008919 -0.790554 0.004382 0.612377
+-0.092877 0.208810 0.008646 -0.790554 0.004382 0.612377
+-0.095269 0.208811 0.005558 -0.790554 0.004382 0.612377
+-0.096305 0.208811 -0.002418 -0.032216 0.998128 -0.051987
+-0.087273 0.208561 -0.012815 -0.032216 0.998128 -0.051987
+-0.093242 0.208561 -0.009116 -0.032216 0.998128 -0.051987
+-0.090235 0.076305 0.011566 -0.639598 0.004187 0.768699
+-0.089739 0.208807 0.011257 -0.639598 0.004187 0.768699
+-0.092877 0.208810 0.008646 -0.639598 0.004187 0.768699
+-0.039181 0.003824 -0.005801 -0.667065 -0.550065 -0.502447
+-0.094712 0.071288 -0.005934 -0.667065 -0.550065 -0.502447
+-0.092211 0.071210 -0.009169 -0.667065 -0.550065 -0.502447
+-0.031072 0.002857 0.012547 -0.015053 -0.005440 0.999872
+-0.001383 -0.001003 0.012973 -0.015053 -0.005440 0.999872
+-0.041857 0.055498 0.012671 -0.015053 -0.005440 0.999872
+-0.031077 0.002857 -0.012547 -0.012333 -0.013910 -0.999827
+-0.084014 0.069053 -0.012815 -0.012333 -0.013910 -0.999827
+-0.042150 0.055658 -0.013145 -0.012333 -0.013910 -0.999827
+-0.042150 0.055658 -0.013145 0.920453 0.387461 -0.051386
+-0.047435 0.068225 -0.013055 0.920453 0.387461 -0.051386
+-0.041745 0.055505 -0.007044 0.920453 0.387461 -0.051386
+-0.041857 0.055498 0.012671 -0.072067 -0.046300 0.996325
+-0.001383 -0.001003 0.012973 -0.072067 -0.046300 0.996325
+-0.037628 0.055414 0.012973 -0.072067 -0.046300 0.996325
+3 0 1 2
+3 3 4 5
+3 6 7 8
+3 9 10 11
+3 12 13 14
+3 15 16 17
+3 18 19 20
+3 21 22 23
+3 24 25 26
+3 27 28 29
+3 30 31 32
+3 33 34 35
+3 36 37 38
+3 39 40 41
+3 42 43 44
+3 45 46 47
+3 48 49 50
+3 51 52 53
+3 54 55 56
+3 57 58 59
+3 60 61 62
+3 63 64 65
+3 66 67 68
+3 69 70 71
+3 72 73 74
+3 75 76 77
+3 78 79 80
+3 81 82 83
+3 84 85 86
+3 87 88 89
+3 90 91 92
+3 93 94 95
+3 96 97 98
+3 99 100 101
+3 102 103 104
+3 105 106 107
+3 108 109 110
+3 111 112 113
+3 114 115 116
+3 117 118 119
+3 120 121 122
+3 123 124 125
+3 126 127 128
+3 129 130 131
+3 54 56 132
+3 133 58 134
+3 36 135 37
+3 136 137 138
+3 139 140 141
+3 142 143 144
+3 145 146 147
+3 148 149 150
+3 151 152 153
+3 154 155 156
+3 157 158 159
+3 160 161 162
+3 163 164 165
+3 166 167 168
+3 169 170 171
+3 172 173 174
+3 175 176 177
+3 178 179 180
+3 181 182 183
+3 184 185 186
+3 187 188 189
+3 190 191 192
+3 193 194 195
+3 196 197 198
+3 199 200 201
+3 202 203 204
+3 205 206 207
+3 208 209 210
+3 211 212 213
+3 214 215 216
+3 217 218 219
+3 220 221 222
+3 223 224 225
+3 226 227 228
+3 229 230 231
+3 232 233 234
+3 235 236 237
+3 17 238 15
+3 239 240 241
+3 242 243 132
+3 133 134 244
+3 245 246 247
+3 248 249 250
+3 251 252 253
+3 254 255 256
+3 257 258 259
+3 260 261 262
+3 263 264 265
+3 266 267 268
+3 269 270 271
+3 272 273 274
+3 275 276 277
+3 278 279 280
+3 281 282 283
+3 37 135 284
+3 285 286 287
+3 288 289 290
+3 291 6 8
+3 292 293 294
+3 295 296 297
+3 298 299 300
+3 301 24 26
+3 64 63 302
+3 303 304 305
+3 306 307 308
+3 309 39 41
+3 310 42 44
+3 311 51 53
+3 312 60 62
+3 313 221 220
+3 314 315 316
+3 317 318 319
+3 320 321 322
+3 323 324 325
+3 326 327 328
+3 329 330 331
+3 332 333 334
+3 335 336 337
+3 338 339 340
+3 341 342 343
+3 344 345 346
+3 347 348 349
+3 350 351 352
+3 353 354 355
+3 356 357 358
+3 359 360 361
+3 362 363 364
+3 365 366 367
+3 368 369 370
+3 371 129 131
+3 56 242 132
+3 58 57 134
+3 372 373 374
+3 375 376 377
+3 378 379 380
+3 381 382 383
+3 384 385 386
+3 387 388 389
+3 390 391 392
+3 393 394 395
+3 396 397 398
+3 399 400 401
+3 402 403 404
+3 405 406 407
+3 408 409 410
+3 411 412 413
+3 414 415 416
+3 417 418 419
+3 420 421 422
+3 423 424 425
+3 426 427 428
+3 429 430 431
+3 432 433 434
+3 435 436 437
+3 438 439 440
+3 441 442 443
+3 444 445 446
+3 447 448 449
+3 450 451 452
+3 453 454 455
+3 456 238 17
+3 457 458 459
+3 460 461 462
+3 463 464 465
+3 466 467 468
+3 469 470 471
+3 472 473 474
+3 475 476 477
+3 478 479 480
+3 481 482 483
+3 484 485 486
+3 487 488 489
diff --git a/Examples/Stapling/Data/Geometry/stapler_handle.mtl b/Examples/Stapling/Data/Geometry/stapler_handle.mtl
new file mode 100644
index 0000000..eaf4a39
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/stapler_handle.mtl
@@ -0,0 +1,11 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl _Mat.1
+Ns 45.098039
+Ka 0.000000 0.000000 0.000000
+Kd 0.640000 0.640000 0.640000
+Ks 1.000000 1.000000 1.000000
+Ni 1.000000
+d 1.000000
+illum 2
diff --git a/Examples/Stapling/Data/Geometry/stapler_handle.obj b/Examples/Stapling/Data/Geometry/stapler_handle.obj
new file mode 100644
index 0000000..fbdf678
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/stapler_handle.obj
@@ -0,0 +1,544 @@
+# Blender v2.69 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib stapler_handle.mtl
+o stapler_handle
+v -0.053356 -0.007044 -0.095488
+v -0.072241 -0.007044 -0.067669
+v -0.051508 -0.007044 -0.081950
+v -0.043540 -0.007044 -0.196716
+v -0.070629 -0.007044 -0.201035
+v -0.054056 -0.007044 -0.171623
+v -0.047435 -0.013055 -0.068225
+v -0.084014 -0.012815 -0.069053
+v -0.087252 -0.012815 -0.072855
+v -0.051680 -0.012967 -0.081963
+v -0.090172 -0.011796 -0.076305
+v -0.093381 -0.009168 -0.075029
+v -0.093242 -0.009116 -0.208561
+v -0.089963 -0.011666 -0.208561
+v -0.051355 -0.007240 -0.081950
+v -0.047093 -0.007144 -0.068200
+v -0.047180 -0.012760 -0.068216
+v -0.051441 -0.012684 -0.082047
+v -0.037628 -0.005132 -0.055414
+v -0.000021 -0.005132 -0.001644
+v -0.000021 -0.012973 -0.001644
+v -0.037628 -0.012973 -0.055414
+v -0.096996 -0.002479 -0.074745
+v -0.096686 -0.002479 -0.072935
+v -0.096695 0.002115 -0.072931
+v -0.097007 0.002114 -0.074745
+v -0.088845 0.011567 -0.071115
+v -0.033798 0.011567 -0.003349
+v -0.031201 0.012815 -0.003042
+v -0.084028 0.012815 -0.069070
+v -0.095624 0.005665 -0.072915
+v -0.095947 0.005665 -0.074745
+v -0.053771 -0.007406 -0.171623
+v -0.043122 -0.007406 -0.197032
+v -0.002554 0.004864 -0.000370
+v -0.004178 0.004864 -0.002917
+v -0.037628 0.004864 -0.055414
+v -0.000021 0.004864 -0.001644
+v -0.000021 -0.012973 0.001946
+v -0.000906 -0.012973 0.002426
+v -0.001383 -0.012973 0.001003
+v -0.087259 0.012815 -0.072864
+v -0.051680 0.012967 -0.081963
+v -0.053440 0.012890 -0.095489
+v -0.087618 0.012815 -0.078007
+v -0.087273 -0.012815 -0.208561
+v -0.054056 -0.012815 -0.171623
+v -0.053440 -0.012890 -0.095489
+v -0.087618 -0.012815 -0.078007
+v -0.072241 0.006777 -0.067669
+v -0.041745 0.006777 -0.055505
+v -0.041745 -0.007044 -0.055505
+v -0.053137 -0.007324 -0.095489
+v -0.053158 -0.012618 -0.095489
+v -0.053770 -0.012553 -0.171623
+v -0.087273 0.012815 -0.208561
+v -0.054056 0.012815 -0.171623
+v -0.090015 0.011440 -0.208561
+v -0.090235 0.011566 -0.076305
+v -0.041927 -0.012815 -0.200565
+v -0.054056 0.006777 -0.171623
+v -0.070629 0.006777 -0.201035
+v -0.043540 0.006777 -0.196716
+v -0.096317 0.002055 -0.208811
+v -0.095269 0.005558 -0.208811
+v -0.092877 0.008646 -0.208810
+v -0.041856 -0.012671 -0.055498
+v -0.095595 0.005688 -0.208561
+v -0.096670 0.002095 -0.208561
+v -0.047093 0.006876 -0.068200
+v -0.047180 0.012760 -0.068216
+v -0.041857 0.012671 -0.055498
+v -0.000021 0.012973 -0.001644
+v -0.000021 0.012973 0.001946
+v -0.000021 0.004864 0.001946
+v -0.088766 -0.011798 -0.071096
+v -0.031201 -0.012815 -0.003042
+v -0.033735 -0.011798 -0.003342
+v -0.037628 0.012973 -0.055414
+v -0.040193 0.002075 -0.003943
+v -0.040181 -0.002438 -0.003942
+v -0.031077 -0.012547 -0.002857
+v -0.031072 0.012547 -0.002857
+v -0.053771 0.007139 -0.171623
+v -0.053770 0.012553 -0.171623
+v -0.053158 0.012618 -0.095489
+v -0.053137 0.007057 -0.095489
+v -0.043122 0.007139 -0.197032
+v -0.041773 0.012553 -0.200249
+v -0.001383 0.012973 0.001003
+v -0.000906 0.012973 0.002426
+v -0.000021 -0.005132 0.001946
+v -0.000906 -0.005132 0.002426
+v -0.040525 0.002115 -0.004145
+v -0.039473 0.005665 -0.004021
+v -0.094701 0.005665 -0.071289
+v -0.095792 0.002115 -0.071328
+v -0.002554 -0.005132 -0.000370
+v -0.001677 0.004864 0.001007
+v -0.001677 -0.005132 0.001007
+v -0.093407 0.008918 -0.075029
+v -0.093263 0.008868 -0.208561
+v -0.041773 -0.012553 -0.200249
+v -0.002026 -0.005132 0.001114
+v -0.002026 0.004864 0.001114
+v -0.001375 -0.012892 -0.000815
+v -0.001275 -0.012884 -0.000923
+v -0.003867 -0.005132 0.000450
+v -0.005627 -0.005132 -0.000208
+v -0.005627 0.004864 -0.000208
+v -0.003867 0.004864 0.000450
+v -0.095959 -0.005934 -0.074745
+v -0.096659 -0.002459 -0.208561
+v -0.095606 -0.005958 -0.208561
+v -0.041927 0.012815 -0.200565
+v -0.042150 -0.013145 -0.055658
+v -0.053356 0.006777 -0.095488
+v -0.051508 0.006777 -0.081950
+v -0.047177 0.006777 -0.068215
+v -0.000906 0.004864 0.002426
+v -0.001677 0.012815 0.001007
+v -0.001677 -0.012815 0.001007
+v -0.087130 -0.012545 -0.208807
+v -0.087126 0.012545 -0.208806
+v -0.042275 0.012545 -0.200876
+v -0.042275 -0.012545 -0.200876
+v -0.010136 0.004864 -0.005870
+v -0.009023 0.004864 -0.008214
+v -0.004513 0.004864 -0.002552
+v -0.042150 0.013145 -0.055658
+v -0.047435 0.013055 -0.068225
+v -0.095780 -0.002479 -0.071327
+v -0.094712 -0.005934 -0.071288
+v -0.039484 -0.005934 -0.004022
+v -0.040513 -0.002479 -0.004144
+v -0.089621 -0.011472 -0.208807
+v -0.092935 -0.008969 -0.208809
+v -0.096305 -0.002418 -0.208811
+v -0.002026 0.012722 0.001114
+v -0.002026 -0.012722 0.001114
+v -0.002179 0.012874 0.001090
+v -0.009023 -0.005132 -0.008214
+v -0.004513 -0.005132 -0.002552
+v -0.002179 -0.012874 0.001090
+v -0.037089 0.008919 -0.003739
+v -0.092240 0.008919 -0.071213
+v -0.002010 -0.013145 0.000955
+v -0.002010 0.013145 0.000955
+v -0.033488 0.011418 -0.003143
+v -0.036794 0.008797 -0.003539
+v -0.039168 0.005534 -0.003822
+v -0.089739 0.011257 -0.208807
+v -0.037064 -0.009169 -0.003735
+v -0.092211 -0.009169 -0.071210
+v -0.039181 -0.005801 -0.003824
+v -0.036639 -0.008986 -0.003521
+v -0.033586 -0.011444 -0.003155
+v -0.095282 -0.005826 -0.208811
+v -0.047177 -0.007044 -0.068215
+v -0.051355 0.006973 -0.081950
+v -0.051441 0.012684 -0.082047
+v -0.089830 -0.011797 -0.072891
+v -0.093182 -0.009169 -0.072891
+v -0.093225 0.008919 -0.072915
+v -0.089894 0.011567 -0.072892
+v -0.095647 -0.005934 -0.072931
+v -0.038934 -0.012891 -0.054405
+v -0.039462 -0.012853 -0.054407
+v -0.001134 -0.012894 -0.001236
+v -0.004178 -0.005132 -0.002917
+v -0.010136 -0.005132 -0.005870
+v -0.001138 0.012889 -0.001234
+v -0.001275 0.012878 -0.000923
+v -0.038933 0.012910 -0.054405
+v -0.039461 0.012873 -0.054406
+v -0.001375 0.012891 -0.000727
+v -0.001111 -0.012277 -0.001252
+v -0.001352 -0.012275 -0.000831
+v -0.039439 -0.012236 -0.054423
+v -0.038911 -0.012273 -0.054421
+v -0.038914 0.012293 -0.054419
+v -0.039442 0.012255 -0.054420
+v -0.001356 0.012273 -0.000741
+v -0.001119 0.012271 -0.001248
+vt 0.000000 0.562083
+vt 0.000000 0.781042
+vt 0.000000 0.439738
+vt 1.000000 0.866987
+vt 1.000000 0.433493
+vt 1.000000 0.000000
+vt 0.921613 0.481195
+vt 0.817866 0.000000
+vt 0.000000 0.000000
+vt 1.000000 1.000000
+vt 0.000000 1.000000
+vt 0.494783 0.000000
+vt 0.494783 1.000000
+vt 0.678493 0.494783
+vt 0.679494 0.493006
+vt 0.957235 0.494783
+vt 0.245385 0.493650
+vt 0.182134 0.000000
+vt 0.325801 0.000000
+vt 0.321507 1.000000
+vt 0.321507 0.000000
+vt 0.674199 0.000000
+vt 0.495038 1.000000
+vt 0.326114 0.494343
+vt 0.326113 0.000437
+vt 0.678177 0.000443
+vt 0.678177 0.494339
+vt 0.678493 1.000000
+vt 0.325801 1.000000
+vt 0.678493 0.000000
+vt 0.678493 0.505217
+vt 0.325801 0.505217
+vt 0.182136 1.000000
+vt 0.042765 0.000000
+vt 0.000000 0.433493
+vt 0.000000 0.866987
+vt 0.747519 0.078780
+vt 0.690647 0.020652
+vt 0.997464 0.000268
+vt 0.495038 0.000000
+vt 0.183352 0.496682
+vt 0.183351 0.998140
+vt 0.002269 0.996481
+vt 0.002321 0.498382
+vt 0.675627 0.940242
+vt 0.504884 0.994241
+vt 0.497999 0.816695
+vt 0.994331 0.841597
+vt 0.678167 0.495250
+vt 0.678167 0.999532
+vt 0.326123 0.999539
+vt 0.326125 0.495246
+vt 0.953820 0.993809
+vt 0.679526 0.998128
+vt 0.679521 0.496646
+vt 0.495038 0.953013
+vt 1.000000 0.953013
+vt 0.504962 0.866987
+vt 0.499219 0.998487
+vt 0.005678 0.998504
+vt 0.499073 0.808825
+vt 0.500029 0.008182
+vt 0.994966 0.008253
+vt 0.995946 0.833742
+vt 1.000000 0.500000
+vt 0.817864 1.000000
+vt 0.957235 0.000000
+vt 0.679499 0.001786
+vt 0.953827 0.006050
+vt 1.000000 0.562083
+vt 0.497895 0.164692
+vt 0.997109 0.164751
+vt 0.995625 0.992722
+vt 0.499413 0.992722
+vt 0.162901 1.000000
+vt 0.002696 0.158197
+vt 0.015534 0.149902
+vt 0.009494 0.006135
+vt 0.489086 0.001282
+vt 0.747391 0.914004
+vt 0.500814 0.007325
+vt 0.991045 0.010874
+vt 0.247391 0.914199
+vt 0.008801 0.010914
+vt 0.488745 0.007487
+vt 0.574360 0.998721
+vt 1.000000 0.011644
+vt 1.000000 0.006206
+vt 1.000000 0.995065
+vt 0.617511 0.001687
+vt 0.000000 0.926841
+vt 0.504962 0.000000
+vt 0.504962 0.926837
+vt 0.000000 0.997615
+vt 0.000000 0.993794
+vt 0.000000 0.023289
+vt 0.333333 0.000000
+vt 1.000000 0.005061
+vt 1.000000 0.996164
+vt 0.333333 1.000000
+vt 0.995199 0.837799
+vt 0.974701 0.080157
+vt 0.646390 0.979032
+vt 0.471913 0.995548
+vt 0.736652 0.755759
+vt 0.642611 0.359030
+vt 0.809815 0.342694
+vt 0.036877 0.021387
+vt 0.936231 0.036984
+vt 0.989220 0.162745
+vt 0.007663 0.989118
+vt 0.471828 0.694838
+vt 0.416059 0.585638
+vt 0.987395 0.504650
+vt 0.018795 0.399015
+vt 0.917102 0.061532
+vt 0.005416 0.834443
+vt 0.004017 0.992877
+vt 0.501629 0.162899
+vt 0.992725 0.019015
+vt 0.992458 0.959969
+vt 0.997835 0.162857
+vt 0.005204 0.003469
+vt 0.558785 0.294143
+vt 0.500000 0.984423
+vt 0.676200 0.942811
+vt 1.000000 0.840593
+vt 0.636293 1.000000
+vt 0.735359 0.764641
+vt 0.641124 0.358876
+vt 0.415485 0.584515
+vt 0.000000 0.386197
+vt 0.000000 0.840593
+vt 0.747519 0.079970
+vt 0.495038 0.159940
+vt 1.000000 0.159941
+vt 0.000000 0.159939
+vt 0.500000 1.000000
+vt 0.183353 0.001806
+vt 0.002261 0.003434
+vt 0.183043 0.494010
+vt 0.001158 0.493024
+vt 0.182847 0.493709
+vt 0.325801 0.494783
+vt 0.183044 0.495574
+vt 0.182842 0.495890
+vt 0.001160 0.496582
+vt 0.504962 1.000000
+vt 0.957235 1.000000
+vt 1.000000 0.439738
+vt 1.000000 0.781042
+vt 0.183356 0.492936
+vt 0.002316 0.491266
+vt 0.182134 1.000000
+vt 0.837099 1.000000
+usemtl _Mat.1
+s 1
+f 1/1 2/2 3/3
+f 4/4 5/5 6/6
+f 7/6 8/6 9/7 10/8
+f 11/9 12/6 13/10 14/11
+f 2/11 1/11 6/9 5/9
+f 15/9 16/6 17/10 18/11
+f 19/12 20/12 21/9 22/9
+f 23/9 24/6 25/10 26/11
+f 27/13 28/13 29/10 30/10
+f 31/9 32/6 26/10 25/11
+f 6/14 33/15 34/16 4/16
+f 35/12 36/12 37/12 38/12
+f 39/9 40/9 41/9
+f 42/17 43/18 44/19 45/19
+f 46/20 47/21 48/22 49/22
+f 2/23 50/10 51/10 52/23
+f 53/24 54/25 55/26 33/27
+f 56/28 45/29 44/19 57/30
+f 58/31 59/32 45/19 56/30
+f 46/33 60/34 47/21
+f 61/9 62/35 63/36
+f 64/37 65/38 66/39
+f 52/12 19/12 22/9 67/9
+f 26/40 32/6 68/10 69/23
+f 70/41 71/42 72/43 51/44
+f 38/12 73/6 74/6 75/12
+f 76/13 8/11 77/11 78/13
+f 51/12 72/6 79/6 37/12
+f 80/45 81/46 82/47 83/48
+f 84/49 85/50 86/51 87/52
+f 14/31 46/28 49/29 11/32
+f 88/16 89/53 85/54 84/55
+f 74/6 90/6 91/6
+f 92/12 93/12 40/9 39/9
+f 94/40 95/6 96/10 97/23
+f 98/56 35/57 99/10 100/23
+f 32/9 101/6 102/10 68/11
+f 88/36 34/58 103/59 89/60
+f 4/58 63/36 62/36 5/58
+f 82/61 104/62 105/63 83/64
+f 41/9 106/9 107/65 39/11
+f 108/9 109/6 110/10 111/11
+f 112/9 23/40 113/23 114/11
+f 56/66 57/30 115/67
+f 7/6 116/6 8/6
+f 117/10 50/10 62/6 61/6
+f 34/16 33/15 55/68 103/69
+f 118/10 119/70 50/10
+f 99/12 120/12 91/6 121/6
+f 38/12 37/12 79/6 73/6
+f 122/9 40/9 93/12 100/12
+f 123/71 124/72 125/73 126/74
+f 110/9 127/6 128/10 129/11
+f 30/75 130/9 131/11
+f 132/23 133/11 134/9 135/40
+f 5/40 62/6 50/10 2/23
+f 123/76 136/77 137/78 138/79
+f 75/12 120/12 99/12
+f 83/80 105/81 139/82
+f 82/83 140/84 104/85
+f 92/12 100/12 93/12
+f 74/6 91/6 120/12 75/12
+f 72/86 121/9 90/6
+f 101/9 59/6 58/10 102/11
+f 139/87 141/88 83/89
+f 67/90 41/10 122/11
+f 129/9 128/6 142/10 143/11
+f 51/91 37/9 19/92 52/93
+f 60/67 103/69 55/68 47/30
+f 23/9 26/6 69/10 113/11
+f 82/94 144/95 140/96
+f 145/9 28/6 27/10 146/11
+f 116/97 67/98 122/10 147/11
+f 148/9 121/6 72/99 130/100
+f 91/6 90/6 121/6
+f 122/9 41/9 40/9
+f 83/101 149/102 150/103 80/104
+f 80/105 150/106 151/107
+f 66/108 152/109 124/110 64/111
+f 78/9 153/6 154/10 76/11
+f 155/112 156/113 81/114
+f 156/115 157/116 82/117 81/118
+f 95/9 145/6 146/10 96/11
+f 123/119 138/120 64/121 124/122
+f 153/9 134/6 133/10 154/11
+f 137/123 158/124 138/125
+f 97/9 132/6 135/10 94/11
+f 12/9 112/6 114/10 13/11
+f 135/23 81/46 80/45 94/126
+f 99/6 105/63 104/62 100/40
+f 121/6 139/82 105/81 99/12
+f 100/12 104/85 140/84 122/9
+f 148/6 141/88 139/87 121/6
+f 29/10 83/89 141/88 148/6
+f 147/11 144/95 82/94 77/11
+f 122/9 140/96 144/95 147/11
+f 28/6 149/102 83/101 29/127
+f 145/128 150/103 149/102 28/6
+f 94/129 80/105 151/107 95/6
+f 95/6 151/107 150/106 145/130
+f 134/11 155/112 81/114 135/65
+f 153/131 156/113 155/112 134/11
+f 78/6 157/116 156/115 153/132
+f 77/133 82/117 157/116 78/6
+f 68/40 65/38 64/37 69/134
+f 102/6 66/39 65/38 68/40
+f 46/135 123/71 126/74 60/23
+f 115/10 125/73 124/72 56/136
+f 60/23 126/74 125/73 115/10
+f 14/137 136/77 123/76 46/137
+f 13/9 137/78 136/77 14/137
+f 58/6 152/109 66/108 102/9
+f 56/136 124/110 152/109 58/6
+f 69/10 64/121 138/120 113/6
+f 114/6 158/124 137/123 13/9
+f 113/138 138/125 158/124 114/6
+f 7/18 17/139 67/140 116/9
+f 159/141 52/142 16/143
+f 4/58 34/58 88/36 63/36
+f 1/144 53/24 33/27 6/14
+f 47/30 55/26 54/25 48/19
+f 119/145 70/146 51/147
+f 160/9 87/6 86/10 161/11
+f 115/11 89/60 103/59 60/148
+f 61/14 84/49 87/52 117/144
+f 44/29 86/51 85/50 57/28
+f 63/16 88/16 84/55 61/14
+f 57/28 85/54 89/53 115/149
+f 11/9 162/6 163/10 12/11
+f 164/9 101/6 32/10 31/11
+f 30/9 29/10 148/6 130/6
+f 1/9 3/6 15/10 53/11
+f 147/9 77/11 8/10 116/9
+f 146/9 164/6 31/10 96/11
+f 118/9 117/6 87/10 160/11
+f 96/9 31/6 25/10 97/11
+f 24/9 132/6 97/10 25/11
+f 162/9 76/6 154/10 163/11
+f 42/9 45/6 59/10 165/11
+f 30/9 42/6 165/10 27/11
+f 76/9 162/6 9/10 8/11
+f 162/9 11/6 49/10 9/11
+f 166/9 112/6 12/10 163/11
+f 133/9 166/6 163/10 154/11
+f 112/9 166/6 24/10 23/11
+f 166/9 133/6 132/10 24/11
+f 164/9 146/6 27/10 165/11
+f 101/9 164/6 165/10 59/11
+f 159/9 3/9 2/11
+f 54/9 18/6 10/10 48/11
+f 42/17 30/75 131/9 43/18
+f 117/150 118/70 50/151
+f 16/152 52/153 67/140 17/139
+f 43/9 131/6 71/10 161/11
+f 70/9 160/6 161/10 71/11
+f 18/9 17/6 7/10 10/11
+f 53/9 15/6 18/10 54/11
+f 44/9 43/6 161/10 86/11
+f 131/154 130/11 72/43 71/42
+f 3/9 159/6 16/10 15/11
+f 119/9 118/6 160/10 70/11
+f 159/6 2/11 52/11
+f 119/6 51/10 50/10
+f 9/7 49/155 48/10 10/11
+f 20/12 92/12 39/9 21/9
+f 22/9 167/9 168/65 67/11
+f 21/9 169/6 167/10 22/11
+f 67/9 168/6 106/10 41/11
+f 75/12 99/12 35/10 38/12
+f 98/12 20/12 19/12 170/12
+f 170/56 19/40 37/6 36/57
+f 98/9 100/12 92/12 20/12
+f 98/9 108/6 111/10 35/11
+f 109/9 98/11 170/9 143/11
+f 36/9 129/6 143/10 170/11
+f 110/9 129/6 36/11 35/9
+f 109/6 108/10 98/11
+f 35/9 111/6 110/11
+f 171/9 142/6 128/10 127/11
+f 109/6 171/6 127/10 110/11
+f 143/9 142/6 171/6 109/6
+f 73/9 172/9 173/65 74/11
+f 79/9 174/6 172/10 73/11
+f 72/9 175/9 174/65 79/11
+f 90/9 176/6 175/10 72/11
+f 74/9 173/9 176/65 90/11
+f 39/9 107/9 169/65 21/11
+f 177/9 178/6 179/10 180/11
+f 181/9 182/6 183/10 184/11
+f 107/6 106/10 169/11
+f 176/6 173/10 172/11
+f 169/9 177/9 180/11 167/11
+f 106/6 178/6 177/9 169/9
+f 168/10 179/10 178/6 106/6
+f 167/11 180/11 179/10 168/10
+f 174/9 181/9 184/11 172/11
+f 175/6 182/6 181/9 174/9
+f 176/10 183/10 182/6 175/6
+f 172/11 184/11 183/10 176/10
diff --git a/Examples/Stapling/Data/Geometry/stapler_indicator.mtl b/Examples/Stapling/Data/Geometry/stapler_indicator.mtl
new file mode 100644
index 0000000..e4fc956
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/stapler_indicator.mtl
@@ -0,0 +1,11 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl _Mat
+Ns 45.098039
+Ka 0.000000 0.000000 0.000000
+Kd 0.000000 0.384000 0.640000
+Ks 1.000000 1.000000 1.000000
+Ni 1.000000
+d 1.000000
+illum 2
diff --git a/Examples/Stapling/Data/Geometry/stapler_indicator.obj b/Examples/Stapling/Data/Geometry/stapler_indicator.obj
new file mode 100644
index 0000000..b60f180
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/stapler_indicator.obj
@@ -0,0 +1,34 @@
+# Blender v2.69 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib stapler_indicator.mtl
+o stapler_indicator
+v -0.028416 -0.013046 -0.036842
+v -0.026082 -0.013046 -0.038503
+v -0.027742 -0.013046 -0.040837
+v -0.028416 -0.013615 -0.036842
+v -0.026082 -0.013615 -0.038503
+v -0.027742 -0.013615 -0.040837
+v -0.028416 0.013547 -0.036842
+v -0.026082 0.013547 -0.038503
+v -0.027742 0.013547 -0.040837
+v -0.028416 0.012978 -0.036842
+v -0.026082 0.012978 -0.038503
+v -0.027742 0.012978 -0.040837
+vt 1.000000 0.000000
+vt 1.000000 1.000000
+vt 0.000000 0.500000
+vt 0.000000 1.000000
+vt 0.000000 0.000000
+vt 1.000000 0.500000
+usemtl _Mat
+s 1
+f 1/1 2/2 3/3
+f 2/4 1/5 4/1 5/2
+f 6/6 5/4 4/5
+f 4/1 1/2 3/4 6/5
+f 2/1 5/2 6/4 3/5
+f 7/1 8/2 9/3
+f 10/1 11/2 8/4 7/5
+f 12/6 11/4 10/5
+f 10/1 7/2 9/4 12/5
+f 8/1 11/2 12/4 9/5
diff --git a/Examples/Stapling/Data/Geometry/stapler_markings.mtl b/Examples/Stapling/Data/Geometry/stapler_markings.mtl
new file mode 100644
index 0000000..dc59153
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/stapler_markings.mtl
@@ -0,0 +1,11 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl _Mat.002
+Ns 45.098039
+Ka 0.000000 0.000000 0.000000
+Kd 0.000000 0.384000 0.640000
+Ks 1.000000 1.000000 1.000000
+Ni 1.000000
+d 1.000000
+illum 2
diff --git a/Examples/Stapling/Data/Geometry/stapler_markings.obj b/Examples/Stapling/Data/Geometry/stapler_markings.obj
new file mode 100644
index 0000000..b9a3af0
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/stapler_markings.obj
@@ -0,0 +1,8114 @@
+# Blender v2.69 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib stapler_markings.mtl
+o stapler_markings
+v -0.026458 0.013011 -0.025189
+v -0.026356 0.013011 -0.025390
+v -0.021093 0.013011 -0.025268
+v -0.021098 0.013011 -0.025065
+v -0.026356 0.013214 -0.025390
+v -0.021093 0.013214 -0.025268
+v -0.026458 0.013214 -0.025189
+v -0.021098 0.013214 -0.025065
+v -0.029057 0.013011 -0.028617
+v -0.028896 0.013011 -0.028740
+v -0.028896 0.013214 -0.028740
+v -0.029057 0.013214 -0.028617
+v -0.021093 0.013011 -0.025268
+v -0.021093 0.013214 -0.025268
+v -0.021098 0.013214 -0.025065
+v -0.021098 0.013011 -0.025065
+v -0.028896 0.013214 -0.028740
+v -0.028896 0.013011 -0.028740
+v -0.029057 0.013011 -0.028617
+v -0.029057 0.013214 -0.028617
+v -0.024182 0.013232 -0.027209
+v -0.024182 0.013002 -0.027209
+v -0.023909 0.013002 -0.027409
+v -0.023909 0.013232 -0.027409
+v -0.024183 0.013232 -0.027308
+v -0.024183 0.013002 -0.027308
+v -0.024193 0.013232 -0.027404
+v -0.024193 0.013002 -0.027404
+v -0.024211 0.013232 -0.027497
+v -0.024211 0.013002 -0.027497
+v -0.024238 0.013232 -0.027587
+v -0.024238 0.013002 -0.027587
+v -0.024273 0.013232 -0.027675
+v -0.024273 0.013002 -0.027675
+v -0.024317 0.013232 -0.027759
+v -0.024317 0.013002 -0.027759
+v -0.024347 0.013232 -0.027807
+v -0.024347 0.013002 -0.027807
+v -0.024379 0.013232 -0.027853
+v -0.024379 0.013002 -0.027853
+v -0.024441 0.013232 -0.027930
+v -0.024441 0.013002 -0.027930
+v -0.024499 0.013232 -0.027990
+v -0.024499 0.013002 -0.027990
+v -0.024552 0.013232 -0.028034
+v -0.024552 0.013002 -0.028034
+v -0.024599 0.013232 -0.028066
+v -0.024599 0.013002 -0.028066
+v -0.024647 0.013232 -0.028094
+v -0.024647 0.013002 -0.028094
+v -0.024697 0.013232 -0.028116
+v -0.024697 0.013002 -0.028116
+v -0.024731 0.013232 -0.028128
+v -0.024731 0.013002 -0.028128
+v -0.024765 0.013232 -0.028138
+v -0.024765 0.013002 -0.028138
+v -0.024817 0.013232 -0.028148
+v -0.024817 0.013002 -0.028148
+v -0.024860 0.013232 -0.028152
+v -0.024860 0.013002 -0.028152
+v -0.024902 0.013232 -0.028152
+v -0.024902 0.013002 -0.028152
+v -0.024944 0.013232 -0.028148
+v -0.024944 0.013002 -0.028148
+v -0.024985 0.013232 -0.028140
+v -0.024985 0.013002 -0.028140
+v -0.025026 0.013232 -0.028128
+v -0.025026 0.013002 -0.028128
+v -0.025067 0.013232 -0.028112
+v -0.025067 0.013002 -0.028112
+v -0.025115 0.013232 -0.028088
+v -0.025115 0.013002 -0.028088
+v -0.025147 0.013232 -0.028069
+v -0.025147 0.013002 -0.028069
+v -0.025178 0.013232 -0.028047
+v -0.025178 0.013002 -0.028047
+v -0.025227 0.013232 -0.028008
+v -0.025227 0.013002 -0.028008
+v -0.025270 0.013232 -0.027967
+v -0.025270 0.013002 -0.027967
+v -0.025306 0.013232 -0.027923
+v -0.025306 0.013002 -0.027923
+v -0.025336 0.013232 -0.027878
+v -0.025336 0.013002 -0.027878
+v -0.025359 0.013232 -0.027830
+v -0.025359 0.013002 -0.027830
+v -0.025376 0.013232 -0.027780
+v -0.025376 0.013002 -0.027780
+v -0.025387 0.013232 -0.027728
+v -0.025387 0.013002 -0.027728
+v -0.025391 0.013232 -0.027674
+v -0.025391 0.013002 -0.027674
+v -0.025389 0.013232 -0.027617
+v -0.025389 0.013002 -0.027617
+v -0.025380 0.013232 -0.027559
+v -0.025380 0.013002 -0.027559
+v -0.025366 0.013232 -0.027498
+v -0.025366 0.013002 -0.027498
+v -0.025336 0.013232 -0.027414
+v -0.025336 0.013002 -0.027414
+v -0.025295 0.013232 -0.027326
+v -0.025295 0.013002 -0.027326
+v -0.025242 0.013232 -0.027234
+v -0.025242 0.013002 -0.027234
+v -0.025161 0.013232 -0.027114
+v -0.025161 0.013002 -0.027114
+v -0.025026 0.013232 -0.026930
+v -0.025026 0.013002 -0.026930
+v -0.025241 0.013232 -0.026773
+v -0.025241 0.013002 -0.026773
+v -0.025369 0.013232 -0.026947
+v -0.025369 0.013002 -0.026947
+v -0.025465 0.013232 -0.027067
+v -0.025465 0.013002 -0.027067
+v -0.025546 0.013232 -0.027147
+v -0.025546 0.013002 -0.027147
+v -0.025610 0.013232 -0.027200
+v -0.025610 0.013002 -0.027200
+v -0.025659 0.013232 -0.027232
+v -0.025659 0.013002 -0.027232
+v -0.025707 0.013232 -0.027258
+v -0.025707 0.013002 -0.027258
+v -0.025756 0.013232 -0.027278
+v -0.025756 0.013002 -0.027278
+v -0.025804 0.013232 -0.027292
+v -0.025804 0.013002 -0.027292
+v -0.025853 0.013232 -0.027300
+v -0.025853 0.013002 -0.027300
+v -0.025901 0.013232 -0.027302
+v -0.025901 0.013002 -0.027302
+v -0.025950 0.013232 -0.027298
+v -0.025950 0.013002 -0.027298
+v -0.025999 0.013232 -0.027287
+v -0.025999 0.013002 -0.027287
+v -0.026048 0.013232 -0.027271
+v -0.026048 0.013002 -0.027271
+v -0.026096 0.013232 -0.027248
+v -0.026096 0.013002 -0.027248
+v -0.026129 0.013232 -0.027230
+v -0.026129 0.013002 -0.027230
+v -0.026178 0.013232 -0.027197
+v -0.026178 0.013002 -0.027197
+v -0.026234 0.013232 -0.027152
+v -0.026234 0.013002 -0.027152
+v -0.026281 0.013232 -0.027105
+v -0.026281 0.013002 -0.027105
+v -0.026311 0.013232 -0.027068
+v -0.026311 0.013002 -0.027068
+v -0.026335 0.013232 -0.027031
+v -0.026335 0.013002 -0.027031
+v -0.026355 0.013232 -0.026993
+v -0.026355 0.013002 -0.026993
+v -0.026370 0.013232 -0.026953
+v -0.026370 0.013002 -0.026953
+v -0.026380 0.013232 -0.026913
+v -0.026380 0.013002 -0.026913
+v -0.026385 0.013232 -0.026871
+v -0.026385 0.013002 -0.026871
+v -0.026385 0.013232 -0.026829
+v -0.026385 0.013002 -0.026829
+v -0.026380 0.013232 -0.026785
+v -0.026380 0.013002 -0.026785
+v -0.026371 0.013232 -0.026741
+v -0.026371 0.013002 -0.026741
+v -0.026356 0.013232 -0.026695
+v -0.026356 0.013002 -0.026695
+v -0.026329 0.013232 -0.026633
+v -0.026329 0.013002 -0.026633
+v -0.026293 0.013232 -0.026568
+v -0.026293 0.013002 -0.026568
+v -0.026248 0.013232 -0.026502
+v -0.026248 0.013002 -0.026502
+v -0.026198 0.013232 -0.026440
+v -0.026198 0.013002 -0.026440
+v -0.026142 0.013232 -0.026383
+v -0.026142 0.013002 -0.026383
+v -0.026080 0.013232 -0.026332
+v -0.026080 0.013002 -0.026332
+v -0.026013 0.013232 -0.026286
+v -0.026013 0.013002 -0.026286
+v -0.025941 0.013232 -0.026246
+v -0.025941 0.013002 -0.026246
+v -0.025863 0.013232 -0.026211
+v -0.025863 0.013002 -0.026211
+v -0.025767 0.013232 -0.026179
+v -0.025767 0.013002 -0.026179
+v -0.026014 0.013232 -0.025998
+v -0.026014 0.013002 -0.025998
+v -0.026103 0.013232 -0.026041
+v -0.026103 0.013002 -0.026041
+v -0.026188 0.013232 -0.026092
+v -0.026188 0.013002 -0.026092
+v -0.026269 0.013232 -0.026153
+v -0.026269 0.013002 -0.026153
+v -0.026356 0.013232 -0.026231
+v -0.026356 0.013002 -0.026231
+v -0.026430 0.013232 -0.026310
+v -0.026430 0.013002 -0.026310
+v -0.026509 0.013232 -0.026409
+v -0.026509 0.013002 -0.026409
+v -0.026569 0.013232 -0.026498
+v -0.026569 0.013002 -0.026498
+v -0.026611 0.013232 -0.026576
+v -0.026611 0.013002 -0.026576
+v -0.026645 0.013232 -0.026654
+v -0.026645 0.013002 -0.026654
+v -0.026666 0.013232 -0.026721
+v -0.026666 0.013002 -0.026721
+v -0.026681 0.013232 -0.026787
+v -0.026681 0.013002 -0.026787
+v -0.026689 0.013232 -0.026853
+v -0.026689 0.013002 -0.026853
+v -0.026691 0.013232 -0.026909
+v -0.026691 0.013002 -0.026909
+v -0.026688 0.013232 -0.026964
+v -0.026688 0.013002 -0.026964
+v -0.026679 0.013232 -0.027029
+v -0.026679 0.013002 -0.027029
+v -0.026664 0.013232 -0.027091
+v -0.026664 0.013002 -0.027091
+v -0.026647 0.013232 -0.027140
+v -0.026647 0.013002 -0.027140
+v -0.026626 0.013232 -0.027186
+v -0.026626 0.013002 -0.027186
+v -0.026601 0.013232 -0.027231
+v -0.026601 0.013002 -0.027231
+v -0.026571 0.013232 -0.027274
+v -0.026571 0.013002 -0.027274
+v -0.026530 0.013232 -0.027322
+v -0.026530 0.013002 -0.027322
+v -0.026484 0.013232 -0.027368
+v -0.026484 0.013002 -0.027368
+v -0.026431 0.013232 -0.027410
+v -0.026431 0.013002 -0.027410
+v -0.026344 0.013232 -0.027467
+v -0.026344 0.013002 -0.027467
+v -0.026273 0.013232 -0.027503
+v -0.026273 0.013002 -0.027503
+v -0.026203 0.013232 -0.027531
+v -0.026203 0.013002 -0.027531
+v -0.026132 0.013232 -0.027550
+v -0.026132 0.013002 -0.027550
+v -0.026079 0.013232 -0.027558
+v -0.026079 0.013002 -0.027558
+v -0.026026 0.013232 -0.027562
+v -0.026026 0.013002 -0.027562
+v -0.025973 0.013232 -0.027561
+v -0.025973 0.013002 -0.027561
+v -0.025920 0.013232 -0.027555
+v -0.025920 0.013002 -0.027555
+v -0.025848 0.013232 -0.027539
+v -0.025848 0.013002 -0.027539
+v -0.025777 0.013232 -0.027515
+v -0.025777 0.013002 -0.027515
+v -0.025705 0.013232 -0.027482
+v -0.025705 0.013002 -0.027482
+v -0.025633 0.013232 -0.027441
+v -0.025633 0.013002 -0.027441
+v -0.025543 0.013232 -0.027377
+v -0.025543 0.013002 -0.027377
+v -0.025537 0.013232 -0.027381
+v -0.025537 0.013002 -0.027381
+v -0.025581 0.013232 -0.027463
+v -0.025581 0.013002 -0.027463
+v -0.025611 0.013232 -0.027534
+v -0.025611 0.013002 -0.027534
+v -0.025634 0.013232 -0.027605
+v -0.025634 0.013002 -0.027605
+v -0.025649 0.013232 -0.027675
+v -0.025649 0.013002 -0.027675
+v -0.025656 0.013232 -0.027735
+v -0.025656 0.013002 -0.027735
+v -0.025657 0.013232 -0.027795
+v -0.025657 0.013002 -0.027795
+v -0.025655 0.013232 -0.027835
+v -0.025655 0.013002 -0.027835
+v -0.025649 0.013232 -0.027885
+v -0.025649 0.013002 -0.027885
+v -0.025636 0.013232 -0.027943
+v -0.025636 0.013002 -0.027943
+v -0.025619 0.013232 -0.027998
+v -0.025619 0.013002 -0.027998
+v -0.025596 0.013232 -0.028051
+v -0.025596 0.013002 -0.028051
+v -0.025569 0.013232 -0.028101
+v -0.025569 0.013002 -0.028101
+v -0.025536 0.013232 -0.028149
+v -0.025536 0.013002 -0.028149
+v -0.025499 0.013232 -0.028194
+v -0.025499 0.013002 -0.028194
+v -0.025456 0.013232 -0.028237
+v -0.025456 0.013002 -0.028237
+v -0.025425 0.013232 -0.028264
+v -0.025425 0.013002 -0.028264
+v -0.025391 0.013232 -0.028290
+v -0.025391 0.013002 -0.028290
+v -0.025319 0.013232 -0.028338
+v -0.025319 0.013002 -0.028338
+v -0.025246 0.013232 -0.028378
+v -0.025246 0.013002 -0.028378
+v -0.025172 0.013232 -0.028408
+v -0.025172 0.013002 -0.028408
+v -0.025108 0.013232 -0.028428
+v -0.025108 0.013002 -0.028428
+v -0.025045 0.013232 -0.028441
+v -0.025045 0.013002 -0.028441
+v -0.024980 0.013232 -0.028448
+v -0.024980 0.013002 -0.028448
+v -0.024914 0.013232 -0.028449
+v -0.024914 0.013002 -0.028449
+v -0.024848 0.013232 -0.028445
+v -0.024848 0.013002 -0.028445
+v -0.024808 0.013232 -0.028439
+v -0.024808 0.013002 -0.028439
+v -0.024754 0.013232 -0.028428
+v -0.024754 0.013002 -0.028428
+v -0.024674 0.013232 -0.028405
+v -0.024674 0.013002 -0.028405
+v -0.024596 0.013232 -0.028374
+v -0.024596 0.013002 -0.028374
+v -0.024520 0.013232 -0.028336
+v -0.024520 0.013002 -0.028336
+v -0.024447 0.013232 -0.028289
+v -0.024447 0.013002 -0.028289
+v -0.024365 0.013232 -0.028225
+v -0.024365 0.013002 -0.028225
+v -0.024286 0.013232 -0.028151
+v -0.024286 0.013002 -0.028151
+v -0.024220 0.013232 -0.028078
+v -0.024220 0.013002 -0.028078
+v -0.024158 0.013232 -0.027998
+v -0.024158 0.013002 -0.027998
+v -0.024066 0.013232 -0.027861
+v -0.024066 0.013002 -0.027861
+v -0.024007 0.013232 -0.027753
+v -0.024007 0.013002 -0.027753
+v -0.023966 0.013232 -0.027658
+v -0.023966 0.013002 -0.027658
+v -0.023936 0.013232 -0.027566
+v -0.023936 0.013002 -0.027566
+v -0.023918 0.013232 -0.027487
+v -0.023918 0.013002 -0.027487
+v -0.023913 0.013232 -0.027453
+v -0.023913 0.013002 -0.027453
+v -0.025436 0.013232 -0.028988
+v -0.025436 0.013002 -0.028988
+v -0.025167 0.013002 -0.029185
+v -0.025167 0.013232 -0.029185
+v -0.025448 0.013232 -0.029105
+v -0.025448 0.013002 -0.029105
+v -0.025469 0.013232 -0.029216
+v -0.025469 0.013002 -0.029216
+v -0.025497 0.013232 -0.029309
+v -0.025497 0.013002 -0.029309
+v -0.025533 0.013232 -0.029397
+v -0.025533 0.013002 -0.029397
+v -0.025576 0.013232 -0.029480
+v -0.025576 0.013002 -0.029480
+v -0.025600 0.013232 -0.029519
+v -0.025600 0.013002 -0.029519
+v -0.025627 0.013232 -0.029557
+v -0.025627 0.013002 -0.029557
+v -0.025684 0.013232 -0.029628
+v -0.025684 0.013002 -0.029628
+v -0.025736 0.013232 -0.029681
+v -0.025736 0.013002 -0.029681
+v -0.025791 0.013232 -0.029727
+v -0.025791 0.013002 -0.029727
+v -0.025840 0.013232 -0.029761
+v -0.025840 0.013002 -0.029761
+v -0.025891 0.013232 -0.029789
+v -0.025891 0.013002 -0.029789
+v -0.025944 0.013232 -0.029812
+v -0.025944 0.013002 -0.029812
+v -0.025990 0.013232 -0.029827
+v -0.025990 0.013002 -0.029827
+v -0.026037 0.013232 -0.029838
+v -0.026037 0.013002 -0.029838
+v -0.026094 0.013232 -0.029846
+v -0.026094 0.013002 -0.029846
+v -0.026141 0.013232 -0.029849
+v -0.026141 0.013002 -0.029849
+v -0.026187 0.013232 -0.029847
+v -0.026187 0.013002 -0.029847
+v -0.026233 0.013232 -0.029841
+v -0.026233 0.013002 -0.029841
+v -0.026279 0.013232 -0.029831
+v -0.026279 0.013002 -0.029831
+v -0.026334 0.013232 -0.029813
+v -0.026334 0.013002 -0.029813
+v -0.026388 0.013232 -0.029789
+v -0.026388 0.013002 -0.029789
+v -0.026441 0.013232 -0.029760
+v -0.026441 0.013002 -0.029760
+v -0.026494 0.013232 -0.029724
+v -0.026494 0.013002 -0.029724
+v -0.026551 0.013232 -0.029678
+v -0.026551 0.013002 -0.029678
+v -0.026593 0.013232 -0.029636
+v -0.026593 0.013002 -0.029636
+v -0.026630 0.013232 -0.029592
+v -0.026630 0.013002 -0.029592
+v -0.026656 0.013232 -0.029553
+v -0.026656 0.013002 -0.029553
+v -0.026678 0.013232 -0.029513
+v -0.026678 0.013002 -0.029513
+v -0.026696 0.013232 -0.029472
+v -0.026696 0.013002 -0.029472
+v -0.026710 0.013232 -0.029429
+v -0.026710 0.013002 -0.029429
+v -0.026720 0.013232 -0.029384
+v -0.026720 0.013002 -0.029384
+v -0.026724 0.013232 -0.029357
+v -0.026724 0.013002 -0.029357
+v -0.026726 0.013232 -0.029329
+v -0.026726 0.013002 -0.029329
+v -0.026727 0.013232 -0.029281
+v -0.026727 0.013002 -0.029281
+v -0.026722 0.013232 -0.029223
+v -0.026722 0.013002 -0.029223
+v -0.026711 0.013232 -0.029164
+v -0.026711 0.013002 -0.029164
+v -0.026694 0.013232 -0.029104
+v -0.026694 0.013002 -0.029104
+v -0.026666 0.013232 -0.029032
+v -0.026666 0.013002 -0.029032
+v -0.026624 0.013232 -0.028948
+v -0.026624 0.013002 -0.028948
+v -0.026586 0.013232 -0.028884
+v -0.026586 0.013002 -0.028884
+v -0.026541 0.013232 -0.028819
+v -0.026541 0.013002 -0.028819
+v -0.026268 0.013232 -0.028485
+v -0.026268 0.013002 -0.028485
+v -0.027378 0.013232 -0.027672
+v -0.027378 0.013002 -0.027672
+v -0.028163 0.013232 -0.028744
+v -0.028163 0.013002 -0.028744
+v -0.027934 0.013232 -0.028911
+v -0.027934 0.013002 -0.028911
+v -0.027325 0.013232 -0.028079
+v -0.027325 0.013002 -0.028079
+v -0.026676 0.013232 -0.028554
+v -0.026676 0.013002 -0.028554
+v -0.026820 0.013232 -0.028738
+v -0.026820 0.013002 -0.028738
+v -0.026904 0.013232 -0.028865
+v -0.026904 0.013002 -0.028865
+v -0.026956 0.013232 -0.028962
+v -0.026956 0.013002 -0.028962
+v -0.026997 0.013232 -0.029058
+v -0.026997 0.013002 -0.029058
+v -0.027022 0.013232 -0.029140
+v -0.027022 0.013002 -0.029140
+v -0.027039 0.013232 -0.029220
+v -0.027039 0.013002 -0.029220
+v -0.027048 0.013232 -0.029300
+v -0.027048 0.013002 -0.029300
+v -0.027049 0.013232 -0.029353
+v -0.027049 0.013002 -0.029353
+v -0.027044 0.013232 -0.029418
+v -0.027044 0.013002 -0.029418
+v -0.027035 0.013232 -0.029482
+v -0.027035 0.013002 -0.029482
+v -0.027019 0.013232 -0.029544
+v -0.027019 0.013002 -0.029544
+v -0.026998 0.013232 -0.029604
+v -0.026998 0.013002 -0.029604
+v -0.026972 0.013232 -0.029663
+v -0.026972 0.013002 -0.029663
+v -0.026940 0.013232 -0.029719
+v -0.026940 0.013002 -0.029719
+v -0.026895 0.013232 -0.029784
+v -0.026895 0.013002 -0.029784
+v -0.026841 0.013232 -0.029846
+v -0.026841 0.013002 -0.029846
+v -0.026780 0.013232 -0.029905
+v -0.026780 0.013002 -0.029905
+v -0.026698 0.013232 -0.029971
+v -0.026698 0.013002 -0.029971
+v -0.026608 0.013232 -0.030031
+v -0.026608 0.013002 -0.030031
+v -0.026529 0.013232 -0.030073
+v -0.026529 0.013002 -0.030073
+v -0.026450 0.013232 -0.030106
+v -0.026450 0.013002 -0.030106
+v -0.026370 0.013232 -0.030131
+v -0.026370 0.013002 -0.030131
+v -0.026303 0.013232 -0.030145
+v -0.026303 0.013002 -0.030145
+v -0.026235 0.013232 -0.030152
+v -0.026235 0.013002 -0.030152
+v -0.026166 0.013232 -0.030154
+v -0.026166 0.013002 -0.030154
+v -0.026097 0.013232 -0.030150
+v -0.026097 0.013002 -0.030150
+v -0.026028 0.013232 -0.030140
+v -0.026028 0.013002 -0.030140
+v -0.025959 0.013232 -0.030124
+v -0.025959 0.013002 -0.030124
+v -0.025878 0.013232 -0.030097
+v -0.025878 0.013002 -0.030097
+v -0.025800 0.013232 -0.030061
+v -0.025800 0.013002 -0.030061
+v -0.025724 0.013232 -0.030017
+v -0.025724 0.013002 -0.030017
+v -0.025650 0.013232 -0.029964
+v -0.025650 0.013002 -0.029964
+v -0.025568 0.013232 -0.029891
+v -0.025568 0.013002 -0.029891
+v -0.025488 0.013232 -0.029806
+v -0.025488 0.013002 -0.029806
+v -0.025412 0.013232 -0.029710
+v -0.025412 0.013002 -0.029710
+v -0.025309 0.013232 -0.029556
+v -0.025309 0.013002 -0.029556
+v -0.025248 0.013232 -0.029446
+v -0.025248 0.013002 -0.029446
+v -0.025208 0.013232 -0.029353
+v -0.025208 0.013002 -0.029353
+v -0.025186 0.013232 -0.029285
+v -0.025186 0.013002 -0.029285
+v -0.025175 0.013232 -0.029239
+v -0.025175 0.013002 -0.029239
+v -0.020762 0.013011 -0.017321
+v -0.020659 0.013011 -0.017522
+v -0.015397 0.013011 -0.017399
+v -0.015401 0.013011 -0.017197
+v -0.020659 0.013214 -0.017522
+v -0.015397 0.013214 -0.017399
+v -0.020762 0.013214 -0.017321
+v -0.015401 0.013214 -0.017197
+v -0.023361 0.013011 -0.020749
+v -0.023199 0.013011 -0.020872
+v -0.023199 0.013214 -0.020872
+v -0.023361 0.013214 -0.020749
+v -0.015397 0.013011 -0.017399
+v -0.015397 0.013214 -0.017399
+v -0.015401 0.013214 -0.017197
+v -0.015401 0.013011 -0.017197
+v -0.023199 0.013214 -0.020872
+v -0.023199 0.013011 -0.020872
+v -0.023361 0.013011 -0.020749
+v -0.023361 0.013214 -0.020749
+v -0.018111 0.013232 -0.019574
+v -0.018111 0.013002 -0.019574
+v -0.019088 0.013002 -0.020908
+v -0.019088 0.013232 -0.020908
+v -0.018336 0.013232 -0.019409
+v -0.018336 0.013002 -0.019409
+v -0.019456 0.013232 -0.019580
+v -0.019456 0.013002 -0.019580
+v -0.019877 0.013232 -0.019629
+v -0.019877 0.013002 -0.019629
+v -0.020010 0.013232 -0.019634
+v -0.020010 0.013002 -0.019634
+v -0.020126 0.013232 -0.019632
+v -0.020126 0.013002 -0.019632
+v -0.020211 0.013232 -0.019623
+v -0.020211 0.013002 -0.019623
+v -0.020284 0.013232 -0.019608
+v -0.020284 0.013002 -0.019608
+v -0.020354 0.013232 -0.019587
+v -0.020354 0.013002 -0.019587
+v -0.020421 0.013232 -0.019559
+v -0.020421 0.013002 -0.019559
+v -0.020485 0.013232 -0.019526
+v -0.020485 0.013002 -0.019526
+v -0.020516 0.013232 -0.019507
+v -0.020516 0.013002 -0.019507
+v -0.020554 0.013232 -0.019480
+v -0.020554 0.013002 -0.019480
+v -0.020611 0.013232 -0.019434
+v -0.020611 0.013002 -0.019434
+v -0.020654 0.013232 -0.019392
+v -0.020654 0.013002 -0.019392
+v -0.020686 0.013232 -0.019355
+v -0.020686 0.013002 -0.019355
+v -0.020712 0.013232 -0.019315
+v -0.020712 0.013002 -0.019315
+v -0.020731 0.013232 -0.019282
+v -0.020731 0.013002 -0.019282
+v -0.020746 0.013232 -0.019248
+v -0.020746 0.013002 -0.019248
+v -0.020758 0.013232 -0.019212
+v -0.020758 0.013002 -0.019212
+v -0.020765 0.013232 -0.019183
+v -0.020765 0.013002 -0.019183
+v -0.020770 0.013232 -0.019154
+v -0.020770 0.013002 -0.019154
+v -0.020774 0.013232 -0.019116
+v -0.020774 0.013002 -0.019116
+v -0.020773 0.013232 -0.019078
+v -0.020773 0.013002 -0.019078
+v -0.020770 0.013232 -0.019040
+v -0.020770 0.013002 -0.019040
+v -0.020761 0.013232 -0.018994
+v -0.020761 0.013002 -0.018994
+v -0.020747 0.013232 -0.018948
+v -0.020747 0.013002 -0.018948
+v -0.020728 0.013232 -0.018901
+v -0.020728 0.013002 -0.018901
+v -0.020699 0.013232 -0.018845
+v -0.020699 0.013002 -0.018845
+v -0.020675 0.013232 -0.018805
+v -0.020675 0.013002 -0.018805
+v -0.020647 0.013232 -0.018765
+v -0.020647 0.013002 -0.018765
+v -0.020592 0.013232 -0.018697
+v -0.020592 0.013002 -0.018697
+v -0.020529 0.013232 -0.018635
+v -0.020529 0.013002 -0.018635
+v -0.020460 0.013232 -0.018579
+v -0.020460 0.013002 -0.018579
+v -0.020385 0.013232 -0.018529
+v -0.020385 0.013002 -0.018529
+v -0.020302 0.013232 -0.018485
+v -0.020302 0.013002 -0.018485
+v -0.020197 0.013232 -0.018441
+v -0.020197 0.013002 -0.018441
+v -0.020100 0.013232 -0.018411
+v -0.020100 0.013002 -0.018411
+v -0.019978 0.013232 -0.018382
+v -0.019978 0.013002 -0.018382
+v -0.020243 0.013232 -0.018188
+v -0.020243 0.013002 -0.018188
+v -0.020350 0.013232 -0.018223
+v -0.020350 0.013002 -0.018223
+v -0.020452 0.013232 -0.018268
+v -0.020452 0.013002 -0.018268
+v -0.020548 0.013232 -0.018322
+v -0.020548 0.013002 -0.018322
+v -0.020639 0.013232 -0.018385
+v -0.020639 0.013002 -0.018385
+v -0.020724 0.013232 -0.018458
+v -0.020724 0.013002 -0.018458
+v -0.020803 0.013232 -0.018539
+v -0.020803 0.013002 -0.018539
+v -0.020888 0.013232 -0.018643
+v -0.020888 0.013002 -0.018643
+v -0.020951 0.013232 -0.018738
+v -0.020951 0.013002 -0.018738
+v -0.020995 0.013232 -0.018820
+v -0.020995 0.013002 -0.018820
+v -0.021030 0.013232 -0.018902
+v -0.021030 0.013002 -0.018902
+v -0.021051 0.013232 -0.018972
+v -0.021051 0.013002 -0.018972
+v -0.021066 0.013232 -0.019042
+v -0.021066 0.013002 -0.019042
+v -0.021073 0.013232 -0.019112
+v -0.021073 0.013002 -0.019112
+v -0.021073 0.013232 -0.019170
+v -0.021073 0.013002 -0.019170
+v -0.021069 0.013232 -0.019227
+v -0.021069 0.013002 -0.019227
+v -0.021057 0.013232 -0.019295
+v -0.021057 0.013002 -0.019295
+v -0.021041 0.013232 -0.019349
+v -0.021041 0.013002 -0.019349
+v -0.021022 0.013232 -0.019402
+v -0.021022 0.013002 -0.019402
+v -0.020997 0.013232 -0.019453
+v -0.020997 0.013002 -0.019453
+v -0.020961 0.013232 -0.019511
+v -0.020961 0.013002 -0.019511
+v -0.020919 0.013232 -0.019568
+v -0.020919 0.013002 -0.019568
+v -0.020870 0.013232 -0.019621
+v -0.020870 0.013002 -0.019621
+v -0.020824 0.013232 -0.019664
+v -0.020824 0.013002 -0.019664
+v -0.020762 0.013232 -0.019712
+v -0.020762 0.013002 -0.019712
+v -0.020692 0.013232 -0.019759
+v -0.020692 0.013002 -0.019759
+v -0.020618 0.013232 -0.019800
+v -0.020618 0.013002 -0.019800
+v -0.020530 0.013232 -0.019837
+v -0.020530 0.013002 -0.019837
+v -0.020437 0.013232 -0.019867
+v -0.020437 0.013002 -0.019867
+v -0.020351 0.013232 -0.019887
+v -0.020351 0.013002 -0.019887
+v -0.020260 0.013232 -0.019900
+v -0.020260 0.013002 -0.019900
+v -0.020122 0.013232 -0.019908
+v -0.020122 0.013002 -0.019908
+v -0.019829 0.013232 -0.019896
+v -0.019829 0.013002 -0.019896
+v -0.019447 0.013232 -0.019848
+v -0.019447 0.013002 -0.019848
+v -0.018568 0.013232 -0.019706
+v -0.018568 0.013002 -0.019706
+v -0.018562 0.013232 -0.019711
+v -0.018562 0.013002 -0.019711
+v -0.019316 0.013232 -0.020741
+v -0.019316 0.013002 -0.020741
+v -0.019759 0.013232 -0.021147
+v -0.019759 0.013002 -0.021147
+v -0.019490 0.013002 -0.021344
+v -0.019490 0.013232 -0.021344
+v -0.019771 0.013232 -0.021264
+v -0.019771 0.013002 -0.021264
+v -0.019793 0.013232 -0.021375
+v -0.019793 0.013002 -0.021375
+v -0.019820 0.013232 -0.021468
+v -0.019820 0.013002 -0.021468
+v -0.019856 0.013232 -0.021556
+v -0.019856 0.013002 -0.021556
+v -0.019899 0.013232 -0.021639
+v -0.019899 0.013002 -0.021639
+v -0.019924 0.013232 -0.021678
+v -0.019924 0.013002 -0.021678
+v -0.019950 0.013232 -0.021716
+v -0.019950 0.013002 -0.021716
+v -0.020007 0.013232 -0.021787
+v -0.020007 0.013002 -0.021787
+v -0.020059 0.013232 -0.021840
+v -0.020059 0.013002 -0.021840
+v -0.020114 0.013232 -0.021886
+v -0.020114 0.013002 -0.021886
+v -0.020163 0.013232 -0.021920
+v -0.020163 0.013002 -0.021920
+v -0.020214 0.013232 -0.021948
+v -0.020214 0.013002 -0.021948
+v -0.020267 0.013232 -0.021971
+v -0.020267 0.013002 -0.021971
+v -0.020313 0.013232 -0.021986
+v -0.020313 0.013002 -0.021986
+v -0.020360 0.013232 -0.021997
+v -0.020360 0.013002 -0.021997
+v -0.020417 0.013232 -0.022006
+v -0.020417 0.013002 -0.022006
+v -0.020464 0.013232 -0.022008
+v -0.020464 0.013002 -0.022008
+v -0.020510 0.013232 -0.022006
+v -0.020510 0.013002 -0.022006
+v -0.020557 0.013232 -0.022000
+v -0.020557 0.013002 -0.022000
+v -0.020602 0.013232 -0.021990
+v -0.020602 0.013002 -0.021990
+v -0.020657 0.013232 -0.021972
+v -0.020657 0.013002 -0.021972
+v -0.020711 0.013232 -0.021948
+v -0.020711 0.013002 -0.021948
+v -0.020764 0.013232 -0.021919
+v -0.020764 0.013002 -0.021919
+v -0.020817 0.013232 -0.021883
+v -0.020817 0.013002 -0.021883
+v -0.020874 0.013232 -0.021837
+v -0.020874 0.013002 -0.021837
+v -0.020917 0.013232 -0.021795
+v -0.020917 0.013002 -0.021795
+v -0.020953 0.013232 -0.021751
+v -0.020953 0.013002 -0.021751
+v -0.020979 0.013232 -0.021712
+v -0.020979 0.013002 -0.021712
+v -0.021001 0.013232 -0.021672
+v -0.021001 0.013002 -0.021672
+v -0.021019 0.013232 -0.021631
+v -0.021019 0.013002 -0.021631
+v -0.021033 0.013232 -0.021588
+v -0.021033 0.013002 -0.021588
+v -0.021043 0.013232 -0.021543
+v -0.021043 0.013002 -0.021543
+v -0.021047 0.013232 -0.021516
+v -0.021047 0.013002 -0.021516
+v -0.021049 0.013232 -0.021488
+v -0.021049 0.013002 -0.021488
+v -0.021050 0.013232 -0.021440
+v -0.021050 0.013002 -0.021440
+v -0.021045 0.013232 -0.021382
+v -0.021045 0.013002 -0.021382
+v -0.021034 0.013232 -0.021323
+v -0.021034 0.013002 -0.021323
+v -0.021017 0.013232 -0.021263
+v -0.021017 0.013002 -0.021263
+v -0.020990 0.013232 -0.021191
+v -0.020990 0.013002 -0.021191
+v -0.020948 0.013232 -0.021107
+v -0.020948 0.013002 -0.021107
+v -0.020909 0.013232 -0.021043
+v -0.020909 0.013002 -0.021043
+v -0.020864 0.013232 -0.020978
+v -0.020864 0.013002 -0.020978
+v -0.020591 0.013232 -0.020645
+v -0.020591 0.013002 -0.020645
+v -0.021701 0.013232 -0.019831
+v -0.021701 0.013002 -0.019831
+v -0.022486 0.013232 -0.020903
+v -0.022486 0.013002 -0.020903
+v -0.022257 0.013232 -0.021070
+v -0.022257 0.013002 -0.021070
+v -0.021648 0.013232 -0.020238
+v -0.021648 0.013002 -0.020238
+v -0.020999 0.013232 -0.020713
+v -0.020999 0.013002 -0.020713
+v -0.021143 0.013232 -0.020897
+v -0.021143 0.013002 -0.020897
+v -0.021227 0.013232 -0.021024
+v -0.021227 0.013002 -0.021024
+v -0.021280 0.013232 -0.021121
+v -0.021280 0.013002 -0.021121
+v -0.021320 0.013232 -0.021217
+v -0.021320 0.013002 -0.021217
+v -0.021346 0.013232 -0.021299
+v -0.021346 0.013002 -0.021299
+v -0.021363 0.013232 -0.021379
+v -0.021363 0.013002 -0.021379
+v -0.021371 0.013232 -0.021459
+v -0.021371 0.013002 -0.021459
+v -0.021372 0.013232 -0.021512
+v -0.021372 0.013002 -0.021512
+v -0.021368 0.013232 -0.021577
+v -0.021368 0.013002 -0.021577
+v -0.021358 0.013232 -0.021641
+v -0.021358 0.013002 -0.021641
+v -0.021342 0.013232 -0.021703
+v -0.021342 0.013002 -0.021703
+v -0.021321 0.013232 -0.021764
+v -0.021321 0.013002 -0.021764
+v -0.021295 0.013232 -0.021822
+v -0.021295 0.013002 -0.021822
+v -0.021263 0.013232 -0.021878
+v -0.021263 0.013002 -0.021878
+v -0.021218 0.013232 -0.021943
+v -0.021218 0.013002 -0.021943
+v -0.021164 0.013232 -0.022005
+v -0.021164 0.013002 -0.022005
+v -0.021103 0.013232 -0.022064
+v -0.021103 0.013002 -0.022064
+v -0.021021 0.013232 -0.022130
+v -0.021021 0.013002 -0.022130
+v -0.020931 0.013232 -0.022190
+v -0.020931 0.013002 -0.022190
+v -0.020853 0.013232 -0.022232
+v -0.020853 0.013002 -0.022232
+v -0.020773 0.013232 -0.022265
+v -0.020773 0.013002 -0.022265
+v -0.020693 0.013232 -0.022290
+v -0.020693 0.013002 -0.022290
+v -0.020626 0.013232 -0.022304
+v -0.020626 0.013002 -0.022304
+v -0.020558 0.013232 -0.022312
+v -0.020558 0.013002 -0.022312
+v -0.020490 0.013232 -0.022313
+v -0.020490 0.013002 -0.022313
+v -0.020421 0.013232 -0.022309
+v -0.020421 0.013002 -0.022309
+v -0.020351 0.013232 -0.022299
+v -0.020351 0.013002 -0.022299
+v -0.020282 0.013232 -0.022283
+v -0.020282 0.013002 -0.022283
+v -0.020201 0.013232 -0.022256
+v -0.020201 0.013002 -0.022256
+v -0.020123 0.013232 -0.022220
+v -0.020123 0.013002 -0.022220
+v -0.020047 0.013232 -0.022176
+v -0.020047 0.013002 -0.022176
+v -0.019974 0.013232 -0.022123
+v -0.019974 0.013002 -0.022123
+v -0.019891 0.013232 -0.022050
+v -0.019891 0.013002 -0.022050
+v -0.019811 0.013232 -0.021965
+v -0.019811 0.013002 -0.021965
+v -0.019735 0.013232 -0.021869
+v -0.019735 0.013002 -0.021869
+v -0.019632 0.013232 -0.021716
+v -0.019632 0.013002 -0.021716
+v -0.019571 0.013232 -0.021605
+v -0.019571 0.013002 -0.021605
+v -0.019531 0.013232 -0.021513
+v -0.019531 0.013002 -0.021513
+v -0.019509 0.013232 -0.021444
+v -0.019509 0.013002 -0.021444
+v -0.019498 0.013232 -0.021398
+v -0.019498 0.013002 -0.021398
+v -0.014943 0.013011 -0.009212
+v -0.014841 0.013011 -0.009412
+v -0.009578 0.013011 -0.009290
+v -0.009583 0.013011 -0.009087
+v -0.014841 0.013214 -0.009412
+v -0.009578 0.013214 -0.009290
+v -0.014943 0.013214 -0.009212
+v -0.009583 0.013214 -0.009087
+v -0.017542 0.013011 -0.012640
+v -0.017381 0.013011 -0.012762
+v -0.017381 0.013214 -0.012762
+v -0.017542 0.013214 -0.012640
+v -0.009578 0.013011 -0.009290
+v -0.009578 0.013214 -0.009290
+v -0.009583 0.013214 -0.009087
+v -0.009583 0.013011 -0.009087
+v -0.017381 0.013214 -0.012762
+v -0.017381 0.013011 -0.012762
+v -0.017542 0.013011 -0.012640
+v -0.017542 0.013214 -0.012640
+v -0.012342 0.013232 -0.011532
+v -0.012342 0.012840 -0.011532
+v -0.013259 0.012840 -0.012784
+v -0.013259 0.013232 -0.012784
+v -0.012565 0.013232 -0.011369
+v -0.012565 0.012840 -0.011369
+v -0.012931 0.013232 -0.011870
+v -0.012931 0.012840 -0.011870
+v -0.014651 0.013232 -0.010610
+v -0.014651 0.012840 -0.010610
+v -0.014123 0.013232 -0.010209
+v -0.014123 0.012840 -0.010209
+v -0.014363 0.013232 -0.010033
+v -0.014363 0.012840 -0.010033
+v -0.015158 0.013232 -0.010628
+v -0.015158 0.012840 -0.010628
+v -0.013116 0.013232 -0.012123
+v -0.013116 0.012840 -0.012123
+v -0.013482 0.013232 -0.012622
+v -0.013482 0.012840 -0.012622
+v -0.013900 0.013232 -0.012982
+v -0.013900 0.012840 -0.012982
+v -0.013631 0.012840 -0.013179
+v -0.013631 0.013232 -0.013179
+v -0.013912 0.013232 -0.013099
+v -0.013912 0.012840 -0.013099
+v -0.013934 0.013232 -0.013210
+v -0.013934 0.012840 -0.013210
+v -0.013961 0.013232 -0.013303
+v -0.013961 0.012840 -0.013303
+v -0.013997 0.013232 -0.013391
+v -0.013997 0.012840 -0.013391
+v -0.014040 0.013232 -0.013473
+v -0.014040 0.012840 -0.013473
+v -0.014065 0.013232 -0.013513
+v -0.014065 0.012840 -0.013513
+v -0.014091 0.013232 -0.013551
+v -0.014091 0.012840 -0.013551
+v -0.014148 0.013232 -0.013621
+v -0.014148 0.012840 -0.013621
+v -0.014200 0.013232 -0.013675
+v -0.014200 0.012840 -0.013675
+v -0.014255 0.013232 -0.013721
+v -0.014255 0.012840 -0.013721
+v -0.014304 0.013232 -0.013755
+v -0.014304 0.012840 -0.013755
+v -0.014355 0.013232 -0.013783
+v -0.014355 0.012840 -0.013783
+v -0.014408 0.013232 -0.013806
+v -0.014408 0.012840 -0.013806
+v -0.014454 0.013232 -0.013821
+v -0.014454 0.012840 -0.013821
+v -0.014501 0.013232 -0.013832
+v -0.014501 0.012840 -0.013832
+v -0.014558 0.013232 -0.013840
+v -0.014558 0.012840 -0.013840
+v -0.014605 0.013232 -0.013842
+v -0.014605 0.012840 -0.013842
+v -0.014651 0.013232 -0.013841
+v -0.014651 0.012840 -0.013841
+v -0.014698 0.013232 -0.013835
+v -0.014698 0.012840 -0.013835
+v -0.014743 0.013232 -0.013824
+v -0.014743 0.012840 -0.013824
+v -0.014798 0.013232 -0.013807
+v -0.014798 0.012840 -0.013807
+v -0.014852 0.013232 -0.013783
+v -0.014852 0.012840 -0.013783
+v -0.014905 0.013232 -0.013753
+v -0.014905 0.012840 -0.013753
+v -0.014958 0.013232 -0.013718
+v -0.014958 0.012840 -0.013718
+v -0.015015 0.013232 -0.013672
+v -0.015015 0.012840 -0.013672
+v -0.015058 0.013232 -0.013630
+v -0.015058 0.012840 -0.013630
+v -0.015094 0.013232 -0.013586
+v -0.015094 0.012840 -0.013586
+v -0.015120 0.013232 -0.013547
+v -0.015120 0.012840 -0.013547
+v -0.015142 0.013232 -0.013507
+v -0.015142 0.012840 -0.013507
+v -0.015160 0.013232 -0.013466
+v -0.015160 0.012840 -0.013466
+v -0.015174 0.013232 -0.013423
+v -0.015174 0.012840 -0.013423
+v -0.015184 0.013232 -0.013378
+v -0.015184 0.012840 -0.013378
+v -0.015188 0.013232 -0.013351
+v -0.015188 0.012840 -0.013351
+v -0.015190 0.013232 -0.013322
+v -0.015190 0.012840 -0.013322
+v -0.015191 0.013232 -0.013275
+v -0.015191 0.012840 -0.013275
+v -0.015186 0.013232 -0.013217
+v -0.015186 0.012840 -0.013217
+v -0.015175 0.013232 -0.013158
+v -0.015175 0.012840 -0.013158
+v -0.015158 0.013232 -0.013097
+v -0.015158 0.012840 -0.013097
+v -0.015131 0.013232 -0.013026
+v -0.015131 0.012840 -0.013026
+v -0.015089 0.013232 -0.012942
+v -0.015089 0.012840 -0.012942
+v -0.015050 0.013232 -0.012878
+v -0.015050 0.012840 -0.012878
+v -0.015005 0.013232 -0.012813
+v -0.015005 0.012840 -0.012813
+v -0.014732 0.013232 -0.012479
+v -0.014732 0.012840 -0.012479
+v -0.015842 0.013232 -0.011666
+v -0.015842 0.012840 -0.011666
+v -0.016627 0.013232 -0.012738
+v -0.016627 0.012840 -0.012738
+v -0.016398 0.013232 -0.012905
+v -0.016398 0.012840 -0.012905
+v -0.015789 0.013232 -0.012073
+v -0.015789 0.012840 -0.012073
+v -0.015140 0.013232 -0.012548
+v -0.015140 0.012840 -0.012548
+v -0.015284 0.013232 -0.012732
+v -0.015284 0.012840 -0.012732
+v -0.015368 0.013232 -0.012859
+v -0.015368 0.012840 -0.012859
+v -0.015421 0.013232 -0.012956
+v -0.015421 0.012840 -0.012956
+v -0.015461 0.013232 -0.013052
+v -0.015461 0.012840 -0.013052
+v -0.015487 0.013232 -0.013134
+v -0.015487 0.012840 -0.013134
+v -0.015504 0.013232 -0.013214
+v -0.015504 0.012840 -0.013214
+v -0.015512 0.013232 -0.013294
+v -0.015512 0.012840 -0.013294
+v -0.015513 0.013232 -0.013347
+v -0.015513 0.012840 -0.013347
+v -0.015509 0.013232 -0.013412
+v -0.015509 0.012840 -0.013412
+v -0.015499 0.013232 -0.013476
+v -0.015499 0.012840 -0.013476
+v -0.015483 0.013232 -0.013538
+v -0.015483 0.012840 -0.013538
+v -0.015462 0.013232 -0.013598
+v -0.015462 0.012840 -0.013598
+v -0.015436 0.013232 -0.013656
+v -0.015436 0.012840 -0.013656
+v -0.015404 0.013232 -0.013713
+v -0.015404 0.012840 -0.013713
+v -0.015359 0.013232 -0.013778
+v -0.015359 0.012840 -0.013778
+v -0.015305 0.013232 -0.013840
+v -0.015305 0.012840 -0.013840
+v -0.015244 0.013232 -0.013899
+v -0.015244 0.012840 -0.013899
+v -0.015162 0.013232 -0.013964
+v -0.015162 0.012840 -0.013964
+v -0.015072 0.013232 -0.014024
+v -0.015072 0.012840 -0.014024
+v -0.014994 0.013232 -0.014066
+v -0.014994 0.012840 -0.014066
+v -0.014914 0.013232 -0.014100
+v -0.014914 0.012840 -0.014100
+v -0.014834 0.013232 -0.014125
+v -0.014834 0.012840 -0.014125
+v -0.014767 0.013232 -0.014138
+v -0.014767 0.012840 -0.014138
+v -0.014699 0.013232 -0.014146
+v -0.014699 0.012840 -0.014146
+v -0.014631 0.013232 -0.014148
+v -0.014631 0.012840 -0.014148
+v -0.014562 0.013232 -0.014144
+v -0.014562 0.012840 -0.014144
+v -0.014492 0.013232 -0.014134
+v -0.014492 0.012840 -0.014134
+v -0.014423 0.013232 -0.014118
+v -0.014423 0.012840 -0.014118
+v -0.014342 0.013232 -0.014091
+v -0.014342 0.012840 -0.014091
+v -0.014264 0.013232 -0.014055
+v -0.014264 0.012840 -0.014055
+v -0.014188 0.013232 -0.014010
+v -0.014188 0.012840 -0.014010
+v -0.014115 0.013232 -0.013957
+v -0.014115 0.012840 -0.013957
+v -0.014032 0.013232 -0.013885
+v -0.014032 0.012840 -0.013885
+v -0.013952 0.013232 -0.013800
+v -0.013952 0.012840 -0.013800
+v -0.013876 0.013232 -0.013704
+v -0.013876 0.012840 -0.013704
+v -0.013773 0.013232 -0.013550
+v -0.013773 0.012840 -0.013550
+v -0.013712 0.013232 -0.013440
+v -0.013712 0.012840 -0.013440
+v -0.013672 0.013232 -0.013347
+v -0.013672 0.012840 -0.013347
+v -0.013650 0.013232 -0.013279
+v -0.013650 0.012840 -0.013279
+v -0.013639 0.013232 -0.013232
+v -0.013639 0.012840 -0.013232
+v -0.026458 -0.013229 -0.025189
+v -0.026356 -0.013229 -0.025390
+v -0.021093 -0.013229 -0.025268
+v -0.021098 -0.013229 -0.025065
+v -0.026356 -0.013026 -0.025390
+v -0.021093 -0.013026 -0.025268
+v -0.026458 -0.013026 -0.025189
+v -0.021098 -0.013026 -0.025065
+v -0.029057 -0.013229 -0.028617
+v -0.028896 -0.013229 -0.028740
+v -0.028896 -0.013026 -0.028740
+v -0.029057 -0.013026 -0.028617
+v -0.021093 -0.013229 -0.025268
+v -0.021093 -0.013026 -0.025268
+v -0.021098 -0.013026 -0.025065
+v -0.021098 -0.013229 -0.025065
+v -0.028896 -0.013026 -0.028740
+v -0.028896 -0.013229 -0.028740
+v -0.029057 -0.013229 -0.028617
+v -0.029057 -0.013026 -0.028617
+v -0.026377 -0.013135 -0.030206
+v -0.026377 -0.012905 -0.030206
+v -0.026104 -0.012905 -0.030406
+v -0.026104 -0.013135 -0.030406
+v -0.026283 -0.013135 -0.030175
+v -0.026283 -0.012905 -0.030175
+v -0.026194 -0.013135 -0.030137
+v -0.026194 -0.012905 -0.030137
+v -0.026111 -0.013135 -0.030092
+v -0.026111 -0.012905 -0.030092
+v -0.026033 -0.013135 -0.030039
+v -0.026033 -0.012905 -0.030039
+v -0.025960 -0.013135 -0.029979
+v -0.025960 -0.012905 -0.029979
+v -0.025893 -0.013135 -0.029911
+v -0.025893 -0.012905 -0.029911
+v -0.025857 -0.013135 -0.029869
+v -0.025857 -0.012905 -0.029869
+v -0.025823 -0.013135 -0.029824
+v -0.025823 -0.012905 -0.029824
+v -0.025768 -0.013135 -0.029741
+v -0.025768 -0.012905 -0.029741
+v -0.025729 -0.013135 -0.029668
+v -0.025729 -0.012905 -0.029668
+v -0.025702 -0.013135 -0.029604
+v -0.025702 -0.012905 -0.029604
+v -0.025685 -0.013135 -0.029550
+v -0.025685 -0.012905 -0.029550
+v -0.025674 -0.013135 -0.029495
+v -0.025674 -0.012905 -0.029495
+v -0.025668 -0.013135 -0.029441
+v -0.025668 -0.012905 -0.029441
+v -0.025666 -0.013135 -0.029406
+v -0.025666 -0.012905 -0.029406
+v -0.025668 -0.013135 -0.029370
+v -0.025668 -0.012905 -0.029370
+v -0.025674 -0.013135 -0.029317
+v -0.025674 -0.012905 -0.029317
+v -0.025683 -0.013135 -0.029275
+v -0.025683 -0.012905 -0.029275
+v -0.025696 -0.013135 -0.029235
+v -0.025696 -0.012905 -0.029235
+v -0.025712 -0.013135 -0.029196
+v -0.025712 -0.012905 -0.029196
+v -0.025732 -0.013135 -0.029159
+v -0.025732 -0.012905 -0.029159
+v -0.025755 -0.013135 -0.029124
+v -0.025755 -0.012905 -0.029124
+v -0.025783 -0.013135 -0.029090
+v -0.025783 -0.012905 -0.029090
+v -0.025820 -0.013135 -0.029051
+v -0.025820 -0.012905 -0.029051
+v -0.025848 -0.013135 -0.029027
+v -0.025848 -0.012905 -0.029027
+v -0.025878 -0.013135 -0.029003
+v -0.025878 -0.012905 -0.029003
+v -0.025930 -0.013135 -0.028968
+v -0.025930 -0.012905 -0.028968
+v -0.025983 -0.013135 -0.028940
+v -0.025983 -0.012905 -0.028940
+v -0.026035 -0.013135 -0.028919
+v -0.026035 -0.012905 -0.028919
+v -0.026088 -0.013135 -0.028904
+v -0.026088 -0.012905 -0.028904
+v -0.026140 -0.013135 -0.028897
+v -0.026140 -0.012905 -0.028897
+v -0.026193 -0.013135 -0.028895
+v -0.026193 -0.012905 -0.028895
+v -0.026246 -0.013135 -0.028901
+v -0.026246 -0.012905 -0.028901
+v -0.026299 -0.013135 -0.028913
+v -0.026299 -0.012905 -0.028913
+v -0.026352 -0.013135 -0.028932
+v -0.026352 -0.012905 -0.028932
+v -0.026405 -0.013135 -0.028958
+v -0.026405 -0.012905 -0.028958
+v -0.026459 -0.013135 -0.028991
+v -0.026459 -0.012905 -0.028991
+v -0.026530 -0.013135 -0.029044
+v -0.026530 -0.012905 -0.029044
+v -0.026601 -0.013135 -0.029110
+v -0.026601 -0.012905 -0.029110
+v -0.026673 -0.013135 -0.029188
+v -0.026673 -0.012905 -0.029188
+v -0.026763 -0.013135 -0.029301
+v -0.026763 -0.012905 -0.029301
+v -0.026898 -0.013135 -0.029485
+v -0.026898 -0.012905 -0.029485
+v -0.027113 -0.013135 -0.029328
+v -0.027113 -0.012905 -0.029328
+v -0.026985 -0.013135 -0.029154
+v -0.026985 -0.012905 -0.029154
+v -0.026900 -0.013135 -0.029026
+v -0.026900 -0.012905 -0.029026
+v -0.026847 -0.013135 -0.028924
+v -0.026847 -0.012905 -0.028924
+v -0.026817 -0.013135 -0.028847
+v -0.026817 -0.012905 -0.028847
+v -0.026801 -0.013135 -0.028791
+v -0.026801 -0.012905 -0.028791
+v -0.026790 -0.013135 -0.028737
+v -0.026790 -0.012905 -0.028737
+v -0.026786 -0.013135 -0.028685
+v -0.026786 -0.012905 -0.028685
+v -0.026787 -0.013135 -0.028634
+v -0.026787 -0.012905 -0.028634
+v -0.026794 -0.013135 -0.028586
+v -0.026794 -0.012905 -0.028586
+v -0.026807 -0.013135 -0.028539
+v -0.026807 -0.012905 -0.028539
+v -0.026826 -0.013135 -0.028494
+v -0.026826 -0.012905 -0.028494
+v -0.026851 -0.013135 -0.028450
+v -0.026851 -0.012905 -0.028450
+v -0.026881 -0.013135 -0.028409
+v -0.026881 -0.012905 -0.028409
+v -0.026917 -0.013135 -0.028369
+v -0.026917 -0.012905 -0.028369
+v -0.026945 -0.013135 -0.028344
+v -0.026945 -0.012905 -0.028344
+v -0.026991 -0.013135 -0.028307
+v -0.026991 -0.012905 -0.028307
+v -0.027051 -0.013135 -0.028267
+v -0.027051 -0.012905 -0.028267
+v -0.027110 -0.013135 -0.028236
+v -0.027110 -0.012905 -0.028236
+v -0.027153 -0.013135 -0.028219
+v -0.027153 -0.012905 -0.028219
+v -0.027196 -0.013135 -0.028207
+v -0.027196 -0.012905 -0.028207
+v -0.027239 -0.013135 -0.028200
+v -0.027239 -0.012905 -0.028200
+v -0.027281 -0.013135 -0.028197
+v -0.027281 -0.012905 -0.028197
+v -0.027323 -0.013135 -0.028200
+v -0.027323 -0.012905 -0.028200
+v -0.027364 -0.013135 -0.028208
+v -0.027364 -0.012905 -0.028208
+v -0.027404 -0.013135 -0.028220
+v -0.027404 -0.012905 -0.028220
+v -0.027444 -0.013135 -0.028238
+v -0.027444 -0.012905 -0.028238
+v -0.027484 -0.013135 -0.028261
+v -0.027484 -0.012905 -0.028261
+v -0.027523 -0.013135 -0.028289
+v -0.027523 -0.012905 -0.028289
+v -0.027574 -0.013135 -0.028333
+v -0.027574 -0.012905 -0.028333
+v -0.027625 -0.013135 -0.028387
+v -0.027625 -0.012905 -0.028387
+v -0.027674 -0.013135 -0.028449
+v -0.027674 -0.012905 -0.028449
+v -0.027718 -0.013135 -0.028516
+v -0.027718 -0.012905 -0.028516
+v -0.027756 -0.013135 -0.028587
+v -0.027756 -0.012905 -0.028587
+v -0.027786 -0.013135 -0.028661
+v -0.027786 -0.012905 -0.028661
+v -0.027809 -0.013135 -0.028739
+v -0.027809 -0.012905 -0.028739
+v -0.027826 -0.013135 -0.028820
+v -0.027826 -0.012905 -0.028820
+v -0.027835 -0.013135 -0.028905
+v -0.027835 -0.012905 -0.028905
+v -0.027837 -0.013135 -0.029006
+v -0.027837 -0.012905 -0.029006
+v -0.028084 -0.013135 -0.028825
+v -0.028084 -0.012905 -0.028825
+v -0.028070 -0.013135 -0.028728
+v -0.028070 -0.012905 -0.028728
+v -0.028047 -0.013135 -0.028631
+v -0.028047 -0.012905 -0.028631
+v -0.028014 -0.013135 -0.028535
+v -0.028014 -0.012905 -0.028535
+v -0.027966 -0.013135 -0.028428
+v -0.027966 -0.012905 -0.028428
+v -0.027913 -0.013135 -0.028334
+v -0.027913 -0.012905 -0.028334
+v -0.027842 -0.013135 -0.028229
+v -0.027842 -0.012905 -0.028229
+v -0.027775 -0.013135 -0.028145
+v -0.027775 -0.012905 -0.028145
+v -0.027714 -0.013135 -0.028081
+v -0.027714 -0.012905 -0.028081
+v -0.027649 -0.013135 -0.028026
+v -0.027649 -0.012905 -0.028026
+v -0.027592 -0.013135 -0.027985
+v -0.027592 -0.012905 -0.027985
+v -0.027533 -0.013135 -0.027951
+v -0.027533 -0.012905 -0.027951
+v -0.027472 -0.013135 -0.027923
+v -0.027472 -0.012905 -0.027923
+v -0.027420 -0.013135 -0.027905
+v -0.027420 -0.012905 -0.027905
+v -0.027367 -0.013135 -0.027890
+v -0.027367 -0.012905 -0.027890
+v -0.027302 -0.013135 -0.027880
+v -0.027302 -0.012905 -0.027880
+v -0.027239 -0.013135 -0.027875
+v -0.027239 -0.012905 -0.027875
+v -0.027187 -0.013135 -0.027877
+v -0.027187 -0.012905 -0.027877
+v -0.027136 -0.013135 -0.027883
+v -0.027136 -0.012905 -0.027883
+v -0.027086 -0.013135 -0.027893
+v -0.027086 -0.012905 -0.027893
+v -0.027036 -0.013135 -0.027909
+v -0.027036 -0.012905 -0.027909
+v -0.026978 -0.013135 -0.027933
+v -0.026978 -0.012905 -0.027933
+v -0.026920 -0.013135 -0.027964
+v -0.026920 -0.012905 -0.027964
+v -0.026864 -0.013135 -0.028001
+v -0.026864 -0.012905 -0.028001
+v -0.026783 -0.013135 -0.028067
+v -0.026783 -0.012905 -0.028067
+v -0.026728 -0.013135 -0.028123
+v -0.026728 -0.012905 -0.028123
+v -0.026680 -0.013135 -0.028182
+v -0.026680 -0.012905 -0.028182
+v -0.026641 -0.013135 -0.028244
+v -0.026641 -0.012905 -0.028244
+v -0.026617 -0.013135 -0.028292
+v -0.026617 -0.012905 -0.028292
+v -0.026597 -0.013135 -0.028341
+v -0.026597 -0.012905 -0.028341
+v -0.026582 -0.013135 -0.028392
+v -0.026582 -0.012905 -0.028392
+v -0.026572 -0.013135 -0.028445
+v -0.026572 -0.012905 -0.028445
+v -0.026565 -0.013135 -0.028518
+v -0.026565 -0.012905 -0.028518
+v -0.026567 -0.013135 -0.028593
+v -0.026567 -0.012905 -0.028593
+v -0.026576 -0.013135 -0.028671
+v -0.026576 -0.012905 -0.028671
+v -0.026594 -0.013135 -0.028752
+v -0.026594 -0.012905 -0.028752
+v -0.026628 -0.013135 -0.028857
+v -0.026628 -0.012905 -0.028857
+v -0.026622 -0.013135 -0.028862
+v -0.026622 -0.012905 -0.028862
+v -0.026557 -0.013135 -0.028796
+v -0.026557 -0.012905 -0.028796
+v -0.026498 -0.013135 -0.028745
+v -0.026498 -0.012905 -0.028745
+v -0.026438 -0.013135 -0.028702
+v -0.026438 -0.012905 -0.028702
+v -0.026375 -0.013135 -0.028667
+v -0.026375 -0.012905 -0.028667
+v -0.026320 -0.013135 -0.028642
+v -0.026320 -0.012905 -0.028642
+v -0.026263 -0.013135 -0.028623
+v -0.026263 -0.012905 -0.028623
+v -0.026225 -0.013135 -0.028613
+v -0.026225 -0.012905 -0.028613
+v -0.026175 -0.013135 -0.028604
+v -0.026175 -0.012905 -0.028604
+v -0.026116 -0.013135 -0.028598
+v -0.026116 -0.012905 -0.028598
+v -0.026058 -0.013135 -0.028598
+v -0.026058 -0.012905 -0.028598
+v -0.026001 -0.013135 -0.028604
+v -0.026001 -0.012905 -0.028604
+v -0.025945 -0.013135 -0.028615
+v -0.025945 -0.012905 -0.028615
+v -0.025889 -0.013135 -0.028631
+v -0.025889 -0.012905 -0.028631
+v -0.025835 -0.013135 -0.028653
+v -0.025835 -0.012905 -0.028653
+v -0.025781 -0.013135 -0.028681
+v -0.025781 -0.012905 -0.028681
+v -0.025746 -0.013135 -0.028703
+v -0.025746 -0.012905 -0.028703
+v -0.025711 -0.013135 -0.028727
+v -0.025711 -0.012905 -0.028727
+v -0.025644 -0.013135 -0.028781
+v -0.025644 -0.012905 -0.028781
+v -0.025584 -0.013135 -0.028839
+v -0.025584 -0.012905 -0.028839
+v -0.025532 -0.013135 -0.028901
+v -0.025532 -0.012905 -0.028901
+v -0.025495 -0.013135 -0.028955
+v -0.025495 -0.012905 -0.028955
+v -0.025463 -0.013135 -0.029012
+v -0.025463 -0.012905 -0.029012
+v -0.025436 -0.013135 -0.029071
+v -0.025436 -0.012905 -0.029071
+v -0.025415 -0.013135 -0.029133
+v -0.025415 -0.012905 -0.029133
+v -0.025400 -0.013135 -0.029198
+v -0.025400 -0.012905 -0.029198
+v -0.025393 -0.013135 -0.029238
+v -0.025393 -0.012905 -0.029238
+v -0.025387 -0.013135 -0.029293
+v -0.025387 -0.012905 -0.029293
+v -0.025385 -0.013135 -0.029376
+v -0.025385 -0.012905 -0.029376
+v -0.025391 -0.013135 -0.029460
+v -0.025391 -0.012905 -0.029460
+v -0.025405 -0.013135 -0.029544
+v -0.025405 -0.012905 -0.029544
+v -0.025427 -0.013135 -0.029627
+v -0.025427 -0.012905 -0.029627
+v -0.025463 -0.013135 -0.029725
+v -0.025463 -0.012905 -0.029725
+v -0.025511 -0.013135 -0.029823
+v -0.025511 -0.012905 -0.029823
+v -0.025560 -0.013135 -0.029907
+v -0.025560 -0.012905 -0.029907
+v -0.025617 -0.013135 -0.029991
+v -0.025617 -0.012905 -0.029991
+v -0.025720 -0.013135 -0.030119
+v -0.025720 -0.012905 -0.030119
+v -0.025806 -0.013135 -0.030209
+v -0.025806 -0.012905 -0.030209
+v -0.025884 -0.013135 -0.030276
+v -0.025884 -0.012905 -0.030276
+v -0.025963 -0.013135 -0.030333
+v -0.025963 -0.012905 -0.030333
+v -0.026033 -0.013135 -0.030374
+v -0.026033 -0.012905 -0.030374
+v -0.026063 -0.013135 -0.030389
+v -0.026063 -0.012905 -0.030389
+v -0.025059 -0.013135 -0.028474
+v -0.025059 -0.012905 -0.028474
+v -0.024790 -0.012905 -0.028671
+v -0.024790 -0.013135 -0.028671
+v -0.024951 -0.013135 -0.028427
+v -0.024951 -0.012905 -0.028427
+v -0.024852 -0.013135 -0.028373
+v -0.024852 -0.012905 -0.028373
+v -0.024772 -0.013135 -0.028318
+v -0.024772 -0.012905 -0.028318
+v -0.024699 -0.013135 -0.028258
+v -0.024699 -0.012905 -0.028258
+v -0.024633 -0.013135 -0.028192
+v -0.024633 -0.012905 -0.028192
+v -0.024603 -0.013135 -0.028157
+v -0.024603 -0.012905 -0.028157
+v -0.024574 -0.013135 -0.028120
+v -0.024574 -0.012905 -0.028120
+v -0.024524 -0.013135 -0.028044
+v -0.024524 -0.012905 -0.028044
+v -0.024489 -0.013135 -0.027978
+v -0.024489 -0.012905 -0.027978
+v -0.024462 -0.013135 -0.027912
+v -0.024462 -0.012905 -0.027912
+v -0.024444 -0.013135 -0.027855
+v -0.024444 -0.012905 -0.027855
+v -0.024433 -0.013135 -0.027798
+v -0.024433 -0.012905 -0.027798
+v -0.024427 -0.013135 -0.027740
+v -0.024427 -0.012905 -0.027740
+v -0.024426 -0.013135 -0.027692
+v -0.024426 -0.012905 -0.027692
+v -0.024430 -0.013135 -0.027644
+v -0.024430 -0.012905 -0.027644
+v -0.024439 -0.013135 -0.027588
+v -0.024439 -0.012905 -0.027588
+v -0.024451 -0.013135 -0.027542
+v -0.024451 -0.012905 -0.027542
+v -0.024467 -0.013135 -0.027498
+v -0.024467 -0.012905 -0.027498
+v -0.024487 -0.013135 -0.027456
+v -0.024487 -0.012905 -0.027456
+v -0.024511 -0.013135 -0.027415
+v -0.024511 -0.012905 -0.027415
+v -0.024544 -0.013135 -0.027369
+v -0.024544 -0.012905 -0.027369
+v -0.024583 -0.013135 -0.027325
+v -0.024583 -0.012905 -0.027325
+v -0.024627 -0.013135 -0.027283
+v -0.024627 -0.012905 -0.027283
+v -0.024677 -0.013135 -0.027243
+v -0.024677 -0.012905 -0.027243
+v -0.024738 -0.013135 -0.027202
+v -0.024738 -0.012905 -0.027202
+v -0.024791 -0.013135 -0.027175
+v -0.024791 -0.012905 -0.027175
+v -0.024844 -0.013135 -0.027153
+v -0.024844 -0.012905 -0.027153
+v -0.024889 -0.013135 -0.027140
+v -0.024889 -0.012905 -0.027140
+v -0.024933 -0.013135 -0.027131
+v -0.024933 -0.012905 -0.027131
+v -0.024978 -0.013135 -0.027126
+v -0.024978 -0.012905 -0.027126
+v -0.025024 -0.013135 -0.027126
+v -0.025024 -0.012905 -0.027126
+v -0.025069 -0.013135 -0.027130
+v -0.025069 -0.012905 -0.027130
+v -0.025096 -0.013135 -0.027135
+v -0.025096 -0.012905 -0.027135
+v -0.025124 -0.013135 -0.027141
+v -0.025124 -0.012905 -0.027141
+v -0.025170 -0.013135 -0.027154
+v -0.025170 -0.012905 -0.027154
+v -0.025223 -0.013135 -0.027176
+v -0.025223 -0.012905 -0.027176
+v -0.025277 -0.013135 -0.027205
+v -0.025277 -0.012905 -0.027205
+v -0.025329 -0.013135 -0.027239
+v -0.025329 -0.012905 -0.027239
+v -0.025389 -0.013135 -0.027287
+v -0.025389 -0.012905 -0.027287
+v -0.025456 -0.013135 -0.027353
+v -0.025456 -0.012905 -0.027353
+v -0.025505 -0.013135 -0.027409
+v -0.025505 -0.012905 -0.027409
+v -0.025554 -0.013135 -0.027471
+v -0.025554 -0.012905 -0.027471
+v -0.025789 -0.013135 -0.027832
+v -0.025789 -0.012905 -0.027832
+v -0.026900 -0.013135 -0.027019
+v -0.026900 -0.012905 -0.027019
+v -0.026115 -0.013135 -0.025948
+v -0.026115 -0.012905 -0.025948
+v -0.025886 -0.013135 -0.026115
+v -0.025886 -0.012905 -0.026115
+v -0.026496 -0.013135 -0.026947
+v -0.026496 -0.012905 -0.026947
+v -0.025847 -0.013135 -0.027422
+v -0.025847 -0.012905 -0.027422
+v -0.025715 -0.013135 -0.027229
+v -0.025715 -0.012905 -0.027229
+v -0.025620 -0.013135 -0.027111
+v -0.025620 -0.012905 -0.027111
+v -0.025543 -0.013135 -0.027032
+v -0.025543 -0.012905 -0.027032
+v -0.025463 -0.013135 -0.026964
+v -0.025463 -0.012905 -0.026964
+v -0.025393 -0.013135 -0.026915
+v -0.025393 -0.012905 -0.026915
+v -0.025322 -0.013135 -0.026875
+v -0.025322 -0.012905 -0.026875
+v -0.025248 -0.013135 -0.026843
+v -0.025248 -0.012905 -0.026843
+v -0.025198 -0.013135 -0.026826
+v -0.025198 -0.012905 -0.026826
+v -0.025135 -0.013135 -0.026810
+v -0.025135 -0.012905 -0.026810
+v -0.025071 -0.013135 -0.026800
+v -0.025071 -0.012905 -0.026800
+v -0.025007 -0.013135 -0.026796
+v -0.025007 -0.012905 -0.026796
+v -0.024943 -0.013135 -0.026798
+v -0.024943 -0.012905 -0.026798
+v -0.024880 -0.013135 -0.026806
+v -0.024880 -0.012905 -0.026806
+v -0.024816 -0.013135 -0.026819
+v -0.024816 -0.012905 -0.026819
+v -0.024741 -0.013135 -0.026843
+v -0.024741 -0.012905 -0.026843
+v -0.024666 -0.013135 -0.026875
+v -0.024666 -0.012905 -0.026875
+v -0.024591 -0.013135 -0.026916
+v -0.024591 -0.012905 -0.026916
+v -0.024503 -0.013135 -0.026974
+v -0.024503 -0.012905 -0.026974
+v -0.024419 -0.013135 -0.027042
+v -0.024419 -0.012905 -0.027042
+v -0.024355 -0.013135 -0.027104
+v -0.024355 -0.012905 -0.027104
+v -0.024300 -0.013135 -0.027169
+v -0.024300 -0.012905 -0.027169
+v -0.024252 -0.013135 -0.027238
+v -0.024252 -0.012905 -0.027238
+v -0.024218 -0.013135 -0.027298
+v -0.024218 -0.012905 -0.027298
+v -0.024190 -0.013135 -0.027361
+v -0.024190 -0.012905 -0.027361
+v -0.024168 -0.013135 -0.027425
+v -0.024168 -0.012905 -0.027425
+v -0.024151 -0.013135 -0.027492
+v -0.024151 -0.012905 -0.027492
+v -0.024139 -0.013135 -0.027562
+v -0.024139 -0.012905 -0.027562
+v -0.024134 -0.013135 -0.027632
+v -0.024134 -0.012905 -0.027632
+v -0.024136 -0.013135 -0.027718
+v -0.024136 -0.012905 -0.027718
+v -0.024146 -0.013135 -0.027803
+v -0.024146 -0.012905 -0.027803
+v -0.024166 -0.013135 -0.027889
+v -0.024166 -0.012905 -0.027889
+v -0.024194 -0.013135 -0.027975
+v -0.024194 -0.012905 -0.027975
+v -0.024238 -0.013135 -0.028076
+v -0.024238 -0.012905 -0.028076
+v -0.024295 -0.013135 -0.028177
+v -0.024295 -0.012905 -0.028177
+v -0.024363 -0.013135 -0.028279
+v -0.024363 -0.012905 -0.028279
+v -0.024479 -0.013135 -0.028424
+v -0.024479 -0.012905 -0.028424
+v -0.024566 -0.013135 -0.028515
+v -0.024566 -0.012905 -0.028515
+v -0.024642 -0.013135 -0.028581
+v -0.024642 -0.012905 -0.028581
+v -0.024700 -0.013135 -0.028623
+v -0.024700 -0.012905 -0.028623
+v -0.024742 -0.013135 -0.028647
+v -0.024742 -0.012905 -0.028647
+v -0.020762 -0.013229 -0.017321
+v -0.020659 -0.013229 -0.017522
+v -0.015397 -0.013229 -0.017399
+v -0.015401 -0.013229 -0.017197
+v -0.020659 -0.013026 -0.017522
+v -0.015397 -0.013026 -0.017399
+v -0.020762 -0.013026 -0.017321
+v -0.015401 -0.013026 -0.017197
+v -0.023361 -0.013229 -0.020749
+v -0.023199 -0.013229 -0.020872
+v -0.023199 -0.013026 -0.020872
+v -0.023361 -0.013026 -0.020749
+v -0.015397 -0.013229 -0.017399
+v -0.015397 -0.013026 -0.017399
+v -0.015401 -0.013026 -0.017197
+v -0.015401 -0.013229 -0.017197
+v -0.023199 -0.013026 -0.020872
+v -0.023199 -0.013229 -0.020872
+v -0.023361 -0.013229 -0.020749
+v -0.023361 -0.013026 -0.020749
+v -0.020345 -0.013135 -0.022625
+v -0.020345 -0.012905 -0.022625
+v -0.019368 -0.012905 -0.021291
+v -0.019368 -0.013135 -0.021291
+v -0.020571 -0.013135 -0.022460
+v -0.020571 -0.012905 -0.022460
+v -0.020745 -0.013135 -0.021341
+v -0.020745 -0.012905 -0.021341
+v -0.020825 -0.013135 -0.020925
+v -0.020825 -0.012905 -0.020925
+v -0.020861 -0.013135 -0.020796
+v -0.020861 -0.012905 -0.020796
+v -0.020898 -0.013135 -0.020687
+v -0.020898 -0.012905 -0.020687
+v -0.020933 -0.013135 -0.020608
+v -0.020933 -0.012905 -0.020608
+v -0.020969 -0.013135 -0.020543
+v -0.020969 -0.012905 -0.020543
+v -0.021010 -0.013135 -0.020483
+v -0.021010 -0.012905 -0.020483
+v -0.021056 -0.013135 -0.020427
+v -0.021056 -0.012905 -0.020427
+v -0.021108 -0.013135 -0.020376
+v -0.021108 -0.012905 -0.020376
+v -0.021135 -0.013135 -0.020352
+v -0.021135 -0.012905 -0.020352
+v -0.021172 -0.013135 -0.020323
+v -0.021172 -0.012905 -0.020323
+v -0.021233 -0.013135 -0.020283
+v -0.021233 -0.012905 -0.020283
+v -0.021286 -0.013135 -0.020255
+v -0.021286 -0.012905 -0.020255
+v -0.021332 -0.013135 -0.020236
+v -0.021332 -0.012905 -0.020236
+v -0.021377 -0.013135 -0.020222
+v -0.021377 -0.012905 -0.020222
+v -0.021414 -0.013135 -0.020215
+v -0.021414 -0.012905 -0.020215
+v -0.021452 -0.013135 -0.020211
+v -0.021452 -0.012905 -0.020211
+v -0.021489 -0.013135 -0.020210
+v -0.021489 -0.012905 -0.020210
+v -0.021519 -0.013135 -0.020212
+v -0.021519 -0.012905 -0.020212
+v -0.021549 -0.013135 -0.020216
+v -0.021549 -0.012905 -0.020216
+v -0.021585 -0.013135 -0.020224
+v -0.021585 -0.012905 -0.020224
+v -0.021621 -0.013135 -0.020236
+v -0.021621 -0.012905 -0.020236
+v -0.021656 -0.013135 -0.020251
+v -0.021656 -0.012905 -0.020251
+v -0.021698 -0.013135 -0.020273
+v -0.021698 -0.012905 -0.020273
+v -0.021738 -0.013135 -0.020301
+v -0.021738 -0.012905 -0.020301
+v -0.021777 -0.013135 -0.020333
+v -0.021777 -0.012905 -0.020333
+v -0.021821 -0.013135 -0.020377
+v -0.021821 -0.012905 -0.020377
+v -0.021852 -0.013135 -0.020412
+v -0.021852 -0.012905 -0.020412
+v -0.021882 -0.013135 -0.020451
+v -0.021882 -0.012905 -0.020451
+v -0.021930 -0.013135 -0.020524
+v -0.021930 -0.012905 -0.020524
+v -0.021970 -0.013135 -0.020603
+v -0.021970 -0.012905 -0.020603
+v -0.022003 -0.013135 -0.020685
+v -0.022003 -0.012905 -0.020685
+v -0.022028 -0.013135 -0.020772
+v -0.022028 -0.012905 -0.020772
+v -0.022045 -0.013135 -0.020865
+v -0.022045 -0.012905 -0.020865
+v -0.022055 -0.013135 -0.020978
+v -0.022055 -0.012905 -0.020978
+v -0.022055 -0.013135 -0.021080
+v -0.022055 -0.012905 -0.021080
+v -0.022045 -0.013135 -0.021205
+v -0.022045 -0.012905 -0.021205
+v -0.022310 -0.013135 -0.021010
+v -0.022310 -0.012905 -0.021010
+v -0.022309 -0.013135 -0.020897
+v -0.022309 -0.012905 -0.020897
+v -0.022297 -0.013135 -0.020787
+v -0.022297 -0.012905 -0.020787
+v -0.022274 -0.013135 -0.020679
+v -0.022274 -0.012905 -0.020679
+v -0.022241 -0.013135 -0.020574
+v -0.022241 -0.012905 -0.020574
+v -0.022198 -0.013135 -0.020471
+v -0.022198 -0.012905 -0.020471
+v -0.022144 -0.013135 -0.020370
+v -0.022144 -0.012905 -0.020370
+v -0.022071 -0.013135 -0.020258
+v -0.022071 -0.012905 -0.020258
+v -0.021999 -0.013135 -0.020170
+v -0.021999 -0.012905 -0.020170
+v -0.021934 -0.013135 -0.020102
+v -0.021934 -0.012905 -0.020102
+v -0.021866 -0.013135 -0.020045
+v -0.021866 -0.012905 -0.020045
+v -0.021806 -0.013135 -0.020003
+v -0.021806 -0.012905 -0.020003
+v -0.021744 -0.013135 -0.019968
+v -0.021744 -0.012905 -0.019968
+v -0.021680 -0.013135 -0.019940
+v -0.021680 -0.012905 -0.019940
+v -0.021625 -0.013135 -0.019923
+v -0.021625 -0.012905 -0.019923
+v -0.021568 -0.013135 -0.019910
+v -0.021568 -0.012905 -0.019910
+v -0.021500 -0.013135 -0.019901
+v -0.021500 -0.012905 -0.019901
+v -0.021444 -0.013135 -0.019899
+v -0.021444 -0.012905 -0.019899
+v -0.021388 -0.013135 -0.019902
+v -0.021388 -0.012905 -0.019902
+v -0.021332 -0.013135 -0.019910
+v -0.021332 -0.012905 -0.019910
+v -0.021265 -0.013135 -0.019926
+v -0.021265 -0.012905 -0.019926
+v -0.021199 -0.013135 -0.019950
+v -0.021199 -0.012905 -0.019950
+v -0.021133 -0.013135 -0.019980
+v -0.021133 -0.012905 -0.019980
+v -0.021078 -0.013135 -0.020011
+v -0.021078 -0.012905 -0.020011
+v -0.021013 -0.013135 -0.020055
+v -0.021013 -0.012905 -0.020055
+v -0.020948 -0.013135 -0.020108
+v -0.020948 -0.012905 -0.020108
+v -0.020887 -0.013135 -0.020166
+v -0.020887 -0.012905 -0.020166
+v -0.020824 -0.013135 -0.020239
+v -0.020824 -0.012905 -0.020239
+v -0.020768 -0.013135 -0.020318
+v -0.020768 -0.012905 -0.020318
+v -0.020723 -0.013135 -0.020395
+v -0.020723 -0.012905 -0.020395
+v -0.020683 -0.013135 -0.020477
+v -0.020683 -0.012905 -0.020477
+v -0.020633 -0.013135 -0.020607
+v -0.020633 -0.012905 -0.020607
+v -0.020557 -0.013135 -0.020890
+v -0.020557 -0.012905 -0.020890
+v -0.020488 -0.013135 -0.021268
+v -0.020488 -0.012905 -0.021268
+v -0.020357 -0.013135 -0.022149
+v -0.020357 -0.012905 -0.022149
+v -0.020351 -0.013135 -0.022153
+v -0.020351 -0.012905 -0.022153
+v -0.019597 -0.013135 -0.021124
+v -0.019597 -0.012905 -0.021124
+v -0.019343 -0.013135 -0.020579
+v -0.019343 -0.012905 -0.020579
+v -0.019074 -0.012905 -0.020776
+v -0.019074 -0.013135 -0.020776
+v -0.019235 -0.013135 -0.020532
+v -0.019235 -0.012905 -0.020532
+v -0.019136 -0.013135 -0.020478
+v -0.019136 -0.012905 -0.020478
+v -0.019055 -0.013135 -0.020424
+v -0.019055 -0.012905 -0.020424
+v -0.018982 -0.013135 -0.020363
+v -0.018982 -0.012905 -0.020363
+v -0.018916 -0.013135 -0.020297
+v -0.018916 -0.012905 -0.020297
+v -0.018886 -0.013135 -0.020262
+v -0.018886 -0.012905 -0.020262
+v -0.018858 -0.013135 -0.020225
+v -0.018858 -0.012905 -0.020225
+v -0.018808 -0.013135 -0.020150
+v -0.018808 -0.012905 -0.020150
+v -0.018773 -0.013135 -0.020083
+v -0.018773 -0.012905 -0.020083
+v -0.018745 -0.013135 -0.020017
+v -0.018745 -0.012905 -0.020017
+v -0.018728 -0.013135 -0.019960
+v -0.018728 -0.012905 -0.019960
+v -0.018716 -0.013135 -0.019903
+v -0.018716 -0.012905 -0.019903
+v -0.018711 -0.013135 -0.019845
+v -0.018711 -0.012905 -0.019845
+v -0.018710 -0.013135 -0.019797
+v -0.018710 -0.012905 -0.019797
+v -0.018714 -0.013135 -0.019749
+v -0.018714 -0.012905 -0.019749
+v -0.018723 -0.013135 -0.019693
+v -0.018723 -0.012905 -0.019693
+v -0.018735 -0.013135 -0.019647
+v -0.018735 -0.012905 -0.019647
+v -0.018751 -0.013135 -0.019603
+v -0.018751 -0.012905 -0.019603
+v -0.018771 -0.013135 -0.019561
+v -0.018771 -0.012905 -0.019561
+v -0.018794 -0.013135 -0.019521
+v -0.018794 -0.012905 -0.019521
+v -0.018827 -0.013135 -0.019474
+v -0.018827 -0.012905 -0.019474
+v -0.018866 -0.013135 -0.019430
+v -0.018866 -0.012905 -0.019430
+v -0.018911 -0.013135 -0.019388
+v -0.018911 -0.012905 -0.019388
+v -0.018960 -0.013135 -0.019348
+v -0.018960 -0.012905 -0.019348
+v -0.019022 -0.013135 -0.019307
+v -0.019022 -0.012905 -0.019307
+v -0.019074 -0.013135 -0.019280
+v -0.019074 -0.012905 -0.019280
+v -0.019128 -0.013135 -0.019258
+v -0.019128 -0.012905 -0.019258
+v -0.019172 -0.013135 -0.019245
+v -0.019172 -0.012905 -0.019245
+v -0.019217 -0.013135 -0.019236
+v -0.019217 -0.012905 -0.019236
+v -0.019262 -0.013135 -0.019231
+v -0.019262 -0.012905 -0.019231
+v -0.019307 -0.013135 -0.019231
+v -0.019307 -0.012905 -0.019231
+v -0.019353 -0.013135 -0.019235
+v -0.019353 -0.012905 -0.019235
+v -0.019380 -0.013135 -0.019240
+v -0.019380 -0.012905 -0.019240
+v -0.019408 -0.013135 -0.019246
+v -0.019408 -0.012905 -0.019246
+v -0.019453 -0.013135 -0.019259
+v -0.019453 -0.012905 -0.019259
+v -0.019507 -0.013135 -0.019281
+v -0.019507 -0.012905 -0.019281
+v -0.019560 -0.013135 -0.019310
+v -0.019560 -0.012905 -0.019310
+v -0.019612 -0.013135 -0.019344
+v -0.019612 -0.012905 -0.019344
+v -0.019672 -0.013135 -0.019392
+v -0.019672 -0.012905 -0.019392
+v -0.019739 -0.013135 -0.019458
+v -0.019739 -0.012905 -0.019458
+v -0.019789 -0.013135 -0.019514
+v -0.019789 -0.012905 -0.019514
+v -0.019837 -0.013135 -0.019576
+v -0.019837 -0.012905 -0.019576
+v -0.020073 -0.013135 -0.019937
+v -0.020073 -0.012905 -0.019937
+v -0.021183 -0.013135 -0.019124
+v -0.021183 -0.012905 -0.019124
+v -0.020398 -0.013135 -0.018053
+v -0.020398 -0.012905 -0.018053
+v -0.020170 -0.013135 -0.018220
+v -0.020170 -0.012905 -0.018220
+v -0.020779 -0.013135 -0.019052
+v -0.020779 -0.012905 -0.019052
+v -0.020131 -0.013135 -0.019527
+v -0.020131 -0.012905 -0.019527
+v -0.019999 -0.013135 -0.019334
+v -0.019999 -0.012905 -0.019334
+v -0.019903 -0.013135 -0.019216
+v -0.019903 -0.012905 -0.019216
+v -0.019826 -0.013135 -0.019137
+v -0.019826 -0.012905 -0.019137
+v -0.019747 -0.013135 -0.019069
+v -0.019747 -0.012905 -0.019069
+v -0.019677 -0.013135 -0.019020
+v -0.019677 -0.012905 -0.019020
+v -0.019605 -0.013135 -0.018980
+v -0.019605 -0.012905 -0.018980
+v -0.019532 -0.013135 -0.018948
+v -0.019532 -0.012905 -0.018948
+v -0.019482 -0.013135 -0.018931
+v -0.019482 -0.012905 -0.018931
+v -0.019418 -0.013135 -0.018915
+v -0.019418 -0.012905 -0.018915
+v -0.019354 -0.013135 -0.018905
+v -0.019354 -0.012905 -0.018905
+v -0.019290 -0.013135 -0.018901
+v -0.019290 -0.012905 -0.018901
+v -0.019227 -0.013135 -0.018903
+v -0.019227 -0.012905 -0.018903
+v -0.019163 -0.013135 -0.018911
+v -0.019163 -0.012905 -0.018911
+v -0.019100 -0.013135 -0.018924
+v -0.019100 -0.012905 -0.018924
+v -0.019024 -0.013135 -0.018948
+v -0.019024 -0.012905 -0.018948
+v -0.018949 -0.013135 -0.018980
+v -0.018949 -0.012905 -0.018980
+v -0.018874 -0.013135 -0.019021
+v -0.018874 -0.012905 -0.019021
+v -0.018787 -0.013135 -0.019079
+v -0.018787 -0.012905 -0.019079
+v -0.018703 -0.013135 -0.019147
+v -0.018703 -0.012905 -0.019147
+v -0.018639 -0.013135 -0.019209
+v -0.018639 -0.012905 -0.019209
+v -0.018583 -0.013135 -0.019274
+v -0.018583 -0.012905 -0.019274
+v -0.018535 -0.013135 -0.019343
+v -0.018535 -0.012905 -0.019343
+v -0.018502 -0.013135 -0.019403
+v -0.018502 -0.012905 -0.019403
+v -0.018474 -0.013135 -0.019466
+v -0.018474 -0.012905 -0.019466
+v -0.018451 -0.013135 -0.019530
+v -0.018451 -0.012905 -0.019530
+v -0.018434 -0.013135 -0.019597
+v -0.018434 -0.012905 -0.019597
+v -0.018423 -0.013135 -0.019667
+v -0.018423 -0.012905 -0.019667
+v -0.018418 -0.013135 -0.019737
+v -0.018418 -0.012905 -0.019737
+v -0.018419 -0.013135 -0.019822
+v -0.018419 -0.012905 -0.019822
+v -0.018430 -0.013135 -0.019908
+v -0.018430 -0.012905 -0.019908
+v -0.018449 -0.013135 -0.019994
+v -0.018449 -0.012905 -0.019994
+v -0.018477 -0.013135 -0.020080
+v -0.018477 -0.012905 -0.020080
+v -0.018522 -0.013135 -0.020181
+v -0.018522 -0.012905 -0.020181
+v -0.018578 -0.013135 -0.020282
+v -0.018578 -0.012905 -0.020282
+v -0.018647 -0.013135 -0.020384
+v -0.018647 -0.012905 -0.020384
+v -0.018762 -0.013135 -0.020529
+v -0.018762 -0.012905 -0.020529
+v -0.018849 -0.013135 -0.020620
+v -0.018849 -0.012905 -0.020620
+v -0.018926 -0.013135 -0.020686
+v -0.018926 -0.012905 -0.020686
+v -0.018984 -0.013135 -0.020728
+v -0.018984 -0.012905 -0.020728
+v -0.019025 -0.013135 -0.020752
+v -0.019025 -0.012905 -0.020752
+v -0.014943 -0.013229 -0.009212
+v -0.014841 -0.013229 -0.009412
+v -0.009579 -0.013229 -0.009290
+v -0.009583 -0.013229 -0.009087
+v -0.014841 -0.013026 -0.009412
+v -0.009579 -0.013026 -0.009290
+v -0.014943 -0.013026 -0.009212
+v -0.009583 -0.013026 -0.009087
+v -0.017542 -0.013229 -0.012640
+v -0.017381 -0.013229 -0.012762
+v -0.017381 -0.013026 -0.012762
+v -0.017542 -0.013026 -0.012640
+v -0.009579 -0.013229 -0.009290
+v -0.009579 -0.013026 -0.009290
+v -0.009583 -0.013026 -0.009087
+v -0.009583 -0.013229 -0.009087
+v -0.017381 -0.013026 -0.012762
+v -0.017381 -0.013229 -0.012762
+v -0.017542 -0.013229 -0.012640
+v -0.017542 -0.013026 -0.012640
+v -0.014477 -0.013152 -0.014447
+v -0.014477 -0.012760 -0.014447
+v -0.013560 -0.012760 -0.013195
+v -0.013560 -0.013152 -0.013195
+v -0.014699 -0.013152 -0.014284
+v -0.014699 -0.012760 -0.014284
+v -0.014333 -0.013152 -0.013784
+v -0.014333 -0.012760 -0.013784
+v -0.016053 -0.013152 -0.012524
+v -0.016053 -0.012760 -0.012524
+v -0.016276 -0.013152 -0.013149
+v -0.016276 -0.012760 -0.013149
+v -0.016517 -0.013152 -0.012973
+v -0.016517 -0.012760 -0.012973
+v -0.016189 -0.013152 -0.012036
+v -0.016189 -0.012760 -0.012036
+v -0.014148 -0.013152 -0.013531
+v -0.014148 -0.012760 -0.013531
+v -0.013782 -0.013152 -0.013032
+v -0.013782 -0.012760 -0.013032
+v -0.013565 -0.013152 -0.012525
+v -0.013565 -0.012760 -0.012525
+v -0.013296 -0.012760 -0.012722
+v -0.013296 -0.013152 -0.012722
+v -0.013457 -0.013152 -0.012478
+v -0.013457 -0.012760 -0.012478
+v -0.013358 -0.013152 -0.012424
+v -0.013358 -0.012760 -0.012424
+v -0.013278 -0.013152 -0.012369
+v -0.013278 -0.012760 -0.012369
+v -0.013205 -0.013152 -0.012309
+v -0.013205 -0.012760 -0.012309
+v -0.013139 -0.013152 -0.012243
+v -0.013139 -0.012760 -0.012243
+v -0.013109 -0.013152 -0.012207
+v -0.013109 -0.012760 -0.012207
+v -0.013080 -0.013152 -0.012171
+v -0.013080 -0.012760 -0.012171
+v -0.013030 -0.013152 -0.012095
+v -0.013030 -0.012760 -0.012095
+v -0.012995 -0.013152 -0.012029
+v -0.012995 -0.012760 -0.012029
+v -0.012968 -0.013152 -0.011963
+v -0.012968 -0.012760 -0.011963
+v -0.012950 -0.013152 -0.011906
+v -0.012950 -0.012760 -0.011906
+v -0.012939 -0.013152 -0.011849
+v -0.012939 -0.012760 -0.011849
+v -0.012933 -0.013152 -0.011791
+v -0.012933 -0.012760 -0.011791
+v -0.012932 -0.013152 -0.011743
+v -0.012932 -0.012760 -0.011743
+v -0.012936 -0.013152 -0.011695
+v -0.012936 -0.012760 -0.011695
+v -0.012945 -0.013152 -0.011639
+v -0.012945 -0.012760 -0.011639
+v -0.012957 -0.013152 -0.011593
+v -0.012957 -0.012760 -0.011593
+v -0.012973 -0.013152 -0.011549
+v -0.012973 -0.012760 -0.011549
+v -0.012993 -0.013152 -0.011507
+v -0.012993 -0.012760 -0.011507
+v -0.013017 -0.013152 -0.011466
+v -0.013017 -0.012760 -0.011466
+v -0.013050 -0.013152 -0.011420
+v -0.013050 -0.012760 -0.011420
+v -0.013089 -0.013152 -0.011375
+v -0.013089 -0.012760 -0.011375
+v -0.013133 -0.013152 -0.011333
+v -0.013133 -0.012760 -0.011333
+v -0.013183 -0.013152 -0.011294
+v -0.013183 -0.012760 -0.011294
+v -0.013244 -0.013152 -0.011253
+v -0.013244 -0.012760 -0.011253
+v -0.013297 -0.013152 -0.011226
+v -0.013297 -0.012760 -0.011226
+v -0.013350 -0.013152 -0.011204
+v -0.013350 -0.012760 -0.011204
+v -0.013395 -0.013152 -0.011191
+v -0.013395 -0.012760 -0.011191
+v -0.013439 -0.013152 -0.011182
+v -0.013439 -0.012760 -0.011182
+v -0.013484 -0.013152 -0.011177
+v -0.013484 -0.012760 -0.011177
+v -0.013530 -0.013152 -0.011177
+v -0.013530 -0.012760 -0.011177
+v -0.013575 -0.013152 -0.011181
+v -0.013575 -0.012760 -0.011181
+v -0.013602 -0.013152 -0.011186
+v -0.013602 -0.012760 -0.011186
+v -0.013630 -0.013152 -0.011192
+v -0.013630 -0.012760 -0.011192
+v -0.013676 -0.013152 -0.011205
+v -0.013676 -0.012760 -0.011205
+v -0.013729 -0.013152 -0.011227
+v -0.013729 -0.012760 -0.011227
+v -0.013783 -0.013152 -0.011256
+v -0.013783 -0.012760 -0.011256
+v -0.013835 -0.013152 -0.011290
+v -0.013835 -0.012760 -0.011290
+v -0.013895 -0.013152 -0.011338
+v -0.013895 -0.012760 -0.011338
+v -0.013962 -0.013152 -0.011404
+v -0.013962 -0.012760 -0.011404
+v -0.014011 -0.013152 -0.011460
+v -0.014011 -0.012760 -0.011460
+v -0.014059 -0.013152 -0.011522
+v -0.014059 -0.012760 -0.011522
+v -0.014295 -0.013152 -0.011883
+v -0.014295 -0.012760 -0.011883
+v -0.015406 -0.013152 -0.011070
+v -0.015406 -0.012760 -0.011070
+v -0.014621 -0.013152 -0.009999
+v -0.014621 -0.012760 -0.009999
+v -0.014392 -0.013152 -0.010166
+v -0.014392 -0.012760 -0.010166
+v -0.015002 -0.013152 -0.010998
+v -0.015002 -0.012760 -0.010998
+v -0.014353 -0.013152 -0.011473
+v -0.014353 -0.012760 -0.011473
+v -0.014221 -0.013152 -0.011280
+v -0.014221 -0.012760 -0.011280
+v -0.014126 -0.013152 -0.011162
+v -0.014126 -0.012760 -0.011162
+v -0.014049 -0.013152 -0.011083
+v -0.014049 -0.012760 -0.011083
+v -0.013969 -0.013152 -0.011015
+v -0.013969 -0.012760 -0.011015
+v -0.013899 -0.013152 -0.010966
+v -0.013899 -0.012760 -0.010966
+v -0.013828 -0.013152 -0.010926
+v -0.013828 -0.012760 -0.010926
+v -0.013754 -0.013152 -0.010894
+v -0.013754 -0.012760 -0.010894
+v -0.013704 -0.013152 -0.010877
+v -0.013704 -0.012760 -0.010877
+v -0.013641 -0.013152 -0.010861
+v -0.013641 -0.012760 -0.010861
+v -0.013577 -0.013152 -0.010851
+v -0.013577 -0.012760 -0.010851
+v -0.013513 -0.013152 -0.010847
+v -0.013513 -0.012760 -0.010847
+v -0.013449 -0.013152 -0.010849
+v -0.013449 -0.012760 -0.010849
+v -0.013386 -0.013152 -0.010857
+v -0.013386 -0.012760 -0.010857
+v -0.013322 -0.013152 -0.010870
+v -0.013322 -0.012760 -0.010870
+v -0.013247 -0.013152 -0.010894
+v -0.013247 -0.012760 -0.010894
+v -0.013172 -0.013152 -0.010926
+v -0.013172 -0.012760 -0.010926
+v -0.013097 -0.013152 -0.010967
+v -0.013097 -0.012760 -0.010967
+v -0.013009 -0.013152 -0.011025
+v -0.013009 -0.012760 -0.011025
+v -0.012925 -0.013152 -0.011093
+v -0.012925 -0.012760 -0.011093
+v -0.012861 -0.013152 -0.011155
+v -0.012861 -0.012760 -0.011155
+v -0.012806 -0.013152 -0.011220
+v -0.012806 -0.012760 -0.011220
+v -0.012758 -0.013152 -0.011289
+v -0.012758 -0.012760 -0.011289
+v -0.012724 -0.013152 -0.011349
+v -0.012724 -0.012760 -0.011349
+v -0.012696 -0.013152 -0.011412
+v -0.012696 -0.012760 -0.011412
+v -0.012674 -0.013152 -0.011476
+v -0.012674 -0.012760 -0.011476
+v -0.012657 -0.013152 -0.011543
+v -0.012657 -0.012760 -0.011543
+v -0.012645 -0.013152 -0.011613
+v -0.012645 -0.012760 -0.011613
+v -0.012640 -0.013152 -0.011683
+v -0.012640 -0.012760 -0.011683
+v -0.012642 -0.013152 -0.011769
+v -0.012642 -0.012760 -0.011769
+v -0.012652 -0.013152 -0.011854
+v -0.012652 -0.012760 -0.011854
+v -0.012672 -0.013152 -0.011940
+v -0.012672 -0.012760 -0.011940
+v -0.012700 -0.013152 -0.012026
+v -0.012700 -0.012760 -0.012026
+v -0.012744 -0.013152 -0.012127
+v -0.012744 -0.012760 -0.012127
+v -0.012801 -0.013152 -0.012228
+v -0.012801 -0.012760 -0.012228
+v -0.012869 -0.013152 -0.012330
+v -0.012869 -0.012760 -0.012330
+v -0.012985 -0.013152 -0.012475
+v -0.012985 -0.012760 -0.012475
+v -0.013072 -0.013152 -0.012565
+v -0.013072 -0.012760 -0.012565
+v -0.013148 -0.013152 -0.012632
+v -0.013148 -0.012760 -0.012632
+v -0.013206 -0.013152 -0.012674
+v -0.013206 -0.012760 -0.012674
+v -0.013248 -0.013152 -0.012698
+v -0.013248 -0.012760 -0.012698
+v -0.012845 0.001552 -0.000351
+v -0.002979 0.000002 0.001014
+v -0.012845 -0.001549 -0.000351
+v -0.012877 0.001552 -0.000121
+v -0.003011 0.000002 0.001245
+v -0.012877 -0.001549 -0.000121
+v -0.015002 -0.012760 -0.010998
+v -0.015406 -0.012760 -0.011070
+v -0.014621 -0.012760 -0.009999
+v -0.014295 -0.012760 -0.011883
+v -0.014392 -0.012760 -0.010166
+v -0.014353 -0.012760 -0.011473
+v -0.014221 -0.012760 -0.011280
+v -0.014059 -0.012760 -0.011522
+v -0.014126 -0.012760 -0.011162
+v -0.014049 -0.012760 -0.011083
+v -0.014011 -0.012760 -0.011460
+v -0.013969 -0.012760 -0.011015
+v -0.013962 -0.012760 -0.011404
+v -0.013899 -0.012760 -0.010966
+v -0.013895 -0.012760 -0.011338
+v -0.013828 -0.012760 -0.010926
+v -0.013835 -0.012760 -0.011290
+v -0.013783 -0.012760 -0.011256
+v -0.013754 -0.012760 -0.010894
+v -0.013729 -0.012760 -0.011227
+v -0.013704 -0.012760 -0.010877
+v -0.013676 -0.012760 -0.011205
+v -0.013641 -0.012760 -0.010861
+v -0.013630 -0.012760 -0.011192
+v -0.013577 -0.012760 -0.010851
+v -0.013602 -0.012760 -0.011186
+v -0.013575 -0.012760 -0.011181
+v -0.013513 -0.012760 -0.010847
+v -0.013530 -0.012760 -0.011177
+v -0.013296 -0.012760 -0.012722
+v -0.013565 -0.012760 -0.012525
+v -0.013457 -0.012760 -0.012478
+v -0.013484 -0.012760 -0.011177
+v -0.013449 -0.012760 -0.010849
+v -0.013439 -0.012760 -0.011182
+v -0.013358 -0.012760 -0.012424
+v -0.013386 -0.012760 -0.010857
+v -0.013395 -0.012760 -0.011191
+v -0.013350 -0.012760 -0.011204
+v -0.013322 -0.012760 -0.010870
+v -0.013278 -0.012760 -0.012369
+v -0.013297 -0.012760 -0.011226
+v -0.013247 -0.012760 -0.010894
+v -0.013244 -0.012760 -0.011253
+v -0.013248 -0.012760 -0.012698
+v -0.013205 -0.012760 -0.012309
+v -0.013206 -0.012760 -0.012674
+v -0.013172 -0.012760 -0.010926
+v -0.013183 -0.012760 -0.011294
+v -0.013148 -0.012760 -0.012632
+v -0.013139 -0.012760 -0.012243
+v -0.013133 -0.012760 -0.011333
+v -0.013097 -0.012760 -0.010967
+v -0.013072 -0.012760 -0.012565
+v -0.013109 -0.012760 -0.012207
+v -0.013089 -0.012760 -0.011375
+v -0.013080 -0.012760 -0.012171
+v -0.013009 -0.012760 -0.011025
+v -0.013050 -0.012760 -0.011420
+v -0.013030 -0.012760 -0.012095
+v -0.012985 -0.012760 -0.012475
+v -0.013017 -0.012760 -0.011466
+v -0.012995 -0.012760 -0.012029
+v -0.012993 -0.012760 -0.011507
+v -0.012925 -0.012760 -0.011093
+v -0.012968 -0.012760 -0.011963
+v -0.012973 -0.012760 -0.011549
+v -0.012869 -0.012760 -0.012330
+v -0.012957 -0.012760 -0.011593
+v -0.012950 -0.012760 -0.011906
+v -0.012945 -0.012760 -0.011639
+v -0.012939 -0.012760 -0.011849
+v -0.012936 -0.012760 -0.011695
+v -0.012933 -0.012760 -0.011791
+v -0.012932 -0.012760 -0.011743
+v -0.012861 -0.012760 -0.011155
+v -0.012801 -0.012760 -0.012228
+v -0.012806 -0.012760 -0.011220
+v -0.012758 -0.012760 -0.011289
+v -0.012744 -0.012760 -0.012127
+v -0.012724 -0.012760 -0.011349
+v -0.012700 -0.012760 -0.012026
+v -0.012696 -0.012760 -0.011412
+v -0.012672 -0.012760 -0.011940
+v -0.012674 -0.012760 -0.011476
+v -0.012657 -0.012760 -0.011543
+v -0.012652 -0.012760 -0.011854
+v -0.012645 -0.012760 -0.011613
+v -0.012642 -0.012760 -0.011769
+v -0.012640 -0.012760 -0.011683
+v -0.016276 -0.012760 -0.013149
+v -0.016517 -0.012760 -0.012973
+v -0.016189 -0.012760 -0.012036
+v -0.016053 -0.012760 -0.012524
+v -0.014148 -0.012760 -0.013531
+v -0.014333 -0.012760 -0.013784
+v -0.014477 -0.012760 -0.014447
+v -0.014699 -0.012760 -0.014284
+v -0.013560 -0.012760 -0.013195
+v -0.013782 -0.012760 -0.013032
+v -0.014621 -0.013152 -0.009999
+v -0.015406 -0.013152 -0.011070
+v -0.015002 -0.013152 -0.010998
+v -0.014295 -0.013152 -0.011883
+v -0.014392 -0.013152 -0.010166
+v -0.014353 -0.013152 -0.011473
+v -0.014221 -0.013152 -0.011280
+v -0.014059 -0.013152 -0.011522
+v -0.014126 -0.013152 -0.011162
+v -0.014049 -0.013152 -0.011083
+v -0.014011 -0.013152 -0.011460
+v -0.013969 -0.013152 -0.011015
+v -0.013962 -0.013152 -0.011404
+v -0.013899 -0.013152 -0.010966
+v -0.013895 -0.013152 -0.011338
+v -0.013828 -0.013152 -0.010926
+v -0.013835 -0.013152 -0.011290
+v -0.013783 -0.013152 -0.011256
+v -0.013754 -0.013152 -0.010894
+v -0.013729 -0.013152 -0.011227
+v -0.013704 -0.013152 -0.010877
+v -0.013676 -0.013152 -0.011205
+v -0.013641 -0.013152 -0.010861
+v -0.013630 -0.013152 -0.011192
+v -0.013577 -0.013152 -0.010851
+v -0.013602 -0.013152 -0.011186
+v -0.013575 -0.013152 -0.011181
+v -0.013513 -0.013152 -0.010847
+v -0.013530 -0.013152 -0.011177
+v -0.013457 -0.013152 -0.012478
+v -0.013565 -0.013152 -0.012525
+v -0.013296 -0.013152 -0.012722
+v -0.013484 -0.013152 -0.011177
+v -0.013449 -0.013152 -0.010849
+v -0.013439 -0.013152 -0.011182
+v -0.013358 -0.013152 -0.012424
+v -0.013386 -0.013152 -0.010857
+v -0.013395 -0.013152 -0.011191
+v -0.013350 -0.013152 -0.011204
+v -0.013322 -0.013152 -0.010870
+v -0.013278 -0.013152 -0.012369
+v -0.013297 -0.013152 -0.011226
+v -0.013247 -0.013152 -0.010894
+v -0.013244 -0.013152 -0.011253
+v -0.013248 -0.013152 -0.012698
+v -0.013205 -0.013152 -0.012309
+v -0.013206 -0.013152 -0.012674
+v -0.013172 -0.013152 -0.010926
+v -0.013183 -0.013152 -0.011294
+v -0.013148 -0.013152 -0.012632
+v -0.013139 -0.013152 -0.012243
+v -0.013133 -0.013152 -0.011333
+v -0.013097 -0.013152 -0.010967
+v -0.013072 -0.013152 -0.012565
+v -0.013109 -0.013152 -0.012207
+v -0.013089 -0.013152 -0.011375
+v -0.013080 -0.013152 -0.012171
+v -0.013009 -0.013152 -0.011025
+v -0.013050 -0.013152 -0.011420
+v -0.013030 -0.013152 -0.012095
+v -0.012985 -0.013152 -0.012475
+v -0.013017 -0.013152 -0.011466
+v -0.012995 -0.013152 -0.012029
+v -0.012993 -0.013152 -0.011507
+v -0.012925 -0.013152 -0.011093
+v -0.012968 -0.013152 -0.011963
+v -0.012973 -0.013152 -0.011549
+v -0.012869 -0.013152 -0.012330
+v -0.012957 -0.013152 -0.011593
+v -0.012950 -0.013152 -0.011906
+v -0.012945 -0.013152 -0.011639
+v -0.012939 -0.013152 -0.011849
+v -0.012936 -0.013152 -0.011695
+v -0.012933 -0.013152 -0.011791
+v -0.012932 -0.013152 -0.011743
+v -0.012861 -0.013152 -0.011155
+v -0.012801 -0.013152 -0.012228
+v -0.012806 -0.013152 -0.011220
+v -0.012758 -0.013152 -0.011289
+v -0.012744 -0.013152 -0.012127
+v -0.012724 -0.013152 -0.011349
+v -0.012700 -0.013152 -0.012026
+v -0.012696 -0.013152 -0.011412
+v -0.012672 -0.013152 -0.011940
+v -0.012674 -0.013152 -0.011476
+v -0.012657 -0.013152 -0.011543
+v -0.012652 -0.013152 -0.011854
+v -0.012645 -0.013152 -0.011613
+v -0.012642 -0.013152 -0.011769
+v -0.012640 -0.013152 -0.011683
+v -0.016189 -0.013152 -0.012036
+v -0.016517 -0.013152 -0.012973
+v -0.016276 -0.013152 -0.013149
+v -0.016053 -0.013152 -0.012524
+v -0.014148 -0.013152 -0.013531
+v -0.014333 -0.013152 -0.013784
+v -0.014699 -0.013152 -0.014284
+v -0.014477 -0.013152 -0.014447
+v -0.013560 -0.013152 -0.013195
+v -0.013782 -0.013152 -0.013032
+v -0.020779 -0.012905 -0.019052
+v -0.021183 -0.012905 -0.019124
+v -0.020398 -0.012905 -0.018053
+v -0.020073 -0.012905 -0.019937
+v -0.020170 -0.012905 -0.018220
+v -0.020131 -0.012905 -0.019527
+v -0.019999 -0.012905 -0.019334
+v -0.019837 -0.012905 -0.019576
+v -0.019903 -0.012905 -0.019216
+v -0.019826 -0.012905 -0.019137
+v -0.019789 -0.012905 -0.019514
+v -0.019747 -0.012905 -0.019069
+v -0.019739 -0.012905 -0.019458
+v -0.019677 -0.012905 -0.019020
+v -0.019672 -0.012905 -0.019392
+v -0.019605 -0.012905 -0.018980
+v -0.019612 -0.012905 -0.019344
+v -0.019560 -0.012905 -0.019310
+v -0.019532 -0.012905 -0.018948
+v -0.019507 -0.012905 -0.019281
+v -0.019482 -0.012905 -0.018931
+v -0.019453 -0.012905 -0.019259
+v -0.019418 -0.012905 -0.018915
+v -0.019408 -0.012905 -0.019246
+v -0.019354 -0.012905 -0.018905
+v -0.019380 -0.012905 -0.019240
+v -0.019353 -0.012905 -0.019235
+v -0.019290 -0.012905 -0.018901
+v -0.019307 -0.012905 -0.019231
+v -0.019074 -0.012905 -0.020776
+v -0.019343 -0.012905 -0.020579
+v -0.019235 -0.012905 -0.020532
+v -0.019262 -0.012905 -0.019231
+v -0.019227 -0.012905 -0.018903
+v -0.019217 -0.012905 -0.019236
+v -0.019136 -0.012905 -0.020478
+v -0.019163 -0.012905 -0.018911
+v -0.019172 -0.012905 -0.019245
+v -0.019128 -0.012905 -0.019258
+v -0.019100 -0.012905 -0.018924
+v -0.019055 -0.012905 -0.020424
+v -0.019074 -0.012905 -0.019280
+v -0.019024 -0.012905 -0.018948
+v -0.019022 -0.012905 -0.019307
+v -0.019025 -0.012905 -0.020752
+v -0.018982 -0.012905 -0.020363
+v -0.018984 -0.012905 -0.020728
+v -0.018949 -0.012905 -0.018980
+v -0.018960 -0.012905 -0.019348
+v -0.018926 -0.012905 -0.020686
+v -0.018916 -0.012905 -0.020297
+v -0.018911 -0.012905 -0.019388
+v -0.018874 -0.012905 -0.019021
+v -0.018849 -0.012905 -0.020620
+v -0.018886 -0.012905 -0.020262
+v -0.018866 -0.012905 -0.019430
+v -0.018858 -0.012905 -0.020225
+v -0.018787 -0.012905 -0.019079
+v -0.018827 -0.012905 -0.019474
+v -0.018808 -0.012905 -0.020150
+v -0.018762 -0.012905 -0.020529
+v -0.018794 -0.012905 -0.019521
+v -0.018773 -0.012905 -0.020083
+v -0.018771 -0.012905 -0.019561
+v -0.018703 -0.012905 -0.019147
+v -0.018745 -0.012905 -0.020017
+v -0.018751 -0.012905 -0.019603
+v -0.018647 -0.012905 -0.020384
+v -0.018735 -0.012905 -0.019647
+v -0.018728 -0.012905 -0.019960
+v -0.018723 -0.012905 -0.019693
+v -0.018716 -0.012905 -0.019903
+v -0.018714 -0.012905 -0.019749
+v -0.018711 -0.012905 -0.019845
+v -0.018710 -0.012905 -0.019797
+v -0.018639 -0.012905 -0.019209
+v -0.018578 -0.012905 -0.020282
+v -0.018583 -0.012905 -0.019274
+v -0.018535 -0.012905 -0.019343
+v -0.018522 -0.012905 -0.020181
+v -0.018502 -0.012905 -0.019403
+v -0.018477 -0.012905 -0.020080
+v -0.018474 -0.012905 -0.019466
+v -0.018449 -0.012905 -0.019994
+v -0.018451 -0.012905 -0.019530
+v -0.018434 -0.012905 -0.019597
+v -0.018430 -0.012905 -0.019908
+v -0.018423 -0.012905 -0.019667
+v -0.018419 -0.012905 -0.019822
+v -0.018418 -0.012905 -0.019737
+v -0.022045 -0.012905 -0.021205
+v -0.022310 -0.012905 -0.021010
+v -0.022309 -0.012905 -0.020897
+v -0.022297 -0.012905 -0.020787
+v -0.022274 -0.012905 -0.020679
+v -0.022241 -0.012905 -0.020574
+v -0.022198 -0.012905 -0.020471
+v -0.022144 -0.012905 -0.020370
+v -0.022055 -0.012905 -0.021080
+v -0.022071 -0.012905 -0.020258
+v -0.022055 -0.012905 -0.020978
+v -0.021999 -0.012905 -0.020170
+v -0.022045 -0.012905 -0.020865
+v -0.022028 -0.012905 -0.020772
+v -0.022003 -0.012905 -0.020685
+v -0.021970 -0.012905 -0.020603
+v -0.021934 -0.012905 -0.020102
+v -0.021930 -0.012905 -0.020524
+v -0.021866 -0.012905 -0.020045
+v -0.021882 -0.012905 -0.020451
+v -0.021852 -0.012905 -0.020412
+v -0.021806 -0.012905 -0.020003
+v -0.021821 -0.012905 -0.020377
+v -0.021777 -0.012905 -0.020333
+v -0.021744 -0.012905 -0.019968
+v -0.021738 -0.012905 -0.020301
+v -0.021680 -0.012905 -0.019940
+v -0.021698 -0.012905 -0.020273
+v -0.021656 -0.012905 -0.020251
+v -0.021625 -0.012905 -0.019923
+v -0.021621 -0.012905 -0.020236
+v -0.021568 -0.012905 -0.019910
+v -0.021585 -0.012905 -0.020224
+v -0.021549 -0.012905 -0.020216
+v -0.021500 -0.012905 -0.019901
+v -0.021519 -0.012905 -0.020212
+v -0.021489 -0.012905 -0.020210
+v -0.021444 -0.012905 -0.019899
+v -0.021452 -0.012905 -0.020211
+v -0.021414 -0.012905 -0.020215
+v -0.021388 -0.012905 -0.019902
+v -0.021377 -0.012905 -0.020222
+v -0.021332 -0.012905 -0.019910
+v -0.021332 -0.012905 -0.020236
+v -0.021265 -0.012905 -0.019926
+v -0.021286 -0.012905 -0.020255
+v -0.021233 -0.012905 -0.020283
+v -0.021199 -0.012905 -0.019950
+v -0.021172 -0.012905 -0.020323
+v -0.021133 -0.012905 -0.019980
+v -0.021135 -0.012905 -0.020352
+v -0.021108 -0.012905 -0.020376
+v -0.021078 -0.012905 -0.020011
+v -0.021056 -0.012905 -0.020427
+v -0.021013 -0.012905 -0.020055
+v -0.021010 -0.012905 -0.020483
+v -0.020948 -0.012905 -0.020108
+v -0.020969 -0.012905 -0.020543
+v -0.020933 -0.012905 -0.020608
+v -0.020887 -0.012905 -0.020166
+v -0.020898 -0.012905 -0.020687
+v -0.020861 -0.012905 -0.020796
+v -0.020824 -0.012905 -0.020239
+v -0.020825 -0.012905 -0.020925
+v -0.020745 -0.012905 -0.021341
+v -0.020768 -0.012905 -0.020318
+v -0.020723 -0.012905 -0.020395
+v -0.020571 -0.012905 -0.022460
+v -0.020683 -0.012905 -0.020477
+v -0.020633 -0.012905 -0.020607
+v -0.020557 -0.012905 -0.020890
+v -0.020345 -0.012905 -0.022625
+v -0.020488 -0.012905 -0.021268
+v -0.020357 -0.012905 -0.022149
+v -0.020351 -0.012905 -0.022153
+v -0.019597 -0.012905 -0.021124
+v -0.019368 -0.012905 -0.021291
+v -0.020398 -0.013135 -0.018053
+v -0.021183 -0.013135 -0.019124
+v -0.020779 -0.013135 -0.019052
+v -0.020073 -0.013135 -0.019937
+v -0.020170 -0.013135 -0.018220
+v -0.020131 -0.013135 -0.019527
+v -0.019999 -0.013135 -0.019334
+v -0.019837 -0.013135 -0.019576
+v -0.019903 -0.013135 -0.019216
+v -0.019826 -0.013135 -0.019137
+v -0.019789 -0.013135 -0.019514
+v -0.019747 -0.013135 -0.019069
+v -0.019739 -0.013135 -0.019458
+v -0.019677 -0.013135 -0.019020
+v -0.019672 -0.013135 -0.019392
+v -0.019605 -0.013135 -0.018980
+v -0.019612 -0.013135 -0.019344
+v -0.019560 -0.013135 -0.019310
+v -0.019532 -0.013135 -0.018948
+v -0.019507 -0.013135 -0.019281
+v -0.019482 -0.013135 -0.018931
+v -0.019453 -0.013135 -0.019259
+v -0.019418 -0.013135 -0.018915
+v -0.019408 -0.013135 -0.019246
+v -0.019354 -0.013135 -0.018905
+v -0.019380 -0.013135 -0.019240
+v -0.019353 -0.013135 -0.019235
+v -0.019290 -0.013135 -0.018901
+v -0.019307 -0.013135 -0.019231
+v -0.019235 -0.013135 -0.020532
+v -0.019343 -0.013135 -0.020579
+v -0.019074 -0.013135 -0.020776
+v -0.019262 -0.013135 -0.019231
+v -0.019227 -0.013135 -0.018903
+v -0.019217 -0.013135 -0.019236
+v -0.019136 -0.013135 -0.020478
+v -0.019163 -0.013135 -0.018911
+v -0.019172 -0.013135 -0.019245
+v -0.019128 -0.013135 -0.019258
+v -0.019100 -0.013135 -0.018924
+v -0.019055 -0.013135 -0.020424
+v -0.019074 -0.013135 -0.019280
+v -0.019024 -0.013135 -0.018948
+v -0.019022 -0.013135 -0.019307
+v -0.019025 -0.013135 -0.020752
+v -0.018982 -0.013135 -0.020363
+v -0.018984 -0.013135 -0.020728
+v -0.018949 -0.013135 -0.018980
+v -0.018960 -0.013135 -0.019348
+v -0.018926 -0.013135 -0.020686
+v -0.018916 -0.013135 -0.020297
+v -0.018911 -0.013135 -0.019388
+v -0.018874 -0.013135 -0.019021
+v -0.018849 -0.013135 -0.020620
+v -0.018886 -0.013135 -0.020262
+v -0.018866 -0.013135 -0.019430
+v -0.018858 -0.013135 -0.020225
+v -0.018787 -0.013135 -0.019079
+v -0.018827 -0.013135 -0.019474
+v -0.018808 -0.013135 -0.020150
+v -0.018762 -0.013135 -0.020529
+v -0.018794 -0.013135 -0.019521
+v -0.018773 -0.013135 -0.020083
+v -0.018771 -0.013135 -0.019561
+v -0.018703 -0.013135 -0.019147
+v -0.018745 -0.013135 -0.020017
+v -0.018751 -0.013135 -0.019603
+v -0.018647 -0.013135 -0.020384
+v -0.018735 -0.013135 -0.019647
+v -0.018728 -0.013135 -0.019960
+v -0.018723 -0.013135 -0.019693
+v -0.018716 -0.013135 -0.019903
+v -0.018714 -0.013135 -0.019749
+v -0.018711 -0.013135 -0.019845
+v -0.018710 -0.013135 -0.019797
+v -0.018639 -0.013135 -0.019209
+v -0.018578 -0.013135 -0.020282
+v -0.018583 -0.013135 -0.019274
+v -0.018535 -0.013135 -0.019343
+v -0.018522 -0.013135 -0.020181
+v -0.018502 -0.013135 -0.019403
+v -0.018477 -0.013135 -0.020080
+v -0.018474 -0.013135 -0.019466
+v -0.018449 -0.013135 -0.019994
+v -0.018451 -0.013135 -0.019530
+v -0.018434 -0.013135 -0.019597
+v -0.018430 -0.013135 -0.019908
+v -0.018423 -0.013135 -0.019667
+v -0.018419 -0.013135 -0.019822
+v -0.018418 -0.013135 -0.019737
+v -0.022309 -0.013135 -0.020897
+v -0.022310 -0.013135 -0.021010
+v -0.022045 -0.013135 -0.021205
+v -0.022297 -0.013135 -0.020787
+v -0.022274 -0.013135 -0.020679
+v -0.022241 -0.013135 -0.020574
+v -0.022198 -0.013135 -0.020471
+v -0.022144 -0.013135 -0.020370
+v -0.022071 -0.013135 -0.020258
+v -0.022055 -0.013135 -0.021080
+v -0.021999 -0.013135 -0.020170
+v -0.022055 -0.013135 -0.020978
+v -0.022045 -0.013135 -0.020865
+v -0.022028 -0.013135 -0.020772
+v -0.022003 -0.013135 -0.020685
+v -0.021970 -0.013135 -0.020603
+v -0.021934 -0.013135 -0.020102
+v -0.021930 -0.013135 -0.020524
+v -0.021866 -0.013135 -0.020045
+v -0.021882 -0.013135 -0.020451
+v -0.021852 -0.013135 -0.020412
+v -0.021806 -0.013135 -0.020003
+v -0.021821 -0.013135 -0.020377
+v -0.021777 -0.013135 -0.020333
+v -0.021744 -0.013135 -0.019968
+v -0.021738 -0.013135 -0.020301
+v -0.021680 -0.013135 -0.019940
+v -0.021698 -0.013135 -0.020273
+v -0.021656 -0.013135 -0.020251
+v -0.021625 -0.013135 -0.019923
+v -0.021621 -0.013135 -0.020236
+v -0.021568 -0.013135 -0.019910
+v -0.021585 -0.013135 -0.020224
+v -0.021549 -0.013135 -0.020216
+v -0.021500 -0.013135 -0.019901
+v -0.021519 -0.013135 -0.020212
+v -0.021489 -0.013135 -0.020210
+v -0.021444 -0.013135 -0.019899
+v -0.021452 -0.013135 -0.020211
+v -0.021414 -0.013135 -0.020215
+v -0.021388 -0.013135 -0.019902
+v -0.021377 -0.013135 -0.020222
+v -0.021332 -0.013135 -0.019910
+v -0.021332 -0.013135 -0.020236
+v -0.021265 -0.013135 -0.019926
+v -0.021286 -0.013135 -0.020255
+v -0.021233 -0.013135 -0.020283
+v -0.021199 -0.013135 -0.019950
+v -0.021172 -0.013135 -0.020323
+v -0.021133 -0.013135 -0.019980
+v -0.021135 -0.013135 -0.020352
+v -0.021108 -0.013135 -0.020376
+v -0.021078 -0.013135 -0.020011
+v -0.021056 -0.013135 -0.020427
+v -0.021013 -0.013135 -0.020055
+v -0.021010 -0.013135 -0.020483
+v -0.020948 -0.013135 -0.020108
+v -0.020969 -0.013135 -0.020543
+v -0.020933 -0.013135 -0.020608
+v -0.020887 -0.013135 -0.020166
+v -0.020898 -0.013135 -0.020687
+v -0.020861 -0.013135 -0.020796
+v -0.020824 -0.013135 -0.020239
+v -0.020825 -0.013135 -0.020925
+v -0.020745 -0.013135 -0.021341
+v -0.020768 -0.013135 -0.020318
+v -0.020723 -0.013135 -0.020395
+v -0.020571 -0.013135 -0.022460
+v -0.020683 -0.013135 -0.020477
+v -0.020633 -0.013135 -0.020607
+v -0.020557 -0.013135 -0.020890
+v -0.020345 -0.013135 -0.022625
+v -0.020488 -0.013135 -0.021268
+v -0.020357 -0.013135 -0.022149
+v -0.020351 -0.013135 -0.022153
+v -0.019597 -0.013135 -0.021124
+v -0.019368 -0.013135 -0.021291
+v -0.026496 -0.012905 -0.026947
+v -0.026900 -0.012905 -0.027019
+v -0.026115 -0.012905 -0.025948
+v -0.025789 -0.012905 -0.027832
+v -0.025886 -0.012905 -0.026115
+v -0.025847 -0.012905 -0.027422
+v -0.025715 -0.012905 -0.027229
+v -0.025554 -0.012905 -0.027471
+v -0.025620 -0.012905 -0.027111
+v -0.025543 -0.012905 -0.027032
+v -0.025505 -0.012905 -0.027409
+v -0.025463 -0.012905 -0.026964
+v -0.025456 -0.012905 -0.027353
+v -0.025393 -0.012905 -0.026915
+v -0.025389 -0.012905 -0.027287
+v -0.025322 -0.012905 -0.026875
+v -0.025329 -0.012905 -0.027239
+v -0.025277 -0.012905 -0.027205
+v -0.025248 -0.012905 -0.026843
+v -0.025223 -0.012905 -0.027176
+v -0.025198 -0.012905 -0.026826
+v -0.025170 -0.012905 -0.027154
+v -0.025135 -0.012905 -0.026810
+v -0.025124 -0.012905 -0.027141
+v -0.025071 -0.012905 -0.026800
+v -0.025096 -0.012905 -0.027135
+v -0.025069 -0.012905 -0.027130
+v -0.025007 -0.012905 -0.026796
+v -0.025024 -0.012905 -0.027126
+v -0.024790 -0.012905 -0.028671
+v -0.025059 -0.012905 -0.028474
+v -0.024951 -0.012905 -0.028427
+v -0.024978 -0.012905 -0.027126
+v -0.024943 -0.012905 -0.026798
+v -0.024933 -0.012905 -0.027131
+v -0.024852 -0.012905 -0.028373
+v -0.024880 -0.012905 -0.026806
+v -0.024889 -0.012905 -0.027140
+v -0.024844 -0.012905 -0.027153
+v -0.024816 -0.012905 -0.026819
+v -0.024772 -0.012905 -0.028318
+v -0.024791 -0.012905 -0.027175
+v -0.024741 -0.012905 -0.026843
+v -0.024738 -0.012905 -0.027202
+v -0.024742 -0.012905 -0.028647
+v -0.024699 -0.012905 -0.028258
+v -0.024700 -0.012905 -0.028623
+v -0.024666 -0.012905 -0.026875
+v -0.024677 -0.012905 -0.027243
+v -0.024642 -0.012905 -0.028581
+v -0.024633 -0.012905 -0.028192
+v -0.024627 -0.012905 -0.027283
+v -0.024591 -0.012905 -0.026916
+v -0.024566 -0.012905 -0.028515
+v -0.024603 -0.012905 -0.028157
+v -0.024583 -0.012905 -0.027325
+v -0.024574 -0.012905 -0.028120
+v -0.024503 -0.012905 -0.026974
+v -0.024544 -0.012905 -0.027369
+v -0.024524 -0.012905 -0.028044
+v -0.024479 -0.012905 -0.028424
+v -0.024511 -0.012905 -0.027415
+v -0.024489 -0.012905 -0.027978
+v -0.024487 -0.012905 -0.027456
+v -0.024419 -0.012905 -0.027042
+v -0.024462 -0.012905 -0.027912
+v -0.024467 -0.012905 -0.027498
+v -0.024363 -0.012905 -0.028279
+v -0.024451 -0.012905 -0.027542
+v -0.024444 -0.012905 -0.027855
+v -0.024439 -0.012905 -0.027588
+v -0.024433 -0.012905 -0.027798
+v -0.024430 -0.012905 -0.027644
+v -0.024427 -0.012905 -0.027740
+v -0.024426 -0.012905 -0.027692
+v -0.024355 -0.012905 -0.027104
+v -0.024295 -0.012905 -0.028177
+v -0.024300 -0.012905 -0.027169
+v -0.024252 -0.012905 -0.027238
+v -0.024238 -0.012905 -0.028076
+v -0.024218 -0.012905 -0.027298
+v -0.024194 -0.012905 -0.027975
+v -0.024190 -0.012905 -0.027361
+v -0.024166 -0.012905 -0.027889
+v -0.024168 -0.012905 -0.027425
+v -0.024151 -0.012905 -0.027492
+v -0.024146 -0.012905 -0.027803
+v -0.024139 -0.012905 -0.027562
+v -0.024136 -0.012905 -0.027718
+v -0.024134 -0.012905 -0.027632
+v -0.027837 -0.012905 -0.029006
+v -0.028084 -0.012905 -0.028825
+v -0.028070 -0.012905 -0.028728
+v -0.028047 -0.012905 -0.028631
+v -0.028014 -0.012905 -0.028535
+v -0.027966 -0.012905 -0.028428
+v -0.027913 -0.012905 -0.028334
+v -0.027842 -0.012905 -0.028229
+v -0.027835 -0.012905 -0.028905
+v -0.027775 -0.012905 -0.028145
+v -0.027826 -0.012905 -0.028820
+v -0.027809 -0.012905 -0.028739
+v -0.027786 -0.012905 -0.028661
+v -0.027756 -0.012905 -0.028587
+v -0.027714 -0.012905 -0.028081
+v -0.027718 -0.012905 -0.028516
+v -0.027674 -0.012905 -0.028449
+v -0.027649 -0.012905 -0.028026
+v -0.027625 -0.012905 -0.028387
+v -0.027592 -0.012905 -0.027985
+v -0.027574 -0.012905 -0.028333
+v -0.027533 -0.012905 -0.027951
+v -0.027523 -0.012905 -0.028289
+v -0.027472 -0.012905 -0.027923
+v -0.027484 -0.012905 -0.028261
+v -0.027444 -0.012905 -0.028238
+v -0.027420 -0.012905 -0.027905
+v -0.027404 -0.012905 -0.028220
+v -0.027367 -0.012905 -0.027890
+v -0.027364 -0.012905 -0.028208
+v -0.027302 -0.012905 -0.027880
+v -0.027323 -0.012905 -0.028200
+v -0.027281 -0.012905 -0.028197
+v -0.027239 -0.012905 -0.027875
+v -0.027239 -0.012905 -0.028200
+v -0.027187 -0.012905 -0.027877
+v -0.027196 -0.012905 -0.028207
+v -0.027153 -0.012905 -0.028219
+v -0.027136 -0.012905 -0.027883
+v -0.027110 -0.012905 -0.028236
+v -0.027086 -0.012905 -0.027893
+v -0.026898 -0.012905 -0.029485
+v -0.027113 -0.012905 -0.029328
+v -0.026985 -0.012905 -0.029154
+v -0.027051 -0.012905 -0.028267
+v -0.027036 -0.012905 -0.027909
+v -0.026991 -0.012905 -0.028307
+v -0.026978 -0.012905 -0.027933
+v -0.026945 -0.012905 -0.028344
+v -0.026900 -0.012905 -0.029026
+v -0.026920 -0.012905 -0.027964
+v -0.026917 -0.012905 -0.028369
+v -0.026864 -0.012905 -0.028001
+v -0.026881 -0.012905 -0.028409
+v -0.026847 -0.012905 -0.028924
+v -0.026763 -0.012905 -0.029301
+v -0.026851 -0.012905 -0.028450
+v -0.026783 -0.012905 -0.028067
+v -0.026826 -0.012905 -0.028494
+v -0.026817 -0.012905 -0.028847
+v -0.026807 -0.012905 -0.028539
+v -0.026801 -0.012905 -0.028791
+v -0.026794 -0.012905 -0.028586
+v -0.026790 -0.012905 -0.028737
+v -0.026787 -0.012905 -0.028634
+v -0.026786 -0.012905 -0.028685
+v -0.026728 -0.012905 -0.028123
+v -0.026673 -0.012905 -0.029188
+v -0.026680 -0.012905 -0.028182
+v -0.026641 -0.012905 -0.028244
+v -0.026628 -0.012905 -0.028857
+v -0.026601 -0.012905 -0.029110
+v -0.026617 -0.012905 -0.028292
+v -0.026594 -0.012905 -0.028752
+v -0.026622 -0.012905 -0.028862
+v -0.026557 -0.012905 -0.028796
+v -0.026597 -0.012905 -0.028341
+v -0.026530 -0.012905 -0.029044
+v -0.026582 -0.012905 -0.028392
+v -0.026576 -0.012905 -0.028671
+v -0.026572 -0.012905 -0.028445
+v -0.026567 -0.012905 -0.028593
+v -0.026565 -0.012905 -0.028518
+v -0.026498 -0.012905 -0.028745
+v -0.026459 -0.012905 -0.028991
+v -0.026438 -0.012905 -0.028702
+v -0.026405 -0.012905 -0.028958
+v -0.026375 -0.012905 -0.028667
+v -0.026352 -0.012905 -0.028932
+v -0.026104 -0.012905 -0.030406
+v -0.026377 -0.012905 -0.030206
+v -0.026283 -0.012905 -0.030175
+v -0.026320 -0.012905 -0.028642
+v -0.026299 -0.012905 -0.028913
+v -0.026263 -0.012905 -0.028623
+v -0.026246 -0.012905 -0.028901
+v -0.026194 -0.012905 -0.030137
+v -0.026225 -0.012905 -0.028613
+v -0.026193 -0.012905 -0.028895
+v -0.026175 -0.012905 -0.028604
+v -0.026111 -0.012905 -0.030092
+v -0.026140 -0.012905 -0.028897
+v -0.026116 -0.012905 -0.028598
+v -0.026088 -0.012905 -0.028904
+v -0.026058 -0.012905 -0.028598
+v -0.026033 -0.012905 -0.030039
+v -0.026063 -0.012905 -0.030389
+v -0.026035 -0.012905 -0.028919
+v -0.026033 -0.012905 -0.030374
+v -0.026001 -0.012905 -0.028604
+v -0.025983 -0.012905 -0.028940
+v -0.025960 -0.012905 -0.029979
+v -0.025963 -0.012905 -0.030333
+v -0.025945 -0.012905 -0.028615
+v -0.025930 -0.012905 -0.028968
+v -0.025884 -0.012905 -0.030276
+v -0.025893 -0.012905 -0.029911
+v -0.025889 -0.012905 -0.028631
+v -0.025878 -0.012905 -0.029003
+v -0.025857 -0.012905 -0.029869
+v -0.025835 -0.012905 -0.028653
+v -0.025806 -0.012905 -0.030209
+v -0.025848 -0.012905 -0.029027
+v -0.025823 -0.012905 -0.029824
+v -0.025820 -0.012905 -0.029051
+v -0.025781 -0.012905 -0.028681
+v -0.025768 -0.012905 -0.029741
+v -0.025783 -0.012905 -0.029090
+v -0.025720 -0.012905 -0.030119
+v -0.025755 -0.012905 -0.029124
+v -0.025746 -0.012905 -0.028703
+v -0.025729 -0.012905 -0.029668
+v -0.025732 -0.012905 -0.029159
+v -0.025711 -0.012905 -0.028727
+v -0.025712 -0.012905 -0.029196
+v -0.025702 -0.012905 -0.029604
+v -0.025617 -0.012905 -0.029991
+v -0.025696 -0.012905 -0.029235
+v -0.025644 -0.012905 -0.028781
+v -0.025685 -0.012905 -0.029550
+v -0.025683 -0.012905 -0.029275
+v -0.025674 -0.012905 -0.029495
+v -0.025674 -0.012905 -0.029317
+v -0.025668 -0.012905 -0.029441
+v -0.025668 -0.012905 -0.029370
+v -0.025666 -0.012905 -0.029406
+v -0.025584 -0.012905 -0.028839
+v -0.025560 -0.012905 -0.029907
+v -0.025532 -0.012905 -0.028901
+v -0.025511 -0.012905 -0.029823
+v -0.025495 -0.012905 -0.028955
+v -0.025463 -0.012905 -0.029725
+v -0.025463 -0.012905 -0.029012
+v -0.025427 -0.012905 -0.029627
+v -0.025436 -0.012905 -0.029071
+v -0.025415 -0.012905 -0.029133
+v -0.025405 -0.012905 -0.029544
+v -0.025400 -0.012905 -0.029198
+v -0.025391 -0.012905 -0.029460
+v -0.025393 -0.012905 -0.029238
+v -0.025387 -0.012905 -0.029293
+v -0.025385 -0.012905 -0.029376
+v -0.026115 -0.013135 -0.025948
+v -0.026900 -0.013135 -0.027019
+v -0.026496 -0.013135 -0.026947
+v -0.025789 -0.013135 -0.027832
+v -0.025886 -0.013135 -0.026115
+v -0.025847 -0.013135 -0.027422
+v -0.025715 -0.013135 -0.027229
+v -0.025554 -0.013135 -0.027471
+v -0.025620 -0.013135 -0.027111
+v -0.025543 -0.013135 -0.027032
+v -0.025505 -0.013135 -0.027409
+v -0.025463 -0.013135 -0.026964
+v -0.025456 -0.013135 -0.027353
+v -0.025393 -0.013135 -0.026915
+v -0.025389 -0.013135 -0.027287
+v -0.025322 -0.013135 -0.026875
+v -0.025329 -0.013135 -0.027239
+v -0.025277 -0.013135 -0.027205
+v -0.025248 -0.013135 -0.026843
+v -0.025223 -0.013135 -0.027176
+v -0.025198 -0.013135 -0.026826
+v -0.025170 -0.013135 -0.027154
+v -0.025135 -0.013135 -0.026810
+v -0.025124 -0.013135 -0.027141
+v -0.025071 -0.013135 -0.026800
+v -0.025096 -0.013135 -0.027135
+v -0.025069 -0.013135 -0.027130
+v -0.025007 -0.013135 -0.026796
+v -0.025024 -0.013135 -0.027126
+v -0.024951 -0.013135 -0.028427
+v -0.025059 -0.013135 -0.028474
+v -0.024790 -0.013135 -0.028671
+v -0.024978 -0.013135 -0.027126
+v -0.024943 -0.013135 -0.026798
+v -0.024933 -0.013135 -0.027131
+v -0.024852 -0.013135 -0.028373
+v -0.024880 -0.013135 -0.026806
+v -0.024889 -0.013135 -0.027140
+v -0.024844 -0.013135 -0.027153
+v -0.024816 -0.013135 -0.026819
+v -0.024772 -0.013135 -0.028318
+v -0.024791 -0.013135 -0.027175
+v -0.024741 -0.013135 -0.026843
+v -0.024738 -0.013135 -0.027202
+v -0.024742 -0.013135 -0.028647
+v -0.024699 -0.013135 -0.028258
+v -0.024700 -0.013135 -0.028623
+v -0.024666 -0.013135 -0.026875
+v -0.024677 -0.013135 -0.027243
+v -0.024642 -0.013135 -0.028581
+v -0.024633 -0.013135 -0.028192
+v -0.024627 -0.013135 -0.027283
+v -0.024591 -0.013135 -0.026916
+v -0.024566 -0.013135 -0.028515
+v -0.024603 -0.013135 -0.028157
+v -0.024583 -0.013135 -0.027325
+v -0.024574 -0.013135 -0.028120
+v -0.024503 -0.013135 -0.026974
+v -0.024544 -0.013135 -0.027369
+v -0.024524 -0.013135 -0.028044
+v -0.024479 -0.013135 -0.028424
+v -0.024511 -0.013135 -0.027415
+v -0.024489 -0.013135 -0.027978
+v -0.024487 -0.013135 -0.027456
+v -0.024419 -0.013135 -0.027042
+v -0.024462 -0.013135 -0.027912
+v -0.024467 -0.013135 -0.027498
+v -0.024363 -0.013135 -0.028279
+v -0.024451 -0.013135 -0.027542
+v -0.024444 -0.013135 -0.027855
+v -0.024439 -0.013135 -0.027588
+v -0.024433 -0.013135 -0.027798
+v -0.024430 -0.013135 -0.027644
+v -0.024427 -0.013135 -0.027740
+v -0.024426 -0.013135 -0.027692
+v -0.024355 -0.013135 -0.027104
+v -0.024295 -0.013135 -0.028177
+v -0.024300 -0.013135 -0.027169
+v -0.024252 -0.013135 -0.027238
+v -0.024238 -0.013135 -0.028076
+v -0.024218 -0.013135 -0.027298
+v -0.024194 -0.013135 -0.027975
+v -0.024190 -0.013135 -0.027361
+v -0.024166 -0.013135 -0.027889
+v -0.024168 -0.013135 -0.027425
+v -0.024151 -0.013135 -0.027492
+v -0.024146 -0.013135 -0.027803
+v -0.024139 -0.013135 -0.027562
+v -0.024136 -0.013135 -0.027718
+v -0.024134 -0.013135 -0.027632
+v -0.028070 -0.013135 -0.028728
+v -0.028084 -0.013135 -0.028825
+v -0.027837 -0.013135 -0.029006
+v -0.028047 -0.013135 -0.028631
+v -0.028014 -0.013135 -0.028535
+v -0.027966 -0.013135 -0.028428
+v -0.027913 -0.013135 -0.028334
+v -0.027842 -0.013135 -0.028229
+v -0.027775 -0.013135 -0.028145
+v -0.027835 -0.013135 -0.028905
+v -0.027826 -0.013135 -0.028820
+v -0.027809 -0.013135 -0.028739
+v -0.027786 -0.013135 -0.028661
+v -0.027756 -0.013135 -0.028587
+v -0.027714 -0.013135 -0.028081
+v -0.027718 -0.013135 -0.028516
+v -0.027674 -0.013135 -0.028449
+v -0.027649 -0.013135 -0.028026
+v -0.027625 -0.013135 -0.028387
+v -0.027592 -0.013135 -0.027985
+v -0.027574 -0.013135 -0.028333
+v -0.027533 -0.013135 -0.027951
+v -0.027523 -0.013135 -0.028289
+v -0.027472 -0.013135 -0.027923
+v -0.027484 -0.013135 -0.028261
+v -0.027444 -0.013135 -0.028238
+v -0.027420 -0.013135 -0.027905
+v -0.027404 -0.013135 -0.028220
+v -0.027367 -0.013135 -0.027890
+v -0.027364 -0.013135 -0.028208
+v -0.027302 -0.013135 -0.027880
+v -0.027323 -0.013135 -0.028200
+v -0.027281 -0.013135 -0.028197
+v -0.027239 -0.013135 -0.027875
+v -0.027239 -0.013135 -0.028200
+v -0.027187 -0.013135 -0.027877
+v -0.027196 -0.013135 -0.028207
+v -0.027153 -0.013135 -0.028219
+v -0.027136 -0.013135 -0.027883
+v -0.027110 -0.013135 -0.028236
+v -0.027086 -0.013135 -0.027893
+v -0.026985 -0.013135 -0.029154
+v -0.027113 -0.013135 -0.029328
+v -0.026898 -0.013135 -0.029485
+v -0.027051 -0.013135 -0.028267
+v -0.027036 -0.013135 -0.027909
+v -0.026991 -0.013135 -0.028307
+v -0.026978 -0.013135 -0.027933
+v -0.026945 -0.013135 -0.028344
+v -0.026900 -0.013135 -0.029026
+v -0.026920 -0.013135 -0.027964
+v -0.026917 -0.013135 -0.028369
+v -0.026864 -0.013135 -0.028001
+v -0.026881 -0.013135 -0.028409
+v -0.026847 -0.013135 -0.028924
+v -0.026763 -0.013135 -0.029301
+v -0.026851 -0.013135 -0.028450
+v -0.026783 -0.013135 -0.028067
+v -0.026826 -0.013135 -0.028494
+v -0.026817 -0.013135 -0.028847
+v -0.026807 -0.013135 -0.028539
+v -0.026801 -0.013135 -0.028791
+v -0.026794 -0.013135 -0.028586
+v -0.026790 -0.013135 -0.028737
+v -0.026787 -0.013135 -0.028634
+v -0.026786 -0.013135 -0.028685
+v -0.026728 -0.013135 -0.028123
+v -0.026673 -0.013135 -0.029188
+v -0.026680 -0.013135 -0.028182
+v -0.026641 -0.013135 -0.028244
+v -0.026628 -0.013135 -0.028857
+v -0.026601 -0.013135 -0.029110
+v -0.026617 -0.013135 -0.028292
+v -0.026594 -0.013135 -0.028752
+v -0.026622 -0.013135 -0.028862
+v -0.026557 -0.013135 -0.028796
+v -0.026597 -0.013135 -0.028341
+v -0.026530 -0.013135 -0.029044
+v -0.026582 -0.013135 -0.028392
+v -0.026576 -0.013135 -0.028671
+v -0.026572 -0.013135 -0.028445
+v -0.026567 -0.013135 -0.028593
+v -0.026565 -0.013135 -0.028518
+v -0.026498 -0.013135 -0.028745
+v -0.026459 -0.013135 -0.028991
+v -0.026438 -0.013135 -0.028702
+v -0.026405 -0.013135 -0.028958
+v -0.026375 -0.013135 -0.028667
+v -0.026352 -0.013135 -0.028932
+v -0.026283 -0.013135 -0.030175
+v -0.026377 -0.013135 -0.030206
+v -0.026104 -0.013135 -0.030406
+v -0.026320 -0.013135 -0.028642
+v -0.026299 -0.013135 -0.028913
+v -0.026263 -0.013135 -0.028623
+v -0.026246 -0.013135 -0.028901
+v -0.026194 -0.013135 -0.030137
+v -0.026225 -0.013135 -0.028613
+v -0.026193 -0.013135 -0.028895
+v -0.026175 -0.013135 -0.028604
+v -0.026111 -0.013135 -0.030092
+v -0.026140 -0.013135 -0.028897
+v -0.026116 -0.013135 -0.028598
+v -0.026088 -0.013135 -0.028904
+v -0.026058 -0.013135 -0.028598
+v -0.026033 -0.013135 -0.030039
+v -0.026063 -0.013135 -0.030389
+v -0.026035 -0.013135 -0.028919
+v -0.026033 -0.013135 -0.030374
+v -0.026001 -0.013135 -0.028604
+v -0.025983 -0.013135 -0.028940
+v -0.025960 -0.013135 -0.029979
+v -0.025963 -0.013135 -0.030333
+v -0.025945 -0.013135 -0.028615
+v -0.025930 -0.013135 -0.028968
+v -0.025884 -0.013135 -0.030276
+v -0.025893 -0.013135 -0.029911
+v -0.025889 -0.013135 -0.028631
+v -0.025878 -0.013135 -0.029003
+v -0.025857 -0.013135 -0.029869
+v -0.025835 -0.013135 -0.028653
+v -0.025806 -0.013135 -0.030209
+v -0.025848 -0.013135 -0.029027
+v -0.025823 -0.013135 -0.029824
+v -0.025820 -0.013135 -0.029051
+v -0.025781 -0.013135 -0.028681
+v -0.025768 -0.013135 -0.029741
+v -0.025783 -0.013135 -0.029090
+v -0.025720 -0.013135 -0.030119
+v -0.025755 -0.013135 -0.029124
+v -0.025746 -0.013135 -0.028703
+v -0.025729 -0.013135 -0.029668
+v -0.025732 -0.013135 -0.029159
+v -0.025711 -0.013135 -0.028727
+v -0.025712 -0.013135 -0.029196
+v -0.025702 -0.013135 -0.029604
+v -0.025617 -0.013135 -0.029991
+v -0.025696 -0.013135 -0.029235
+v -0.025644 -0.013135 -0.028781
+v -0.025685 -0.013135 -0.029550
+v -0.025683 -0.013135 -0.029275
+v -0.025674 -0.013135 -0.029495
+v -0.025674 -0.013135 -0.029317
+v -0.025668 -0.013135 -0.029441
+v -0.025668 -0.013135 -0.029370
+v -0.025666 -0.013135 -0.029406
+v -0.025584 -0.013135 -0.028839
+v -0.025560 -0.013135 -0.029907
+v -0.025532 -0.013135 -0.028901
+v -0.025511 -0.013135 -0.029823
+v -0.025495 -0.013135 -0.028955
+v -0.025463 -0.013135 -0.029725
+v -0.025463 -0.013135 -0.029012
+v -0.025427 -0.013135 -0.029627
+v -0.025436 -0.013135 -0.029071
+v -0.025415 -0.013135 -0.029133
+v -0.025405 -0.013135 -0.029544
+v -0.025400 -0.013135 -0.029198
+v -0.025391 -0.013135 -0.029460
+v -0.025393 -0.013135 -0.029238
+v -0.025387 -0.013135 -0.029293
+v -0.025385 -0.013135 -0.029376
+v -0.015842 0.012840 -0.011666
+v -0.016627 0.012840 -0.012738
+v -0.016398 0.012840 -0.012905
+v -0.015789 0.012840 -0.012073
+v -0.014732 0.012840 -0.012479
+v -0.015140 0.012840 -0.012548
+v -0.015512 0.012840 -0.013294
+v -0.015513 0.012840 -0.013347
+v -0.015509 0.012840 -0.013412
+v -0.015504 0.012840 -0.013214
+v -0.015499 0.012840 -0.013476
+v -0.015487 0.012840 -0.013134
+v -0.015483 0.012840 -0.013538
+v -0.015461 0.012840 -0.013052
+v -0.015462 0.012840 -0.013598
+v -0.015436 0.012840 -0.013656
+v -0.015421 0.012840 -0.012956
+v -0.015404 0.012840 -0.013713
+v -0.015368 0.012840 -0.012859
+v -0.015359 0.012840 -0.013778
+v -0.015284 0.012840 -0.012732
+v -0.015305 0.012840 -0.013840
+v -0.015244 0.012840 -0.013899
+v -0.015191 0.012840 -0.013275
+v -0.015190 0.012840 -0.013322
+v -0.015162 0.012840 -0.013964
+v -0.015186 0.012840 -0.013217
+v -0.015188 0.012840 -0.013351
+v -0.015184 0.012840 -0.013378
+v -0.015175 0.012840 -0.013158
+v -0.015174 0.012840 -0.013423
+v -0.015158 0.012840 -0.013097
+v -0.015160 0.012840 -0.013466
+v -0.015072 0.012840 -0.014024
+v -0.015142 0.012840 -0.013507
+v -0.015131 0.012840 -0.013026
+v -0.015120 0.012840 -0.013547
+v -0.015005 0.012840 -0.012813
+v -0.015050 0.012840 -0.012878
+v -0.015089 0.012840 -0.012942
+v -0.015094 0.012840 -0.013586
+v -0.015058 0.012840 -0.013630
+v -0.014994 0.012840 -0.014066
+v -0.015015 0.012840 -0.013672
+v -0.014958 0.012840 -0.013718
+v -0.014914 0.012840 -0.014100
+v -0.014905 0.012840 -0.013753
+v -0.014834 0.012840 -0.014125
+v -0.014852 0.012840 -0.013783
+v -0.014798 0.012840 -0.013807
+v -0.014767 0.012840 -0.014138
+v -0.014743 0.012840 -0.013824
+v -0.014699 0.012840 -0.014146
+v -0.014698 0.012840 -0.013835
+v -0.014631 0.012840 -0.014148
+v -0.014651 0.012840 -0.013841
+v -0.014605 0.012840 -0.013842
+v -0.014562 0.012840 -0.014144
+v -0.014558 0.012840 -0.013840
+v -0.014492 0.012840 -0.014134
+v -0.014501 0.012840 -0.013832
+v -0.014454 0.012840 -0.013821
+v -0.014423 0.012840 -0.014118
+v -0.014408 0.012840 -0.013806
+v -0.014342 0.012840 -0.014091
+v -0.014355 0.012840 -0.013783
+v -0.014304 0.012840 -0.013755
+v -0.014264 0.012840 -0.014055
+v -0.014255 0.012840 -0.013721
+v -0.014188 0.012840 -0.014010
+v -0.014200 0.012840 -0.013675
+v -0.014148 0.012840 -0.013621
+v -0.014115 0.012840 -0.013957
+v -0.014091 0.012840 -0.013551
+v -0.014032 0.012840 -0.013885
+v -0.014065 0.012840 -0.013513
+v -0.014040 0.012840 -0.013473
+v -0.013997 0.012840 -0.013391
+v -0.013952 0.012840 -0.013800
+v -0.013961 0.012840 -0.013303
+v -0.013934 0.012840 -0.013210
+v -0.013876 0.012840 -0.013704
+v -0.013912 0.012840 -0.013099
+v -0.013900 0.012840 -0.012982
+v -0.013631 0.012840 -0.013179
+v -0.013773 0.012840 -0.013550
+v -0.013712 0.012840 -0.013440
+v -0.013672 0.012840 -0.013347
+v -0.013650 0.012840 -0.013279
+v -0.013639 0.012840 -0.013232
+v -0.014363 0.012840 -0.010033
+v -0.015158 0.012840 -0.010628
+v -0.014651 0.012840 -0.010610
+v -0.013116 0.012840 -0.012123
+v -0.014123 0.012840 -0.010209
+v -0.012931 0.012840 -0.011870
+v -0.013482 0.012840 -0.012622
+v -0.013259 0.012840 -0.012784
+v -0.012342 0.012840 -0.011532
+v -0.012565 0.012840 -0.011369
+v -0.016398 0.013232 -0.012905
+v -0.016627 0.013232 -0.012738
+v -0.015842 0.013232 -0.011666
+v -0.015789 0.013232 -0.012073
+v -0.014732 0.013232 -0.012479
+v -0.015140 0.013232 -0.012548
+v -0.015509 0.013232 -0.013412
+v -0.015513 0.013232 -0.013347
+v -0.015512 0.013232 -0.013294
+v -0.015504 0.013232 -0.013214
+v -0.015499 0.013232 -0.013476
+v -0.015487 0.013232 -0.013134
+v -0.015483 0.013232 -0.013538
+v -0.015461 0.013232 -0.013052
+v -0.015462 0.013232 -0.013598
+v -0.015436 0.013232 -0.013656
+v -0.015421 0.013232 -0.012956
+v -0.015404 0.013232 -0.013713
+v -0.015368 0.013232 -0.012859
+v -0.015359 0.013232 -0.013778
+v -0.015284 0.013232 -0.012732
+v -0.015305 0.013232 -0.013840
+v -0.015244 0.013232 -0.013899
+v -0.015191 0.013232 -0.013275
+v -0.015190 0.013232 -0.013322
+v -0.015162 0.013232 -0.013964
+v -0.015186 0.013232 -0.013217
+v -0.015188 0.013232 -0.013351
+v -0.015184 0.013232 -0.013378
+v -0.015175 0.013232 -0.013158
+v -0.015174 0.013232 -0.013423
+v -0.015158 0.013232 -0.013097
+v -0.015160 0.013232 -0.013466
+v -0.015072 0.013232 -0.014024
+v -0.015142 0.013232 -0.013507
+v -0.015131 0.013232 -0.013026
+v -0.015120 0.013232 -0.013547
+v -0.015005 0.013232 -0.012813
+v -0.015050 0.013232 -0.012878
+v -0.015089 0.013232 -0.012942
+v -0.015094 0.013232 -0.013586
+v -0.015058 0.013232 -0.013630
+v -0.014994 0.013232 -0.014066
+v -0.015015 0.013232 -0.013672
+v -0.014958 0.013232 -0.013718
+v -0.014914 0.013232 -0.014100
+v -0.014905 0.013232 -0.013753
+v -0.014834 0.013232 -0.014125
+v -0.014852 0.013232 -0.013783
+v -0.014798 0.013232 -0.013807
+v -0.014767 0.013232 -0.014138
+v -0.014743 0.013232 -0.013824
+v -0.014699 0.013232 -0.014146
+v -0.014698 0.013232 -0.013835
+v -0.014631 0.013232 -0.014148
+v -0.014651 0.013232 -0.013841
+v -0.014605 0.013232 -0.013842
+v -0.014562 0.013232 -0.014144
+v -0.014558 0.013232 -0.013840
+v -0.014492 0.013232 -0.014134
+v -0.014501 0.013232 -0.013832
+v -0.014454 0.013232 -0.013821
+v -0.014423 0.013232 -0.014118
+v -0.014408 0.013232 -0.013806
+v -0.014342 0.013232 -0.014091
+v -0.014355 0.013232 -0.013783
+v -0.014304 0.013232 -0.013755
+v -0.014264 0.013232 -0.014055
+v -0.014255 0.013232 -0.013721
+v -0.014188 0.013232 -0.014010
+v -0.014200 0.013232 -0.013675
+v -0.014148 0.013232 -0.013621
+v -0.014115 0.013232 -0.013957
+v -0.014091 0.013232 -0.013551
+v -0.014032 0.013232 -0.013885
+v -0.014065 0.013232 -0.013513
+v -0.014040 0.013232 -0.013473
+v -0.013997 0.013232 -0.013391
+v -0.013952 0.013232 -0.013800
+v -0.013961 0.013232 -0.013303
+v -0.013934 0.013232 -0.013210
+v -0.013876 0.013232 -0.013704
+v -0.013912 0.013232 -0.013099
+v -0.013900 0.013232 -0.012982
+v -0.013631 0.013232 -0.013179
+v -0.013773 0.013232 -0.013550
+v -0.013712 0.013232 -0.013440
+v -0.013672 0.013232 -0.013347
+v -0.013650 0.013232 -0.013279
+v -0.013639 0.013232 -0.013232
+v -0.014651 0.013232 -0.010610
+v -0.015158 0.013232 -0.010628
+v -0.014363 0.013232 -0.010033
+v -0.013116 0.013232 -0.012123
+v -0.014123 0.013232 -0.010209
+v -0.012931 0.013232 -0.011870
+v -0.013259 0.013232 -0.012784
+v -0.013482 0.013232 -0.012622
+v -0.012342 0.013232 -0.011532
+v -0.012565 0.013232 -0.011369
+v -0.021701 0.013002 -0.019831
+v -0.022486 0.013002 -0.020903
+v -0.022257 0.013002 -0.021070
+v -0.021648 0.013002 -0.020238
+v -0.020591 0.013002 -0.020645
+v -0.020999 0.013002 -0.020713
+v -0.021371 0.013002 -0.021459
+v -0.021372 0.013002 -0.021512
+v -0.021368 0.013002 -0.021577
+v -0.021363 0.013002 -0.021379
+v -0.021358 0.013002 -0.021641
+v -0.021346 0.013002 -0.021299
+v -0.021342 0.013002 -0.021703
+v -0.021320 0.013002 -0.021217
+v -0.021321 0.013002 -0.021764
+v -0.021295 0.013002 -0.021822
+v -0.021280 0.013002 -0.021121
+v -0.021263 0.013002 -0.021878
+v -0.021227 0.013002 -0.021024
+v -0.021218 0.013002 -0.021943
+v -0.021143 0.013002 -0.020897
+v -0.021164 0.013002 -0.022005
+v -0.021103 0.013002 -0.022064
+v -0.021050 0.013002 -0.021440
+v -0.021049 0.013002 -0.021488
+v -0.021021 0.013002 -0.022130
+v -0.021045 0.013002 -0.021382
+v -0.021047 0.013002 -0.021516
+v -0.021043 0.013002 -0.021543
+v -0.021034 0.013002 -0.021323
+v -0.021033 0.013002 -0.021588
+v -0.021017 0.013002 -0.021263
+v -0.021019 0.013002 -0.021631
+v -0.020931 0.013002 -0.022190
+v -0.021001 0.013002 -0.021672
+v -0.020990 0.013002 -0.021191
+v -0.020979 0.013002 -0.021712
+v -0.020864 0.013002 -0.020978
+v -0.020909 0.013002 -0.021043
+v -0.020948 0.013002 -0.021107
+v -0.020953 0.013002 -0.021751
+v -0.020917 0.013002 -0.021795
+v -0.020853 0.013002 -0.022232
+v -0.020874 0.013002 -0.021837
+v -0.020817 0.013002 -0.021883
+v -0.020773 0.013002 -0.022265
+v -0.020764 0.013002 -0.021919
+v -0.020693 0.013002 -0.022290
+v -0.020711 0.013002 -0.021948
+v -0.020657 0.013002 -0.021972
+v -0.020626 0.013002 -0.022304
+v -0.020602 0.013002 -0.021990
+v -0.020558 0.013002 -0.022312
+v -0.020557 0.013002 -0.022000
+v -0.020490 0.013002 -0.022313
+v -0.020510 0.013002 -0.022006
+v -0.020464 0.013002 -0.022008
+v -0.020421 0.013002 -0.022309
+v -0.020417 0.013002 -0.022006
+v -0.020351 0.013002 -0.022299
+v -0.020360 0.013002 -0.021997
+v -0.020313 0.013002 -0.021986
+v -0.020282 0.013002 -0.022283
+v -0.020267 0.013002 -0.021971
+v -0.020201 0.013002 -0.022256
+v -0.020214 0.013002 -0.021948
+v -0.020163 0.013002 -0.021920
+v -0.020123 0.013002 -0.022220
+v -0.020114 0.013002 -0.021886
+v -0.020047 0.013002 -0.022176
+v -0.020059 0.013002 -0.021840
+v -0.020007 0.013002 -0.021787
+v -0.019974 0.013002 -0.022123
+v -0.019950 0.013002 -0.021716
+v -0.019891 0.013002 -0.022050
+v -0.019924 0.013002 -0.021678
+v -0.019899 0.013002 -0.021639
+v -0.019856 0.013002 -0.021556
+v -0.019811 0.013002 -0.021965
+v -0.019820 0.013002 -0.021468
+v -0.019793 0.013002 -0.021375
+v -0.019735 0.013002 -0.021869
+v -0.019771 0.013002 -0.021264
+v -0.019759 0.013002 -0.021147
+v -0.019490 0.013002 -0.021344
+v -0.019632 0.013002 -0.021716
+v -0.019571 0.013002 -0.021605
+v -0.019531 0.013002 -0.021513
+v -0.019509 0.013002 -0.021444
+v -0.019498 0.013002 -0.021398
+v -0.021073 0.013002 -0.019112
+v -0.021073 0.013002 -0.019170
+v -0.021069 0.013002 -0.019227
+v -0.021066 0.013002 -0.019042
+v -0.021057 0.013002 -0.019295
+v -0.021051 0.013002 -0.018972
+v -0.021041 0.013002 -0.019349
+v -0.021030 0.013002 -0.018902
+v -0.021022 0.013002 -0.019402
+v -0.020995 0.013002 -0.018820
+v -0.020997 0.013002 -0.019453
+v -0.020961 0.013002 -0.019511
+v -0.020951 0.013002 -0.018738
+v -0.020919 0.013002 -0.019568
+v -0.020888 0.013002 -0.018643
+v -0.020870 0.013002 -0.019621
+v -0.020803 0.013002 -0.018539
+v -0.020824 0.013002 -0.019664
+v -0.020762 0.013002 -0.019712
+v -0.020724 0.013002 -0.018458
+v -0.020773 0.013002 -0.019078
+v -0.020774 0.013002 -0.019116
+v -0.020770 0.013002 -0.019154
+v -0.020770 0.013002 -0.019040
+v -0.020765 0.013002 -0.019183
+v -0.020761 0.013002 -0.018994
+v -0.020758 0.013002 -0.019212
+v -0.020692 0.013002 -0.019759
+v -0.020747 0.013002 -0.018948
+v -0.020746 0.013002 -0.019248
+v -0.020728 0.013002 -0.018901
+v -0.020731 0.013002 -0.019282
+v -0.020712 0.013002 -0.019315
+v -0.020699 0.013002 -0.018845
+v -0.020639 0.013002 -0.018385
+v -0.020686 0.013002 -0.019355
+v -0.020675 0.013002 -0.018805
+v -0.020618 0.013002 -0.019800
+v -0.020654 0.013002 -0.019392
+v -0.020647 0.013002 -0.018765
+v -0.020611 0.013002 -0.019434
+v -0.020592 0.013002 -0.018697
+v -0.020548 0.013002 -0.018322
+v -0.020530 0.013002 -0.019837
+v -0.020554 0.013002 -0.019480
+v -0.020529 0.013002 -0.018635
+v -0.020516 0.013002 -0.019507
+v -0.020452 0.013002 -0.018268
+v -0.020437 0.013002 -0.019867
+v -0.020460 0.013002 -0.018579
+v -0.020485 0.013002 -0.019526
+v -0.020421 0.013002 -0.019559
+v -0.020385 0.013002 -0.018529
+v -0.020350 0.013002 -0.018223
+v -0.020351 0.013002 -0.019887
+v -0.020354 0.013002 -0.019587
+v -0.020302 0.013002 -0.018485
+v -0.020284 0.013002 -0.019608
+v -0.020260 0.013002 -0.019900
+v -0.020243 0.013002 -0.018188
+v -0.020197 0.013002 -0.018441
+v -0.020211 0.013002 -0.019623
+v -0.020122 0.013002 -0.019908
+v -0.019978 0.013002 -0.018382
+v -0.020100 0.013002 -0.018411
+v -0.020126 0.013002 -0.019632
+v -0.020010 0.013002 -0.019634
+v -0.019829 0.013002 -0.019896
+v -0.019877 0.013002 -0.019629
+v -0.019456 0.013002 -0.019580
+v -0.019447 0.013002 -0.019848
+v -0.018336 0.013002 -0.019409
+v -0.018568 0.013002 -0.019706
+v -0.018562 0.013002 -0.019711
+v -0.019316 0.013002 -0.020741
+v -0.019088 0.013002 -0.020908
+v -0.018111 0.013002 -0.019574
+v -0.022257 0.013232 -0.021070
+v -0.022486 0.013232 -0.020903
+v -0.021701 0.013232 -0.019831
+v -0.021648 0.013232 -0.020238
+v -0.020591 0.013232 -0.020645
+v -0.020999 0.013232 -0.020713
+v -0.021368 0.013232 -0.021577
+v -0.021372 0.013232 -0.021512
+v -0.021371 0.013232 -0.021459
+v -0.021363 0.013232 -0.021379
+v -0.021358 0.013232 -0.021641
+v -0.021346 0.013232 -0.021299
+v -0.021342 0.013232 -0.021703
+v -0.021320 0.013232 -0.021217
+v -0.021321 0.013232 -0.021764
+v -0.021295 0.013232 -0.021822
+v -0.021280 0.013232 -0.021121
+v -0.021263 0.013232 -0.021878
+v -0.021227 0.013232 -0.021024
+v -0.021218 0.013232 -0.021943
+v -0.021143 0.013232 -0.020897
+v -0.021164 0.013232 -0.022005
+v -0.021103 0.013232 -0.022064
+v -0.021050 0.013232 -0.021440
+v -0.021049 0.013232 -0.021488
+v -0.021021 0.013232 -0.022130
+v -0.021045 0.013232 -0.021382
+v -0.021047 0.013232 -0.021516
+v -0.021043 0.013232 -0.021543
+v -0.021034 0.013232 -0.021323
+v -0.021033 0.013232 -0.021588
+v -0.021017 0.013232 -0.021263
+v -0.021019 0.013232 -0.021631
+v -0.020931 0.013232 -0.022190
+v -0.021001 0.013232 -0.021672
+v -0.020990 0.013232 -0.021191
+v -0.020979 0.013232 -0.021712
+v -0.020864 0.013232 -0.020978
+v -0.020909 0.013232 -0.021043
+v -0.020948 0.013232 -0.021107
+v -0.020953 0.013232 -0.021751
+v -0.020917 0.013232 -0.021795
+v -0.020853 0.013232 -0.022232
+v -0.020874 0.013232 -0.021837
+v -0.020817 0.013232 -0.021883
+v -0.020773 0.013232 -0.022265
+v -0.020764 0.013232 -0.021919
+v -0.020693 0.013232 -0.022290
+v -0.020711 0.013232 -0.021948
+v -0.020657 0.013232 -0.021972
+v -0.020626 0.013232 -0.022304
+v -0.020602 0.013232 -0.021990
+v -0.020558 0.013232 -0.022312
+v -0.020557 0.013232 -0.022000
+v -0.020490 0.013232 -0.022313
+v -0.020510 0.013232 -0.022006
+v -0.020464 0.013232 -0.022008
+v -0.020421 0.013232 -0.022309
+v -0.020417 0.013232 -0.022006
+v -0.020351 0.013232 -0.022299
+v -0.020360 0.013232 -0.021997
+v -0.020313 0.013232 -0.021986
+v -0.020282 0.013232 -0.022283
+v -0.020267 0.013232 -0.021971
+v -0.020201 0.013232 -0.022256
+v -0.020214 0.013232 -0.021948
+v -0.020163 0.013232 -0.021920
+v -0.020123 0.013232 -0.022220
+v -0.020114 0.013232 -0.021886
+v -0.020047 0.013232 -0.022176
+v -0.020059 0.013232 -0.021840
+v -0.020007 0.013232 -0.021787
+v -0.019974 0.013232 -0.022123
+v -0.019950 0.013232 -0.021716
+v -0.019891 0.013232 -0.022050
+v -0.019924 0.013232 -0.021678
+v -0.019899 0.013232 -0.021639
+v -0.019856 0.013232 -0.021556
+v -0.019811 0.013232 -0.021965
+v -0.019820 0.013232 -0.021468
+v -0.019793 0.013232 -0.021375
+v -0.019735 0.013232 -0.021869
+v -0.019771 0.013232 -0.021264
+v -0.019759 0.013232 -0.021147
+v -0.019490 0.013232 -0.021344
+v -0.019632 0.013232 -0.021716
+v -0.019571 0.013232 -0.021605
+v -0.019531 0.013232 -0.021513
+v -0.019509 0.013232 -0.021444
+v -0.019498 0.013232 -0.021398
+v -0.021069 0.013232 -0.019227
+v -0.021073 0.013232 -0.019170
+v -0.021073 0.013232 -0.019112
+v -0.021066 0.013232 -0.019042
+v -0.021057 0.013232 -0.019295
+v -0.021051 0.013232 -0.018972
+v -0.021041 0.013232 -0.019349
+v -0.021030 0.013232 -0.018902
+v -0.021022 0.013232 -0.019402
+v -0.020995 0.013232 -0.018820
+v -0.020997 0.013232 -0.019453
+v -0.020961 0.013232 -0.019511
+v -0.020951 0.013232 -0.018738
+v -0.020919 0.013232 -0.019568
+v -0.020888 0.013232 -0.018643
+v -0.020870 0.013232 -0.019621
+v -0.020803 0.013232 -0.018539
+v -0.020824 0.013232 -0.019664
+v -0.020762 0.013232 -0.019712
+v -0.020773 0.013232 -0.019078
+v -0.020724 0.013232 -0.018458
+v -0.020774 0.013232 -0.019116
+v -0.020770 0.013232 -0.019154
+v -0.020770 0.013232 -0.019040
+v -0.020765 0.013232 -0.019183
+v -0.020761 0.013232 -0.018994
+v -0.020758 0.013232 -0.019212
+v -0.020692 0.013232 -0.019759
+v -0.020747 0.013232 -0.018948
+v -0.020746 0.013232 -0.019248
+v -0.020728 0.013232 -0.018901
+v -0.020731 0.013232 -0.019282
+v -0.020712 0.013232 -0.019315
+v -0.020699 0.013232 -0.018845
+v -0.020639 0.013232 -0.018385
+v -0.020686 0.013232 -0.019355
+v -0.020675 0.013232 -0.018805
+v -0.020618 0.013232 -0.019800
+v -0.020654 0.013232 -0.019392
+v -0.020647 0.013232 -0.018765
+v -0.020611 0.013232 -0.019434
+v -0.020592 0.013232 -0.018697
+v -0.020548 0.013232 -0.018322
+v -0.020530 0.013232 -0.019837
+v -0.020554 0.013232 -0.019480
+v -0.020529 0.013232 -0.018635
+v -0.020516 0.013232 -0.019507
+v -0.020452 0.013232 -0.018268
+v -0.020437 0.013232 -0.019867
+v -0.020460 0.013232 -0.018579
+v -0.020485 0.013232 -0.019526
+v -0.020421 0.013232 -0.019559
+v -0.020385 0.013232 -0.018529
+v -0.020350 0.013232 -0.018223
+v -0.020351 0.013232 -0.019887
+v -0.020354 0.013232 -0.019587
+v -0.020302 0.013232 -0.018485
+v -0.020284 0.013232 -0.019608
+v -0.020260 0.013232 -0.019900
+v -0.020243 0.013232 -0.018188
+v -0.020197 0.013232 -0.018441
+v -0.020211 0.013232 -0.019623
+v -0.020122 0.013232 -0.019908
+v -0.020100 0.013232 -0.018411
+v -0.019978 0.013232 -0.018382
+v -0.020126 0.013232 -0.019632
+v -0.020010 0.013232 -0.019634
+v -0.019829 0.013232 -0.019896
+v -0.019877 0.013232 -0.019629
+v -0.019456 0.013232 -0.019580
+v -0.019447 0.013232 -0.019848
+v -0.018336 0.013232 -0.019409
+v -0.018568 0.013232 -0.019706
+v -0.019088 0.013232 -0.020908
+v -0.019316 0.013232 -0.020741
+v -0.018562 0.013232 -0.019711
+v -0.018111 0.013232 -0.019574
+v -0.027378 0.013002 -0.027672
+v -0.028163 0.013002 -0.028744
+v -0.027934 0.013002 -0.028911
+v -0.027325 0.013002 -0.028079
+v -0.026268 0.013002 -0.028485
+v -0.026676 0.013002 -0.028554
+v -0.027048 0.013002 -0.029300
+v -0.027049 0.013002 -0.029353
+v -0.027044 0.013002 -0.029418
+v -0.027039 0.013002 -0.029220
+v -0.027035 0.013002 -0.029482
+v -0.027022 0.013002 -0.029140
+v -0.027019 0.013002 -0.029544
+v -0.026997 0.013002 -0.029058
+v -0.026998 0.013002 -0.029604
+v -0.026972 0.013002 -0.029663
+v -0.026956 0.013002 -0.028962
+v -0.026940 0.013002 -0.029719
+v -0.026904 0.013002 -0.028865
+v -0.026895 0.013002 -0.029784
+v -0.026820 0.013002 -0.028738
+v -0.026841 0.013002 -0.029846
+v -0.026780 0.013002 -0.029905
+v -0.026727 0.013002 -0.029281
+v -0.026726 0.013002 -0.029329
+v -0.026698 0.013002 -0.029971
+v -0.026722 0.013002 -0.029223
+v -0.026724 0.013002 -0.029357
+v -0.026720 0.013002 -0.029384
+v -0.026711 0.013002 -0.029164
+v -0.026710 0.013002 -0.029429
+v -0.026694 0.013002 -0.029104
+v -0.026696 0.013002 -0.029472
+v -0.026608 0.013002 -0.030031
+v -0.026678 0.013002 -0.029513
+v -0.026666 0.013002 -0.029032
+v -0.026656 0.013002 -0.029553
+v -0.026541 0.013002 -0.028819
+v -0.026586 0.013002 -0.028884
+v -0.026624 0.013002 -0.028948
+v -0.026630 0.013002 -0.029592
+v -0.026593 0.013002 -0.029636
+v -0.026529 0.013002 -0.030073
+v -0.026551 0.013002 -0.029678
+v -0.026494 0.013002 -0.029724
+v -0.026450 0.013002 -0.030106
+v -0.026441 0.013002 -0.029760
+v -0.026370 0.013002 -0.030131
+v -0.026388 0.013002 -0.029789
+v -0.026334 0.013002 -0.029813
+v -0.026303 0.013002 -0.030145
+v -0.026279 0.013002 -0.029831
+v -0.026235 0.013002 -0.030152
+v -0.026233 0.013002 -0.029841
+v -0.026166 0.013002 -0.030154
+v -0.026187 0.013002 -0.029847
+v -0.026141 0.013002 -0.029849
+v -0.026097 0.013002 -0.030150
+v -0.026094 0.013002 -0.029846
+v -0.026028 0.013002 -0.030140
+v -0.026037 0.013002 -0.029838
+v -0.025990 0.013002 -0.029827
+v -0.025959 0.013002 -0.030124
+v -0.025944 0.013002 -0.029812
+v -0.025878 0.013002 -0.030097
+v -0.025891 0.013002 -0.029789
+v -0.025840 0.013002 -0.029761
+v -0.025800 0.013002 -0.030061
+v -0.025791 0.013002 -0.029727
+v -0.025724 0.013002 -0.030017
+v -0.025736 0.013002 -0.029681
+v -0.025684 0.013002 -0.029628
+v -0.025650 0.013002 -0.029964
+v -0.025627 0.013002 -0.029557
+v -0.025568 0.013002 -0.029891
+v -0.025600 0.013002 -0.029519
+v -0.025576 0.013002 -0.029480
+v -0.025533 0.013002 -0.029397
+v -0.025488 0.013002 -0.029806
+v -0.025497 0.013002 -0.029309
+v -0.025469 0.013002 -0.029216
+v -0.025412 0.013002 -0.029710
+v -0.025448 0.013002 -0.029105
+v -0.025436 0.013002 -0.028988
+v -0.025167 0.013002 -0.029185
+v -0.025309 0.013002 -0.029556
+v -0.025248 0.013002 -0.029446
+v -0.025208 0.013002 -0.029353
+v -0.025186 0.013002 -0.029285
+v -0.025175 0.013002 -0.029239
+v -0.026689 0.013002 -0.026853
+v -0.026691 0.013002 -0.026909
+v -0.026688 0.013002 -0.026964
+v -0.026681 0.013002 -0.026787
+v -0.026679 0.013002 -0.027029
+v -0.026666 0.013002 -0.026721
+v -0.026664 0.013002 -0.027091
+v -0.026645 0.013002 -0.026654
+v -0.026647 0.013002 -0.027140
+v -0.026626 0.013002 -0.027186
+v -0.026611 0.013002 -0.026576
+v -0.026601 0.013002 -0.027231
+v -0.026569 0.013002 -0.026498
+v -0.026571 0.013002 -0.027274
+v -0.026530 0.013002 -0.027322
+v -0.026509 0.013002 -0.026409
+v -0.026484 0.013002 -0.027368
+v -0.026430 0.013002 -0.026310
+v -0.026431 0.013002 -0.027410
+v -0.026344 0.013002 -0.027467
+v -0.026356 0.013002 -0.026231
+v -0.026385 0.013002 -0.026829
+v -0.026385 0.013002 -0.026871
+v -0.026380 0.013002 -0.026785
+v -0.026380 0.013002 -0.026913
+v -0.026371 0.013002 -0.026741
+v -0.026370 0.013002 -0.026953
+v -0.026356 0.013002 -0.026695
+v -0.026355 0.013002 -0.026993
+v -0.026269 0.013002 -0.026153
+v -0.026329 0.013002 -0.026633
+v -0.026335 0.013002 -0.027031
+v -0.026273 0.013002 -0.027503
+v -0.026311 0.013002 -0.027068
+v -0.026293 0.013002 -0.026568
+v -0.026281 0.013002 -0.027105
+v -0.026248 0.013002 -0.026502
+v -0.026234 0.013002 -0.027152
+v -0.026203 0.013002 -0.027531
+v -0.026188 0.013002 -0.026092
+v -0.026198 0.013002 -0.026440
+v -0.026178 0.013002 -0.027197
+v -0.026132 0.013002 -0.027550
+v -0.026142 0.013002 -0.026383
+v -0.026103 0.013002 -0.026041
+v -0.026129 0.013002 -0.027230
+v -0.026080 0.013002 -0.026332
+v -0.026079 0.013002 -0.027558
+v -0.026096 0.013002 -0.027248
+v -0.026014 0.013002 -0.025998
+v -0.026048 0.013002 -0.027271
+v -0.026013 0.013002 -0.026286
+v -0.026026 0.013002 -0.027562
+v -0.025999 0.013002 -0.027287
+v -0.025973 0.013002 -0.027561
+v -0.025767 0.013002 -0.026179
+v -0.025863 0.013002 -0.026211
+v -0.025941 0.013002 -0.026246
+v -0.025950 0.013002 -0.027298
+v -0.025920 0.013002 -0.027555
+v -0.025901 0.013002 -0.027302
+v -0.025848 0.013002 -0.027539
+v -0.025853 0.013002 -0.027300
+v -0.025804 0.013002 -0.027292
+v -0.025777 0.013002 -0.027515
+v -0.025756 0.013002 -0.027278
+v -0.025705 0.013002 -0.027482
+v -0.025707 0.013002 -0.027258
+v -0.025659 0.013002 -0.027232
+v -0.025633 0.013002 -0.027441
+v -0.025610 0.013002 -0.027200
+v -0.025656 0.013002 -0.027735
+v -0.025657 0.013002 -0.027795
+v -0.025655 0.013002 -0.027835
+v -0.025649 0.013002 -0.027675
+v -0.025649 0.013002 -0.027885
+v -0.025634 0.013002 -0.027605
+v -0.025636 0.013002 -0.027943
+v -0.025619 0.013002 -0.027998
+v -0.025611 0.013002 -0.027534
+v -0.025543 0.013002 -0.027377
+v -0.025596 0.013002 -0.028051
+v -0.025581 0.013002 -0.027463
+v -0.025546 0.013002 -0.027147
+v -0.025569 0.013002 -0.028101
+v -0.025537 0.013002 -0.027381
+v -0.025536 0.013002 -0.028149
+v -0.025465 0.013002 -0.027067
+v -0.025499 0.013002 -0.028194
+v -0.025456 0.013002 -0.028237
+v -0.025369 0.013002 -0.026947
+v -0.025425 0.013002 -0.028264
+v -0.025391 0.013002 -0.027674
+v -0.025391 0.013002 -0.028290
+v -0.025387 0.013002 -0.027728
+v -0.025319 0.013002 -0.028338
+v -0.025389 0.013002 -0.027617
+v -0.025380 0.013002 -0.027559
+v -0.025376 0.013002 -0.027780
+v -0.025366 0.013002 -0.027498
+v -0.025359 0.013002 -0.027830
+v -0.025241 0.013002 -0.026773
+v -0.025336 0.013002 -0.027414
+v -0.025336 0.013002 -0.027878
+v -0.025295 0.013002 -0.027326
+v -0.025306 0.013002 -0.027923
+v -0.025246 0.013002 -0.028378
+v -0.025270 0.013002 -0.027967
+v -0.025242 0.013002 -0.027234
+v -0.025227 0.013002 -0.028008
+v -0.025172 0.013002 -0.028408
+v -0.025161 0.013002 -0.027114
+v -0.025026 0.013002 -0.026930
+v -0.025178 0.013002 -0.028047
+v -0.025147 0.013002 -0.028069
+v -0.025108 0.013002 -0.028428
+v -0.025115 0.013002 -0.028088
+v -0.025067 0.013002 -0.028112
+v -0.025045 0.013002 -0.028441
+v -0.025026 0.013002 -0.028128
+v -0.024980 0.013002 -0.028448
+v -0.024985 0.013002 -0.028140
+v -0.024944 0.013002 -0.028148
+v -0.024914 0.013002 -0.028449
+v -0.024902 0.013002 -0.028152
+v -0.024848 0.013002 -0.028445
+v -0.024860 0.013002 -0.028152
+v -0.024817 0.013002 -0.028148
+v -0.024808 0.013002 -0.028439
+v -0.024765 0.013002 -0.028138
+v -0.024754 0.013002 -0.028428
+v -0.024731 0.013002 -0.028128
+v -0.024674 0.013002 -0.028405
+v -0.024697 0.013002 -0.028116
+v -0.024647 0.013002 -0.028094
+v -0.024596 0.013002 -0.028374
+v -0.024599 0.013002 -0.028066
+v -0.024552 0.013002 -0.028034
+v -0.024520 0.013002 -0.028336
+v -0.024499 0.013002 -0.027990
+v -0.024447 0.013002 -0.028289
+v -0.024441 0.013002 -0.027930
+v -0.024365 0.013002 -0.028225
+v -0.024379 0.013002 -0.027853
+v -0.024347 0.013002 -0.027807
+v -0.024286 0.013002 -0.028151
+v -0.024317 0.013002 -0.027759
+v -0.024273 0.013002 -0.027675
+v -0.024220 0.013002 -0.028078
+v -0.024238 0.013002 -0.027587
+v -0.024211 0.013002 -0.027497
+v -0.024158 0.013002 -0.027998
+v -0.024193 0.013002 -0.027404
+v -0.024183 0.013002 -0.027308
+v -0.024182 0.013002 -0.027209
+v -0.023909 0.013002 -0.027409
+v -0.024066 0.013002 -0.027861
+v -0.024007 0.013002 -0.027753
+v -0.023966 0.013002 -0.027658
+v -0.023936 0.013002 -0.027566
+v -0.023918 0.013002 -0.027487
+v -0.023913 0.013002 -0.027453
+v -0.027934 0.013232 -0.028911
+v -0.028163 0.013232 -0.028744
+v -0.027378 0.013232 -0.027672
+v -0.027325 0.013232 -0.028079
+v -0.026268 0.013232 -0.028485
+v -0.026676 0.013232 -0.028554
+v -0.027044 0.013232 -0.029418
+v -0.027049 0.013232 -0.029353
+v -0.027048 0.013232 -0.029300
+v -0.027039 0.013232 -0.029220
+v -0.027035 0.013232 -0.029482
+v -0.027022 0.013232 -0.029140
+v -0.027019 0.013232 -0.029544
+v -0.026997 0.013232 -0.029058
+v -0.026998 0.013232 -0.029604
+v -0.026972 0.013232 -0.029663
+v -0.026956 0.013232 -0.028962
+v -0.026940 0.013232 -0.029719
+v -0.026904 0.013232 -0.028865
+v -0.026895 0.013232 -0.029784
+v -0.026820 0.013232 -0.028738
+v -0.026841 0.013232 -0.029846
+v -0.026780 0.013232 -0.029905
+v -0.026727 0.013232 -0.029281
+v -0.026726 0.013232 -0.029329
+v -0.026698 0.013232 -0.029971
+v -0.026722 0.013232 -0.029223
+v -0.026724 0.013232 -0.029357
+v -0.026720 0.013232 -0.029384
+v -0.026711 0.013232 -0.029164
+v -0.026710 0.013232 -0.029429
+v -0.026694 0.013232 -0.029104
+v -0.026696 0.013232 -0.029472
+v -0.026608 0.013232 -0.030031
+v -0.026678 0.013232 -0.029513
+v -0.026666 0.013232 -0.029032
+v -0.026656 0.013232 -0.029553
+v -0.026541 0.013232 -0.028819
+v -0.026586 0.013232 -0.028884
+v -0.026624 0.013232 -0.028948
+v -0.026630 0.013232 -0.029592
+v -0.026593 0.013232 -0.029636
+v -0.026529 0.013232 -0.030073
+v -0.026551 0.013232 -0.029678
+v -0.026494 0.013232 -0.029724
+v -0.026450 0.013232 -0.030106
+v -0.026441 0.013232 -0.029760
+v -0.026370 0.013232 -0.030131
+v -0.026388 0.013232 -0.029789
+v -0.026334 0.013232 -0.029813
+v -0.026303 0.013232 -0.030145
+v -0.026279 0.013232 -0.029831
+v -0.026235 0.013232 -0.030152
+v -0.026233 0.013232 -0.029841
+v -0.026166 0.013232 -0.030154
+v -0.026187 0.013232 -0.029847
+v -0.026141 0.013232 -0.029849
+v -0.026097 0.013232 -0.030150
+v -0.026094 0.013232 -0.029846
+v -0.026028 0.013232 -0.030140
+v -0.026037 0.013232 -0.029838
+v -0.025990 0.013232 -0.029827
+v -0.025959 0.013232 -0.030124
+v -0.025944 0.013232 -0.029812
+v -0.025878 0.013232 -0.030097
+v -0.025891 0.013232 -0.029789
+v -0.025840 0.013232 -0.029761
+v -0.025800 0.013232 -0.030061
+v -0.025791 0.013232 -0.029727
+v -0.025724 0.013232 -0.030017
+v -0.025736 0.013232 -0.029681
+v -0.025684 0.013232 -0.029628
+v -0.025650 0.013232 -0.029964
+v -0.025627 0.013232 -0.029557
+v -0.025568 0.013232 -0.029891
+v -0.025600 0.013232 -0.029519
+v -0.025576 0.013232 -0.029480
+v -0.025533 0.013232 -0.029397
+v -0.025488 0.013232 -0.029806
+v -0.025497 0.013232 -0.029309
+v -0.025469 0.013232 -0.029216
+v -0.025412 0.013232 -0.029710
+v -0.025448 0.013232 -0.029105
+v -0.025436 0.013232 -0.028988
+v -0.025167 0.013232 -0.029185
+v -0.025309 0.013232 -0.029556
+v -0.025248 0.013232 -0.029446
+v -0.025208 0.013232 -0.029353
+v -0.025186 0.013232 -0.029285
+v -0.025175 0.013232 -0.029239
+v -0.026688 0.013232 -0.026964
+v -0.026691 0.013232 -0.026909
+v -0.026689 0.013232 -0.026853
+v -0.026681 0.013232 -0.026787
+v -0.026679 0.013232 -0.027029
+v -0.026666 0.013232 -0.026721
+v -0.026664 0.013232 -0.027091
+v -0.026645 0.013232 -0.026654
+v -0.026647 0.013232 -0.027140
+v -0.026626 0.013232 -0.027186
+v -0.026611 0.013232 -0.026576
+v -0.026601 0.013232 -0.027231
+v -0.026569 0.013232 -0.026498
+v -0.026571 0.013232 -0.027274
+v -0.026530 0.013232 -0.027322
+v -0.026509 0.013232 -0.026409
+v -0.026484 0.013232 -0.027368
+v -0.026430 0.013232 -0.026310
+v -0.026431 0.013232 -0.027410
+v -0.026344 0.013232 -0.027467
+v -0.026385 0.013232 -0.026829
+v -0.026356 0.013232 -0.026231
+v -0.026385 0.013232 -0.026871
+v -0.026380 0.013232 -0.026785
+v -0.026380 0.013232 -0.026913
+v -0.026371 0.013232 -0.026741
+v -0.026370 0.013232 -0.026953
+v -0.026356 0.013232 -0.026695
+v -0.026355 0.013232 -0.026993
+v -0.026269 0.013232 -0.026153
+v -0.026329 0.013232 -0.026633
+v -0.026335 0.013232 -0.027031
+v -0.026273 0.013232 -0.027503
+v -0.026311 0.013232 -0.027068
+v -0.026293 0.013232 -0.026568
+v -0.026281 0.013232 -0.027105
+v -0.026248 0.013232 -0.026502
+v -0.026234 0.013232 -0.027152
+v -0.026203 0.013232 -0.027531
+v -0.026188 0.013232 -0.026092
+v -0.026198 0.013232 -0.026440
+v -0.026178 0.013232 -0.027197
+v -0.026132 0.013232 -0.027550
+v -0.026142 0.013232 -0.026383
+v -0.026103 0.013232 -0.026041
+v -0.026129 0.013232 -0.027230
+v -0.026080 0.013232 -0.026332
+v -0.026079 0.013232 -0.027558
+v -0.026096 0.013232 -0.027248
+v -0.026014 0.013232 -0.025998
+v -0.026048 0.013232 -0.027271
+v -0.026013 0.013232 -0.026286
+v -0.026026 0.013232 -0.027562
+v -0.025999 0.013232 -0.027287
+v -0.025973 0.013232 -0.027561
+v -0.025863 0.013232 -0.026211
+v -0.025767 0.013232 -0.026179
+v -0.025941 0.013232 -0.026246
+v -0.025950 0.013232 -0.027298
+v -0.025920 0.013232 -0.027555
+v -0.025901 0.013232 -0.027302
+v -0.025848 0.013232 -0.027539
+v -0.025853 0.013232 -0.027300
+v -0.025804 0.013232 -0.027292
+v -0.025777 0.013232 -0.027515
+v -0.025756 0.013232 -0.027278
+v -0.025705 0.013232 -0.027482
+v -0.025707 0.013232 -0.027258
+v -0.025659 0.013232 -0.027232
+v -0.025633 0.013232 -0.027441
+v -0.025610 0.013232 -0.027200
+v -0.025655 0.013232 -0.027835
+v -0.025657 0.013232 -0.027795
+v -0.025656 0.013232 -0.027735
+v -0.025649 0.013232 -0.027675
+v -0.025649 0.013232 -0.027885
+v -0.025634 0.013232 -0.027605
+v -0.025636 0.013232 -0.027943
+v -0.025619 0.013232 -0.027998
+v -0.025611 0.013232 -0.027534
+v -0.025543 0.013232 -0.027377
+v -0.025596 0.013232 -0.028051
+v -0.025581 0.013232 -0.027463
+v -0.025546 0.013232 -0.027147
+v -0.025569 0.013232 -0.028101
+v -0.025537 0.013232 -0.027381
+v -0.025536 0.013232 -0.028149
+v -0.025465 0.013232 -0.027067
+v -0.025499 0.013232 -0.028194
+v -0.025456 0.013232 -0.028237
+v -0.025369 0.013232 -0.026947
+v -0.025425 0.013232 -0.028264
+v -0.025391 0.013232 -0.027674
+v -0.025391 0.013232 -0.028290
+v -0.025387 0.013232 -0.027728
+v -0.025319 0.013232 -0.028338
+v -0.025389 0.013232 -0.027617
+v -0.025380 0.013232 -0.027559
+v -0.025376 0.013232 -0.027780
+v -0.025366 0.013232 -0.027498
+v -0.025359 0.013232 -0.027830
+v -0.025241 0.013232 -0.026773
+v -0.025336 0.013232 -0.027414
+v -0.025336 0.013232 -0.027878
+v -0.025295 0.013232 -0.027326
+v -0.025306 0.013232 -0.027923
+v -0.025246 0.013232 -0.028378
+v -0.025270 0.013232 -0.027967
+v -0.025242 0.013232 -0.027234
+v -0.025227 0.013232 -0.028008
+v -0.025172 0.013232 -0.028408
+v -0.025161 0.013232 -0.027114
+v -0.025026 0.013232 -0.026930
+v -0.025178 0.013232 -0.028047
+v -0.025147 0.013232 -0.028069
+v -0.025108 0.013232 -0.028428
+v -0.025115 0.013232 -0.028088
+v -0.025067 0.013232 -0.028112
+v -0.025045 0.013232 -0.028441
+v -0.025026 0.013232 -0.028128
+v -0.024980 0.013232 -0.028448
+v -0.024985 0.013232 -0.028140
+v -0.024944 0.013232 -0.028148
+v -0.024914 0.013232 -0.028449
+v -0.024902 0.013232 -0.028152
+v -0.024848 0.013232 -0.028445
+v -0.024860 0.013232 -0.028152
+v -0.024817 0.013232 -0.028148
+v -0.024808 0.013232 -0.028439
+v -0.024765 0.013232 -0.028138
+v -0.024754 0.013232 -0.028428
+v -0.024731 0.013232 -0.028128
+v -0.024674 0.013232 -0.028405
+v -0.024697 0.013232 -0.028116
+v -0.024647 0.013232 -0.028094
+v -0.024596 0.013232 -0.028374
+v -0.024599 0.013232 -0.028066
+v -0.024552 0.013232 -0.028034
+v -0.024520 0.013232 -0.028336
+v -0.024499 0.013232 -0.027990
+v -0.024447 0.013232 -0.028289
+v -0.024441 0.013232 -0.027930
+v -0.024365 0.013232 -0.028225
+v -0.024379 0.013232 -0.027853
+v -0.024347 0.013232 -0.027807
+v -0.024286 0.013232 -0.028151
+v -0.024317 0.013232 -0.027759
+v -0.024273 0.013232 -0.027675
+v -0.024220 0.013232 -0.028078
+v -0.024238 0.013232 -0.027587
+v -0.024211 0.013232 -0.027497
+v -0.024158 0.013232 -0.027998
+v -0.024193 0.013232 -0.027404
+v -0.024183 0.013232 -0.027308
+v -0.024182 0.013232 -0.027209
+v -0.023909 0.013232 -0.027409
+v -0.024066 0.013232 -0.027861
+v -0.024007 0.013232 -0.027753
+v -0.023966 0.013232 -0.027658
+v -0.023936 0.013232 -0.027566
+v -0.023918 0.013232 -0.027487
+v -0.023913 0.013232 -0.027453
+vt 0.000000 0.444594
+vt 0.250000 0.444594
+vt 0.250000 1.000000
+vt 0.000000 1.000000
+vt 0.500000 0.444594
+vt 0.500000 1.000000
+vt 0.750000 0.444594
+vt 0.750000 1.000000
+vt 1.000000 0.444594
+vt 1.000000 1.000000
+vt 0.000000 0.000000
+vt 0.250000 0.000000
+vt 0.500000 0.000000
+vt 0.750000 0.000000
+vt 1.000000 0.000000
+vt 0.027692 1.000000
+vt 0.027692 0.000000
+vt 0.035774 1.000000
+vt 0.035774 0.000000
+vt 0.043661 1.000000
+vt 0.043661 0.000000
+vt 0.051421 1.000000
+vt 0.051421 0.000000
+vt 0.059125 1.000000
+vt 0.059125 0.000000
+vt 0.066846 1.000000
+vt 0.066846 0.000000
+vt 0.074655 1.000000
+vt 0.074655 0.000000
+vt 0.079186 1.000000
+vt 0.079186 0.000000
+vt 0.083780 1.000000
+vt 0.083780 0.000000
+vt 0.091931 1.000000
+vt 0.091931 0.000000
+vt 0.098723 1.000000
+vt 0.098723 0.000000
+vt 0.104371 1.000000
+vt 0.104371 0.000000
+vt 0.109031 1.000000
+vt 0.109031 0.000000
+vt 0.113563 1.000000
+vt 0.113563 0.000000
+vt 0.118005 1.000000
+vt 0.118005 0.000000
+vt 0.120937 1.000000
+vt 0.120937 0.000000
+vt 0.123859 1.000000
+vt 0.123859 0.000000
+vt 0.128170 1.000000
+vt 0.128170 0.000000
+vt 0.131675 1.000000
+vt 0.131675 0.000000
+vt 0.135131 1.000000
+vt 0.135131 0.000000
+vt 0.138567 1.000000
+vt 0.138567 0.000000
+vt 0.142015 1.000000
+vt 0.142015 0.000000
+vt 0.145503 1.000000
+vt 0.145503 0.000000
+vt 0.149062 1.000000
+vt 0.149062 0.000000
+vt 0.153464 1.000000
+vt 0.153464 0.000000
+vt 0.156499 1.000000
+vt 0.156499 0.000000
+vt 0.159627 1.000000
+vt 0.159627 0.000000
+vt 0.164749 1.000000
+vt 0.164749 0.000000
+vt 0.169597 1.000000
+vt 0.169597 0.000000
+vt 0.174220 1.000000
+vt 0.174220 0.000000
+vt 0.178675 1.000000
+vt 0.178675 0.000000
+vt 0.183025 1.000000
+vt 0.183025 0.000000
+vt 0.187338 1.000000
+vt 0.187338 0.000000
+vt 0.191684 1.000000
+vt 0.191684 0.000000
+vt 0.196132 1.000000
+vt 0.196132 0.000000
+vt 0.200745 1.000000
+vt 0.200745 0.000000
+vt 0.205580 1.000000
+vt 0.205580 0.000000
+vt 0.210687 1.000000
+vt 0.210687 0.000000
+vt 0.217990 1.000000
+vt 0.217990 0.000000
+vt 0.225933 1.000000
+vt 0.225933 0.000000
+vt 0.234578 1.000000
+vt 0.234578 0.000000
+vt 0.246447 1.000000
+vt 0.246447 0.000000
+vt 0.265065 1.000000
+vt 0.265065 0.000000
+vt 0.286812 1.000000
+vt 0.286812 0.000000
+vt 0.304491 1.000000
+vt 0.304491 0.000000
+vt 0.317035 1.000000
+vt 0.317035 0.000000
+vt 0.326361 1.000000
+vt 0.326361 0.000000
+vt 0.333158 1.000000
+vt 0.333158 0.000000
+vt 0.337913 1.000000
+vt 0.337913 0.000000
+vt 0.342415 1.000000
+vt 0.342415 0.000000
+vt 0.346706 1.000000
+vt 0.346706 0.000000
+vt 0.350836 1.000000
+vt 0.350836 0.000000
+vt 0.354860 1.000000
+vt 0.354860 0.000000
+vt 0.358839 1.000000
+vt 0.358839 0.000000
+vt 0.362834 1.000000
+vt 0.362834 0.000000
+vt 0.366908 1.000000
+vt 0.366908 0.000000
+vt 0.371118 1.000000
+vt 0.371118 0.000000
+vt 0.375516 1.000000
+vt 0.375516 0.000000
+vt 0.378576 1.000000
+vt 0.378576 0.000000
+vt 0.383388 1.000000
+vt 0.383388 0.000000
+vt 0.389258 1.000000
+vt 0.389258 0.000000
+vt 0.394694 1.000000
+vt 0.394694 0.000000
+vt 0.398529 1.000000
+vt 0.398529 0.000000
+vt 0.402192 1.000000
+vt 0.402192 0.000000
+vt 0.405725 1.000000
+vt 0.405725 0.000000
+vt 0.409171 1.000000
+vt 0.409171 0.000000
+vt 0.412577 1.000000
+vt 0.412577 0.000000
+vt 0.415994 1.000000
+vt 0.415994 0.000000
+vt 0.419469 1.000000
+vt 0.419469 0.000000
+vt 0.423050 1.000000
+vt 0.423050 0.000000
+vt 0.426779 1.000000
+vt 0.426779 0.000000
+vt 0.430694 1.000000
+vt 0.430694 0.000000
+vt 0.436257 1.000000
+vt 0.436257 0.000000
+vt 0.442271 1.000000
+vt 0.442271 0.000000
+vt 0.448788 1.000000
+vt 0.448788 0.000000
+vt 0.455358 1.000000
+vt 0.455358 0.000000
+vt 0.461886 1.000000
+vt 0.461886 0.000000
+vt 0.468433 1.000000
+vt 0.468433 0.000000
+vt 0.475063 1.000000
+vt 0.475063 0.000000
+vt 0.481834 1.000000
+vt 0.481834 0.000000
+vt 0.488803 1.000000
+vt 0.488803 0.000000
+vt 0.497075 1.000000
+vt 0.497075 0.000000
+vt 0.522108 1.000000
+vt 0.522108 0.000000
+vt 0.530158 1.000000
+vt 0.530158 0.000000
+vt 0.538290 1.000000
+vt 0.538290 0.000000
+vt 0.546575 1.000000
+vt 0.546575 0.000000
+vt 0.556161 1.000000
+vt 0.556161 0.000000
+vt 0.564987 1.000000
+vt 0.564987 0.000000
+vt 0.575330 1.000000
+vt 0.575330 0.000000
+vt 0.584110 1.000000
+vt 0.584110 0.000000
+vt 0.591373 1.000000
+vt 0.591373 0.000000
+vt 0.598304 1.000000
+vt 0.598304 0.000000
+vt 0.604030 1.000000
+vt 0.604030 0.000000
+vt 0.609606 1.000000
+vt 0.609606 0.000000
+vt 0.615081 1.000000
+vt 0.615081 0.000000
+vt 0.619606 1.000000
+vt 0.619606 0.000000
+vt 0.624129 1.000000
+vt 0.624129 0.000000
+vt 0.629472 1.000000
+vt 0.629472 0.000000
+vt 0.634660 1.000000
+vt 0.634660 0.000000
+vt 0.638905 1.000000
+vt 0.638905 0.000000
+vt 0.643113 1.000000
+vt 0.643113 0.000000
+vt 0.647317 1.000000
+vt 0.647317 0.000000
+vt 0.651553 1.000000
+vt 0.651553 0.000000
+vt 0.656724 1.000000
+vt 0.656724 0.000000
+vt 0.662042 1.000000
+vt 0.662042 0.000000
+vt 0.667558 1.000000
+vt 0.667558 0.000000
+vt 0.676109 1.000000
+vt 0.676109 0.000000
+vt 0.682565 1.000000
+vt 0.682565 0.000000
+vt 0.688743 1.000000
+vt 0.688743 0.000000
+vt 0.694716 1.000000
+vt 0.694716 0.000000
+vt 0.699108 1.000000
+vt 0.699108 0.000000
+vt 0.703463 1.000000
+vt 0.703463 0.000000
+vt 0.707817 1.000000
+vt 0.707817 0.000000
+vt 0.712205 1.000000
+vt 0.712205 0.000000
+vt 0.718170 1.000000
+vt 0.718170 0.000000
+vt 0.724337 1.000000
+vt 0.724337 0.000000
+vt 0.730777 1.000000
+vt 0.730777 0.000000
+vt 0.737554 1.000000
+vt 0.737554 0.000000
+vt 0.746577 1.000000
+vt 0.746577 0.000000
+vt 0.747203 1.000000
+vt 0.747203 0.000000
+vt 0.754784 1.000000
+vt 0.754784 0.000000
+vt 0.761102 1.000000
+vt 0.761102 0.000000
+vt 0.767177 1.000000
+vt 0.767177 0.000000
+vt 0.773065 1.000000
+vt 0.773065 0.000000
+vt 0.778009 1.000000
+vt 0.778009 0.000000
+vt 0.782900 1.000000
+vt 0.782900 0.000000
+vt 0.786153 1.000000
+vt 0.786153 0.000000
+vt 0.790231 1.000000
+vt 0.790231 0.000000
+vt 0.795079 1.000000
+vt 0.795079 0.000000
+vt 0.799829 1.000000
+vt 0.799829 0.000000
+vt 0.804526 1.000000
+vt 0.804526 0.000000
+vt 0.809215 1.000000
+vt 0.809215 0.000000
+vt 0.813941 1.000000
+vt 0.813941 0.000000
+vt 0.818748 1.000000
+vt 0.818748 0.000000
+vt 0.823679 1.000000
+vt 0.823679 0.000000
+vt 0.827055 1.000000
+vt 0.827055 0.000000
+vt 0.830514 1.000000
+vt 0.830514 0.000000
+vt 0.837600 1.000000
+vt 0.837600 0.000000
+vt 0.844408 1.000000
+vt 0.844408 0.000000
+vt 0.851003 1.000000
+vt 0.851003 0.000000
+vt 0.856388 1.000000
+vt 0.856388 0.000000
+vt 0.861718 1.000000
+vt 0.861718 0.000000
+vt 0.867038 1.000000
+vt 0.867038 0.000000
+vt 0.872394 1.000000
+vt 0.872394 0.000000
+vt 0.877830 1.000000
+vt 0.877830 0.000000
+vt 0.881149 1.000000
+vt 0.881149 0.000000
+vt 0.885657 1.000000
+vt 0.885657 0.000000
+vt 0.892487 1.000000
+vt 0.892487 0.000000
+vt 0.899338 1.000000
+vt 0.899338 0.000000
+vt 0.906274 1.000000
+vt 0.906274 0.000000
+vt 0.913357 1.000000
+vt 0.913357 0.000000
+vt 0.921883 1.000000
+vt 0.921883 0.000000
+vt 0.930772 1.000000
+vt 0.930772 0.000000
+vt 0.938738 1.000000
+vt 0.938738 0.000000
+vt 0.947066 1.000000
+vt 0.947066 0.000000
+vt 0.960493 1.000000
+vt 0.960493 0.000000
+vt 0.970624 1.000000
+vt 0.970624 0.000000
+vt 0.979048 1.000000
+vt 0.979048 0.000000
+vt 0.986994 1.000000
+vt 0.986994 0.000000
+vt 0.993618 1.000000
+vt 0.993618 0.000000
+vt 0.996379 1.000000
+vt 0.996379 0.000000
+vt 0.027926 1.000000
+vt 0.027926 0.000000
+vt 0.037801 1.000000
+vt 0.037801 0.000000
+vt 0.047263 1.000000
+vt 0.047263 0.000000
+vt 0.055398 1.000000
+vt 0.055398 0.000000
+vt 0.063345 1.000000
+vt 0.063345 0.000000
+vt 0.071178 1.000000
+vt 0.071178 0.000000
+vt 0.075076 1.000000
+vt 0.075076 0.000000
+vt 0.078976 1.000000
+vt 0.078976 0.000000
+vt 0.086548 1.000000
+vt 0.086548 0.000000
+vt 0.092832 1.000000
+vt 0.092832 0.000000
+vt 0.098854 1.000000
+vt 0.098854 0.000000
+vt 0.103854 1.000000
+vt 0.103854 0.000000
+vt 0.108752 1.000000
+vt 0.108752 0.000000
+vt 0.113591 1.000000
+vt 0.113591 0.000000
+vt 0.117616 1.000000
+vt 0.117616 0.000000
+vt 0.121661 1.000000
+vt 0.121661 0.000000
+vt 0.126476 1.000000
+vt 0.126476 0.000000
+vt 0.130418 1.000000
+vt 0.130418 0.000000
+vt 0.134327 1.000000
+vt 0.134327 0.000000
+vt 0.138235 1.000000
+vt 0.138235 0.000000
+vt 0.142172 1.000000
+vt 0.142172 0.000000
+vt 0.146978 1.000000
+vt 0.146978 0.000000
+vt 0.151918 1.000000
+vt 0.151918 0.000000
+vt 0.157040 1.000000
+vt 0.157040 0.000000
+vt 0.162385 1.000000
+vt 0.162385 0.000000
+vt 0.168533 1.000000
+vt 0.168533 0.000000
+vt 0.173541 1.000000
+vt 0.173541 0.000000
+vt 0.178352 1.000000
+vt 0.178352 0.000000
+vt 0.182249 1.000000
+vt 0.182249 0.000000
+vt 0.186078 1.000000
+vt 0.186078 0.000000
+vt 0.189873 1.000000
+vt 0.189873 0.000000
+vt 0.193667 1.000000
+vt 0.193667 0.000000
+vt 0.197497 1.000000
+vt 0.197497 0.000000
+vt 0.199826 1.000000
+vt 0.199826 0.000000
+vt 0.202187 1.000000
+vt 0.202187 0.000000
+vt 0.206178 1.000000
+vt 0.206178 0.000000
+vt 0.211064 1.000000
+vt 0.211064 0.000000
+vt 0.216108 1.000000
+vt 0.216108 0.000000
+vt 0.221359 1.000000
+vt 0.221359 0.000000
+vt 0.227801 1.000000
+vt 0.227801 0.000000
+vt 0.235651 1.000000
+vt 0.235651 0.000000
+vt 0.241924 1.000000
+vt 0.241924 0.000000
+vt 0.248555 1.000000
+vt 0.248555 0.000000
+vt 0.284717 1.000000
+vt 0.284717 0.000000
+vt 0.400111 1.000000
+vt 0.400111 0.000000
+vt 0.511493 1.000000
+vt 0.511493 0.000000
+vt 0.535246 1.000000
+vt 0.535246 0.000000
+vt 0.621752 1.000000
+vt 0.621752 0.000000
+vt 0.689159 1.000000
+vt 0.689159 0.000000
+vt 0.708749 1.000000
+vt 0.708749 0.000000
+vt 0.721511 1.000000
+vt 0.721511 0.000000
+vt 0.730770 1.000000
+vt 0.730770 0.000000
+vt 0.739520 1.000000
+vt 0.739520 0.000000
+vt 0.746681 1.000000
+vt 0.746681 0.000000
+vt 0.753590 1.000000
+vt 0.753590 0.000000
+vt 0.760316 1.000000
+vt 0.760316 0.000000
+vt 0.764736 1.000000
+vt 0.764736 0.000000
+vt 0.770226 1.000000
+vt 0.770226 0.000000
+vt 0.775661 1.000000
+vt 0.775661 0.000000
+vt 0.781026 1.000000
+vt 0.781026 0.000000
+vt 0.786365 1.000000
+vt 0.786365 0.000000
+vt 0.791723 1.000000
+vt 0.791723 0.000000
+vt 0.797143 1.000000
+vt 0.797143 0.000000
+vt 0.803788 1.000000
+vt 0.803788 0.000000
+vt 0.810654 1.000000
+vt 0.810654 0.000000
+vt 0.817804 1.000000
+vt 0.817804 0.000000
+vt 0.826577 1.000000
+vt 0.826577 0.000000
+vt 0.835679 1.000000
+vt 0.835679 0.000000
+vt 0.843140 1.000000
+vt 0.843140 0.000000
+vt 0.850349 1.000000
+vt 0.850349 0.000000
+vt 0.857374 1.000000
+vt 0.857374 0.000000
+vt 0.863141 1.000000
+vt 0.863141 0.000000
+vt 0.868873 1.000000
+vt 0.868873 0.000000
+vt 0.874616 1.000000
+vt 0.874616 0.000000
+vt 0.880414 1.000000
+vt 0.880414 0.000000
+vt 0.886310 1.000000
+vt 0.886310 0.000000
+vt 0.892255 1.000000
+vt 0.892255 0.000000
+vt 0.899395 1.000000
+vt 0.899395 0.000000
+vt 0.906615 1.000000
+vt 0.906615 0.000000
+vt 0.913992 1.000000
+vt 0.913992 0.000000
+vt 0.921596 1.000000
+vt 0.921596 0.000000
+vt 0.930839 1.000000
+vt 0.930839 0.000000
+vt 0.940570 1.000000
+vt 0.940570 0.000000
+vt 0.950867 1.000000
+vt 0.950867 0.000000
+vt 0.966392 1.000000
+vt 0.966392 0.000000
+vt 0.976921 1.000000
+vt 0.976921 0.000000
+vt 0.985419 1.000000
+vt 0.985419 0.000000
+vt 0.991441 1.000000
+vt 0.991441 0.000000
+vt 0.995458 1.000000
+vt 0.995458 0.000000
+vt 0.138054 1.000000
+vt 0.138054 0.000000
+vt 0.161382 1.000000
+vt 0.161382 0.000000
+vt 0.255951 1.000000
+vt 0.255951 0.000000
+vt 0.291332 1.000000
+vt 0.291332 0.000000
+vt 0.302483 1.000000
+vt 0.302483 0.000000
+vt 0.312123 1.000000
+vt 0.312123 0.000000
+vt 0.319287 1.000000
+vt 0.319287 0.000000
+vt 0.325492 1.000000
+vt 0.325492 0.000000
+vt 0.331597 1.000000
+vt 0.331597 0.000000
+vt 0.337654 1.000000
+vt 0.337654 0.000000
+vt 0.343717 1.000000
+vt 0.343717 0.000000
+vt 0.346768 1.000000
+vt 0.346768 0.000000
+vt 0.350613 1.000000
+vt 0.350613 0.000000
+vt 0.356731 1.000000
+vt 0.356731 0.000000
+vt 0.361756 1.000000
+vt 0.361756 0.000000
+vt 0.365857 1.000000
+vt 0.365857 0.000000
+vt 0.369804 1.000000
+vt 0.369804 0.000000
+vt 0.373004 1.000000
+vt 0.373004 0.000000
+vt 0.376149 1.000000
+vt 0.376149 0.000000
+vt 0.379265 1.000000
+vt 0.379265 0.000000
+vt 0.381756 1.000000
+vt 0.381756 0.000000
+vt 0.384258 1.000000
+vt 0.384258 0.000000
+vt 0.387397 1.000000
+vt 0.387397 0.000000
+vt 0.390551 1.000000
+vt 0.390551 0.000000
+vt 0.393746 1.000000
+vt 0.393746 0.000000
+vt 0.397670 1.000000
+vt 0.397670 0.000000
+vt 0.401730 1.000000
+vt 0.401730 0.000000
+vt 0.405964 1.000000
+vt 0.405964 0.000000
+vt 0.411165 1.000000
+vt 0.411165 0.000000
+vt 0.415078 1.000000
+vt 0.415078 0.000000
+vt 0.419170 1.000000
+vt 0.419170 0.000000
+vt 0.426505 1.000000
+vt 0.426505 0.000000
+vt 0.433846 1.000000
+vt 0.433846 0.000000
+vt 0.441268 1.000000
+vt 0.441268 0.000000
+vt 0.448848 1.000000
+vt 0.448848 0.000000
+vt 0.456657 1.000000
+vt 0.456657 0.000000
+vt 0.466141 1.000000
+vt 0.466141 0.000000
+vt 0.474658 1.000000
+vt 0.474658 0.000000
+vt 0.485115 1.000000
+vt 0.485115 0.000000
+vt 0.512598 1.000000
+vt 0.512598 0.000000
+vt 0.522023 1.000000
+vt 0.522023 0.000000
+vt 0.531295 1.000000
+vt 0.531295 0.000000
+vt 0.540498 1.000000
+vt 0.540498 0.000000
+vt 0.549718 1.000000
+vt 0.549718 0.000000
+vt 0.559040 1.000000
+vt 0.559040 0.000000
+vt 0.568547 1.000000
+vt 0.568547 0.000000
+vt 0.579737 1.000000
+vt 0.579737 0.000000
+vt 0.589228 1.000000
+vt 0.589228 0.000000
+vt 0.597048 1.000000
+vt 0.597048 0.000000
+vt 0.604483 1.000000
+vt 0.604483 0.000000
+vt 0.610607 1.000000
+vt 0.610607 0.000000
+vt 0.616558 1.000000
+vt 0.616558 0.000000
+vt 0.622395 1.000000
+vt 0.622395 0.000000
+vt 0.627218 1.000000
+vt 0.627218 0.000000
+vt 0.632041 1.000000
+vt 0.632041 0.000000
+vt 0.637782 1.000000
+vt 0.637782 0.000000
+vt 0.642501 1.000000
+vt 0.642501 0.000000
+vt 0.647200 1.000000
+vt 0.647200 0.000000
+vt 0.651915 1.000000
+vt 0.651915 0.000000
+vt 0.657647 1.000000
+vt 0.657647 0.000000
+vt 0.663517 1.000000
+vt 0.663517 0.000000
+vt 0.669581 1.000000
+vt 0.669581 0.000000
+vt 0.674822 1.000000
+vt 0.674822 0.000000
+vt 0.681375 1.000000
+vt 0.681375 0.000000
+vt 0.688411 1.000000
+vt 0.688411 0.000000
+vt 0.695449 1.000000
+vt 0.695449 0.000000
+vt 0.703438 1.000000
+vt 0.703438 0.000000
+vt 0.711575 1.000000
+vt 0.711575 0.000000
+vt 0.718992 1.000000
+vt 0.718992 0.000000
+vt 0.726630 1.000000
+vt 0.726630 0.000000
+vt 0.738216 1.000000
+vt 0.738216 0.000000
+vt 0.762683 1.000000
+vt 0.762683 0.000000
+vt 0.794791 1.000000
+vt 0.794791 0.000000
+vt 0.869137 1.000000
+vt 0.869137 0.000000
+vt 0.869776 1.000000
+vt 0.869776 0.000000
+vt 0.976352 1.000000
+vt 0.976352 0.000000
+vt 0.155874 1.000000
+vt 0.155874 0.000000
+vt 0.183551 1.000000
+vt 0.183551 0.000000
+vt 0.245823 1.000000
+vt 0.245823 0.000000
+vt 0.459934 1.000000
+vt 0.459934 0.000000
+vt 0.526521 1.000000
+vt 0.526521 0.000000
+vt 0.556504 1.000000
+vt 0.556504 0.000000
+vt 0.656155 1.000000
+vt 0.656155 0.000000
+vt 0.910243 1.000000
+vt 0.910243 0.000000
+vt 0.972323 1.000000
+vt 0.972323 0.000000
+vt 0.000000 0.500000
+vt 1.000000 0.500000
+usemtl _Mat.002
+s 1
+f 1/1 2/2 3/3 4/4
+f 2/2 5/5 6/6 3/3
+f 5/5 7/7 8/8 6/6
+f 7/7 1/9 4/10 8/8
+f 2/2 1/1 9/11 10/12
+f 10/12 11/13 5/5 2/2
+f 11/13 12/14 7/7 5/5
+f 1/9 7/7 12/14 9/15
+f 13/4 14/4 15/4
+f 16/4 13/4 15/4
+f 17/4 18/4 19/4
+f 20/4 17/4 19/4
+f 21/16 22/17 23/11 24/4
+f 25/18 26/19 22/17 21/16
+f 27/20 28/21 26/19 25/18
+f 29/22 30/23 28/21 27/20
+f 31/24 32/25 30/23 29/22
+f 33/26 34/27 32/25 31/24
+f 35/28 36/29 34/27 33/26
+f 37/30 38/31 36/29 35/28
+f 39/32 40/33 38/31 37/30
+f 41/34 42/35 40/33 39/32
+f 43/36 44/37 42/35 41/34
+f 45/38 46/39 44/37 43/36
+f 47/40 48/41 46/39 45/38
+f 49/42 50/43 48/41 47/40
+f 51/44 52/45 50/43 49/42
+f 53/46 54/47 52/45 51/44
+f 55/48 56/49 54/47 53/46
+f 57/50 58/51 56/49 55/48
+f 59/52 60/53 58/51 57/50
+f 61/54 62/55 60/53 59/52
+f 63/56 64/57 62/55 61/54
+f 65/58 66/59 64/57 63/56
+f 67/60 68/61 66/59 65/58
+f 69/62 70/63 68/61 67/60
+f 71/64 72/65 70/63 69/62
+f 73/66 74/67 72/65 71/64
+f 75/68 76/69 74/67 73/66
+f 77/70 78/71 76/69 75/68
+f 79/72 80/73 78/71 77/70
+f 81/74 82/75 80/73 79/72
+f 83/76 84/77 82/75 81/74
+f 85/78 86/79 84/77 83/76
+f 87/80 88/81 86/79 85/78
+f 89/82 90/83 88/81 87/80
+f 91/84 92/85 90/83 89/82
+f 93/86 94/87 92/85 91/84
+f 95/88 96/89 94/87 93/86
+f 97/90 98/91 96/89 95/88
+f 99/92 100/93 98/91 97/90
+f 101/94 102/95 100/93 99/92
+f 103/96 104/97 102/95 101/94
+f 105/98 106/99 104/97 103/96
+f 107/100 108/101 106/99 105/98
+f 109/102 110/103 108/101 107/100
+f 111/104 112/105 110/103 109/102
+f 113/106 114/107 112/105 111/104
+f 115/108 116/109 114/107 113/106
+f 117/110 118/111 116/109 115/108
+f 119/112 120/113 118/111 117/110
+f 121/114 122/115 120/113 119/112
+f 123/116 124/117 122/115 121/114
+f 125/118 126/119 124/117 123/116
+f 127/120 128/121 126/119 125/118
+f 129/122 130/123 128/121 127/120
+f 131/124 132/125 130/123 129/122
+f 133/126 134/127 132/125 131/124
+f 135/128 136/129 134/127 133/126
+f 137/130 138/131 136/129 135/128
+f 139/132 140/133 138/131 137/130
+f 141/134 142/135 140/133 139/132
+f 143/136 144/137 142/135 141/134
+f 145/138 146/139 144/137 143/136
+f 147/140 148/141 146/139 145/138
+f 149/142 150/143 148/141 147/140
+f 151/144 152/145 150/143 149/142
+f 153/146 154/147 152/145 151/144
+f 155/148 156/149 154/147 153/146
+f 157/150 158/151 156/149 155/148
+f 159/152 160/153 158/151 157/150
+f 161/154 162/155 160/153 159/152
+f 163/156 164/157 162/155 161/154
+f 165/158 166/159 164/157 163/156
+f 167/160 168/161 166/159 165/158
+f 169/162 170/163 168/161 167/160
+f 171/164 172/165 170/163 169/162
+f 173/166 174/167 172/165 171/164
+f 175/168 176/169 174/167 173/166
+f 177/170 178/171 176/169 175/168
+f 179/172 180/173 178/171 177/170
+f 181/174 182/175 180/173 179/172
+f 183/176 184/177 182/175 181/174
+f 185/178 186/179 184/177 183/176
+f 187/180 188/181 186/179 185/178
+f 189/182 190/183 188/181 187/180
+f 191/184 192/185 190/183 189/182
+f 193/186 194/187 192/185 191/184
+f 195/188 196/189 194/187 193/186
+f 197/190 198/191 196/189 195/188
+f 199/192 200/193 198/191 197/190
+f 201/194 202/195 200/193 199/192
+f 203/196 204/197 202/195 201/194
+f 205/198 206/199 204/197 203/196
+f 207/200 208/201 206/199 205/198
+f 209/202 210/203 208/201 207/200
+f 211/204 212/205 210/203 209/202
+f 213/206 214/207 212/205 211/204
+f 215/208 216/209 214/207 213/206
+f 217/210 218/211 216/209 215/208
+f 219/212 220/213 218/211 217/210
+f 221/214 222/215 220/213 219/212
+f 223/216 224/217 222/215 221/214
+f 225/218 226/219 224/217 223/216
+f 227/220 228/221 226/219 225/218
+f 229/222 230/223 228/221 227/220
+f 231/224 232/225 230/223 229/222
+f 233/226 234/227 232/225 231/224
+f 235/228 236/229 234/227 233/226
+f 237/230 238/231 236/229 235/228
+f 239/232 240/233 238/231 237/230
+f 241/234 242/235 240/233 239/232
+f 243/236 244/237 242/235 241/234
+f 245/238 246/239 244/237 243/236
+f 247/240 248/241 246/239 245/238
+f 249/242 250/243 248/241 247/240
+f 251/244 252/245 250/243 249/242
+f 253/246 254/247 252/245 251/244
+f 255/248 256/249 254/247 253/246
+f 257/250 258/251 256/249 255/248
+f 259/252 260/253 258/251 257/250
+f 261/254 262/255 260/253 259/252
+f 263/256 264/257 262/255 261/254
+f 265/258 266/259 264/257 263/256
+f 267/260 268/261 266/259 265/258
+f 269/262 270/263 268/261 267/260
+f 271/264 272/265 270/263 269/262
+f 273/266 274/267 272/265 271/264
+f 275/268 276/269 274/267 273/266
+f 277/270 278/271 276/269 275/268
+f 279/272 280/273 278/271 277/270
+f 281/274 282/275 280/273 279/272
+f 283/276 284/277 282/275 281/274
+f 285/278 286/279 284/277 283/276
+f 287/280 288/281 286/279 285/278
+f 289/282 290/283 288/281 287/280
+f 291/284 292/285 290/283 289/282
+f 293/286 294/287 292/285 291/284
+f 295/288 296/289 294/287 293/286
+f 297/290 298/291 296/289 295/288
+f 299/292 300/293 298/291 297/290
+f 301/294 302/295 300/293 299/292
+f 303/296 304/297 302/295 301/294
+f 305/298 306/299 304/297 303/296
+f 307/300 308/301 306/299 305/298
+f 309/302 310/303 308/301 307/300
+f 311/304 312/305 310/303 309/302
+f 313/306 314/307 312/305 311/304
+f 315/308 316/309 314/307 313/306
+f 317/310 318/311 316/309 315/308
+f 319/312 320/313 318/311 317/310
+f 321/314 322/315 320/313 319/312
+f 323/316 324/317 322/315 321/314
+f 325/318 326/319 324/317 323/316
+f 327/320 328/321 326/319 325/318
+f 329/322 330/323 328/321 327/320
+f 331/324 332/325 330/323 329/322
+f 333/326 334/327 332/325 331/324
+f 335/328 336/329 334/327 333/326
+f 337/330 338/331 336/329 335/328
+f 339/332 340/333 338/331 337/330
+f 341/334 342/335 340/333 339/332
+f 343/336 344/337 342/335 341/334
+f 24/10 23/15 344/337 343/336
+f 345/338 346/339 347/11 348/4
+f 349/340 350/341 346/339 345/338
+f 351/342 352/343 350/341 349/340
+f 353/344 354/345 352/343 351/342
+f 355/346 356/347 354/345 353/344
+f 357/348 358/349 356/347 355/346
+f 359/350 360/351 358/349 357/348
+f 361/352 362/353 360/351 359/350
+f 363/354 364/355 362/353 361/352
+f 365/356 366/357 364/355 363/354
+f 367/358 368/359 366/357 365/356
+f 369/360 370/361 368/359 367/358
+f 371/362 372/363 370/361 369/360
+f 373/364 374/365 372/363 371/362
+f 375/366 376/367 374/365 373/364
+f 377/368 378/369 376/367 375/366
+f 379/370 380/371 378/369 377/368
+f 381/372 382/373 380/371 379/370
+f 383/374 384/375 382/373 381/372
+f 385/376 386/377 384/375 383/374
+f 387/378 388/379 386/377 385/376
+f 389/380 390/381 388/379 387/378
+f 391/382 392/383 390/381 389/380
+f 393/384 394/385 392/383 391/382
+f 395/386 396/387 394/385 393/384
+f 397/388 398/389 396/387 395/386
+f 399/390 400/391 398/389 397/388
+f 401/392 402/393 400/391 399/390
+f 403/394 404/395 402/393 401/392
+f 405/396 406/397 404/395 403/394
+f 407/398 408/399 406/397 405/396
+f 409/400 410/401 408/399 407/398
+f 411/402 412/403 410/401 409/400
+f 413/404 414/405 412/403 411/402
+f 415/406 416/407 414/405 413/404
+f 417/408 418/409 416/407 415/406
+f 419/410 420/411 418/409 417/408
+f 421/412 422/413 420/411 419/410
+f 423/414 424/415 422/413 421/412
+f 425/416 426/417 424/415 423/414
+f 427/418 428/419 426/417 425/416
+f 429/420 430/421 428/419 427/418
+f 431/422 432/423 430/421 429/420
+f 433/424 434/425 432/423 431/422
+f 435/426 436/427 434/425 433/424
+f 437/428 438/429 436/427 435/426
+f 439/430 440/431 438/429 437/428
+f 441/432 442/433 440/431 439/430
+f 443/434 444/435 442/433 441/432
+f 445/436 446/437 444/435 443/434
+f 447/438 448/439 446/437 445/436
+f 449/440 450/441 448/439 447/438
+f 451/442 452/443 450/441 449/440
+f 453/444 454/445 452/443 451/442
+f 455/446 456/447 454/445 453/444
+f 457/448 458/449 456/447 455/446
+f 459/450 460/451 458/449 457/448
+f 461/452 462/453 460/451 459/450
+f 463/454 464/455 462/453 461/452
+f 465/456 466/457 464/455 463/454
+f 467/458 468/459 466/457 465/456
+f 469/460 470/461 468/459 467/458
+f 471/462 472/463 470/461 469/460
+f 473/464 474/465 472/463 471/462
+f 475/466 476/467 474/465 473/464
+f 477/468 478/469 476/467 475/466
+f 479/470 480/471 478/469 477/468
+f 481/472 482/473 480/471 479/470
+f 483/474 484/475 482/473 481/472
+f 485/476 486/477 484/475 483/474
+f 487/478 488/479 486/477 485/476
+f 489/480 490/481 488/479 487/478
+f 491/482 492/483 490/481 489/480
+f 493/484 494/485 492/483 491/482
+f 495/486 496/487 494/485 493/484
+f 497/488 498/489 496/487 495/486
+f 499/490 500/491 498/489 497/488
+f 501/492 502/493 500/491 499/490
+f 503/494 504/495 502/493 501/492
+f 505/496 506/497 504/495 503/494
+f 507/498 508/499 506/497 505/496
+f 509/500 510/501 508/499 507/498
+f 511/502 512/503 510/501 509/500
+f 513/504 514/505 512/503 511/502
+f 515/506 516/507 514/505 513/504
+f 517/508 518/509 516/507 515/506
+f 519/510 520/511 518/509 517/508
+f 521/512 522/513 520/511 519/510
+f 523/514 524/515 522/513 521/512
+f 348/10 347/15 524/515 523/514
+f 525/1 526/2 527/3 528/4
+f 526/2 529/5 530/6 527/3
+f 529/5 531/7 532/8 530/6
+f 531/7 525/9 528/10 532/8
+f 533/11 534/12 526/2 525/1
+f 534/12 535/13 529/5 526/2
+f 535/13 536/14 531/7 529/5
+f 536/14 533/15 525/9 531/7
+f 537/4 538/4 539/4
+f 540/4 537/4 539/4
+f 541/4 542/4 543/4
+f 544/4 541/4 543/4
+f 545/516 546/517 547/11 548/4
+f 549/518 550/519 546/517 545/516
+f 551/520 552/521 550/519 549/518
+f 553/522 554/523 552/521 551/520
+f 555/524 556/525 554/523 553/522
+f 557/526 558/527 556/525 555/524
+f 559/528 560/529 558/527 557/526
+f 561/530 562/531 560/529 559/528
+f 563/532 564/533 562/531 561/530
+f 565/534 566/535 564/533 563/532
+f 567/536 568/537 566/535 565/534
+f 569/538 570/539 568/537 567/536
+f 571/540 572/541 570/539 569/538
+f 573/542 574/543 572/541 571/540
+f 575/544 576/545 574/543 573/542
+f 577/546 578/547 576/545 575/544
+f 579/548 580/549 578/547 577/546
+f 581/550 582/551 580/549 579/548
+f 583/552 584/553 582/551 581/550
+f 585/554 586/555 584/553 583/552
+f 587/556 588/557 586/555 585/554
+f 589/558 590/559 588/557 587/556
+f 591/560 592/561 590/559 589/558
+f 593/562 594/563 592/561 591/560
+f 595/564 596/565 594/563 593/562
+f 597/566 598/567 596/565 595/564
+f 599/568 600/569 598/567 597/566
+f 601/570 602/571 600/569 599/568
+f 603/572 604/573 602/571 601/570
+f 605/574 606/575 604/573 603/572
+f 607/576 608/577 606/575 605/574
+f 609/578 610/579 608/577 607/576
+f 611/580 612/581 610/579 609/578
+f 613/582 614/583 612/581 611/580
+f 615/584 616/585 614/583 613/582
+f 617/586 618/587 616/585 615/584
+f 619/588 620/589 618/587 617/586
+f 621/590 622/591 620/589 619/588
+f 623/592 624/593 622/591 621/590
+f 625/594 626/595 624/593 623/592
+f 627/596 628/597 626/595 625/594
+f 629/598 630/599 628/597 627/596
+f 631/600 632/601 630/599 629/598
+f 633/602 634/603 632/601 631/600
+f 635/604 636/605 634/603 633/602
+f 637/606 638/607 636/605 635/604
+f 639/608 640/609 638/607 637/606
+f 641/610 642/611 640/609 639/608
+f 643/612 644/613 642/611 641/610
+f 645/614 646/615 644/613 643/612
+f 647/616 648/617 646/615 645/614
+f 649/618 650/619 648/617 647/616
+f 651/620 652/621 650/619 649/618
+f 653/622 654/623 652/621 651/620
+f 655/624 656/625 654/623 653/622
+f 657/626 658/627 656/625 655/624
+f 659/628 660/629 658/627 657/626
+f 661/630 662/631 660/629 659/628
+f 663/632 664/633 662/631 661/630
+f 665/634 666/635 664/633 663/632
+f 667/636 668/637 666/635 665/634
+f 669/638 670/639 668/637 667/636
+f 671/640 672/641 670/639 669/638
+f 673/642 674/643 672/641 671/640
+f 675/644 676/645 674/643 673/642
+f 677/646 678/647 676/645 675/644
+f 679/648 680/649 678/647 677/646
+f 681/650 682/651 680/649 679/648
+f 683/652 684/653 682/651 681/650
+f 685/654 686/655 684/653 683/652
+f 687/656 688/657 686/655 685/654
+f 689/658 690/659 688/657 687/656
+f 691/660 692/661 690/659 689/658
+f 693/662 694/663 692/661 691/660
+f 695/664 696/665 694/663 693/662
+f 697/666 698/667 696/665 695/664
+f 548/10 547/15 698/667 697/666
+f 699/338 700/339 701/11 702/4
+f 703/340 704/341 700/339 699/338
+f 705/342 706/343 704/341 703/340
+f 707/344 708/345 706/343 705/342
+f 709/346 710/347 708/345 707/344
+f 711/348 712/349 710/347 709/346
+f 713/350 714/351 712/349 711/348
+f 715/352 716/353 714/351 713/350
+f 717/354 718/355 716/353 715/352
+f 719/356 720/357 718/355 717/354
+f 721/358 722/359 720/357 719/356
+f 723/360 724/361 722/359 721/358
+f 725/362 726/363 724/361 723/360
+f 727/364 728/365 726/363 725/362
+f 729/366 730/367 728/365 727/364
+f 731/368 732/369 730/367 729/366
+f 733/370 734/371 732/369 731/368
+f 735/372 736/373 734/371 733/370
+f 737/374 738/375 736/373 735/372
+f 739/376 740/377 738/375 737/374
+f 741/378 742/379 740/377 739/376
+f 743/380 744/381 742/379 741/378
+f 745/382 746/383 744/381 743/380
+f 747/384 748/385 746/383 745/382
+f 749/386 750/387 748/385 747/384
+f 751/388 752/389 750/387 749/386
+f 753/390 754/391 752/389 751/388
+f 755/392 756/393 754/391 753/390
+f 757/394 758/395 756/393 755/392
+f 759/396 760/397 758/395 757/394
+f 761/398 762/399 760/397 759/396
+f 763/400 764/401 762/399 761/398
+f 765/402 766/403 764/401 763/400
+f 767/404 768/405 766/403 765/402
+f 769/406 770/407 768/405 767/404
+f 771/408 772/409 770/407 769/406
+f 773/410 774/411 772/409 771/408
+f 775/412 776/413 774/411 773/410
+f 777/414 778/415 776/413 775/412
+f 779/416 780/417 778/415 777/414
+f 781/418 782/419 780/417 779/416
+f 783/420 784/421 782/419 781/418
+f 785/422 786/423 784/421 783/420
+f 787/424 788/425 786/423 785/422
+f 789/426 790/427 788/425 787/424
+f 791/428 792/429 790/427 789/426
+f 793/430 794/431 792/429 791/428
+f 795/432 796/433 794/431 793/430
+f 797/434 798/435 796/433 795/432
+f 799/436 800/437 798/435 797/434
+f 801/438 802/439 800/437 799/436
+f 803/440 804/441 802/439 801/438
+f 805/442 806/443 804/441 803/440
+f 807/444 808/445 806/443 805/442
+f 809/446 810/447 808/445 807/444
+f 811/448 812/449 810/447 809/446
+f 813/450 814/451 812/449 811/448
+f 815/452 816/453 814/451 813/450
+f 817/454 818/455 816/453 815/452
+f 819/456 820/457 818/455 817/454
+f 821/458 822/459 820/457 819/456
+f 823/460 824/461 822/459 821/458
+f 825/462 826/463 824/461 823/460
+f 827/464 828/465 826/463 825/462
+f 829/466 830/467 828/465 827/464
+f 831/468 832/469 830/467 829/466
+f 833/470 834/471 832/469 831/468
+f 835/472 836/473 834/471 833/470
+f 837/474 838/475 836/473 835/472
+f 839/476 840/477 838/475 837/474
+f 841/478 842/479 840/477 839/476
+f 843/480 844/481 842/479 841/478
+f 845/482 846/483 844/481 843/480
+f 847/484 848/485 846/483 845/482
+f 849/486 850/487 848/485 847/484
+f 851/488 852/489 850/487 849/486
+f 853/490 854/491 852/489 851/488
+f 855/492 856/493 854/491 853/490
+f 857/494 858/495 856/493 855/492
+f 859/496 860/497 858/495 857/494
+f 861/498 862/499 860/497 859/496
+f 863/500 864/501 862/499 861/498
+f 865/502 866/503 864/501 863/500
+f 867/504 868/505 866/503 865/502
+f 869/506 870/507 868/505 867/504
+f 871/508 872/509 870/507 869/506
+f 873/510 874/511 872/509 871/508
+f 875/512 876/513 874/511 873/510
+f 877/514 878/515 876/513 875/512
+f 702/10 701/15 878/515 877/514
+f 879/1 880/2 881/3 882/4
+f 880/2 883/5 884/6 881/3
+f 883/5 885/7 886/8 884/6
+f 885/7 879/9 882/10 886/8
+f 887/11 888/12 880/2 879/1
+f 888/12 889/13 883/5 880/2
+f 889/13 890/14 885/7 883/5
+f 890/14 887/15 879/9 885/7
+f 891/4 892/4 893/4
+f 894/4 891/4 893/4
+f 895/4 896/4 897/4
+f 898/4 895/4 897/4
+f 899/668 900/669 901/11 902/4
+f 903/670 904/671 900/669 899/668
+f 905/672 906/673 904/671 903/670
+f 907/674 908/675 906/673 905/672
+f 909/676 910/677 908/675 907/674
+f 911/678 912/679 910/677 909/676
+f 913/680 914/681 912/679 911/678
+f 915/682 916/683 914/681 913/680
+f 917/684 918/685 916/683 915/682
+f 902/10 901/15 918/685 917/684
+f 919/338 920/339 921/11 922/4
+f 923/340 924/341 920/339 919/338
+f 925/342 926/343 924/341 923/340
+f 927/344 928/345 926/343 925/342
+f 929/346 930/347 928/345 927/344
+f 931/348 932/349 930/347 929/346
+f 933/350 934/351 932/349 931/348
+f 935/352 936/353 934/351 933/350
+f 937/354 938/355 936/353 935/352
+f 939/356 940/357 938/355 937/354
+f 941/358 942/359 940/357 939/356
+f 943/360 944/361 942/359 941/358
+f 945/362 946/363 944/361 943/360
+f 947/364 948/365 946/363 945/362
+f 949/366 950/367 948/365 947/364
+f 951/368 952/369 950/367 949/366
+f 953/370 954/371 952/369 951/368
+f 955/372 956/373 954/371 953/370
+f 957/374 958/375 956/373 955/372
+f 959/376 960/377 958/375 957/374
+f 961/378 962/379 960/377 959/376
+f 963/380 964/381 962/379 961/378
+f 965/382 966/383 964/381 963/380
+f 967/384 968/385 966/383 965/382
+f 969/386 970/387 968/385 967/384
+f 971/388 972/389 970/387 969/386
+f 973/390 974/391 972/389 971/388
+f 975/392 976/393 974/391 973/390
+f 977/394 978/395 976/393 975/392
+f 979/396 980/397 978/395 977/394
+f 981/398 982/399 980/397 979/396
+f 983/400 984/401 982/399 981/398
+f 985/402 986/403 984/401 983/400
+f 987/404 988/405 986/403 985/402
+f 989/406 990/407 988/405 987/404
+f 991/408 992/409 990/407 989/406
+f 993/410 994/411 992/409 991/408
+f 995/412 996/413 994/411 993/410
+f 997/414 998/415 996/413 995/412
+f 999/416 1000/417 998/415 997/414
+f 1001/418 1002/419 1000/417 999/416
+f 1003/420 1004/421 1002/419 1001/418
+f 1005/422 1006/423 1004/421 1003/420
+f 1007/424 1008/425 1006/423 1005/422
+f 1009/426 1010/427 1008/425 1007/424
+f 1011/428 1012/429 1010/427 1009/426
+f 1013/430 1014/431 1012/429 1011/428
+f 1015/432 1016/433 1014/431 1013/430
+f 1017/434 1018/435 1016/433 1015/432
+f 1019/436 1020/437 1018/435 1017/434
+f 1021/438 1022/439 1020/437 1019/436
+f 1023/440 1024/441 1022/439 1021/438
+f 1025/442 1026/443 1024/441 1023/440
+f 1027/444 1028/445 1026/443 1025/442
+f 1029/446 1030/447 1028/445 1027/444
+f 1031/448 1032/449 1030/447 1029/446
+f 1033/450 1034/451 1032/449 1031/448
+f 1035/452 1036/453 1034/451 1033/450
+f 1037/454 1038/455 1036/453 1035/452
+f 1039/456 1040/457 1038/455 1037/454
+f 1041/458 1042/459 1040/457 1039/456
+f 1043/460 1044/461 1042/459 1041/458
+f 1045/462 1046/463 1044/461 1043/460
+f 1047/464 1048/465 1046/463 1045/462
+f 1049/466 1050/467 1048/465 1047/464
+f 1051/468 1052/469 1050/467 1049/466
+f 1053/470 1054/471 1052/469 1051/468
+f 1055/472 1056/473 1054/471 1053/470
+f 1057/474 1058/475 1056/473 1055/472
+f 1059/476 1060/477 1058/475 1057/474
+f 1061/478 1062/479 1060/477 1059/476
+f 1063/480 1064/481 1062/479 1061/478
+f 1065/482 1066/483 1064/481 1063/480
+f 1067/484 1068/485 1066/483 1065/482
+f 1069/486 1070/487 1068/485 1067/484
+f 1071/488 1072/489 1070/487 1069/486
+f 1073/490 1074/491 1072/489 1071/488
+f 1075/492 1076/493 1074/491 1073/490
+f 1077/494 1078/495 1076/493 1075/492
+f 1079/496 1080/497 1078/495 1077/494
+f 1081/498 1082/499 1080/497 1079/496
+f 1083/500 1084/501 1082/499 1081/498
+f 1085/502 1086/503 1084/501 1083/500
+f 1087/504 1088/505 1086/503 1085/502
+f 1089/506 1090/507 1088/505 1087/504
+f 1091/508 1092/509 1090/507 1089/506
+f 1093/510 1094/511 1092/509 1091/508
+f 1095/512 1096/513 1094/511 1093/510
+f 1097/514 1098/515 1096/513 1095/512
+f 922/10 921/15 1098/515 1097/514
+f 1099/1 1100/2 1101/3 1102/4
+f 1100/2 1103/5 1104/6 1101/3
+f 1103/5 1105/7 1106/8 1104/6
+f 1105/7 1099/9 1102/10 1106/8
+f 1107/11 1108/12 1100/2 1099/1
+f 1108/12 1109/13 1103/5 1100/2
+f 1109/13 1110/14 1105/7 1103/5
+f 1110/14 1107/15 1099/9 1105/7
+f 1111/4 1112/4 1113/4
+f 1114/4 1111/4 1113/4
+f 1115/4 1116/4 1117/4
+f 1118/4 1115/4 1117/4
+f 1119/16 1120/17 1121/11 1122/4
+f 1123/18 1124/19 1120/17 1119/16
+f 1125/20 1126/21 1124/19 1123/18
+f 1127/22 1128/23 1126/21 1125/20
+f 1129/24 1130/25 1128/23 1127/22
+f 1131/26 1132/27 1130/25 1129/24
+f 1133/28 1134/29 1132/27 1131/26
+f 1135/30 1136/31 1134/29 1133/28
+f 1137/32 1138/33 1136/31 1135/30
+f 1139/34 1140/35 1138/33 1137/32
+f 1141/36 1142/37 1140/35 1139/34
+f 1143/38 1144/39 1142/37 1141/36
+f 1145/40 1146/41 1144/39 1143/38
+f 1147/42 1148/43 1146/41 1145/40
+f 1149/44 1150/45 1148/43 1147/42
+f 1151/46 1152/47 1150/45 1149/44
+f 1153/48 1154/49 1152/47 1151/46
+f 1155/50 1156/51 1154/49 1153/48
+f 1157/52 1158/53 1156/51 1155/50
+f 1159/54 1160/55 1158/53 1157/52
+f 1161/56 1162/57 1160/55 1159/54
+f 1163/58 1164/59 1162/57 1161/56
+f 1165/60 1166/61 1164/59 1163/58
+f 1167/62 1168/63 1166/61 1165/60
+f 1169/64 1170/65 1168/63 1167/62
+f 1171/66 1172/67 1170/65 1169/64
+f 1173/68 1174/69 1172/67 1171/66
+f 1175/70 1176/71 1174/69 1173/68
+f 1177/72 1178/73 1176/71 1175/70
+f 1179/74 1180/75 1178/73 1177/72
+f 1181/76 1182/77 1180/75 1179/74
+f 1183/78 1184/79 1182/77 1181/76
+f 1185/80 1186/81 1184/79 1183/78
+f 1187/82 1188/83 1186/81 1185/80
+f 1189/84 1190/85 1188/83 1187/82
+f 1191/86 1192/87 1190/85 1189/84
+f 1193/88 1194/89 1192/87 1191/86
+f 1195/90 1196/91 1194/89 1193/88
+f 1197/92 1198/93 1196/91 1195/90
+f 1199/94 1200/95 1198/93 1197/92
+f 1201/96 1202/97 1200/95 1199/94
+f 1203/98 1204/99 1202/97 1201/96
+f 1205/100 1206/101 1204/99 1203/98
+f 1207/102 1208/103 1206/101 1205/100
+f 1209/104 1210/105 1208/103 1207/102
+f 1211/106 1212/107 1210/105 1209/104
+f 1213/108 1214/109 1212/107 1211/106
+f 1215/110 1216/111 1214/109 1213/108
+f 1217/112 1218/113 1216/111 1215/110
+f 1219/114 1220/115 1218/113 1217/112
+f 1221/116 1222/117 1220/115 1219/114
+f 1223/118 1224/119 1222/117 1221/116
+f 1225/120 1226/121 1224/119 1223/118
+f 1227/122 1228/123 1226/121 1225/120
+f 1229/124 1230/125 1228/123 1227/122
+f 1231/126 1232/127 1230/125 1229/124
+f 1233/128 1234/129 1232/127 1231/126
+f 1235/130 1236/131 1234/129 1233/128
+f 1237/132 1238/133 1236/131 1235/130
+f 1239/134 1240/135 1238/133 1237/132
+f 1241/136 1242/137 1240/135 1239/134
+f 1243/138 1244/139 1242/137 1241/136
+f 1245/140 1246/141 1244/139 1243/138
+f 1247/142 1248/143 1246/141 1245/140
+f 1249/144 1250/145 1248/143 1247/142
+f 1251/146 1252/147 1250/145 1249/144
+f 1253/148 1254/149 1252/147 1251/146
+f 1255/150 1256/151 1254/149 1253/148
+f 1257/152 1258/153 1256/151 1255/150
+f 1259/154 1260/155 1258/153 1257/152
+f 1261/156 1262/157 1260/155 1259/154
+f 1263/158 1264/159 1262/157 1261/156
+f 1265/160 1266/161 1264/159 1263/158
+f 1267/162 1268/163 1266/161 1265/160
+f 1269/164 1270/165 1268/163 1267/162
+f 1271/166 1272/167 1270/165 1269/164
+f 1273/168 1274/169 1272/167 1271/166
+f 1275/170 1276/171 1274/169 1273/168
+f 1277/172 1278/173 1276/171 1275/170
+f 1279/174 1280/175 1278/173 1277/172
+f 1281/176 1282/177 1280/175 1279/174
+f 1283/178 1284/179 1282/177 1281/176
+f 1285/180 1286/181 1284/179 1283/178
+f 1287/182 1288/183 1286/181 1285/180
+f 1289/184 1290/185 1288/183 1287/182
+f 1291/186 1292/187 1290/185 1289/184
+f 1293/188 1294/189 1292/187 1291/186
+f 1295/190 1296/191 1294/189 1293/188
+f 1297/192 1298/193 1296/191 1295/190
+f 1299/194 1300/195 1298/193 1297/192
+f 1301/196 1302/197 1300/195 1299/194
+f 1303/198 1304/199 1302/197 1301/196
+f 1305/200 1306/201 1304/199 1303/198
+f 1307/202 1308/203 1306/201 1305/200
+f 1309/204 1310/205 1308/203 1307/202
+f 1311/206 1312/207 1310/205 1309/204
+f 1313/208 1314/209 1312/207 1311/206
+f 1315/210 1316/211 1314/209 1313/208
+f 1317/212 1318/213 1316/211 1315/210
+f 1319/214 1320/215 1318/213 1317/212
+f 1321/216 1322/217 1320/215 1319/214
+f 1323/218 1324/219 1322/217 1321/216
+f 1325/220 1326/221 1324/219 1323/218
+f 1327/222 1328/223 1326/221 1325/220
+f 1329/224 1330/225 1328/223 1327/222
+f 1331/226 1332/227 1330/225 1329/224
+f 1333/228 1334/229 1332/227 1331/226
+f 1335/230 1336/231 1334/229 1333/228
+f 1337/232 1338/233 1336/231 1335/230
+f 1339/234 1340/235 1338/233 1337/232
+f 1341/236 1342/237 1340/235 1339/234
+f 1343/238 1344/239 1342/237 1341/236
+f 1345/240 1346/241 1344/239 1343/238
+f 1347/242 1348/243 1346/241 1345/240
+f 1349/244 1350/245 1348/243 1347/242
+f 1351/246 1352/247 1350/245 1349/244
+f 1353/248 1354/249 1352/247 1351/246
+f 1355/250 1356/251 1354/249 1353/248
+f 1357/252 1358/253 1356/251 1355/250
+f 1359/254 1360/255 1358/253 1357/252
+f 1361/256 1362/257 1360/255 1359/254
+f 1363/258 1364/259 1362/257 1361/256
+f 1365/260 1366/261 1364/259 1363/258
+f 1367/262 1368/263 1366/261 1365/260
+f 1369/264 1370/265 1368/263 1367/262
+f 1371/266 1372/267 1370/265 1369/264
+f 1373/268 1374/269 1372/267 1371/266
+f 1375/270 1376/271 1374/269 1373/268
+f 1377/272 1378/273 1376/271 1375/270
+f 1379/274 1380/275 1378/273 1377/272
+f 1381/276 1382/277 1380/275 1379/274
+f 1383/278 1384/279 1382/277 1381/276
+f 1385/280 1386/281 1384/279 1383/278
+f 1387/282 1388/283 1386/281 1385/280
+f 1389/284 1390/285 1388/283 1387/282
+f 1391/286 1392/287 1390/285 1389/284
+f 1393/288 1394/289 1392/287 1391/286
+f 1395/290 1396/291 1394/289 1393/288
+f 1397/292 1398/293 1396/291 1395/290
+f 1399/294 1400/295 1398/293 1397/292
+f 1401/296 1402/297 1400/295 1399/294
+f 1403/298 1404/299 1402/297 1401/296
+f 1405/300 1406/301 1404/299 1403/298
+f 1407/302 1408/303 1406/301 1405/300
+f 1409/304 1410/305 1408/303 1407/302
+f 1411/306 1412/307 1410/305 1409/304
+f 1413/308 1414/309 1412/307 1411/306
+f 1415/310 1416/311 1414/309 1413/308
+f 1417/312 1418/313 1416/311 1415/310
+f 1419/314 1420/315 1418/313 1417/312
+f 1421/316 1422/317 1420/315 1419/314
+f 1423/318 1424/319 1422/317 1421/316
+f 1425/320 1426/321 1424/319 1423/318
+f 1427/322 1428/323 1426/321 1425/320
+f 1429/324 1430/325 1428/323 1427/322
+f 1431/326 1432/327 1430/325 1429/324
+f 1433/328 1434/329 1432/327 1431/326
+f 1435/330 1436/331 1434/329 1433/328
+f 1437/332 1438/333 1436/331 1435/330
+f 1439/334 1440/335 1438/333 1437/332
+f 1441/336 1442/337 1440/335 1439/334
+f 1122/10 1121/15 1442/337 1441/336
+f 1443/338 1444/339 1445/11 1446/4
+f 1447/340 1448/341 1444/339 1443/338
+f 1449/342 1450/343 1448/341 1447/340
+f 1451/344 1452/345 1450/343 1449/342
+f 1453/346 1454/347 1452/345 1451/344
+f 1455/348 1456/349 1454/347 1453/346
+f 1457/350 1458/351 1456/349 1455/348
+f 1459/352 1460/353 1458/351 1457/350
+f 1461/354 1462/355 1460/353 1459/352
+f 1463/356 1464/357 1462/355 1461/354
+f 1465/358 1466/359 1464/357 1463/356
+f 1467/360 1468/361 1466/359 1465/358
+f 1469/362 1470/363 1468/361 1467/360
+f 1471/364 1472/365 1470/363 1469/362
+f 1473/366 1474/367 1472/365 1471/364
+f 1475/368 1476/369 1474/367 1473/366
+f 1477/370 1478/371 1476/369 1475/368
+f 1479/372 1480/373 1478/371 1477/370
+f 1481/374 1482/375 1480/373 1479/372
+f 1483/376 1484/377 1482/375 1481/374
+f 1485/378 1486/379 1484/377 1483/376
+f 1487/380 1488/381 1486/379 1485/378
+f 1489/382 1490/383 1488/381 1487/380
+f 1491/384 1492/385 1490/383 1489/382
+f 1493/386 1494/387 1492/385 1491/384
+f 1495/388 1496/389 1494/387 1493/386
+f 1497/390 1498/391 1496/389 1495/388
+f 1499/392 1500/393 1498/391 1497/390
+f 1501/394 1502/395 1500/393 1499/392
+f 1503/396 1504/397 1502/395 1501/394
+f 1505/398 1506/399 1504/397 1503/396
+f 1507/400 1508/401 1506/399 1505/398
+f 1509/402 1510/403 1508/401 1507/400
+f 1511/404 1512/405 1510/403 1509/402
+f 1513/406 1514/407 1512/405 1511/404
+f 1515/408 1516/409 1514/407 1513/406
+f 1517/410 1518/411 1516/409 1515/408
+f 1519/412 1520/413 1518/411 1517/410
+f 1521/414 1522/415 1520/413 1519/412
+f 1523/416 1524/417 1522/415 1521/414
+f 1525/418 1526/419 1524/417 1523/416
+f 1527/420 1528/421 1526/419 1525/418
+f 1529/422 1530/423 1528/421 1527/420
+f 1531/424 1532/425 1530/423 1529/422
+f 1533/426 1534/427 1532/425 1531/424
+f 1535/428 1536/429 1534/427 1533/426
+f 1537/430 1538/431 1536/429 1535/428
+f 1539/432 1540/433 1538/431 1537/430
+f 1541/434 1542/435 1540/433 1539/432
+f 1543/436 1544/437 1542/435 1541/434
+f 1545/438 1546/439 1544/437 1543/436
+f 1547/440 1548/441 1546/439 1545/438
+f 1549/442 1550/443 1548/441 1547/440
+f 1551/444 1552/445 1550/443 1549/442
+f 1553/446 1554/447 1552/445 1551/444
+f 1555/448 1556/449 1554/447 1553/446
+f 1557/450 1558/451 1556/449 1555/448
+f 1559/452 1560/453 1558/451 1557/450
+f 1561/454 1562/455 1560/453 1559/452
+f 1563/456 1564/457 1562/455 1561/454
+f 1565/458 1566/459 1564/457 1563/456
+f 1567/460 1568/461 1566/459 1565/458
+f 1569/462 1570/463 1568/461 1567/460
+f 1571/464 1572/465 1570/463 1569/462
+f 1573/466 1574/467 1572/465 1571/464
+f 1575/468 1576/469 1574/467 1573/466
+f 1577/470 1578/471 1576/469 1575/468
+f 1579/472 1580/473 1578/471 1577/470
+f 1581/474 1582/475 1580/473 1579/472
+f 1583/476 1584/477 1582/475 1581/474
+f 1585/478 1586/479 1584/477 1583/476
+f 1587/480 1588/481 1586/479 1585/478
+f 1589/482 1590/483 1588/481 1587/480
+f 1591/484 1592/485 1590/483 1589/482
+f 1593/486 1594/487 1592/485 1591/484
+f 1595/488 1596/489 1594/487 1593/486
+f 1597/490 1598/491 1596/489 1595/488
+f 1599/492 1600/493 1598/491 1597/490
+f 1601/494 1602/495 1600/493 1599/492
+f 1603/496 1604/497 1602/495 1601/494
+f 1605/498 1606/499 1604/497 1603/496
+f 1607/500 1608/501 1606/499 1605/498
+f 1609/502 1610/503 1608/501 1607/500
+f 1611/504 1612/505 1610/503 1609/502
+f 1613/506 1614/507 1612/505 1611/504
+f 1615/508 1616/509 1614/507 1613/506
+f 1617/510 1618/511 1616/509 1615/508
+f 1619/512 1620/513 1618/511 1617/510
+f 1621/514 1622/515 1620/513 1619/512
+f 1446/10 1445/15 1622/515 1621/514
+f 1623/1 1624/2 1625/3 1626/4
+f 1624/2 1627/5 1628/6 1625/3
+f 1627/5 1629/7 1630/8 1628/6
+f 1629/7 1623/9 1626/10 1630/8
+f 1631/11 1632/12 1624/2 1623/1
+f 1632/12 1633/13 1627/5 1624/2
+f 1633/13 1634/14 1629/7 1627/5
+f 1634/14 1631/15 1623/9 1629/7
+f 1635/4 1636/4 1637/4
+f 1638/4 1635/4 1637/4
+f 1639/4 1640/4 1641/4
+f 1642/4 1639/4 1641/4
+f 1643/516 1644/517 1645/11 1646/4
+f 1647/518 1648/519 1644/517 1643/516
+f 1649/520 1650/521 1648/519 1647/518
+f 1651/522 1652/523 1650/521 1649/520
+f 1653/524 1654/525 1652/523 1651/522
+f 1655/526 1656/527 1654/525 1653/524
+f 1657/528 1658/529 1656/527 1655/526
+f 1659/530 1660/531 1658/529 1657/528
+f 1661/532 1662/533 1660/531 1659/530
+f 1663/534 1664/535 1662/533 1661/532
+f 1665/536 1666/537 1664/535 1663/534
+f 1667/538 1668/539 1666/537 1665/536
+f 1669/540 1670/541 1668/539 1667/538
+f 1671/542 1672/543 1670/541 1669/540
+f 1673/544 1674/545 1672/543 1671/542
+f 1675/546 1676/547 1674/545 1673/544
+f 1677/548 1678/549 1676/547 1675/546
+f 1679/550 1680/551 1678/549 1677/548
+f 1681/552 1682/553 1680/551 1679/550
+f 1683/554 1684/555 1682/553 1681/552
+f 1685/556 1686/557 1684/555 1683/554
+f 1687/558 1688/559 1686/557 1685/556
+f 1689/560 1690/561 1688/559 1687/558
+f 1691/562 1692/563 1690/561 1689/560
+f 1693/564 1694/565 1692/563 1691/562
+f 1695/566 1696/567 1694/565 1693/564
+f 1697/568 1698/569 1696/567 1695/566
+f 1699/570 1700/571 1698/569 1697/568
+f 1701/572 1702/573 1700/571 1699/570
+f 1703/574 1704/575 1702/573 1701/572
+f 1705/576 1706/577 1704/575 1703/574
+f 1707/578 1708/579 1706/577 1705/576
+f 1709/580 1710/581 1708/579 1707/578
+f 1711/582 1712/583 1710/581 1709/580
+f 1713/584 1714/585 1712/583 1711/582
+f 1715/586 1716/587 1714/585 1713/584
+f 1717/588 1718/589 1716/587 1715/586
+f 1719/590 1720/591 1718/589 1717/588
+f 1721/592 1722/593 1720/591 1719/590
+f 1723/594 1724/595 1722/593 1721/592
+f 1725/596 1726/597 1724/595 1723/594
+f 1727/598 1728/599 1726/597 1725/596
+f 1729/600 1730/601 1728/599 1727/598
+f 1731/602 1732/603 1730/601 1729/600
+f 1733/604 1734/605 1732/603 1731/602
+f 1735/606 1736/607 1734/605 1733/604
+f 1737/608 1738/609 1736/607 1735/606
+f 1739/610 1740/611 1738/609 1737/608
+f 1741/612 1742/613 1740/611 1739/610
+f 1743/614 1744/615 1742/613 1741/612
+f 1745/616 1746/617 1744/615 1743/614
+f 1747/618 1748/619 1746/617 1745/616
+f 1749/620 1750/621 1748/619 1747/618
+f 1751/622 1752/623 1750/621 1749/620
+f 1753/624 1754/625 1752/623 1751/622
+f 1755/626 1756/627 1754/625 1753/624
+f 1757/628 1758/629 1756/627 1755/626
+f 1759/630 1760/631 1758/629 1757/628
+f 1761/632 1762/633 1760/631 1759/630
+f 1763/634 1764/635 1762/633 1761/632
+f 1765/636 1766/637 1764/635 1763/634
+f 1767/638 1768/639 1766/637 1765/636
+f 1769/640 1770/641 1768/639 1767/638
+f 1771/642 1772/643 1770/641 1769/640
+f 1773/644 1774/645 1772/643 1771/642
+f 1775/646 1776/647 1774/645 1773/644
+f 1777/648 1778/649 1776/647 1775/646
+f 1779/650 1780/651 1778/649 1777/648
+f 1781/652 1782/653 1780/651 1779/650
+f 1783/654 1784/655 1782/653 1781/652
+f 1785/656 1786/657 1784/655 1783/654
+f 1787/658 1788/659 1786/657 1785/656
+f 1789/660 1790/661 1788/659 1787/658
+f 1791/662 1792/663 1790/661 1789/660
+f 1793/664 1794/665 1792/663 1791/662
+f 1795/666 1796/667 1794/665 1793/664
+f 1646/10 1645/15 1796/667 1795/666
+f 1797/338 1798/339 1799/11 1800/4
+f 1801/340 1802/341 1798/339 1797/338
+f 1803/342 1804/343 1802/341 1801/340
+f 1805/344 1806/345 1804/343 1803/342
+f 1807/346 1808/347 1806/345 1805/344
+f 1809/348 1810/349 1808/347 1807/346
+f 1811/350 1812/351 1810/349 1809/348
+f 1813/352 1814/353 1812/351 1811/350
+f 1815/354 1816/355 1814/353 1813/352
+f 1817/356 1818/357 1816/355 1815/354
+f 1819/358 1820/359 1818/357 1817/356
+f 1821/360 1822/361 1820/359 1819/358
+f 1823/362 1824/363 1822/361 1821/360
+f 1825/364 1826/365 1824/363 1823/362
+f 1827/366 1828/367 1826/365 1825/364
+f 1829/368 1830/369 1828/367 1827/366
+f 1831/370 1832/371 1830/369 1829/368
+f 1833/372 1834/373 1832/371 1831/370
+f 1835/374 1836/375 1834/373 1833/372
+f 1837/376 1838/377 1836/375 1835/374
+f 1839/378 1840/379 1838/377 1837/376
+f 1841/380 1842/381 1840/379 1839/378
+f 1843/382 1844/383 1842/381 1841/380
+f 1845/384 1846/385 1844/383 1843/382
+f 1847/386 1848/387 1846/385 1845/384
+f 1849/388 1850/389 1848/387 1847/386
+f 1851/390 1852/391 1850/389 1849/388
+f 1853/392 1854/393 1852/391 1851/390
+f 1855/394 1856/395 1854/393 1853/392
+f 1857/396 1858/397 1856/395 1855/394
+f 1859/398 1860/399 1858/397 1857/396
+f 1861/400 1862/401 1860/399 1859/398
+f 1863/402 1864/403 1862/401 1861/400
+f 1865/404 1866/405 1864/403 1863/402
+f 1867/406 1868/407 1866/405 1865/404
+f 1869/408 1870/409 1868/407 1867/406
+f 1871/410 1872/411 1870/409 1869/408
+f 1873/412 1874/413 1872/411 1871/410
+f 1875/414 1876/415 1874/413 1873/412
+f 1877/416 1878/417 1876/415 1875/414
+f 1879/418 1880/419 1878/417 1877/416
+f 1881/420 1882/421 1880/419 1879/418
+f 1883/422 1884/423 1882/421 1881/420
+f 1885/424 1886/425 1884/423 1883/422
+f 1887/426 1888/427 1886/425 1885/424
+f 1889/428 1890/429 1888/427 1887/426
+f 1891/430 1892/431 1890/429 1889/428
+f 1893/432 1894/433 1892/431 1891/430
+f 1895/434 1896/435 1894/433 1893/432
+f 1897/436 1898/437 1896/435 1895/434
+f 1899/438 1900/439 1898/437 1897/436
+f 1901/440 1902/441 1900/439 1899/438
+f 1903/442 1904/443 1902/441 1901/440
+f 1905/444 1906/445 1904/443 1903/442
+f 1907/446 1908/447 1906/445 1905/444
+f 1909/448 1910/449 1908/447 1907/446
+f 1911/450 1912/451 1910/449 1909/448
+f 1913/452 1914/453 1912/451 1911/450
+f 1915/454 1916/455 1914/453 1913/452
+f 1917/456 1918/457 1916/455 1915/454
+f 1919/458 1920/459 1918/457 1917/456
+f 1921/460 1922/461 1920/459 1919/458
+f 1923/462 1924/463 1922/461 1921/460
+f 1925/464 1926/465 1924/463 1923/462
+f 1927/466 1928/467 1926/465 1925/464
+f 1929/468 1930/469 1928/467 1927/466
+f 1931/470 1932/471 1930/469 1929/468
+f 1933/472 1934/473 1932/471 1931/470
+f 1935/474 1936/475 1934/473 1933/472
+f 1937/476 1938/477 1936/475 1935/474
+f 1939/478 1940/479 1938/477 1937/476
+f 1941/480 1942/481 1940/479 1939/478
+f 1943/482 1944/483 1942/481 1941/480
+f 1945/484 1946/485 1944/483 1943/482
+f 1947/486 1948/487 1946/485 1945/484
+f 1949/488 1950/489 1948/487 1947/486
+f 1951/490 1952/491 1950/489 1949/488
+f 1953/492 1954/493 1952/491 1951/490
+f 1955/494 1956/495 1954/493 1953/492
+f 1957/496 1958/497 1956/495 1955/494
+f 1959/498 1960/499 1958/497 1957/496
+f 1961/500 1962/501 1960/499 1959/498
+f 1963/502 1964/503 1962/501 1961/500
+f 1965/504 1966/505 1964/503 1963/502
+f 1967/506 1968/507 1966/505 1965/504
+f 1969/508 1970/509 1968/507 1967/506
+f 1971/510 1972/511 1970/509 1969/508
+f 1973/512 1974/513 1972/511 1971/510
+f 1975/514 1976/515 1974/513 1973/512
+f 1800/10 1799/15 1976/515 1975/514
+f 1977/1 1978/2 1979/3 1980/4
+f 1978/2 1981/5 1982/6 1979/3
+f 1981/5 1983/7 1984/8 1982/6
+f 1983/7 1977/9 1980/10 1984/8
+f 1985/11 1986/12 1978/2 1977/1
+f 1986/12 1987/13 1981/5 1978/2
+f 1987/13 1988/14 1983/7 1981/5
+f 1988/14 1985/15 1977/9 1983/7
+f 1989/4 1990/4 1991/4
+f 1992/4 1989/4 1991/4
+f 1993/4 1994/4 1995/4
+f 1996/4 1993/4 1995/4
+f 1997/668 1998/669 1999/11 2000/4
+f 2001/670 2002/671 1998/669 1997/668
+f 2003/672 2004/673 2002/671 2001/670
+f 2005/674 2006/675 2004/673 2003/672
+f 2007/676 2008/677 2006/675 2005/674
+f 2009/678 2010/679 2008/677 2007/676
+f 2011/680 2012/681 2010/679 2009/678
+f 2013/682 2014/683 2012/681 2011/680
+f 2015/684 2016/685 2014/683 2013/682
+f 2000/10 1999/15 2016/685 2015/684
+f 2017/338 2018/339 2019/11 2020/4
+f 2021/340 2022/341 2018/339 2017/338
+f 2023/342 2024/343 2022/341 2021/340
+f 2025/344 2026/345 2024/343 2023/342
+f 2027/346 2028/347 2026/345 2025/344
+f 2029/348 2030/349 2028/347 2027/346
+f 2031/350 2032/351 2030/349 2029/348
+f 2033/352 2034/353 2032/351 2031/350
+f 2035/354 2036/355 2034/353 2033/352
+f 2037/356 2038/357 2036/355 2035/354
+f 2039/358 2040/359 2038/357 2037/356
+f 2041/360 2042/361 2040/359 2039/358
+f 2043/362 2044/363 2042/361 2041/360
+f 2045/364 2046/365 2044/363 2043/362
+f 2047/366 2048/367 2046/365 2045/364
+f 2049/368 2050/369 2048/367 2047/366
+f 2051/370 2052/371 2050/369 2049/368
+f 2053/372 2054/373 2052/371 2051/370
+f 2055/374 2056/375 2054/373 2053/372
+f 2057/376 2058/377 2056/375 2055/374
+f 2059/378 2060/379 2058/377 2057/376
+f 2061/380 2062/381 2060/379 2059/378
+f 2063/382 2064/383 2062/381 2061/380
+f 2065/384 2066/385 2064/383 2063/382
+f 2067/386 2068/387 2066/385 2065/384
+f 2069/388 2070/389 2068/387 2067/386
+f 2071/390 2072/391 2070/389 2069/388
+f 2073/392 2074/393 2072/391 2071/390
+f 2075/394 2076/395 2074/393 2073/392
+f 2077/396 2078/397 2076/395 2075/394
+f 2079/398 2080/399 2078/397 2077/396
+f 2081/400 2082/401 2080/399 2079/398
+f 2083/402 2084/403 2082/401 2081/400
+f 2085/404 2086/405 2084/403 2083/402
+f 2087/406 2088/407 2086/405 2085/404
+f 2089/408 2090/409 2088/407 2087/406
+f 2091/410 2092/411 2090/409 2089/408
+f 2093/412 2094/413 2092/411 2091/410
+f 2095/414 2096/415 2094/413 2093/412
+f 2097/416 2098/417 2096/415 2095/414
+f 2099/418 2100/419 2098/417 2097/416
+f 2101/420 2102/421 2100/419 2099/418
+f 2103/422 2104/423 2102/421 2101/420
+f 2105/424 2106/425 2104/423 2103/422
+f 2107/426 2108/427 2106/425 2105/424
+f 2109/428 2110/429 2108/427 2107/426
+f 2111/430 2112/431 2110/429 2109/428
+f 2113/432 2114/433 2112/431 2111/430
+f 2115/434 2116/435 2114/433 2113/432
+f 2117/436 2118/437 2116/435 2115/434
+f 2119/438 2120/439 2118/437 2117/436
+f 2121/440 2122/441 2120/439 2119/438
+f 2123/442 2124/443 2122/441 2121/440
+f 2125/444 2126/445 2124/443 2123/442
+f 2127/446 2128/447 2126/445 2125/444
+f 2129/448 2130/449 2128/447 2127/446
+f 2131/450 2132/451 2130/449 2129/448
+f 2133/452 2134/453 2132/451 2131/450
+f 2135/454 2136/455 2134/453 2133/452
+f 2137/456 2138/457 2136/455 2135/454
+f 2139/458 2140/459 2138/457 2137/456
+f 2141/460 2142/461 2140/459 2139/458
+f 2143/462 2144/463 2142/461 2141/460
+f 2145/464 2146/465 2144/463 2143/462
+f 2147/466 2148/467 2146/465 2145/464
+f 2149/468 2150/469 2148/467 2147/466
+f 2151/470 2152/471 2150/469 2149/468
+f 2153/472 2154/473 2152/471 2151/470
+f 2155/474 2156/475 2154/473 2153/472
+f 2157/476 2158/477 2156/475 2155/474
+f 2159/478 2160/479 2158/477 2157/476
+f 2161/480 2162/481 2160/479 2159/478
+f 2163/482 2164/483 2162/481 2161/480
+f 2165/484 2166/485 2164/483 2163/482
+f 2167/486 2168/487 2166/485 2165/484
+f 2169/488 2170/489 2168/487 2167/486
+f 2171/490 2172/491 2170/489 2169/488
+f 2173/492 2174/493 2172/491 2171/490
+f 2175/494 2176/495 2174/493 2173/492
+f 2177/496 2178/497 2176/495 2175/494
+f 2179/498 2180/499 2178/497 2177/496
+f 2181/500 2182/501 2180/499 2179/498
+f 2183/502 2184/503 2182/501 2181/500
+f 2185/504 2186/505 2184/503 2183/502
+f 2187/506 2188/507 2186/505 2185/504
+f 2189/508 2190/509 2188/507 2187/506
+f 2191/510 2192/511 2190/509 2189/508
+f 2193/512 2194/513 2192/511 2191/510
+f 2195/514 2196/515 2194/513 2193/512
+f 2020/10 2019/15 2196/515 2195/514
+f 2197/15 2198/10 2199/686
+f 2200/15 2201/10 2198/4 2197/11
+f 2202/687 2201/4 2200/11
+f 2200/15 2197/10 2199/4 2202/11
+f 2198/15 2201/10 2202/4 2199/11
+f 2203/4 2204/4 2205/4
+f 2206/4 2204/4 2203/4
+f 2207/4 2203/4 2205/4
+f 2206/4 2203/4 2208/4
+f 2206/4 2208/4 2209/4
+f 2210/4 2206/4 2209/4
+f 2210/4 2209/4 2211/4
+f 2210/4 2211/4 2212/4
+f 2213/4 2210/4 2212/4
+f 2213/4 2212/4 2214/4
+f 2215/4 2213/4 2214/4
+f 2215/4 2214/4 2216/4
+f 2217/4 2215/4 2216/4
+f 2217/4 2216/4 2218/4
+f 2219/4 2217/4 2218/4
+f 2220/4 2219/4 2218/4
+f 2220/4 2218/4 2221/4
+f 2222/4 2220/4 2221/4
+f 2222/4 2221/4 2223/4
+f 2224/4 2222/4 2223/4
+f 2224/4 2223/4 2225/4
+f 2226/4 2224/4 2225/4
+f 2226/4 2225/4 2227/4
+f 2228/4 2226/4 2227/4
+f 2229/4 2228/4 2227/4
+f 2229/4 2227/4 2230/4
+f 2231/4 2229/4 2230/4
+f 2232/4 2233/4 2234/4
+f 2235/4 2231/4 2230/4
+f 2235/4 2230/4 2236/4
+f 2237/4 2235/4 2236/4
+f 2232/4 2234/4 2238/4
+f 2237/4 2236/4 2239/4
+f 2240/4 2237/4 2239/4
+f 2241/4 2240/4 2239/4
+f 2241/4 2239/4 2242/4
+f 2232/4 2238/4 2243/4
+f 2244/4 2241/4 2242/4
+f 2244/4 2242/4 2245/4
+f 2246/4 2244/4 2245/4
+f 2247/4 2232/4 2243/4
+f 2247/4 2243/4 2248/4
+f 2249/4 2247/4 2248/4
+f 2246/4 2245/4 2250/4
+f 2251/4 2246/4 2250/4
+f 2252/4 2249/4 2248/4
+f 2252/4 2248/4 2253/4
+f 2254/4 2251/4 2250/4
+f 2254/4 2250/4 2255/4
+f 2256/4 2252/4 2253/4
+f 2256/4 2253/4 2257/4
+f 2258/4 2254/4 2255/4
+f 2256/4 2257/4 2259/4
+f 2258/4 2255/4 2260/4
+f 2261/4 2258/4 2260/4
+f 2256/4 2259/4 2262/4
+f 2263/4 2256/4 2262/4
+f 2264/4 2261/4 2260/4
+f 2263/4 2262/4 2265/4
+f 2266/4 2264/4 2260/4
+f 2266/4 2260/4 2267/4
+f 2263/4 2265/4 2268/4
+f 2269/4 2266/4 2267/4
+f 2270/4 2263/4 2268/4
+f 2271/4 2269/4 2267/4
+f 2270/4 2268/4 2272/4
+f 2273/4 2271/4 2267/4
+f 2270/4 2272/4 2274/4
+f 2275/4 2273/4 2267/4
+f 2270/4 2274/4 2276/4
+f 2277/4 2275/4 2267/4
+f 2270/4 2276/4 2277/4
+f 2270/4 2277/4 2267/4
+f 2270/4 2267/4 2278/4
+f 2279/4 2270/4 2278/4
+f 2279/4 2278/4 2280/4
+f 2279/4 2280/4 2281/4
+f 2282/4 2279/4 2281/4
+f 2282/4 2281/4 2283/4
+f 2284/4 2282/4 2283/4
+f 2284/4 2283/4 2285/4
+f 2286/4 2284/4 2285/4
+f 2286/4 2285/4 2287/4
+f 2286/4 2287/4 2288/4
+f 2289/4 2286/4 2288/4
+f 2289/4 2288/4 2290/4
+f 2291/4 2289/4 2290/4
+f 2291/4 2290/4 2292/4
+f 2293/4 2294/4 2295/4
+f 2296/4 2293/4 2295/4
+f 2296/4 2295/4 2297/4
+f 2298/4 2296/4 2297/4
+f 2299/4 2300/4 2298/4
+f 2301/4 2299/4 2298/4
+f 2301/4 2298/4 2297/4
+f 2301/4 2297/4 2302/4
+f 2303/4 2304/4 2305/4
+f 2305/4 2304/4 2306/4
+f 2303/4 2305/4 2307/4
+f 2308/4 2305/4 2306/4
+f 2309/4 2308/4 2306/4
+f 2309/4 2306/4 2310/4
+f 2311/4 2309/4 2310/4
+f 2312/4 2311/4 2310/4
+f 2312/4 2310/4 2313/4
+f 2314/4 2312/4 2313/4
+f 2314/4 2313/4 2315/4
+f 2316/4 2314/4 2315/4
+f 2316/4 2315/4 2317/4
+f 2318/4 2316/4 2317/4
+f 2318/4 2317/4 2319/4
+f 2318/4 2319/4 2320/4
+f 2321/4 2318/4 2320/4
+f 2321/4 2320/4 2322/4
+f 2323/4 2321/4 2322/4
+f 2323/4 2322/4 2324/4
+f 2325/4 2323/4 2324/4
+f 2325/4 2324/4 2326/4
+f 2327/4 2325/4 2326/4
+f 2327/4 2326/4 2328/4
+f 2327/4 2328/4 2329/4
+f 2330/4 2327/4 2329/4
+f 2330/4 2329/4 2331/4
+f 2332/4 2333/4 2334/4
+f 2330/4 2331/4 2335/4
+f 2336/4 2330/4 2335/4
+f 2336/4 2335/4 2337/4
+f 2338/4 2332/4 2334/4
+f 2339/4 2336/4 2337/4
+f 2339/4 2337/4 2340/4
+f 2339/4 2340/4 2341/4
+f 2342/4 2339/4 2341/4
+f 2343/4 2338/4 2334/4
+f 2342/4 2341/4 2344/4
+f 2345/4 2342/4 2344/4
+f 2345/4 2344/4 2346/4
+f 2343/4 2334/4 2347/4
+f 2348/4 2343/4 2347/4
+f 2348/4 2347/4 2349/4
+f 2350/4 2345/4 2346/4
+f 2350/4 2346/4 2351/4
+f 2348/4 2349/4 2352/4
+f 2353/4 2348/4 2352/4
+f 2350/4 2351/4 2354/4
+f 2355/4 2350/4 2354/4
+f 2353/4 2352/4 2356/4
+f 2357/4 2353/4 2356/4
+f 2355/4 2354/4 2358/4
+f 2359/4 2357/4 2356/4
+f 2360/4 2355/4 2358/4
+f 2360/4 2358/4 2361/4
+f 2362/4 2359/4 2356/4
+f 2362/4 2356/4 2363/4
+f 2360/4 2361/4 2364/4
+f 2365/4 2362/4 2363/4
+f 2360/4 2364/4 2366/4
+f 2367/4 2360/4 2366/4
+f 2368/4 2365/4 2363/4
+f 2367/4 2366/4 2369/4
+f 2368/4 2363/4 2370/4
+f 2367/4 2369/4 2371/4
+f 2372/4 2368/4 2370/4
+f 2367/4 2371/4 2373/4
+f 2374/4 2372/4 2370/4
+f 2367/4 2373/4 2375/4
+f 2376/4 2374/4 2370/4
+f 2367/4 2375/4 2377/4
+f 2377/4 2376/4 2370/4
+f 2367/4 2377/4 2370/4
+f 2378/4 2367/4 2370/4
+f 2378/4 2370/4 2379/4
+f 2380/4 2378/4 2379/4
+f 2381/4 2380/4 2379/4
+f 2381/4 2379/4 2382/4
+f 2383/4 2381/4 2382/4
+f 2383/4 2382/4 2384/4
+f 2385/4 2383/4 2384/4
+f 2385/4 2384/4 2386/4
+f 2387/4 2385/4 2386/4
+f 2388/4 2387/4 2386/4
+f 2388/4 2386/4 2389/4
+f 2390/4 2388/4 2389/4
+f 2390/4 2389/4 2391/4
+f 2392/4 2390/4 2391/4
+f 2393/4 2394/4 2395/4
+f 2393/4 2395/4 2396/4
+f 2397/4 2393/4 2396/4
+f 2397/4 2396/4 2398/4
+f 2398/4 2399/4 2400/4
+f 2398/4 2400/4 2401/4
+f 2397/4 2398/4 2401/4
+f 2402/4 2397/4 2401/4
+f 2403/4 2404/4 2405/4
+f 2406/4 2404/4 2403/4
+f 2407/4 2403/4 2405/4
+f 2406/4 2403/4 2408/4
+f 2406/4 2408/4 2409/4
+f 2410/4 2406/4 2409/4
+f 2410/4 2409/4 2411/4
+f 2410/4 2411/4 2412/4
+f 2413/4 2410/4 2412/4
+f 2413/4 2412/4 2414/4
+f 2415/4 2413/4 2414/4
+f 2415/4 2414/4 2416/4
+f 2417/4 2415/4 2416/4
+f 2417/4 2416/4 2418/4
+f 2419/4 2417/4 2418/4
+f 2420/4 2419/4 2418/4
+f 2420/4 2418/4 2421/4
+f 2422/4 2420/4 2421/4
+f 2422/4 2421/4 2423/4
+f 2424/4 2422/4 2423/4
+f 2424/4 2423/4 2425/4
+f 2426/4 2424/4 2425/4
+f 2426/4 2425/4 2427/4
+f 2428/4 2426/4 2427/4
+f 2429/4 2428/4 2427/4
+f 2429/4 2427/4 2430/4
+f 2431/4 2429/4 2430/4
+f 2432/4 2433/4 2434/4
+f 2435/4 2431/4 2430/4
+f 2435/4 2430/4 2436/4
+f 2437/4 2435/4 2436/4
+f 2432/4 2434/4 2438/4
+f 2437/4 2436/4 2439/4
+f 2440/4 2437/4 2439/4
+f 2441/4 2440/4 2439/4
+f 2441/4 2439/4 2442/4
+f 2432/4 2438/4 2443/4
+f 2444/4 2441/4 2442/4
+f 2444/4 2442/4 2445/4
+f 2446/4 2444/4 2445/4
+f 2447/4 2432/4 2443/4
+f 2447/4 2443/4 2448/4
+f 2449/4 2447/4 2448/4
+f 2446/4 2445/4 2450/4
+f 2451/4 2446/4 2450/4
+f 2452/4 2449/4 2448/4
+f 2452/4 2448/4 2453/4
+f 2454/4 2451/4 2450/4
+f 2454/4 2450/4 2455/4
+f 2456/4 2452/4 2453/4
+f 2456/4 2453/4 2457/4
+f 2458/4 2454/4 2455/4
+f 2456/4 2457/4 2459/4
+f 2458/4 2455/4 2460/4
+f 2461/4 2458/4 2460/4
+f 2456/4 2459/4 2462/4
+f 2463/4 2456/4 2462/4
+f 2464/4 2461/4 2460/4
+f 2463/4 2462/4 2465/4
+f 2466/4 2464/4 2460/4
+f 2466/4 2460/4 2467/4
+f 2463/4 2465/4 2468/4
+f 2469/4 2466/4 2467/4
+f 2470/4 2463/4 2468/4
+f 2471/4 2469/4 2467/4
+f 2470/4 2468/4 2472/4
+f 2473/4 2471/4 2467/4
+f 2470/4 2472/4 2474/4
+f 2475/4 2473/4 2467/4
+f 2470/4 2474/4 2476/4
+f 2477/4 2475/4 2467/4
+f 2470/4 2476/4 2477/4
+f 2470/4 2477/4 2467/4
+f 2470/4 2467/4 2478/4
+f 2479/4 2470/4 2478/4
+f 2479/4 2478/4 2480/4
+f 2479/4 2480/4 2481/4
+f 2482/4 2479/4 2481/4
+f 2482/4 2481/4 2483/4
+f 2484/4 2482/4 2483/4
+f 2484/4 2483/4 2485/4
+f 2486/4 2484/4 2485/4
+f 2486/4 2485/4 2487/4
+f 2486/4 2487/4 2488/4
+f 2489/4 2486/4 2488/4
+f 2489/4 2488/4 2490/4
+f 2491/4 2489/4 2490/4
+f 2491/4 2490/4 2492/4
+f 2493/4 2494/4 2495/4
+f 2493/4 2495/4 2496/4
+f 2493/4 2496/4 2497/4
+f 2493/4 2497/4 2498/4
+f 2493/4 2498/4 2499/4
+f 2493/4 2499/4 2500/4
+f 2501/4 2500/4 2502/4
+f 2493/4 2500/4 2501/4
+f 2503/4 2502/4 2504/4
+f 2501/4 2502/4 2503/4
+f 2505/4 2503/4 2504/4
+f 2506/4 2505/4 2504/4
+f 2507/4 2506/4 2504/4
+f 2508/4 2507/4 2504/4
+f 2508/4 2504/4 2509/4
+f 2510/4 2508/4 2509/4
+f 2510/4 2509/4 2511/4
+f 2512/4 2510/4 2511/4
+f 2513/4 2512/4 2511/4
+f 2513/4 2511/4 2514/4
+f 2515/4 2513/4 2514/4
+f 2516/4 2515/4 2514/4
+f 2516/4 2514/4 2517/4
+f 2518/4 2516/4 2517/4
+f 2518/4 2517/4 2519/4
+f 2520/4 2518/4 2519/4
+f 2521/4 2520/4 2519/4
+f 2521/4 2519/4 2522/4
+f 2523/4 2521/4 2522/4
+f 2523/4 2522/4 2524/4
+f 2525/4 2523/4 2524/4
+f 2526/4 2525/4 2524/4
+f 2526/4 2524/4 2527/4
+f 2528/4 2526/4 2527/4
+f 2529/4 2528/4 2527/4
+f 2529/4 2527/4 2530/4
+f 2531/4 2529/4 2530/4
+f 2532/4 2531/4 2530/4
+f 2532/4 2530/4 2533/4
+f 2534/4 2532/4 2533/4
+f 2534/4 2533/4 2535/4
+f 2536/4 2534/4 2535/4
+f 2536/4 2535/4 2537/4
+f 2538/4 2536/4 2537/4
+f 2539/4 2538/4 2537/4
+f 2539/4 2537/4 2540/4
+f 2541/4 2539/4 2540/4
+f 2541/4 2540/4 2542/4
+f 2543/4 2541/4 2542/4
+f 2544/4 2543/4 2542/4
+f 2544/4 2542/4 2545/4
+f 2546/4 2544/4 2545/4
+f 2546/4 2545/4 2547/4
+f 2548/4 2546/4 2547/4
+f 2548/4 2547/4 2549/4
+f 2550/4 2548/4 2549/4
+f 2551/4 2550/4 2549/4
+f 2551/4 2549/4 2552/4
+f 2553/4 2551/4 2552/4
+f 2554/4 2553/4 2552/4
+f 2554/4 2552/4 2555/4
+f 2556/4 2554/4 2555/4
+f 2557/4 2556/4 2555/4
+f 2557/4 2555/4 2558/4
+f 2557/4 2558/4 2559/4
+f 2560/4 2557/4 2559/4
+f 2560/4 2559/4 2561/4
+f 2560/4 2561/4 2562/4
+f 2560/4 2562/4 2563/4
+f 2564/4 2560/4 2563/4
+f 2564/4 2563/4 2565/4
+f 2564/4 2565/4 2566/4
+f 2564/4 2566/4 2567/4
+f 2564/4 2567/4 2568/4
+f 2569/4 2564/4 2568/4
+f 2570/4 2571/4 2572/4
+f 2572/4 2571/4 2573/4
+f 2570/4 2572/4 2574/4
+f 2575/4 2572/4 2573/4
+f 2576/4 2575/4 2573/4
+f 2576/4 2573/4 2577/4
+f 2578/4 2576/4 2577/4
+f 2579/4 2578/4 2577/4
+f 2579/4 2577/4 2580/4
+f 2581/4 2579/4 2580/4
+f 2581/4 2580/4 2582/4
+f 2583/4 2581/4 2582/4
+f 2583/4 2582/4 2584/4
+f 2585/4 2583/4 2584/4
+f 2585/4 2584/4 2586/4
+f 2585/4 2586/4 2587/4
+f 2588/4 2585/4 2587/4
+f 2588/4 2587/4 2589/4
+f 2590/4 2588/4 2589/4
+f 2590/4 2589/4 2591/4
+f 2592/4 2590/4 2591/4
+f 2592/4 2591/4 2593/4
+f 2594/4 2592/4 2593/4
+f 2594/4 2593/4 2595/4
+f 2594/4 2595/4 2596/4
+f 2597/4 2594/4 2596/4
+f 2597/4 2596/4 2598/4
+f 2599/4 2600/4 2601/4
+f 2597/4 2598/4 2602/4
+f 2603/4 2597/4 2602/4
+f 2603/4 2602/4 2604/4
+f 2605/4 2599/4 2601/4
+f 2606/4 2603/4 2604/4
+f 2606/4 2604/4 2607/4
+f 2606/4 2607/4 2608/4
+f 2609/4 2606/4 2608/4
+f 2610/4 2605/4 2601/4
+f 2609/4 2608/4 2611/4
+f 2612/4 2609/4 2611/4
+f 2612/4 2611/4 2613/4
+f 2610/4 2601/4 2614/4
+f 2615/4 2610/4 2614/4
+f 2615/4 2614/4 2616/4
+f 2617/4 2612/4 2613/4
+f 2617/4 2613/4 2618/4
+f 2615/4 2616/4 2619/4
+f 2620/4 2615/4 2619/4
+f 2617/4 2618/4 2621/4
+f 2622/4 2617/4 2621/4
+f 2620/4 2619/4 2623/4
+f 2624/4 2620/4 2623/4
+f 2622/4 2621/4 2625/4
+f 2626/4 2624/4 2623/4
+f 2627/4 2622/4 2625/4
+f 2627/4 2625/4 2628/4
+f 2629/4 2626/4 2623/4
+f 2629/4 2623/4 2630/4
+f 2627/4 2628/4 2631/4
+f 2632/4 2629/4 2630/4
+f 2627/4 2631/4 2633/4
+f 2634/4 2627/4 2633/4
+f 2635/4 2632/4 2630/4
+f 2634/4 2633/4 2636/4
+f 2635/4 2630/4 2637/4
+f 2634/4 2636/4 2638/4
+f 2639/4 2635/4 2637/4
+f 2634/4 2638/4 2640/4
+f 2641/4 2639/4 2637/4
+f 2634/4 2640/4 2642/4
+f 2643/4 2641/4 2637/4
+f 2634/4 2642/4 2644/4
+f 2644/4 2643/4 2637/4
+f 2634/4 2644/4 2637/4
+f 2645/4 2634/4 2637/4
+f 2645/4 2637/4 2646/4
+f 2647/4 2645/4 2646/4
+f 2648/4 2647/4 2646/4
+f 2648/4 2646/4 2649/4
+f 2650/4 2648/4 2649/4
+f 2650/4 2649/4 2651/4
+f 2652/4 2650/4 2651/4
+f 2652/4 2651/4 2653/4
+f 2654/4 2652/4 2653/4
+f 2655/4 2654/4 2653/4
+f 2655/4 2653/4 2656/4
+f 2657/4 2655/4 2656/4
+f 2657/4 2656/4 2658/4
+f 2659/4 2657/4 2658/4
+f 2660/4 2661/4 2662/4
+f 2663/4 2660/4 2662/4
+f 2664/4 2663/4 2662/4
+f 2665/4 2664/4 2662/4
+f 2666/4 2665/4 2662/4
+f 2667/4 2666/4 2662/4
+f 2668/4 2667/4 2669/4
+f 2669/4 2667/4 2662/4
+f 2670/4 2668/4 2671/4
+f 2671/4 2668/4 2669/4
+f 2670/4 2671/4 2672/4
+f 2670/4 2672/4 2673/4
+f 2670/4 2673/4 2674/4
+f 2670/4 2674/4 2675/4
+f 2676/4 2670/4 2675/4
+f 2676/4 2675/4 2677/4
+f 2678/4 2676/4 2677/4
+f 2678/4 2677/4 2679/4
+f 2678/4 2679/4 2680/4
+f 2681/4 2678/4 2680/4
+f 2681/4 2680/4 2682/4
+f 2681/4 2682/4 2683/4
+f 2684/4 2681/4 2683/4
+f 2684/4 2683/4 2685/4
+f 2686/4 2684/4 2685/4
+f 2686/4 2685/4 2687/4
+f 2686/4 2687/4 2688/4
+f 2689/4 2686/4 2688/4
+f 2689/4 2688/4 2690/4
+f 2691/4 2689/4 2690/4
+f 2691/4 2690/4 2692/4
+f 2691/4 2692/4 2693/4
+f 2694/4 2691/4 2693/4
+f 2694/4 2693/4 2695/4
+f 2694/4 2695/4 2696/4
+f 2697/4 2694/4 2696/4
+f 2697/4 2696/4 2698/4
+f 2697/4 2698/4 2699/4
+f 2700/4 2697/4 2699/4
+f 2700/4 2699/4 2701/4
+f 2702/4 2700/4 2701/4
+f 2702/4 2701/4 2703/4
+f 2704/4 2702/4 2703/4
+f 2704/4 2703/4 2705/4
+f 2704/4 2705/4 2706/4
+f 2707/4 2704/4 2706/4
+f 2707/4 2706/4 2708/4
+f 2709/4 2707/4 2708/4
+f 2709/4 2708/4 2710/4
+f 2709/4 2710/4 2711/4
+f 2712/4 2709/4 2711/4
+f 2712/4 2711/4 2713/4
+f 2714/4 2712/4 2713/4
+f 2714/4 2713/4 2715/4
+f 2716/4 2714/4 2715/4
+f 2716/4 2715/4 2717/4
+f 2716/4 2717/4 2718/4
+f 2719/4 2716/4 2718/4
+f 2719/4 2718/4 2720/4
+f 2719/4 2720/4 2721/4
+f 2722/4 2719/4 2721/4
+f 2722/4 2721/4 2723/4
+f 2722/4 2723/4 2724/4
+f 2725/4 2722/4 2724/4
+f 2726/4 2725/4 2724/4
+f 2726/4 2724/4 2727/4
+f 2728/4 2726/4 2727/4
+f 2729/4 2728/4 2727/4
+f 2730/4 2729/4 2727/4
+f 2730/4 2727/4 2731/4
+f 2732/4 2730/4 2731/4
+f 2733/4 2732/4 2731/4
+f 2734/4 2733/4 2731/4
+f 2735/4 2734/4 2731/4
+f 2735/4 2731/4 2736/4
+f 2737/4 2738/4 2739/4
+f 2740/4 2738/4 2737/4
+f 2741/4 2737/4 2739/4
+f 2740/4 2737/4 2742/4
+f 2740/4 2742/4 2743/4
+f 2744/4 2740/4 2743/4
+f 2744/4 2743/4 2745/4
+f 2744/4 2745/4 2746/4
+f 2747/4 2744/4 2746/4
+f 2747/4 2746/4 2748/4
+f 2749/4 2747/4 2748/4
+f 2749/4 2748/4 2750/4
+f 2751/4 2749/4 2750/4
+f 2751/4 2750/4 2752/4
+f 2753/4 2751/4 2752/4
+f 2754/4 2753/4 2752/4
+f 2754/4 2752/4 2755/4
+f 2756/4 2754/4 2755/4
+f 2756/4 2755/4 2757/4
+f 2758/4 2756/4 2757/4
+f 2758/4 2757/4 2759/4
+f 2760/4 2758/4 2759/4
+f 2760/4 2759/4 2761/4
+f 2762/4 2760/4 2761/4
+f 2763/4 2762/4 2761/4
+f 2763/4 2761/4 2764/4
+f 2765/4 2763/4 2764/4
+f 2766/4 2767/4 2768/4
+f 2769/4 2765/4 2764/4
+f 2769/4 2764/4 2770/4
+f 2771/4 2769/4 2770/4
+f 2766/4 2768/4 2772/4
+f 2771/4 2770/4 2773/4
+f 2774/4 2771/4 2773/4
+f 2775/4 2774/4 2773/4
+f 2775/4 2773/4 2776/4
+f 2766/4 2772/4 2777/4
+f 2778/4 2775/4 2776/4
+f 2778/4 2776/4 2779/4
+f 2780/4 2778/4 2779/4
+f 2781/4 2766/4 2777/4
+f 2781/4 2777/4 2782/4
+f 2783/4 2781/4 2782/4
+f 2780/4 2779/4 2784/4
+f 2785/4 2780/4 2784/4
+f 2786/4 2783/4 2782/4
+f 2786/4 2782/4 2787/4
+f 2788/4 2785/4 2784/4
+f 2788/4 2784/4 2789/4
+f 2790/4 2786/4 2787/4
+f 2790/4 2787/4 2791/4
+f 2792/4 2788/4 2789/4
+f 2790/4 2791/4 2793/4
+f 2792/4 2789/4 2794/4
+f 2795/4 2792/4 2794/4
+f 2790/4 2793/4 2796/4
+f 2797/4 2790/4 2796/4
+f 2798/4 2795/4 2794/4
+f 2797/4 2796/4 2799/4
+f 2800/4 2798/4 2794/4
+f 2800/4 2794/4 2801/4
+f 2797/4 2799/4 2802/4
+f 2803/4 2800/4 2801/4
+f 2804/4 2797/4 2802/4
+f 2805/4 2803/4 2801/4
+f 2804/4 2802/4 2806/4
+f 2807/4 2805/4 2801/4
+f 2804/4 2806/4 2808/4
+f 2809/4 2807/4 2801/4
+f 2804/4 2808/4 2810/4
+f 2811/4 2809/4 2801/4
+f 2804/4 2810/4 2811/4
+f 2804/4 2811/4 2801/4
+f 2804/4 2801/4 2812/4
+f 2813/4 2804/4 2812/4
+f 2813/4 2812/4 2814/4
+f 2813/4 2814/4 2815/4
+f 2816/4 2813/4 2815/4
+f 2816/4 2815/4 2817/4
+f 2818/4 2816/4 2817/4
+f 2818/4 2817/4 2819/4
+f 2820/4 2818/4 2819/4
+f 2820/4 2819/4 2821/4
+f 2820/4 2821/4 2822/4
+f 2823/4 2820/4 2822/4
+f 2823/4 2822/4 2824/4
+f 2825/4 2823/4 2824/4
+f 2825/4 2824/4 2826/4
+f 2827/4 2828/4 2829/4
+f 2827/4 2829/4 2830/4
+f 2827/4 2830/4 2831/4
+f 2827/4 2831/4 2832/4
+f 2827/4 2832/4 2833/4
+f 2827/4 2833/4 2834/4
+f 2835/4 2834/4 2836/4
+f 2827/4 2834/4 2835/4
+f 2837/4 2835/4 2836/4
+f 2838/4 2837/4 2836/4
+f 2839/4 2838/4 2836/4
+f 2840/4 2839/4 2836/4
+f 2840/4 2836/4 2841/4
+f 2842/4 2840/4 2841/4
+f 2843/4 2842/4 2841/4
+f 2843/4 2841/4 2844/4
+f 2845/4 2843/4 2844/4
+f 2845/4 2844/4 2846/4
+f 2847/4 2845/4 2846/4
+f 2847/4 2846/4 2848/4
+f 2849/4 2847/4 2848/4
+f 2849/4 2848/4 2850/4
+f 2851/4 2849/4 2850/4
+f 2852/4 2851/4 2850/4
+f 2852/4 2850/4 2853/4
+f 2854/4 2852/4 2853/4
+f 2854/4 2853/4 2855/4
+f 2856/4 2854/4 2855/4
+f 2856/4 2855/4 2857/4
+f 2858/4 2856/4 2857/4
+f 2859/4 2858/4 2857/4
+f 2859/4 2857/4 2860/4
+f 2861/4 2859/4 2860/4
+f 2861/4 2860/4 2862/4
+f 2863/4 2861/4 2862/4
+f 2864/4 2863/4 2862/4
+f 2864/4 2862/4 2865/4
+f 2866/4 2864/4 2865/4
+f 2866/4 2865/4 2867/4
+f 2868/4 2869/4 2870/4
+f 2871/4 2866/4 2867/4
+f 2871/4 2867/4 2872/4
+f 2873/4 2871/4 2872/4
+f 2873/4 2872/4 2874/4
+f 2875/4 2873/4 2874/4
+f 2868/4 2870/4 2876/4
+f 2875/4 2874/4 2877/4
+f 2878/4 2875/4 2877/4
+f 2878/4 2877/4 2879/4
+f 2880/4 2878/4 2879/4
+f 2868/4 2876/4 2881/4
+f 2882/4 2868/4 2881/4
+f 2883/4 2880/4 2879/4
+f 2883/4 2879/4 2884/4
+f 2885/4 2883/4 2884/4
+f 2882/4 2881/4 2886/4
+f 2887/4 2885/4 2884/4
+f 2882/4 2886/4 2888/4
+f 2889/4 2887/4 2884/4
+f 2882/4 2888/4 2890/4
+f 2891/4 2889/4 2884/4
+f 2882/4 2890/4 2892/4
+f 2892/4 2891/4 2884/4
+f 2882/4 2892/4 2884/4
+f 2882/4 2884/4 2893/4
+f 2894/4 2882/4 2893/4
+f 2894/4 2893/4 2895/4
+f 2894/4 2895/4 2896/4
+f 2897/4 2894/4 2896/4
+f 2898/4 2894/4 2897/4
+f 2897/4 2896/4 2899/4
+f 2900/4 2897/4 2899/4
+f 2898/4 2897/4 2901/4
+f 2898/4 2901/4 2902/4
+f 2900/4 2899/4 2903/4
+f 2904/4 2898/4 2902/4
+f 2900/4 2903/4 2905/4
+f 2906/4 2900/4 2905/4
+f 2906/4 2905/4 2907/4
+f 2908/4 2906/4 2907/4
+f 2908/4 2907/4 2909/4
+f 2904/4 2902/4 2910/4
+f 2911/4 2904/4 2910/4
+f 2911/4 2910/4 2912/4
+f 2913/4 2911/4 2912/4
+f 2913/4 2912/4 2914/4
+f 2915/4 2913/4 2914/4
+f 2916/4 2917/4 2918/4
+f 2915/4 2914/4 2919/4
+f 2920/4 2915/4 2919/4
+f 2920/4 2919/4 2921/4
+f 2922/4 2920/4 2921/4
+f 2916/4 2918/4 2923/4
+f 2922/4 2921/4 2924/4
+f 2925/4 2922/4 2924/4
+f 2925/4 2924/4 2926/4
+f 2916/4 2923/4 2927/4
+f 2928/4 2925/4 2926/4
+f 2928/4 2926/4 2929/4
+f 2930/4 2928/4 2929/4
+f 2930/4 2929/4 2931/4
+f 2916/4 2927/4 2932/4
+f 2933/4 2916/4 2932/4
+f 2934/4 2930/4 2931/4
+f 2935/4 2933/4 2932/4
+f 2934/4 2931/4 2936/4
+f 2937/4 2934/4 2936/4
+f 2935/4 2932/4 2938/4
+f 2939/4 2935/4 2938/4
+f 2937/4 2936/4 2940/4
+f 2941/4 2937/4 2940/4
+f 2942/4 2939/4 2938/4
+f 2942/4 2938/4 2943/4
+f 2941/4 2940/4 2944/4
+f 2945/4 2941/4 2944/4
+f 2942/4 2943/4 2946/4
+f 2945/4 2944/4 2947/4
+f 2948/4 2942/4 2946/4
+f 2949/4 2945/4 2947/4
+f 2948/4 2946/4 2950/4
+f 2951/4 2949/4 2947/4
+f 2951/4 2947/4 2952/4
+f 2948/4 2950/4 2953/4
+f 2954/4 2951/4 2952/4
+f 2955/4 2948/4 2953/4
+f 2956/4 2954/4 2952/4
+f 2956/4 2952/4 2957/4
+f 2955/4 2953/4 2958/4
+f 2959/4 2956/4 2957/4
+f 2959/4 2957/4 2960/4
+f 2961/4 2959/4 2960/4
+f 2955/4 2958/4 2962/4
+f 2963/4 2955/4 2962/4
+f 2964/4 2961/4 2960/4
+f 2964/4 2960/4 2965/4
+f 2963/4 2962/4 2966/4
+f 2967/4 2964/4 2965/4
+f 2963/4 2966/4 2968/4
+f 2969/4 2967/4 2965/4
+f 2963/4 2968/4 2970/4
+f 2971/4 2969/4 2965/4
+f 2963/4 2970/4 2972/4
+f 2972/4 2971/4 2965/4
+f 2963/4 2972/4 2965/4
+f 2963/4 2965/4 2973/4
+f 2974/4 2963/4 2973/4
+f 2974/4 2973/4 2975/4
+f 2976/4 2974/4 2975/4
+f 2976/4 2975/4 2977/4
+f 2978/4 2976/4 2977/4
+f 2978/4 2977/4 2979/4
+f 2980/4 2978/4 2979/4
+f 2980/4 2979/4 2981/4
+f 2980/4 2981/4 2982/4
+f 2983/4 2980/4 2982/4
+f 2983/4 2982/4 2984/4
+f 2985/4 2983/4 2984/4
+f 2985/4 2984/4 2986/4
+f 2985/4 2986/4 2987/4
+f 2988/4 2985/4 2987/4
+f 2989/4 2990/4 2991/4
+f 2991/4 2990/4 2992/4
+f 2989/4 2991/4 2993/4
+f 2994/4 2991/4 2992/4
+f 2995/4 2994/4 2992/4
+f 2995/4 2992/4 2996/4
+f 2997/4 2995/4 2996/4
+f 2998/4 2997/4 2996/4
+f 2998/4 2996/4 2999/4
+f 3000/4 2998/4 2999/4
+f 3000/4 2999/4 3001/4
+f 3002/4 3000/4 3001/4
+f 3002/4 3001/4 3003/4
+f 3004/4 3002/4 3003/4
+f 3004/4 3003/4 3005/4
+f 3004/4 3005/4 3006/4
+f 3007/4 3004/4 3006/4
+f 3007/4 3006/4 3008/4
+f 3009/4 3007/4 3008/4
+f 3009/4 3008/4 3010/4
+f 3011/4 3009/4 3010/4
+f 3011/4 3010/4 3012/4
+f 3013/4 3011/4 3012/4
+f 3013/4 3012/4 3014/4
+f 3013/4 3014/4 3015/4
+f 3016/4 3013/4 3015/4
+f 3016/4 3015/4 3017/4
+f 3018/4 3019/4 3020/4
+f 3016/4 3017/4 3021/4
+f 3022/4 3016/4 3021/4
+f 3022/4 3021/4 3023/4
+f 3024/4 3018/4 3020/4
+f 3025/4 3022/4 3023/4
+f 3025/4 3023/4 3026/4
+f 3025/4 3026/4 3027/4
+f 3028/4 3025/4 3027/4
+f 3029/4 3024/4 3020/4
+f 3028/4 3027/4 3030/4
+f 3031/4 3028/4 3030/4
+f 3031/4 3030/4 3032/4
+f 3029/4 3020/4 3033/4
+f 3034/4 3029/4 3033/4
+f 3034/4 3033/4 3035/4
+f 3036/4 3031/4 3032/4
+f 3036/4 3032/4 3037/4
+f 3034/4 3035/4 3038/4
+f 3039/4 3034/4 3038/4
+f 3036/4 3037/4 3040/4
+f 3041/4 3036/4 3040/4
+f 3039/4 3038/4 3042/4
+f 3043/4 3039/4 3042/4
+f 3041/4 3040/4 3044/4
+f 3045/4 3043/4 3042/4
+f 3046/4 3041/4 3044/4
+f 3046/4 3044/4 3047/4
+f 3048/4 3045/4 3042/4
+f 3048/4 3042/4 3049/4
+f 3046/4 3047/4 3050/4
+f 3051/4 3048/4 3049/4
+f 3046/4 3050/4 3052/4
+f 3053/4 3046/4 3052/4
+f 3054/4 3051/4 3049/4
+f 3053/4 3052/4 3055/4
+f 3054/4 3049/4 3056/4
+f 3053/4 3055/4 3057/4
+f 3058/4 3054/4 3056/4
+f 3053/4 3057/4 3059/4
+f 3060/4 3058/4 3056/4
+f 3053/4 3059/4 3061/4
+f 3062/4 3060/4 3056/4
+f 3053/4 3061/4 3063/4
+f 3063/4 3062/4 3056/4
+f 3053/4 3063/4 3056/4
+f 3064/4 3053/4 3056/4
+f 3064/4 3056/4 3065/4
+f 3066/4 3064/4 3065/4
+f 3067/4 3066/4 3065/4
+f 3067/4 3065/4 3068/4
+f 3069/4 3067/4 3068/4
+f 3069/4 3068/4 3070/4
+f 3071/4 3069/4 3070/4
+f 3071/4 3070/4 3072/4
+f 3073/4 3071/4 3072/4
+f 3074/4 3073/4 3072/4
+f 3074/4 3072/4 3075/4
+f 3076/4 3074/4 3075/4
+f 3076/4 3075/4 3077/4
+f 3078/4 3076/4 3077/4
+f 3079/4 3080/4 3081/4
+f 3082/4 3079/4 3081/4
+f 3083/4 3082/4 3081/4
+f 3084/4 3083/4 3081/4
+f 3085/4 3084/4 3081/4
+f 3086/4 3085/4 3081/4
+f 3087/4 3086/4 3088/4
+f 3088/4 3086/4 3081/4
+f 3087/4 3088/4 3089/4
+f 3087/4 3089/4 3090/4
+f 3087/4 3090/4 3091/4
+f 3087/4 3091/4 3092/4
+f 3093/4 3087/4 3092/4
+f 3093/4 3092/4 3094/4
+f 3093/4 3094/4 3095/4
+f 3096/4 3093/4 3095/4
+f 3096/4 3095/4 3097/4
+f 3098/4 3096/4 3097/4
+f 3098/4 3097/4 3099/4
+f 3100/4 3098/4 3099/4
+f 3100/4 3099/4 3101/4
+f 3102/4 3100/4 3101/4
+f 3102/4 3101/4 3103/4
+f 3102/4 3103/4 3104/4
+f 3105/4 3102/4 3104/4
+f 3105/4 3104/4 3106/4
+f 3107/4 3105/4 3106/4
+f 3107/4 3106/4 3108/4
+f 3109/4 3107/4 3108/4
+f 3109/4 3108/4 3110/4
+f 3109/4 3110/4 3111/4
+f 3112/4 3109/4 3111/4
+f 3112/4 3111/4 3113/4
+f 3114/4 3112/4 3113/4
+f 3114/4 3113/4 3115/4
+f 3114/4 3115/4 3116/4
+f 3117/4 3114/4 3116/4
+f 3117/4 3116/4 3118/4
+f 3119/4 3117/4 3118/4
+f 3120/4 3121/4 3122/4
+f 3119/4 3118/4 3123/4
+f 3124/4 3119/4 3123/4
+f 3124/4 3123/4 3125/4
+f 3126/4 3124/4 3125/4
+f 3126/4 3125/4 3127/4
+f 3128/4 3120/4 3122/4
+f 3129/4 3126/4 3127/4
+f 3129/4 3127/4 3130/4
+f 3131/4 3129/4 3130/4
+f 3131/4 3130/4 3132/4
+f 3133/4 3128/4 3122/4
+f 3133/4 3122/4 3134/4
+f 3131/4 3132/4 3135/4
+f 3136/4 3131/4 3135/4
+f 3136/4 3135/4 3137/4
+f 3138/4 3133/4 3134/4
+f 3136/4 3137/4 3139/4
+f 3140/4 3138/4 3134/4
+f 3136/4 3139/4 3141/4
+f 3142/4 3140/4 3134/4
+f 3136/4 3141/4 3143/4
+f 3144/4 3142/4 3134/4
+f 3136/4 3143/4 3144/4
+f 3136/4 3144/4 3134/4
+f 3145/4 3136/4 3134/4
+f 3145/4 3134/4 3146/4
+f 3147/4 3145/4 3146/4
+f 3148/4 3147/4 3146/4
+f 3148/4 3146/4 3149/4
+f 3149/4 3146/4 3150/4
+f 3151/4 3148/4 3149/4
+f 3151/4 3149/4 3152/4
+f 3153/4 3149/4 3150/4
+f 3154/4 3153/4 3150/4
+f 3155/4 3151/4 3152/4
+f 3154/4 3150/4 3156/4
+f 3157/4 3155/4 3152/4
+f 3157/4 3152/4 3158/4
+f 3159/4 3157/4 3158/4
+f 3159/4 3158/4 3160/4
+f 3161/4 3159/4 3160/4
+f 3162/4 3154/4 3156/4
+f 3162/4 3156/4 3163/4
+f 3164/4 3162/4 3163/4
+f 3164/4 3163/4 3165/4
+f 3166/4 3164/4 3165/4
+f 3166/4 3165/4 3167/4
+f 3168/4 3169/4 3170/4
+f 3171/4 3166/4 3167/4
+f 3171/4 3167/4 3172/4
+f 3173/4 3171/4 3172/4
+f 3173/4 3172/4 3174/4
+f 3175/4 3168/4 3170/4
+f 3176/4 3173/4 3174/4
+f 3176/4 3174/4 3177/4
+f 3178/4 3176/4 3177/4
+f 3179/4 3175/4 3170/4
+f 3178/4 3177/4 3180/4
+f 3181/4 3178/4 3180/4
+f 3181/4 3180/4 3182/4
+f 3183/4 3181/4 3182/4
+f 3184/4 3179/4 3170/4
+f 3184/4 3170/4 3185/4
+f 3183/4 3182/4 3186/4
+f 3184/4 3185/4 3187/4
+f 3188/4 3183/4 3186/4
+f 3188/4 3186/4 3189/4
+f 3190/4 3184/4 3187/4
+f 3190/4 3187/4 3191/4
+f 3192/4 3188/4 3189/4
+f 3192/4 3189/4 3193/4
+f 3190/4 3191/4 3194/4
+f 3195/4 3190/4 3194/4
+f 3196/4 3192/4 3193/4
+f 3196/4 3193/4 3197/4
+f 3198/4 3195/4 3194/4
+f 3199/4 3196/4 3197/4
+f 3198/4 3194/4 3200/4
+f 3199/4 3197/4 3201/4
+f 3202/4 3198/4 3200/4
+f 3199/4 3201/4 3203/4
+f 3204/4 3199/4 3203/4
+f 3205/4 3202/4 3200/4
+f 3204/4 3203/4 3206/4
+f 3205/4 3200/4 3207/4
+f 3204/4 3206/4 3208/4
+f 3209/4 3204/4 3208/4
+f 3210/4 3205/4 3207/4
+f 3209/4 3208/4 3211/4
+f 3212/4 3209/4 3211/4
+f 3212/4 3211/4 3213/4
+f 3214/4 3210/4 3207/4
+f 3214/4 3207/4 3215/4
+f 3212/4 3213/4 3216/4
+f 3217/4 3212/4 3216/4
+f 3218/4 3214/4 3215/4
+f 3217/4 3216/4 3219/4
+f 3220/4 3218/4 3215/4
+f 3217/4 3219/4 3221/4
+f 3222/4 3220/4 3215/4
+f 3217/4 3221/4 3223/4
+f 3224/4 3222/4 3215/4
+f 3217/4 3223/4 3224/4
+f 3217/4 3224/4 3215/4
+f 3225/4 3217/4 3215/4
+f 3225/4 3215/4 3226/4
+f 3227/4 3225/4 3226/4
+f 3227/4 3226/4 3228/4
+f 3229/4 3227/4 3228/4
+f 3229/4 3228/4 3230/4
+f 3231/4 3229/4 3230/4
+f 3231/4 3230/4 3232/4
+f 3233/4 3231/4 3232/4
+f 3234/4 3233/4 3232/4
+f 3234/4 3232/4 3235/4
+f 3236/4 3234/4 3235/4
+f 3236/4 3235/4 3237/4
+f 3238/4 3236/4 3237/4
+f 3239/4 3238/4 3237/4
+f 3239/4 3237/4 3240/4
+f 3241/4 3242/4 3243/4
+f 3241/4 3243/4 3244/4
+f 3245/4 3241/4 3244/4
+f 3245/4 3244/4 3246/4
+f 3247/4 3248/4 3249/4
+f 3250/4 3247/4 3249/4
+f 3250/4 3249/4 3251/4
+f 3252/4 3250/4 3251/4
+f 3252/4 3251/4 3253/4
+f 3254/4 3252/4 3253/4
+f 3254/4 3253/4 3255/4
+f 3254/4 3255/4 3256/4
+f 3257/4 3254/4 3256/4
+f 3257/4 3256/4 3258/4
+f 3259/4 3257/4 3258/4
+f 3259/4 3258/4 3260/4
+f 3261/4 3259/4 3260/4
+f 3261/4 3260/4 3262/4
+f 3261/4 3262/4 3263/4
+f 3246/4 3261/4 3263/4
+f 3246/4 3263/4 3264/4
+f 3264/4 3263/4 3265/4
+f 3265/4 3263/4 3266/4
+f 3246/4 3264/4 3267/4
+f 3268/4 3265/4 3266/4
+f 3269/4 3268/4 3266/4
+f 3246/4 3267/4 3270/4
+f 3271/4 3269/4 3266/4
+f 3246/4 3270/4 3272/4
+f 3273/4 3271/4 3266/4
+f 3273/4 3266/4 3274/4
+f 3275/4 3273/4 3274/4
+f 3246/4 3272/4 3276/4
+f 3277/4 3275/4 3274/4
+f 3245/4 3246/4 3278/4
+f 3278/4 3246/4 3279/4
+f 3279/4 3246/4 3280/4
+f 3280/4 3246/4 3276/4
+f 3281/4 3277/4 3274/4
+f 3282/4 3281/4 3274/4
+f 3282/4 3274/4 3283/4
+f 3284/4 3282/4 3283/4
+f 3285/4 3284/4 3283/4
+f 3285/4 3283/4 3286/4
+f 3287/4 3285/4 3286/4
+f 3287/4 3286/4 3288/4
+f 3289/4 3287/4 3288/4
+f 3290/4 3289/4 3288/4
+f 3290/4 3288/4 3291/4
+f 3292/4 3290/4 3291/4
+f 3292/4 3291/4 3293/4
+f 3294/4 3292/4 3293/4
+f 3294/4 3293/4 3295/4
+f 3296/4 3294/4 3295/4
+f 3297/4 3296/4 3295/4
+f 3297/4 3295/4 3298/4
+f 3299/4 3297/4 3298/4
+f 3299/4 3298/4 3300/4
+f 3301/4 3299/4 3300/4
+f 3302/4 3301/4 3300/4
+f 3302/4 3300/4 3303/4
+f 3304/4 3302/4 3303/4
+f 3304/4 3303/4 3305/4
+f 3306/4 3304/4 3305/4
+f 3307/4 3306/4 3305/4
+f 3307/4 3305/4 3308/4
+f 3309/4 3307/4 3308/4
+f 3309/4 3308/4 3310/4
+f 3311/4 3309/4 3310/4
+f 3312/4 3311/4 3310/4
+f 3312/4 3310/4 3313/4
+f 3314/4 3312/4 3313/4
+f 3314/4 3313/4 3315/4
+f 3316/4 3314/4 3315/4
+f 3317/4 3316/4 3315/4
+f 3318/4 3317/4 3315/4
+f 3318/4 3315/4 3319/4
+f 3320/4 3318/4 3319/4
+f 3321/4 3320/4 3319/4
+f 3321/4 3319/4 3322/4
+f 3323/4 3321/4 3322/4
+f 3324/4 3323/4 3322/4
+f 3325/4 3324/4 3322/4
+f 3325/4 3322/4 3326/4
+f 3325/4 3326/4 3327/4
+f 3325/4 3327/4 3328/4
+f 3325/4 3328/4 3329/4
+f 3325/4 3329/4 3330/4
+f 3331/4 3332/4 3333/4
+f 3333/4 3332/4 3334/4
+f 3331/4 3333/4 3335/4
+f 3336/4 3333/4 3334/4
+f 3334/4 3337/4 3338/4
+f 3334/4 3338/4 3339/4
+f 3336/4 3334/4 3339/4
+f 3340/4 3336/4 3339/4
+f 3341/4 3342/4 3343/4
+f 3344/4 3341/4 3343/4
+f 3344/4 3343/4 3345/4
+f 3346/4 3344/4 3345/4
+f 3347/4 3348/4 3349/4
+f 3347/4 3349/4 3350/4
+f 3351/4 3347/4 3350/4
+f 3351/4 3350/4 3352/4
+f 3353/4 3351/4 3352/4
+f 3353/4 3352/4 3354/4
+f 3355/4 3353/4 3354/4
+f 3356/4 3355/4 3354/4
+f 3356/4 3354/4 3357/4
+f 3358/4 3356/4 3357/4
+f 3358/4 3357/4 3359/4
+f 3360/4 3358/4 3359/4
+f 3360/4 3359/4 3361/4
+f 3362/4 3360/4 3361/4
+f 3363/4 3362/4 3361/4
+f 3363/4 3361/4 3346/4
+f 3364/4 3363/4 3346/4
+f 3365/4 3363/4 3364/4
+f 3366/4 3363/4 3365/4
+f 3367/4 3364/4 3346/4
+f 3366/4 3365/4 3368/4
+f 3366/4 3368/4 3369/4
+f 3370/4 3367/4 3346/4
+f 3366/4 3369/4 3371/4
+f 3372/4 3370/4 3346/4
+f 3366/4 3371/4 3373/4
+f 3374/4 3366/4 3373/4
+f 3374/4 3373/4 3375/4
+f 3376/4 3372/4 3346/4
+f 3374/4 3375/4 3377/4
+f 3378/4 3346/4 3345/4
+f 3379/4 3346/4 3378/4
+f 3380/4 3346/4 3379/4
+f 3376/4 3346/4 3380/4
+f 3374/4 3377/4 3381/4
+f 3374/4 3381/4 3382/4
+f 3383/4 3374/4 3382/4
+f 3383/4 3382/4 3384/4
+f 3383/4 3384/4 3385/4
+f 3386/4 3383/4 3385/4
+f 3386/4 3385/4 3387/4
+f 3388/4 3386/4 3387/4
+f 3388/4 3387/4 3389/4
+f 3388/4 3389/4 3390/4
+f 3391/4 3388/4 3390/4
+f 3391/4 3390/4 3392/4
+f 3393/4 3391/4 3392/4
+f 3393/4 3392/4 3394/4
+f 3395/4 3393/4 3394/4
+f 3395/4 3394/4 3396/4
+f 3395/4 3396/4 3397/4
+f 3398/4 3395/4 3397/4
+f 3398/4 3397/4 3399/4
+f 3400/4 3398/4 3399/4
+f 3400/4 3399/4 3401/4
+f 3400/4 3401/4 3402/4
+f 3403/4 3400/4 3402/4
+f 3403/4 3402/4 3404/4
+f 3405/4 3403/4 3404/4
+f 3405/4 3404/4 3406/4
+f 3405/4 3406/4 3407/4
+f 3408/4 3405/4 3407/4
+f 3408/4 3407/4 3409/4
+f 3410/4 3408/4 3409/4
+f 3410/4 3409/4 3411/4
+f 3410/4 3411/4 3412/4
+f 3413/4 3410/4 3412/4
+f 3413/4 3412/4 3414/4
+f 3415/4 3413/4 3414/4
+f 3415/4 3414/4 3416/4
+f 3415/4 3416/4 3417/4
+f 3415/4 3417/4 3418/4
+f 3419/4 3415/4 3418/4
+f 3419/4 3418/4 3420/4
+f 3419/4 3420/4 3421/4
+f 3422/4 3419/4 3421/4
+f 3422/4 3421/4 3423/4
+f 3422/4 3423/4 3424/4
+f 3422/4 3424/4 3425/4
+f 3426/4 3422/4 3425/4
+f 3427/4 3426/4 3425/4
+f 3428/4 3427/4 3425/4
+f 3429/4 3428/4 3425/4
+f 3430/4 3429/4 3425/4
+f 3431/4 3432/4 3433/4
+f 3434/4 3432/4 3431/4
+f 3435/4 3431/4 3433/4
+f 3434/4 3431/4 3436/4
+f 3437/4 3438/4 3434/4
+f 3439/4 3437/4 3434/4
+f 3439/4 3434/4 3436/4
+f 3439/4 3436/4 3440/4
+f 3441/4 3442/4 3443/4
+f 3441/4 3443/4 3444/4
+f 3445/4 3441/4 3444/4
+f 3445/4 3444/4 3446/4
+f 3447/4 3448/4 3449/4
+f 3450/4 3447/4 3449/4
+f 3450/4 3449/4 3451/4
+f 3452/4 3450/4 3451/4
+f 3452/4 3451/4 3453/4
+f 3454/4 3452/4 3453/4
+f 3454/4 3453/4 3455/4
+f 3454/4 3455/4 3456/4
+f 3457/4 3454/4 3456/4
+f 3457/4 3456/4 3458/4
+f 3459/4 3457/4 3458/4
+f 3459/4 3458/4 3460/4
+f 3461/4 3459/4 3460/4
+f 3461/4 3460/4 3462/4
+f 3461/4 3462/4 3463/4
+f 3446/4 3461/4 3463/4
+f 3446/4 3463/4 3464/4
+f 3464/4 3463/4 3465/4
+f 3465/4 3463/4 3466/4
+f 3446/4 3464/4 3467/4
+f 3468/4 3465/4 3466/4
+f 3469/4 3468/4 3466/4
+f 3446/4 3467/4 3470/4
+f 3471/4 3469/4 3466/4
+f 3446/4 3470/4 3472/4
+f 3473/4 3471/4 3466/4
+f 3473/4 3466/4 3474/4
+f 3475/4 3473/4 3474/4
+f 3446/4 3472/4 3476/4
+f 3477/4 3475/4 3474/4
+f 3445/4 3446/4 3478/4
+f 3478/4 3446/4 3479/4
+f 3479/4 3446/4 3480/4
+f 3480/4 3446/4 3476/4
+f 3481/4 3477/4 3474/4
+f 3482/4 3481/4 3474/4
+f 3482/4 3474/4 3483/4
+f 3484/4 3482/4 3483/4
+f 3485/4 3484/4 3483/4
+f 3485/4 3483/4 3486/4
+f 3487/4 3485/4 3486/4
+f 3487/4 3486/4 3488/4
+f 3489/4 3487/4 3488/4
+f 3490/4 3489/4 3488/4
+f 3490/4 3488/4 3491/4
+f 3492/4 3490/4 3491/4
+f 3492/4 3491/4 3493/4
+f 3494/4 3492/4 3493/4
+f 3494/4 3493/4 3495/4
+f 3496/4 3494/4 3495/4
+f 3497/4 3496/4 3495/4
+f 3497/4 3495/4 3498/4
+f 3499/4 3497/4 3498/4
+f 3499/4 3498/4 3500/4
+f 3501/4 3499/4 3500/4
+f 3502/4 3501/4 3500/4
+f 3502/4 3500/4 3503/4
+f 3504/4 3502/4 3503/4
+f 3504/4 3503/4 3505/4
+f 3506/4 3504/4 3505/4
+f 3507/4 3506/4 3505/4
+f 3507/4 3505/4 3508/4
+f 3509/4 3507/4 3508/4
+f 3509/4 3508/4 3510/4
+f 3511/4 3509/4 3510/4
+f 3512/4 3511/4 3510/4
+f 3512/4 3510/4 3513/4
+f 3514/4 3512/4 3513/4
+f 3514/4 3513/4 3515/4
+f 3516/4 3514/4 3515/4
+f 3517/4 3516/4 3515/4
+f 3518/4 3517/4 3515/4
+f 3518/4 3515/4 3519/4
+f 3520/4 3518/4 3519/4
+f 3521/4 3520/4 3519/4
+f 3521/4 3519/4 3522/4
+f 3523/4 3521/4 3522/4
+f 3524/4 3523/4 3522/4
+f 3525/4 3524/4 3522/4
+f 3525/4 3522/4 3526/4
+f 3525/4 3526/4 3527/4
+f 3525/4 3527/4 3528/4
+f 3525/4 3528/4 3529/4
+f 3525/4 3529/4 3530/4
+f 3531/4 3532/4 3533/4
+f 3534/4 3531/4 3533/4
+f 3534/4 3533/4 3535/4
+f 3536/4 3534/4 3535/4
+f 3536/4 3535/4 3537/4
+f 3538/4 3536/4 3537/4
+f 3538/4 3537/4 3539/4
+f 3540/4 3538/4 3539/4
+f 3540/4 3539/4 3541/4
+f 3540/4 3541/4 3542/4
+f 3543/4 3540/4 3542/4
+f 3543/4 3542/4 3544/4
+f 3545/4 3543/4 3544/4
+f 3545/4 3544/4 3546/4
+f 3547/4 3545/4 3546/4
+f 3547/4 3546/4 3548/4
+f 3547/4 3548/4 3549/4
+f 3550/4 3547/4 3551/4
+f 3551/4 3547/4 3552/4
+f 3552/4 3547/4 3549/4
+f 3553/4 3552/4 3549/4
+f 3550/4 3551/4 3554/4
+f 3555/4 3553/4 3549/4
+f 3550/4 3554/4 3556/4
+f 3557/4 3555/4 3549/4
+f 3557/4 3549/4 3558/4
+f 3550/4 3556/4 3559/4
+f 3560/4 3557/4 3558/4
+f 3550/4 3559/4 3561/4
+f 3562/4 3560/4 3558/4
+f 3563/4 3562/4 3558/4
+f 3550/4 3561/4 3564/4
+f 3565/4 3550/4 3564/4
+f 3566/4 3563/4 3558/4
+f 3565/4 3564/4 3567/4
+f 3566/4 3558/4 3568/4
+f 3569/4 3566/4 3568/4
+f 3565/4 3567/4 3570/4
+f 3571/4 3569/4 3568/4
+f 3565/4 3570/4 3572/4
+f 3573/4 3565/4 3572/4
+f 3571/4 3568/4 3574/4
+f 3575/4 3571/4 3574/4
+f 3573/4 3572/4 3576/4
+f 3577/4 3575/4 3574/4
+f 3578/4 3573/4 3576/4
+f 3577/4 3574/4 3579/4
+f 3578/4 3576/4 3580/4
+f 3581/4 3577/4 3579/4
+f 3582/4 3581/4 3579/4
+f 3578/4 3580/4 3583/4
+f 3584/4 3578/4 3583/4
+f 3582/4 3579/4 3585/4
+f 3586/4 3582/4 3585/4
+f 3584/4 3583/4 3587/4
+f 3588/4 3586/4 3585/4
+f 3588/4 3585/4 3589/4
+f 3590/4 3584/4 3587/4
+f 3590/4 3587/4 3591/4
+f 3592/4 3588/4 3589/4
+f 3592/4 3589/4 3593/4
+f 3594/4 3590/4 3595/4
+f 3595/4 3590/4 3591/4
+f 3596/4 3592/4 3593/4
+f 3597/4 3596/4 3593/4
+f 3597/4 3593/4 3598/4
+f 3599/4 3597/4 3598/4
+f 3600/4 3599/4 3598/4
+f 3600/4 3598/4 3601/4
+f 3602/4 3600/4 3601/4
+f 3602/4 3601/4 3603/4
+f 3604/4 3605/4 3606/4
+f 3604/4 3606/4 3607/4
+f 3602/4 3603/4 3604/4
+f 3602/4 3604/4 3607/4
+f 3608/4 3609/4 3610/4
+f 3611/4 3608/4 3610/4
+f 3611/4 3610/4 3612/4
+f 3613/4 3611/4 3612/4
+f 3614/4 3615/4 3616/4
+f 3614/4 3616/4 3617/4
+f 3618/4 3614/4 3617/4
+f 3618/4 3617/4 3619/4
+f 3620/4 3618/4 3619/4
+f 3620/4 3619/4 3621/4
+f 3622/4 3620/4 3621/4
+f 3623/4 3622/4 3621/4
+f 3623/4 3621/4 3624/4
+f 3625/4 3623/4 3624/4
+f 3625/4 3624/4 3626/4
+f 3627/4 3625/4 3626/4
+f 3627/4 3626/4 3628/4
+f 3629/4 3627/4 3628/4
+f 3630/4 3629/4 3628/4
+f 3630/4 3628/4 3613/4
+f 3631/4 3630/4 3613/4
+f 3632/4 3630/4 3631/4
+f 3633/4 3630/4 3632/4
+f 3634/4 3631/4 3613/4
+f 3633/4 3632/4 3635/4
+f 3633/4 3635/4 3636/4
+f 3637/4 3634/4 3613/4
+f 3633/4 3636/4 3638/4
+f 3639/4 3637/4 3613/4
+f 3633/4 3638/4 3640/4
+f 3641/4 3633/4 3640/4
+f 3641/4 3640/4 3642/4
+f 3643/4 3639/4 3613/4
+f 3641/4 3642/4 3644/4
+f 3645/4 3613/4 3612/4
+f 3646/4 3613/4 3645/4
+f 3647/4 3613/4 3646/4
+f 3643/4 3613/4 3647/4
+f 3641/4 3644/4 3648/4
+f 3641/4 3648/4 3649/4
+f 3650/4 3641/4 3649/4
+f 3650/4 3649/4 3651/4
+f 3650/4 3651/4 3652/4
+f 3653/4 3650/4 3652/4
+f 3653/4 3652/4 3654/4
+f 3655/4 3653/4 3654/4
+f 3655/4 3654/4 3656/4
+f 3655/4 3656/4 3657/4
+f 3658/4 3655/4 3657/4
+f 3658/4 3657/4 3659/4
+f 3660/4 3658/4 3659/4
+f 3660/4 3659/4 3661/4
+f 3662/4 3660/4 3661/4
+f 3662/4 3661/4 3663/4
+f 3662/4 3663/4 3664/4
+f 3665/4 3662/4 3664/4
+f 3665/4 3664/4 3666/4
+f 3667/4 3665/4 3666/4
+f 3667/4 3666/4 3668/4
+f 3667/4 3668/4 3669/4
+f 3670/4 3667/4 3669/4
+f 3670/4 3669/4 3671/4
+f 3672/4 3670/4 3671/4
+f 3672/4 3671/4 3673/4
+f 3672/4 3673/4 3674/4
+f 3675/4 3672/4 3674/4
+f 3675/4 3674/4 3676/4
+f 3677/4 3675/4 3676/4
+f 3677/4 3676/4 3678/4
+f 3677/4 3678/4 3679/4
+f 3680/4 3677/4 3679/4
+f 3680/4 3679/4 3681/4
+f 3682/4 3680/4 3681/4
+f 3682/4 3681/4 3683/4
+f 3682/4 3683/4 3684/4
+f 3682/4 3684/4 3685/4
+f 3686/4 3682/4 3685/4
+f 3686/4 3685/4 3687/4
+f 3686/4 3687/4 3688/4
+f 3689/4 3686/4 3688/4
+f 3689/4 3688/4 3690/4
+f 3689/4 3690/4 3691/4
+f 3689/4 3691/4 3692/4
+f 3693/4 3689/4 3692/4
+f 3694/4 3693/4 3692/4
+f 3695/4 3694/4 3692/4
+f 3696/4 3695/4 3692/4
+f 3697/4 3696/4 3692/4
+f 3698/4 3699/4 3700/4
+f 3698/4 3700/4 3701/4
+f 3702/4 3698/4 3701/4
+f 3702/4 3701/4 3703/4
+f 3704/4 3702/4 3703/4
+f 3704/4 3703/4 3705/4
+f 3706/4 3704/4 3705/4
+f 3706/4 3705/4 3707/4
+f 3708/4 3706/4 3707/4
+f 3709/4 3708/4 3707/4
+f 3709/4 3707/4 3710/4
+f 3711/4 3709/4 3710/4
+f 3711/4 3710/4 3712/4
+f 3713/4 3711/4 3712/4
+f 3713/4 3712/4 3714/4
+f 3715/4 3713/4 3714/4
+f 3716/4 3715/4 3714/4
+f 3717/4 3714/4 3718/4
+f 3719/4 3714/4 3717/4
+f 3716/4 3714/4 3719/4
+f 3716/4 3719/4 3720/4
+f 3721/4 3717/4 3718/4
+f 3716/4 3720/4 3722/4
+f 3723/4 3721/4 3718/4
+f 3716/4 3722/4 3724/4
+f 3725/4 3716/4 3724/4
+f 3726/4 3723/4 3718/4
+f 3725/4 3724/4 3727/4
+f 3728/4 3726/4 3718/4
+f 3725/4 3727/4 3729/4
+f 3725/4 3729/4 3730/4
+f 3731/4 3728/4 3718/4
+f 3731/4 3718/4 3732/4
+f 3725/4 3730/4 3733/4
+f 3734/4 3731/4 3732/4
+f 3735/4 3725/4 3733/4
+f 3735/4 3733/4 3736/4
+f 3737/4 3734/4 3732/4
+f 3735/4 3736/4 3738/4
+f 3739/4 3737/4 3732/4
+f 3739/4 3732/4 3740/4
+f 3741/4 3735/4 3738/4
+f 3741/4 3738/4 3742/4
+f 3743/4 3739/4 3740/4
+f 3741/4 3742/4 3744/4
+f 3743/4 3740/4 3745/4
+f 3746/4 3741/4 3744/4
+f 3747/4 3743/4 3745/4
+f 3746/4 3744/4 3748/4
+f 3746/4 3748/4 3749/4
+f 3750/4 3747/4 3745/4
+f 3750/4 3745/4 3751/4
+f 3752/4 3746/4 3749/4
+f 3752/4 3749/4 3753/4
+f 3754/4 3750/4 3751/4
+f 3752/4 3753/4 3755/4
+f 3756/4 3752/4 3755/4
+f 3754/4 3751/4 3757/4
+f 3758/4 3754/4 3757/4
+f 3756/4 3755/4 3759/4
+f 3760/4 3756/4 3759/4
+f 3761/4 3757/4 3762/4
+f 3758/4 3757/4 3761/4
+f 3760/4 3759/4 3763/4
+f 3760/4 3763/4 3764/4
+f 3765/4 3760/4 3764/4
+f 3765/4 3764/4 3766/4
+f 3765/4 3766/4 3767/4
+f 3768/4 3765/4 3767/4
+f 3768/4 3767/4 3769/4
+f 3770/4 3768/4 3769/4
+f 3771/4 3772/4 3773/4
+f 3774/4 3771/4 3773/4
+f 3773/4 3770/4 3769/4
+f 3774/4 3773/4 3769/4
+f 3775/4 3776/4 3777/4
+f 3775/4 3777/4 3778/4
+f 3779/4 3775/4 3778/4
+f 3779/4 3778/4 3780/4
+f 3781/4 3782/4 3783/4
+f 3784/4 3781/4 3783/4
+f 3784/4 3783/4 3785/4
+f 3786/4 3784/4 3785/4
+f 3786/4 3785/4 3787/4
+f 3788/4 3786/4 3787/4
+f 3788/4 3787/4 3789/4
+f 3788/4 3789/4 3790/4
+f 3791/4 3788/4 3790/4
+f 3791/4 3790/4 3792/4
+f 3793/4 3791/4 3792/4
+f 3793/4 3792/4 3794/4
+f 3795/4 3793/4 3794/4
+f 3795/4 3794/4 3796/4
+f 3795/4 3796/4 3797/4
+f 3780/4 3795/4 3797/4
+f 3780/4 3797/4 3798/4
+f 3798/4 3797/4 3799/4
+f 3799/4 3797/4 3800/4
+f 3780/4 3798/4 3801/4
+f 3802/4 3799/4 3800/4
+f 3803/4 3802/4 3800/4
+f 3780/4 3801/4 3804/4
+f 3805/4 3803/4 3800/4
+f 3780/4 3804/4 3806/4
+f 3807/4 3805/4 3800/4
+f 3807/4 3800/4 3808/4
+f 3809/4 3807/4 3808/4
+f 3780/4 3806/4 3810/4
+f 3811/4 3809/4 3808/4
+f 3779/4 3780/4 3812/4
+f 3812/4 3780/4 3813/4
+f 3813/4 3780/4 3814/4
+f 3814/4 3780/4 3810/4
+f 3815/4 3811/4 3808/4
+f 3816/4 3815/4 3808/4
+f 3816/4 3808/4 3817/4
+f 3818/4 3816/4 3817/4
+f 3819/4 3818/4 3817/4
+f 3819/4 3817/4 3820/4
+f 3821/4 3819/4 3820/4
+f 3821/4 3820/4 3822/4
+f 3823/4 3821/4 3822/4
+f 3824/4 3823/4 3822/4
+f 3824/4 3822/4 3825/4
+f 3826/4 3824/4 3825/4
+f 3826/4 3825/4 3827/4
+f 3828/4 3826/4 3827/4
+f 3828/4 3827/4 3829/4
+f 3830/4 3828/4 3829/4
+f 3831/4 3830/4 3829/4
+f 3831/4 3829/4 3832/4
+f 3833/4 3831/4 3832/4
+f 3833/4 3832/4 3834/4
+f 3835/4 3833/4 3834/4
+f 3836/4 3835/4 3834/4
+f 3836/4 3834/4 3837/4
+f 3838/4 3836/4 3837/4
+f 3838/4 3837/4 3839/4
+f 3840/4 3838/4 3839/4
+f 3841/4 3840/4 3839/4
+f 3841/4 3839/4 3842/4
+f 3843/4 3841/4 3842/4
+f 3843/4 3842/4 3844/4
+f 3845/4 3843/4 3844/4
+f 3846/4 3845/4 3844/4
+f 3846/4 3844/4 3847/4
+f 3848/4 3846/4 3847/4
+f 3848/4 3847/4 3849/4
+f 3850/4 3848/4 3849/4
+f 3851/4 3850/4 3849/4
+f 3852/4 3851/4 3849/4
+f 3852/4 3849/4 3853/4
+f 3854/4 3852/4 3853/4
+f 3855/4 3854/4 3853/4
+f 3855/4 3853/4 3856/4
+f 3857/4 3855/4 3856/4
+f 3858/4 3857/4 3856/4
+f 3859/4 3858/4 3856/4
+f 3859/4 3856/4 3860/4
+f 3859/4 3860/4 3861/4
+f 3859/4 3861/4 3862/4
+f 3859/4 3862/4 3863/4
+f 3859/4 3863/4 3864/4
+f 3865/4 3866/4 3867/4
+f 3868/4 3865/4 3867/4
+f 3868/4 3867/4 3869/4
+f 3870/4 3868/4 3869/4
+f 3870/4 3869/4 3871/4
+f 3872/4 3870/4 3871/4
+f 3872/4 3871/4 3873/4
+f 3872/4 3873/4 3874/4
+f 3875/4 3872/4 3874/4
+f 3875/4 3874/4 3876/4
+f 3877/4 3875/4 3876/4
+f 3877/4 3876/4 3878/4
+f 3877/4 3878/4 3879/4
+f 3880/4 3877/4 3879/4
+f 3880/4 3879/4 3881/4
+f 3882/4 3880/4 3881/4
+f 3882/4 3881/4 3883/4
+f 3882/4 3883/4 3884/4
+f 3885/4 3882/4 3886/4
+f 3886/4 3882/4 3887/4
+f 3887/4 3882/4 3884/4
+f 3885/4 3886/4 3888/4
+f 3889/4 3887/4 3884/4
+f 3885/4 3888/4 3890/4
+f 3891/4 3889/4 3884/4
+f 3885/4 3890/4 3892/4
+f 3893/4 3891/4 3884/4
+f 3894/4 3885/4 3892/4
+f 3894/4 3892/4 3895/4
+f 3896/4 3893/4 3884/4
+f 3896/4 3884/4 3897/4
+f 3898/4 3896/4 3897/4
+f 3894/4 3895/4 3899/4
+f 3900/4 3898/4 3897/4
+f 3894/4 3899/4 3901/4
+f 3902/4 3900/4 3897/4
+f 3902/4 3897/4 3903/4
+f 3904/4 3894/4 3901/4
+f 3904/4 3901/4 3905/4
+f 3906/4 3902/4 3903/4
+f 3906/4 3903/4 3907/4
+f 3904/4 3905/4 3908/4
+f 3909/4 3904/4 3908/4
+f 3910/4 3906/4 3907/4
+f 3909/4 3908/4 3911/4
+f 3910/4 3907/4 3912/4
+f 3913/4 3910/4 3912/4
+f 3914/4 3909/4 3911/4
+f 3915/4 3913/4 3912/4
+f 3914/4 3911/4 3916/4
+f 3915/4 3912/4 3917/4
+f 3918/4 3915/4 3917/4
+f 3918/4 3917/4 3919/4
+f 3920/4 3914/4 3921/4
+f 3921/4 3914/4 3922/4
+f 3922/4 3914/4 3916/4
+f 3923/4 3918/4 3919/4
+f 3923/4 3919/4 3924/4
+f 3925/4 3923/4 3924/4
+f 3925/4 3924/4 3926/4
+f 3927/4 3925/4 3926/4
+f 3928/4 3927/4 3926/4
+f 3928/4 3926/4 3929/4
+f 3930/4 3928/4 3929/4
+f 3930/4 3929/4 3931/4
+f 3932/4 3930/4 3931/4
+f 3933/4 3932/4 3931/4
+f 3933/4 3931/4 3934/4
+f 3935/4 3933/4 3934/4
+f 3936/4 3937/4 3938/4
+f 3939/4 3936/4 3938/4
+f 3939/4 3938/4 3940/4
+f 3941/4 3939/4 3940/4
+f 3941/4 3940/4 3942/4
+f 3941/4 3942/4 3943/4
+f 3944/4 3941/4 3943/4
+f 3935/4 3934/4 3945/4
+f 3944/4 3943/4 3946/4
+f 3947/4 3944/4 3946/4
+f 3948/4 3935/4 3945/4
+f 3947/4 3946/4 3949/4
+f 3950/4 3947/4 3949/4
+f 3950/4 3949/4 3951/4
+f 3952/4 3948/4 3945/4
+f 3952/4 3945/4 3950/4
+f 3952/4 3950/4 3951/4
+f 3952/4 3951/4 3953/4
+f 3952/4 3953/4 3954/4
+f 3955/4 3952/4 3954/4
+f 3955/4 3954/4 3956/4
+f 3955/4 3956/4 3957/4
+f 3957/4 3956/4 3958/4
+f 3957/4 3958/4 3959/4
+f 3959/4 3958/4 3960/4
+f 3955/4 3957/4 3961/4
+f 3955/4 3961/4 3962/4
+f 3963/4 3959/4 3960/4
+f 3955/4 3962/4 3964/4
+f 3965/4 3963/4 3960/4
+f 3966/4 3955/4 3964/4
+f 3966/4 3964/4 3967/4
+f 3968/4 3965/4 3960/4
+f 3966/4 3967/4 3969/4
+f 3970/4 3968/4 3960/4
+f 3970/4 3960/4 3971/4
+f 3972/4 3970/4 3971/4
+f 3966/4 3969/4 3973/4
+f 3974/4 3972/4 3971/4
+f 3974/4 3971/4 3975/4
+f 3966/4 3973/4 3976/4
+f 3977/4 3966/4 3976/4
+f 3978/4 3974/4 3975/4
+f 3979/4 3978/4 3975/4
+f 3979/4 3975/4 3980/4
+f 3981/4 3979/4 3980/4
+f 3982/4 3981/4 3980/4
+f 3982/4 3980/4 3983/4
+f 3984/4 3982/4 3983/4
+f 3984/4 3983/4 3985/4
+f 3986/4 3984/4 3985/4
+f 3987/4 3986/4 3985/4
+f 3987/4 3985/4 3988/4
+f 3989/4 3987/4 3988/4
+f 3989/4 3988/4 3990/4
+f 3991/4 3989/4 3990/4
+f 3992/4 3991/4 3990/4
+f 3992/4 3990/4 3993/4
+f 3994/4 3992/4 3993/4
+f 3994/4 3993/4 3995/4
+f 3996/4 3994/4 3995/4
+f 3996/4 3995/4 3997/4
+f 3998/4 3996/4 3997/4
+f 3999/4 3998/4 3997/4
+f 3999/4 3997/4 4000/4
+f 4001/4 3999/4 4000/4
+f 4002/4 4001/4 4000/4
+f 4002/4 4000/4 4003/4
+f 4004/4 4002/4 4003/4
+f 4004/4 4003/4 4005/4
+f 4006/4 4004/4 4005/4
+f 4006/4 4005/4 4007/4
+f 4008/4 4006/4 4007/4
+f 4009/4 4008/4 4007/4
+f 4009/4 4007/4 4010/4
+f 4011/4 4009/4 4010/4
+f 4012/4 4011/4 4010/4
+f 4012/4 4010/4 4013/4
+f 4014/4 4012/4 4013/4
+f 4015/4 4014/4 4013/4
+f 4015/4 4013/4 4016/4
+f 4017/4 4015/4 4016/4
+f 4018/4 4017/4 4016/4
+f 4019/4 4018/4 4016/4
+f 4020/4 4019/4 4016/4
+f 4020/4 4016/4 4021/4
+f 4020/4 4021/4 4022/4
+f 4020/4 4022/4 4023/4
+f 4020/4 4023/4 4024/4
+f 4020/4 4024/4 4025/4
+f 4020/4 4025/4 4026/4
+f 4027/4 4028/4 4029/4
+f 4030/4 4027/4 4029/4
+f 4030/4 4029/4 4031/4
+f 4032/4 4030/4 4031/4
+f 4033/4 4034/4 4035/4
+f 4033/4 4035/4 4036/4
+f 4037/4 4033/4 4036/4
+f 4037/4 4036/4 4038/4
+f 4039/4 4037/4 4038/4
+f 4039/4 4038/4 4040/4
+f 4041/4 4039/4 4040/4
+f 4042/4 4041/4 4040/4
+f 4042/4 4040/4 4043/4
+f 4044/4 4042/4 4043/4
+f 4044/4 4043/4 4045/4
+f 4046/4 4044/4 4045/4
+f 4046/4 4045/4 4047/4
+f 4048/4 4046/4 4047/4
+f 4049/4 4048/4 4047/4
+f 4049/4 4047/4 4032/4
+f 4050/4 4049/4 4032/4
+f 4051/4 4049/4 4050/4
+f 4052/4 4049/4 4051/4
+f 4053/4 4050/4 4032/4
+f 4052/4 4051/4 4054/4
+f 4052/4 4054/4 4055/4
+f 4056/4 4053/4 4032/4
+f 4052/4 4055/4 4057/4
+f 4058/4 4056/4 4032/4
+f 4052/4 4057/4 4059/4
+f 4060/4 4052/4 4059/4
+f 4060/4 4059/4 4061/4
+f 4062/4 4058/4 4032/4
+f 4060/4 4061/4 4063/4
+f 4064/4 4032/4 4031/4
+f 4065/4 4032/4 4064/4
+f 4066/4 4032/4 4065/4
+f 4062/4 4032/4 4066/4
+f 4060/4 4063/4 4067/4
+f 4060/4 4067/4 4068/4
+f 4069/4 4060/4 4068/4
+f 4069/4 4068/4 4070/4
+f 4069/4 4070/4 4071/4
+f 4072/4 4069/4 4071/4
+f 4072/4 4071/4 4073/4
+f 4074/4 4072/4 4073/4
+f 4074/4 4073/4 4075/4
+f 4074/4 4075/4 4076/4
+f 4077/4 4074/4 4076/4
+f 4077/4 4076/4 4078/4
+f 4079/4 4077/4 4078/4
+f 4079/4 4078/4 4080/4
+f 4081/4 4079/4 4080/4
+f 4081/4 4080/4 4082/4
+f 4081/4 4082/4 4083/4
+f 4084/4 4081/4 4083/4
+f 4084/4 4083/4 4085/4
+f 4086/4 4084/4 4085/4
+f 4086/4 4085/4 4087/4
+f 4086/4 4087/4 4088/4
+f 4089/4 4086/4 4088/4
+f 4089/4 4088/4 4090/4
+f 4091/4 4089/4 4090/4
+f 4091/4 4090/4 4092/4
+f 4091/4 4092/4 4093/4
+f 4094/4 4091/4 4093/4
+f 4094/4 4093/4 4095/4
+f 4096/4 4094/4 4095/4
+f 4096/4 4095/4 4097/4
+f 4096/4 4097/4 4098/4
+f 4099/4 4096/4 4098/4
+f 4099/4 4098/4 4100/4
+f 4101/4 4099/4 4100/4
+f 4101/4 4100/4 4102/4
+f 4101/4 4102/4 4103/4
+f 4101/4 4103/4 4104/4
+f 4105/4 4101/4 4104/4
+f 4105/4 4104/4 4106/4
+f 4105/4 4106/4 4107/4
+f 4108/4 4105/4 4107/4
+f 4108/4 4107/4 4109/4
+f 4108/4 4109/4 4110/4
+f 4108/4 4110/4 4111/4
+f 4112/4 4108/4 4111/4
+f 4113/4 4112/4 4111/4
+f 4114/4 4113/4 4111/4
+f 4115/4 4114/4 4111/4
+f 4116/4 4115/4 4111/4
+f 4117/4 4118/4 4119/4
+f 4117/4 4119/4 4120/4
+f 4121/4 4117/4 4120/4
+f 4121/4 4120/4 4122/4
+f 4123/4 4121/4 4122/4
+f 4123/4 4122/4 4124/4
+f 4125/4 4123/4 4124/4
+f 4126/4 4125/4 4124/4
+f 4126/4 4124/4 4127/4
+f 4128/4 4126/4 4127/4
+f 4128/4 4127/4 4129/4
+f 4130/4 4128/4 4129/4
+f 4131/4 4130/4 4129/4
+f 4131/4 4129/4 4132/4
+f 4133/4 4131/4 4132/4
+f 4133/4 4132/4 4134/4
+f 4135/4 4133/4 4134/4
+f 4136/4 4135/4 4134/4
+f 4137/4 4134/4 4138/4
+f 4139/4 4134/4 4137/4
+f 4136/4 4134/4 4139/4
+f 4140/4 4137/4 4138/4
+f 4136/4 4139/4 4141/4
+f 4142/4 4140/4 4138/4
+f 4136/4 4141/4 4143/4
+f 4144/4 4142/4 4138/4
+f 4136/4 4143/4 4145/4
+f 4144/4 4138/4 4146/4
+f 4147/4 4144/4 4146/4
+f 4136/4 4145/4 4148/4
+f 4149/4 4136/4 4148/4
+f 4149/4 4148/4 4150/4
+f 4151/4 4147/4 4146/4
+f 4149/4 4150/4 4152/4
+f 4153/4 4151/4 4146/4
+f 4149/4 4152/4 4154/4
+f 4155/4 4149/4 4154/4
+f 4153/4 4146/4 4156/4
+f 4157/4 4153/4 4156/4
+f 4155/4 4154/4 4158/4
+f 4159/4 4155/4 4158/4
+f 4160/4 4157/4 4156/4
+f 4160/4 4156/4 4161/4
+f 4159/4 4158/4 4162/4
+f 4163/4 4160/4 4161/4
+f 4164/4 4159/4 4162/4
+f 4164/4 4162/4 4165/4
+f 4163/4 4161/4 4166/4
+f 4164/4 4165/4 4167/4
+f 4168/4 4163/4 4166/4
+f 4169/4 4164/4 4167/4
+f 4169/4 4167/4 4170/4
+f 4171/4 4169/4 4170/4
+f 4172/4 4166/4 4173/4
+f 4174/4 4166/4 4172/4
+f 4168/4 4166/4 4174/4
+f 4171/4 4170/4 4175/4
+f 4176/4 4171/4 4175/4
+f 4176/4 4175/4 4177/4
+f 4178/4 4176/4 4177/4
+f 4178/4 4177/4 4179/4
+f 4178/4 4179/4 4180/4
+f 4181/4 4178/4 4180/4
+f 4181/4 4180/4 4182/4
+f 4183/4 4181/4 4182/4
+f 4183/4 4182/4 4184/4
+f 4183/4 4184/4 4185/4
+f 4186/4 4183/4 4185/4
+f 4186/4 4185/4 4187/4
+f 4188/4 4189/4 4190/4
+f 4188/4 4190/4 4191/4
+f 4192/4 4188/4 4191/4
+f 4192/4 4191/4 4193/4
+f 4194/4 4192/4 4193/4
+f 4195/4 4194/4 4193/4
+f 4195/4 4193/4 4196/4
+f 4197/4 4186/4 4187/4
+f 4198/4 4195/4 4196/4
+f 4198/4 4196/4 4199/4
+f 4197/4 4187/4 4200/4
+f 4201/4 4198/4 4199/4
+f 4201/4 4199/4 4202/4
+f 4203/4 4201/4 4202/4
+f 4197/4 4200/4 4204/4
+f 4202/4 4197/4 4204/4
+f 4203/4 4202/4 4204/4
+f 4205/4 4203/4 4204/4
+f 4206/4 4205/4 4204/4
+f 4206/4 4204/4 4207/4
+f 4208/4 4206/4 4207/4
+f 4209/4 4208/4 4207/4
+f 4210/4 4208/4 4209/4
+f 4211/4 4210/4 4209/4
+f 4212/4 4210/4 4211/4
+f 4213/4 4209/4 4207/4
+f 4214/4 4213/4 4207/4
+f 4212/4 4211/4 4215/4
+f 4216/4 4214/4 4207/4
+f 4212/4 4215/4 4217/4
+f 4216/4 4207/4 4218/4
+f 4219/4 4216/4 4218/4
+f 4212/4 4217/4 4220/4
+f 4221/4 4219/4 4218/4
+f 4212/4 4220/4 4222/4
+f 4223/4 4212/4 4222/4
+f 4223/4 4222/4 4224/4
+f 4225/4 4221/4 4218/4
+f 4223/4 4224/4 4226/4
+f 4227/4 4223/4 4226/4
+f 4228/4 4225/4 4218/4
+f 4228/4 4218/4 4229/4
+f 4227/4 4226/4 4230/4
+f 4227/4 4230/4 4231/4
+f 4232/4 4227/4 4231/4
+f 4232/4 4231/4 4233/4
+f 4232/4 4233/4 4234/4
+f 4235/4 4232/4 4234/4
+f 4235/4 4234/4 4236/4
+f 4237/4 4235/4 4236/4
+f 4237/4 4236/4 4238/4
+f 4237/4 4238/4 4239/4
+f 4240/4 4237/4 4239/4
+f 4240/4 4239/4 4241/4
+f 4242/4 4240/4 4241/4
+f 4242/4 4241/4 4243/4
+f 4242/4 4243/4 4244/4
+f 4245/4 4242/4 4244/4
+f 4245/4 4244/4 4246/4
+f 4247/4 4245/4 4246/4
+f 4247/4 4246/4 4248/4
+f 4249/4 4247/4 4248/4
+f 4249/4 4248/4 4250/4
+f 4249/4 4250/4 4251/4
+f 4252/4 4249/4 4251/4
+f 4252/4 4251/4 4253/4
+f 4252/4 4253/4 4254/4
+f 4255/4 4252/4 4254/4
+f 4255/4 4254/4 4256/4
+f 4257/4 4255/4 4256/4
+f 4257/4 4256/4 4258/4
+f 4259/4 4257/4 4258/4
+f 4259/4 4258/4 4260/4
+f 4259/4 4260/4 4261/4
+f 4262/4 4259/4 4261/4
+f 4262/4 4261/4 4263/4
+f 4262/4 4263/4 4264/4
+f 4265/4 4262/4 4264/4
+f 4265/4 4264/4 4266/4
+f 4265/4 4266/4 4267/4
+f 4268/4 4265/4 4267/4
+f 4268/4 4267/4 4269/4
+f 4268/4 4269/4 4270/4
+f 4268/4 4270/4 4271/4
+f 4268/4 4271/4 4272/4
+f 4273/4 4268/4 4272/4
+f 4274/4 4273/4 4272/4
+f 4275/4 4274/4 4272/4
+f 4276/4 4275/4 4272/4
+f 4277/4 4276/4 4272/4
+f 4278/4 4277/4 4272/4
diff --git a/Examples/Stapling/Data/Geometry/stapler_trigger.mtl b/Examples/Stapling/Data/Geometry/stapler_trigger.mtl
new file mode 100644
index 0000000..cbcb070
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/stapler_trigger.mtl
@@ -0,0 +1,11 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl _Mat.003
+Ns 45.098039
+Ka 0.000000 0.000000 0.000000
+Kd 0.640000 0.640000 0.640000
+Ks 1.000000 1.000000 1.000000
+Ni 1.000000
+d 1.000000
+illum 2
diff --git a/Examples/Stapling/Data/Geometry/stapler_trigger.obj b/Examples/Stapling/Data/Geometry/stapler_trigger.obj
new file mode 100644
index 0000000..75344d5
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/stapler_trigger.obj
@@ -0,0 +1,357 @@
+# Blender v2.69 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib stapler_trigger.mtl
+o stapler_trigger
+v -0.042081 -0.006419 -0.111377
+v -0.042081 0.006068 -0.111377
+v -0.041799 0.006068 -0.057137
+v -0.041799 -0.006419 -0.057137
+v -0.036635 -0.006419 -0.168180
+v -0.036635 0.006068 -0.168180
+v -0.027803 0.006068 -0.189866
+v -0.027803 -0.006419 -0.189866
+v -0.083131 -0.002459 -0.108502
+v -0.046824 -0.002459 -0.111552
+v -0.046573 -0.002459 -0.063532
+v -0.067945 -0.002459 -0.069992
+v -0.067945 0.002108 -0.069992
+v -0.046573 0.002108 -0.063532
+v -0.046824 0.002108 -0.111552
+v -0.083131 0.002108 -0.108502
+v -0.071514 -0.006419 -0.066118
+v -0.071514 0.006068 -0.066118
+v -0.087436 -0.002459 -0.168619
+v -0.041295 -0.002459 -0.169246
+v -0.041295 0.002108 -0.169246
+v -0.087436 0.002108 -0.168619
+v -0.082674 0.006068 -0.190857
+v -0.072338 0.006068 -0.194373
+v -0.072338 -0.006419 -0.194373
+v -0.082674 -0.006419 -0.190857
+v -0.086930 0.002108 -0.169685
+v -0.071873 0.002108 -0.189509
+v -0.079279 0.002108 -0.186832
+v -0.079279 -0.002459 -0.186832
+v -0.071873 -0.002459 -0.189509
+v -0.086930 -0.002459 -0.169685
+v -0.060506 0.002108 -0.191317
+v -0.060506 -0.002459 -0.191317
+v -0.041190 -0.002459 -0.169479
+v -0.039868 0.006068 -0.194198
+v -0.039868 -0.006419 -0.194198
+v -0.041158 0.002108 -0.169643
+v -0.034244 0.002108 -0.186620
+v -0.040456 0.002108 -0.189294
+v -0.050472 0.002108 -0.191135
+v -0.040377 -0.002459 -0.189270
+v -0.034244 -0.002459 -0.186620
+v -0.040961 -0.002459 -0.170128
+v -0.061414 0.006068 -0.196034
+v -0.050242 0.006068 -0.195907
+v -0.050242 -0.006419 -0.195907
+v -0.061414 -0.006419 -0.196034
+v -0.087515 -0.006725 -0.107551
+v -0.083745 -0.006725 -0.108407
+v -0.068407 -0.006725 -0.069492
+v -0.071284 -0.006725 -0.066369
+v -0.042108 -0.006725 -0.057550
+v -0.045956 -0.006725 -0.062706
+v -0.046210 -0.006725 -0.111564
+v -0.042387 -0.006725 -0.111391
+v -0.091934 -0.006725 -0.169353
+v -0.088053 -0.006725 -0.168670
+v -0.040687 -0.006725 -0.169176
+v -0.036936 -0.006725 -0.168254
+v -0.082459 -0.006725 -0.190601
+v -0.079719 -0.006725 -0.187332
+v -0.072289 -0.006725 -0.194068
+v -0.071666 -0.006725 -0.190196
+v -0.050268 -0.006725 -0.195601
+v -0.050592 -0.006725 -0.191765
+v -0.060760 -0.006725 -0.191912
+v -0.061365 -0.006725 -0.195729
+v -0.028199 -0.006725 -0.189664
+v -0.033461 -0.006725 -0.186980
+v -0.040252 -0.006725 -0.189872
+v -0.039896 -0.006725 -0.193888
+v -0.071284 0.006374 -0.066369
+v -0.068407 0.006374 -0.069492
+v -0.083745 0.006374 -0.108407
+v -0.087515 0.006374 -0.107551
+v -0.042108 0.006374 -0.057550
+v -0.045956 0.006374 -0.062706
+v -0.042387 0.006374 -0.111391
+v -0.046210 0.006374 -0.111564
+v -0.088053 0.006374 -0.168670
+v -0.091934 0.006374 -0.169353
+v -0.036936 0.006374 -0.168254
+v -0.040687 0.006374 -0.169176
+v -0.079719 0.006374 -0.187332
+v -0.082459 0.006374 -0.190601
+v -0.071666 0.006374 -0.190196
+v -0.072289 0.006374 -0.194068
+v -0.061365 0.006374 -0.195729
+v -0.060760 0.006374 -0.191912
+v -0.050592 0.006374 -0.191765
+v -0.050268 0.006374 -0.195601
+v -0.028199 0.006374 -0.189664
+v -0.033461 0.006374 -0.186980
+v -0.039896 0.006374 -0.193888
+v -0.040252 0.006374 -0.189872
+v -0.083443 -0.006419 -0.108476
+v -0.083443 -0.002765 -0.108476
+v -0.068176 -0.002765 -0.069742
+v -0.068176 -0.006419 -0.069742
+v -0.046265 -0.006419 -0.063119
+v -0.046265 -0.002765 -0.063119
+v -0.046516 -0.002765 -0.111578
+v -0.046516 -0.006419 -0.111578
+v -0.087743 -0.006419 -0.168615
+v -0.087743 -0.002765 -0.168615
+v -0.040987 -0.002765 -0.169250
+v -0.040987 -0.006419 -0.169250
+v -0.079505 -0.006419 -0.187076
+v -0.079505 -0.002765 -0.187076
+v -0.071613 -0.006419 -0.189893
+v -0.071573 -0.002765 -0.189907
+v -0.050618 -0.006419 -0.191459
+v -0.050612 -0.002765 -0.191459
+v -0.060760 -0.002765 -0.191608
+v -0.060715 -0.006419 -0.191607
+v -0.033847 -0.006419 -0.186783
+v -0.033847 -0.002765 -0.186783
+v -0.040361 -0.002765 -0.189587
+v -0.040286 -0.006419 -0.189560
+v -0.068176 0.006068 -0.069742
+v -0.068176 0.002415 -0.069742
+v -0.083443 0.002415 -0.108476
+v -0.083443 0.006068 -0.108476
+v -0.046265 0.006068 -0.063119
+v -0.046265 0.002415 -0.063119
+v -0.046516 0.006068 -0.111578
+v -0.046516 0.002415 -0.111578
+v -0.087743 0.002415 -0.168615
+v -0.087743 0.006068 -0.168615
+v -0.040987 0.006068 -0.169250
+v -0.040987 0.002415 -0.169250
+v -0.079505 0.002414 -0.187076
+v -0.079505 0.006068 -0.187076
+v -0.071573 0.002414 -0.189907
+v -0.071613 0.006068 -0.189893
+v -0.060715 0.006068 -0.191607
+v -0.060760 0.002414 -0.191608
+v -0.050612 0.002414 -0.191459
+v -0.050618 0.006068 -0.191459
+v -0.033847 0.006068 -0.186783
+v -0.033847 0.002414 -0.186783
+v -0.040286 0.006068 -0.189560
+v -0.040361 0.002414 -0.189587
+v -0.092245 -0.006419 -0.169407
+v -0.092245 0.006068 -0.169407
+v -0.087817 -0.006419 -0.107482
+v -0.087817 0.006068 -0.107482
+v -0.041190 0.002108 -0.169479
+v -0.050642 -0.002459 -0.191153
+vt 0.000000 0.000000
+vt 1.000000 0.000000
+vt 1.000000 1.000000
+vt 0.000000 1.000000
+vt 0.000000 0.866987
+vt 1.000000 0.866987
+vt 0.000000 0.039759
+vt 0.000000 0.038214
+vt 0.000000 0.890896
+vt 0.000000 0.892077
+vt 1.000000 0.892077
+vt 1.000000 0.890896
+vt 1.000000 0.038214
+vt 1.000000 0.039759
+vt 0.000000 0.044141
+vt 0.000000 0.027875
+vt 0.000000 0.958666
+vt 0.000000 0.956815
+vt 1.000000 0.956815
+vt 1.000000 0.958666
+vt 1.000000 0.027875
+vt 1.000000 0.044141
+vt 0.000000 0.061429
+vt 0.000000 0.448069
+vt 0.000000 0.649313
+vt 1.000000 0.649313
+vt 1.000000 0.742011
+vt 1.000000 0.061429
+vt 0.000000 0.050673
+vt 0.000000 0.064990
+vt 0.000000 0.764868
+vt 0.000000 0.726115
+vt 1.000000 0.032495
+vt 1.000000 0.025337
+vt 0.000000 0.062837
+vt 0.000000 0.732803
+vt 0.000000 0.945155
+vt 1.000000 0.041968
+vt 1.000000 0.549985
+vt 1.000000 0.029180
+vt 1.000000 0.834708
+vt 1.000000 0.713809
+vt 1.000000 0.070436
+vt 0.000000 0.834708
+vt 1.000000 0.816038
+vt 0.834403 0.000000
+vt 0.338579 0.009244
+vt 0.000000 0.414929
+usemtl _Mat.003
+s 1
+f 1/1 2/2 3/3 4/4
+f 2/3 1/4 5/1 6/2
+f 7/5 6/1 5/2 8/6
+f 9/7 10/8 11/9 12/10
+f 13/11 14/12 15/13 16/14
+f 17/4 4/4 3/3 18/3
+f 19/15 20/16 10/17 9/18
+f 16/19 15/20 21/21 22/22
+f 23/5 24/5 25/6 26/6
+f 27/23 28/24 29/25
+f 30/26 31/27 32/28
+f 27/29 21/30 33/31 28/32
+f 34/33 35/34 20/33
+f 36/5 7/5 8/6 37/6
+f 38/35 39/36 40/36 41/37
+f 42/38 43/39 44/40
+f 45/1 46/2 47/3 48/4
+f 49/1 50/7 51/10 52/4
+f 53/4 54/9 55/8 56/1
+f 52/4 51/10 54/9 53/4
+f 57/1 58/15 50/18 49/4
+f 56/4 55/17 59/16 60/1
+f 61/6 62/26 58/28 57/2
+f 63/6 64/41 62/26 61/6
+f 65/6 66/42 67/43 68/2
+f 69/6 70/39 71/38 72/2
+f 60/2 59/40 70/39 69/6
+f 73/3 74/11 75/14 76/2
+f 77/3 78/12 74/11 73/3
+f 79/2 80/13 78/12 77/3
+f 76/3 75/19 81/22 82/2
+f 83/2 84/21 80/20 79/3
+f 82/1 81/23 85/25 86/5
+f 86/5 85/25 87/44 88/5
+f 89/5 90/32 91/31 92/5
+f 93/5 94/36 84/35 83/1
+f 95/4 96/37 94/36 93/5
+f 97/7 98/7 99/10 100/10
+f 101/9 102/9 103/8 104/8
+f 100/10 99/10 102/9 101/9
+f 105/15 106/15 98/18 97/18
+f 104/17 103/17 107/16 108/16
+f 109/26 110/26 106/28 105/28
+f 111/41 112/41 110/26 109/26
+f 113/42 114/42 115/43 116/43
+f 117/39 118/39 119/38 120/38
+f 108/40 107/40 118/39 117/39
+f 121/11 122/11 123/14 124/14
+f 125/12 126/12 122/11 121/11
+f 127/13 128/13 126/12 125/12
+f 124/19 123/19 129/22 130/22
+f 131/21 132/21 128/20 127/20
+f 130/23 129/23 133/25 134/25
+f 134/25 133/25 135/44 136/44
+f 137/32 138/32 139/31 140/31
+f 141/36 142/36 132/35 131/35
+f 143/37 144/37 142/36 141/36
+f 23/45 26/45 145/46 146/46
+f 145/46 147/47 148/47 146/46
+f 147/47 17/4 18/4 148/47
+f 149/30 38/30 41/48 33/4
+f 150/42 44/42 35/33 34/4
+f 24/1 45/2 48/3 25/4
+f 87/1 90/2 89/3 88/4
+f 63/1 68/2 67/3 64/4
+f 111/1 116/2 115/3 112/4
+f 135/1 138/2 137/3 136/4
+f 139/1 144/2 143/3 140/4
+f 113/1 120/2 119/3 114/4
+f 37/1 47/2 46/3 36/4
+f 91/1 96/2 95/3 92/4
+f 65/1 72/2 71/3 66/4
+f 21/2 149/3 33/4
+f 27/2 22/3 21/4
+f 32/1 31/2 34/3 20/4
+f 19/2 32/3 20/4
+f 150/2 42/3 44/4
+f 26/1 61/2 57/3 145/4
+f 61/1 26/2 25/3 63/4
+f 86/1 23/2 146/3 82/4
+f 23/1 86/2 88/3 24/4
+f 148/1 76/2 82/3 146/4
+f 18/1 73/2 76/3 148/4
+f 52/1 17/2 147/3 49/4
+f 53/1 4/2 17/3 52/4
+f 3/1 77/2 73/3 18/4
+f 145/1 57/2 49/3 147/4
+f 5/1 60/2 69/3 8/4
+f 8/1 69/2 72/3 37/4
+f 83/1 6/2 7/3 93/4
+f 93/1 7/2 36/3 95/4
+f 79/1 2/2 6/3 83/4
+f 77/1 3/2 2/3 79/4
+f 56/3 1/4 4/1 53/2
+f 1/3 56/4 60/1 5/2
+f 48/1 68/2 63/3 25/4
+f 65/1 47/2 37/3 72/4
+f 95/1 36/2 46/3 92/4
+f 89/1 45/2 24/3 88/4
+f 105/1 58/2 62/3 109/4
+f 109/1 62/2 64/3 111/4
+f 100/1 51/2 50/3 97/4
+f 101/1 54/2 51/3 100/4
+f 97/1 50/2 58/3 105/4
+f 117/1 70/2 59/3 108/4
+f 120/1 71/2 70/3 117/4
+f 104/1 55/2 54/3 101/4
+f 108/1 59/2 55/3 104/4
+f 141/1 94/2 96/3 143/4
+f 91/1 140/2 143/3 96/4
+f 133/1 29/2 28/3 135/4
+f 33/1 138/2 135/3 28/4
+f 81/1 130/2 134/3 85/4
+f 136/1 87/2 85/3 134/4
+f 75/1 124/2 130/3 81/4
+f 74/1 121/2 124/3 75/4
+f 78/1 125/2 121/3 74/4
+f 131/1 84/2 94/3 141/4
+f 127/1 80/2 84/3 131/4
+f 125/1 78/2 80/3 127/4
+f 140/1 91/2 90/3 137/4
+f 144/1 40/2 39/3 142/4
+f 40/1 144/2 139/3 41/4
+f 110/1 30/2 32/3 106/4
+f 30/1 110/2 112/3 31/4
+f 98/1 9/2 12/3 99/4
+f 11/1 102/2 99/3 12/4
+f 9/1 98/2 106/3 19/4
+f 107/1 44/2 43/3 118/4
+f 42/1 119/2 118/3 43/4
+f 10/1 103/2 102/3 11/4
+f 103/1 10/2 20/3 107/4
+f 119/1 42/2 150/3 114/4
+f 114/1 150/2 34/3 115/4
+f 129/1 27/2 29/3 133/4
+f 123/1 16/2 22/3 129/4
+f 122/1 13/2 16/3 123/4
+f 126/1 14/2 13/3 122/4
+f 142/1 39/2 38/3 132/4
+f 15/1 128/2 132/3 21/4
+f 14/1 126/2 128/3 15/4
+f 115/1 34/2 31/3 112/4
+f 138/1 33/2 41/3 139/4
+f 68/1 48/2 47/3 65/4
+f 92/1 46/2 45/3 89/4
+f 67/1 116/2 111/3 64/4
+f 66/1 113/2 116/3 67/4
+f 137/1 90/2 87/3 136/4
+f 113/1 66/2 71/3 120/4
+f 106/2 32/3 19/4
+f 20/1 35/2 44/3 107/4
+f 129/2 22/3 27/4
+f 21/1 132/2 38/3 149/4
diff --git a/Examples/Stapling/Data/Geometry/upperarm.osgb b/Examples/Stapling/Data/Geometry/upperarm.osgb
new file mode 100644
index 0000000..06b9f88
Binary files /dev/null and b/Examples/Stapling/Data/Geometry/upperarm.osgb differ
diff --git a/Examples/Stapling/Data/Geometry/upperarm.png b/Examples/Stapling/Data/Geometry/upperarm.png
new file mode 100644
index 0000000..03d16c6
Binary files /dev/null and b/Examples/Stapling/Data/Geometry/upperarm.png differ
diff --git a/Examples/Stapling/Data/Geometry/upperarm_normal.png b/Examples/Stapling/Data/Geometry/upperarm_normal.png
new file mode 100644
index 0000000..7a549cf
Binary files /dev/null and b/Examples/Stapling/Data/Geometry/upperarm_normal.png differ
diff --git a/Examples/Stapling/Data/Geometry/virtual_staple_1.ply b/Examples/Stapling/Data/Geometry/virtual_staple_1.ply
new file mode 100644
index 0000000..a480e82
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/virtual_staple_1.ply
@@ -0,0 +1,28 @@
+ply
+format ascii 1.0
+comment This file is a part of the OpenSurgSim project.
+comment Copyright 2013, SimQuest Solutions Inc.
+comment
+comment Licensed under the Apache License, Version 2.0 (the "License");
+comment you may not use this file except in compliance with the License.
+comment You may obtain a copy of the License at
+comment
+comment http://www.apache.org/licenses/LICENSE-2.0
+comment
+comment Unless required by applicable law or agreed to in writing, software
+comment distributed under the License is distributed on an "AS IS" BASIS,
+comment WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+comment See the License for the specific language governing permissions and
+comment limitations under the License.
+comment
+element vertex 3
+property double x
+property double y
+property double z
+element face 1
+property list uint uint vertex_indices
+end_header
+0.00005 0.000855 0.00531
+0.00005 -0.000855 0.00531
+0.003871 0.0 0.00533
+3 0 1 2
diff --git a/Examples/Stapling/Data/Geometry/virtual_staple_2.ply b/Examples/Stapling/Data/Geometry/virtual_staple_2.ply
new file mode 100644
index 0000000..33c378a
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/virtual_staple_2.ply
@@ -0,0 +1,28 @@
+ply
+format ascii 1.0
+comment This file is a part of the OpenSurgSim project.
+comment Copyright 2013, SimQuest Solutions Inc.
+comment
+comment Licensed under the Apache License, Version 2.0 (the "License");
+comment you may not use this file except in compliance with the License.
+comment You may obtain a copy of the License at
+comment
+comment http://www.apache.org/licenses/LICENSE-2.0
+comment
+comment Unless required by applicable law or agreed to in writing, software
+comment distributed under the License is distributed on an "AS IS" BASIS,
+comment WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+comment See the License for the specific language governing permissions and
+comment limitations under the License.
+comment
+element vertex 3
+property double x
+property double y
+property double z
+element face 1
+property list uint uint vertex_indices
+end_header
+0.00005 0.000855 -0.00531
+0.00005 -0.000855 -0.00531
+0.003871 0.0 -0.00533
+3 0 1 2
diff --git a/Examples/Stapling/Data/Geometry/wound.png b/Examples/Stapling/Data/Geometry/wound.png
new file mode 100644
index 0000000..75922e4
Binary files /dev/null and b/Examples/Stapling/Data/Geometry/wound.png differ
diff --git a/Examples/Stapling/Data/Geometry/wound_deformable.ply b/Examples/Stapling/Data/Geometry/wound_deformable.ply
new file mode 100644
index 0000000..e71b564
--- /dev/null
+++ b/Examples/Stapling/Data/Geometry/wound_deformable.ply
@@ -0,0 +1,2391 @@
+ply
+format ascii 1.0
+comment This file is a part of the OpenSurgSim project.
+comment Copyright 2013, SimQuest Solutions Inc.
+comment
+comment Licensed under the Apache License, Version 2.0 (the "License");
+comment you may not use this file except in compliance with the License.
+comment You may obtain a copy of the License at
+comment
+comment http://www.apache.org/licenses/LICENSE-2.0
+comment
+comment Unless required by applicable law or agreed to in writing, software
+comment distributed under the License is distributed on an "AS IS" BASIS,
+comment WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+comment See the License for the specific language governing permissions and
+comment limitations under the License.
+comment
+comment This file provides the geometry which describes a tetrahedral volume
+comment mesh and triangular surface mesh of an arm wound.
+comment
+element vertex 391
+property double x
+property double y
+property double z
+property double s
+property double t
+element face 407
+property list uint uint vertex_indices
+element 3d_element 1476
+property list uint uint vertex_indices
+element boundary_condition 79
+property uint vertex_index
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+-0.017638 0.001427 -0.012363 0.707716 0.681378
+-0.018984 0.002069 -0.016867 0.736310 0.634209
+-0.014913 -0.001358 -0.015090 0.680908 0.590891
+-0.018090 -0.001832 -0.017359 0.707848 0.596931
+0.012643 -0.002455 0.001226 0.403742 0.411127
+0.009376 -0.006324 0.003409 0.0 0.0
+0.015479 -0.002289 0.003265 0.375952 0.394412
+0.014119 -0.002367 0.007046 0.0 0.0
+-0.000236 -0.006438 -0.015323 0.0 0.0
+-0.003732 -0.004010 -0.013609 0.0 0.0
+0.002155 -0.000546 -0.013612 0.0 0.0
+0.001262 -0.001523 -0.018049 0.0 0.0
+0.019667 -0.017785 0.008674 0.0 0.0
+0.029299 -0.005262 0.004546 0.0 0.0
+0.046990 -0.020879 0.007375 0.0 0.0
+0.037332 -0.020768 -0.005328 0.0 0.0
+0.033586 -0.016523 0.018450 0.0 0.0
+0.034101 -0.002031 0.016309 0.0 0.0
+0.033392 -0.002474 0.009623 0.0 0.0
+0.025349 -0.003774 0.014192 0.0 0.0
+0.019145 0.004850 0.012264 0.307965 0.463751
+0.020216 0.004879 0.011113 0.302951 0.437069
+0.020968 0.001399 0.010219 0.315752 0.400181
+0.018687 0.001314 0.008476 0.333716 0.411611
+-0.007352 0.001018 0.005860 0.538316 0.697416
+-0.007333 -0.005722 0.014291 0.0 0.0
+-0.013652 -0.000173 0.002866 0.599904 0.744518
+-0.007113 -0.008223 -0.002577 0.0 0.0
+-0.006477 -0.004144 -0.015932 0.0 0.0
+-0.010957 -0.004967 -0.018490 0.0 0.0
+-0.007629 -0.006377 -0.018801 0.0 0.0
+-0.009906 -0.007928 -0.014976 0.0 0.0
+0.014105 -0.020914 -0.009294 0.0 0.0
+0.009112 -0.006286 -0.008604 0.0 0.0
+0.006268 -0.009189 -0.014543 0.0 0.0
+0.002300 -0.008261 -0.009596 0.0 0.0
+0.006635 0.000026 -0.006193 0.478446 0.422446
+0.010115 -0.000309 -0.007852 0.0 0.0
+0.005102 -0.004418 -0.008996 0.0 0.0
+-0.006609 0.001420 -0.025080 0.683895 0.418703
+-0.011057 -0.007486 -0.026293 0.0 0.0
+-0.008811 0.000184 -0.034044 0.750872 0.396926
+-0.005338 -0.005470 -0.024596 0.0 0.0
+0.001259 0.002603 0.004418 0.477874 0.590676
+0.000123 0.002377 -0.000362 0.508779 0.579936
+0.002998 0.002713 0.001375 0.478745 0.561899
+0.002769 -0.002124 0.003090 0.0 0.0
+-0.020005 -0.002711 -0.020006 0.725914 0.579430
+-0.021803 0.001971 -0.019803 0.769228 0.624707
+-0.021059 0.001058 -0.014582 0.742789 0.686283
+0.014631 0.000524 0.027783 0.0 0.0
+0.022184 -0.002620 0.021520 0.0 0.0
+0.018739 -0.011699 0.030717 0.0 0.0
+0.015337 -0.003630 0.015467 0.0 0.0
+0.016049 0.003671 -0.002811 0.385336 0.303910
+0.018898 0.003898 -0.000351 0.355721 0.293064
+0.014287 0.000683 -0.000350 0.398533 0.377274
+0.019319 -0.000205 -0.003032 0.0 0.0
+0.046990 -0.007619 0.007375 0.0 0.0
+0.040075 -0.002131 -0.002012 0.275562 0.097586
+0.037332 -0.007509 -0.005328 0.0 0.0
+-0.011140 0.001039 -0.002776 0.605865 0.671648
+-0.014033 -0.006140 -0.001762 0.0 0.0
+-0.016684 0.000633 -0.005670 0.664721 0.711067
+-0.011809 -0.002190 -0.009170 0.0 0.0
+0.024204 0.005929 0.015670 0.261423 0.435611
+0.026606 0.006410 0.017058 0.241027 0.413001
+0.023576 0.006924 0.019679 0.249385 0.456824
+0.024566 0.001831 0.017759 0.0 0.0
+0.013743 0.004124 0.008547 0.363455 0.496876
+0.011935 0.004109 0.005502 0.386672 0.485966
+0.014670 0.004333 0.007344 0.356430 0.470987
+0.012704 0.000753 0.008033 0.0 0.0
+-0.003034 -0.009956 -0.020825 0.0 0.0
+-0.006038 -0.007999 -0.012282 0.0 0.0
+0.014736 0.003042 -0.006304 0.410541 0.288505
+0.014286 -0.003192 -0.009813 0.0 0.0
+0.014065 0.002151 -0.010793 0.441123 0.277848
+0.019818 -0.002939 -0.008113 0.0 0.0
+0.036755 0.006575 0.019271 0.171882 0.285622
+0.036907 0.001084 0.018946 0.0 0.0
+0.033263 0.001097 0.023070 0.0 0.0
+0.033376 0.002597 0.017918 0.0 0.0
+0.022543 0.001069 0.006601 0.319550 0.337176
+0.020245 -0.001947 0.007184 0.330922 0.370668
+0.019727 0.001041 0.004292 0.343634 0.348013
+0.023642 -0.002413 0.005541 0.0 0.0
+0.029219 0.001517 -0.003210 0.316280 0.186932
+0.022506 -0.007309 -0.002980 0.0 0.0
+0.013584 0.001132 0.004599 0.380549 0.438335
+0.015737 0.001218 0.006218 0.360614 0.429612
+0.015374 -0.005687 -0.005108 0.0 0.0
+0.019027 -0.006775 -0.014839 0.0 0.0
+0.030475 0.001257 0.015552 0.262159 0.346781
+0.027297 -0.001544 0.019319 0.0 0.0
+-0.006877 -0.002740 -0.012407 0.607383 0.519549
+-0.009683 -0.005474 -0.007552 0.0 0.0
+-0.008963 -0.000571 -0.011342 0.619375 0.564274
+-0.009123 -0.005772 -0.011405 0.0 0.0
+0.006753 0.004254 0.022359 0.380109 0.683747
+0.010408 0.004267 0.016270 0.359520 0.575078
+0.015903 0.005679 0.020105 0.304442 0.542677
+0.017492 0.000471 0.018668 0.0 0.0
+-0.018916 -0.006283 -0.013171 0.0 0.0
+-0.013552 -0.004660 -0.011679 0.0 0.0
+-0.015191 -0.008193 -0.008642 0.0 0.0
+-0.014454 -0.008852 -0.015804 0.0 0.0
+0.016241 0.004334 0.010184 0.336917 0.483939
+0.017210 0.004543 0.009049 0.332410 0.456022
+0.012207 0.000507 -0.002006 0.420299 0.390013
+0.015357 0.000245 -0.003791 0.0 0.0
+0.007154 -0.001854 0.005752 0.0 0.0
+0.011373 -0.002515 0.009976 0.0 0.0
+0.009600 -0.002702 0.003200 0.0 0.0
+0.022251 -0.002063 0.015960 0.0 0.0
+0.021468 -0.001789 0.012242 0.0 0.0
+0.017030 0.000365 0.013057 0.0 0.0
+0.020308 0.001792 0.013974 0.0 0.0
+0.013639 -0.003166 -0.002562 0.0 0.0
+0.011924 -0.003711 -0.006304 0.0 0.0
+-0.014146 -0.020429 -0.047689 0.0 0.0
+-0.014146 -0.007170 -0.047689 0.0 0.0
+-0.024140 -0.017967 -0.040188 0.0 0.0
+-0.012259 -0.006676 -0.033836 0.0 0.0
+0.002537 -0.008403 -0.037874 0.0 0.0
+-0.001499 -0.000057 -0.029561 0.672858 0.347281
+-0.000610 -0.002936 -0.039936 0.735672 0.277619
+0.031048 0.003384 0.005728 0.258962 0.227713
+0.031021 0.004616 0.009841 0.239710 0.251763
+0.029058 -0.002228 0.009819 0.0 0.0
+-0.017817 -0.005609 -0.020805 0.0 0.0
+-0.021077 -0.009144 -0.020053 0.0 0.0
+-0.019782 -0.005521 -0.017072 0.0 0.0
+-0.021431 -0.005952 -0.022335 0.0 0.0
+0.028581 0.004545 0.037841 0.0 0.0
+0.022848 0.002598 0.033651 0.0 0.0
+0.026518 0.008415 0.028378 0.199337 0.482877
+-0.008942 -0.024739 -0.008176 0.0 0.0
+-0.020167 -0.001455 -0.000035 0.663670 0.794459
+-0.019937 -0.006669 -0.033633 0.0 0.0
+-0.011533 -0.022757 -0.029013 0.0 0.0
+0.025681 0.001454 0.013298 0.276295 0.369130
+0.021352 0.005330 0.013823 0.286985 0.451165
+0.022555 0.005159 0.012575 0.281559 0.420259
+-0.018697 -0.003565 -0.023866 0.0 0.0
+-0.003741 -0.007976 -0.007476 0.0 0.0
+-0.004960 -0.004029 -0.006795 0.0 0.0
+0.035352 0.005531 0.016102 0.190755 0.270198
+0.033038 0.004995 0.012719 0.216780 0.264971
+0.036201 0.003833 0.011247 0.209145 0.220919
+-0.014781 0.002213 -0.020373 0.719005 0.520347
+-0.016880 -0.002824 -0.021921 0.0 0.0
+-0.016398 0.001977 -0.024375 0.754476 0.519638
+-0.013707 -0.002037 -0.021513 0.0 0.0
+-0.001967 0.002359 -0.017593 0.600145 0.399436
+-0.005030 0.002252 -0.019030 0.632366 0.420192
+-0.000406 0.001476 -0.021158 0.610992 0.370987
+-0.003015 -0.003057 -0.018944 0.0 0.0
+0.005527 0.000425 -0.000868 0.462746 0.487855
+0.005663 0.003045 0.003152 0.449655 0.544815
+0.003921 0.003399 0.000160 0.473347 0.534593
+-0.015527 -0.005152 -0.022539 0.0 0.0
+-0.015560 -0.006666 -0.026900 0.0 0.0
+-0.014996 -0.002896 -0.024296 0.0 0.0
+-0.013917 -0.007956 -0.024021 0.0 0.0
+0.004887 -0.021029 0.001820 0.0 0.0
+0.002057 -0.006870 -0.003212 0.0 0.0
+0.022552 -0.001675 0.009108 0.314427 0.363307
+-0.024489 0.000741 -0.016918 0.777588 0.684517
+-0.024391 0.001936 -0.022406 0.801319 0.612368
+-0.024601 -0.004739 -0.018778 0.0 0.0
+0.006850 -0.006797 -0.004277 0.0 0.0
+0.004032 -0.004089 -0.000551 0.0 0.0
+0.012333 0.004106 0.011509 0.361086 0.521100
+0.005130 0.003275 0.012820 0.411354 0.608085
+0.007317 0.003436 0.008224 0.411549 0.550627
+-0.004058 0.001878 0.001264 0.531496 0.626286
+-0.014203 0.001575 -0.029189 0.762778 0.468814
+-0.010570 -0.002455 -0.021860 0.0 0.0
+-0.012199 0.002106 -0.022621 0.713800 0.485157
+-0.025827 -0.009187 -0.021112 0.0 0.0
+-0.021601 -0.004637 -0.026044 0.0 0.0
+-0.017290 -0.008529 -0.023047 0.0 0.0
+-0.013967 -0.002130 -0.018341 0.682695 0.540233
+-0.014398 -0.004396 -0.019489 0.0 0.0
+-0.016250 -0.002623 -0.019238 0.704893 0.552676
+0.002921 -0.006003 -0.021793 0.0 0.0
+0.009600 0.002834 -0.010048 0.466575 0.318768
+0.012195 0.002939 -0.008156 0.437269 0.303618
+0.014492 -0.021349 -0.029300 0.0 0.0
+0.026117 -0.020765 -0.017965 0.0 0.0
+0.014492 -0.008090 -0.029300 0.0 0.0
+0.002621 -0.007830 -0.029082 0.0 0.0
+0.008423 0.000639 0.001153 0.432663 0.470788
+0.030678 0.007333 0.019744 0.205235 0.378927
+0.026656 0.007758 0.022058 0.221464 0.439517
+0.020949 0.007122 0.023900 0.253990 0.513600
+0.024451 0.002704 0.023354 0.0 0.0
+-0.023157 -0.024020 -0.019768 0.0 0.0
+0.029177 0.003608 0.018712 0.0 0.0
+0.012003 0.005360 0.025917 0.328309 0.649173
+0.004293 -0.015189 0.020860 0.0 0.0
+0.002537 -0.021663 -0.037874 0.0 0.0
+0.017564 -0.002515 -0.026912 0.535047 0.158602
+0.011421 -0.002833 -0.031689 0.602351 0.194529
+0.011215 0.000322 -0.020415 0.517135 0.253575
+0.002635 0.003021 -0.012333 0.529341 0.378660
+0.001235 0.002490 -0.015739 0.562597 0.374265
+0.004272 0.002632 -0.013690 0.525222 0.350674
+-0.002997 0.002057 -0.002458 0.541143 0.598152
+-0.002614 -0.004314 -0.002919 0.0 0.0
+0.003125 -0.022994 -0.021280 0.0 0.0
+-0.005395 -0.000823 -0.013764 0.605695 0.495888
+-0.008954 -0.001258 -0.015629 0.641873 0.519288
+-0.003834 0.002652 -0.016057 0.603320 0.424604
+-0.007047 -0.000971 -0.018908 0.0 0.0
+0.010433 -0.002408 -0.000327 0.425311 0.423985
+0.007502 -0.002301 -0.002415 0.454583 0.442237
+0.009372 0.000321 -0.004108 0.449153 0.405214
+0.025152 0.001195 0.009053 0.296717 0.330843
+0.016960 0.000956 0.001995 0.370882 0.360310
+0.019875 -0.003369 -0.000014 0.0 0.0
+0.030751 0.008190 0.022981 0.192338 0.395364
+0.006170 -0.006701 0.007879 0.0 0.0
+0.004293 -0.001930 0.020860 0.0 0.0
+-0.000653 -0.004919 0.008637 0.0 0.0
+-0.014173 -0.021266 0.011121 0.0 0.0
+-0.017277 -0.006019 -0.017796 0.0 0.0
+0.023544 -0.000939 0.002037 0.0 0.0
+-0.009768 -0.009134 -0.022027 0.0 0.0
+-0.009580 -0.002948 -0.014062 0.636321 0.535441
+-0.011797 -0.001691 -0.017176 0.666623 0.531485
+-0.010204 0.002309 -0.009891 0.632801 0.615829
+-0.011598 0.001574 -0.008061 0.635984 0.649988
+-0.008708 0.001627 -0.006285 0.605513 0.633471
+0.034635 0.001794 0.002947 0.257173 0.179657
+0.039472 0.002062 0.008665 0.205123 0.177019
+-0.015907 -0.007981 -0.018944 0.0 0.0
+-0.020598 0.001813 -0.025718 0.792131 0.555168
+-0.045621 -0.006163 -0.019740 0.935849 0.983660
+-0.040582 -0.008149 -0.025424 0.0 0.0
+-0.042936 -0.004004 -0.023210 0.932866 0.916952
+-0.038274 -0.003561 -0.017712 0.876293 0.872123
+-0.025030 -0.004697 -0.013625 0.0 0.0
+-0.023215 -0.000003 -0.009663 0.733754 0.749496
+-0.021118 -0.002997 -0.014161 0.0 0.0
+-0.004112 0.000695 0.015891 0.483989 0.762601
+-0.016389 -0.004557 -0.019826 0.0 0.0
+-0.003547 -0.002715 -0.010257 0.571529 0.501819
+-0.001840 -0.000524 -0.011726 0.569397 0.474849
+-0.000903 -0.002832 -0.008443 0.541931 0.487581
+-0.001161 -0.006198 -0.011121 0.0 0.0
+0.026165 -0.000632 0.016453 0.0 0.0
+0.025651 0.005503 0.014190 0.255812 0.397905
+-0.045621 -0.011579 -0.019740 0.0 0.0
+-0.040582 -0.021408 -0.025424 0.0 0.0
+-0.032217 -0.008366 -0.016728 0.0 0.0
+0.000124 -0.006312 0.001727 0.0 0.0
+0.017517 -0.002166 0.004909 0.355779 0.383708
+0.011949 -0.006639 -0.000263 0.0 0.0
+0.023286 0.001530 0.011808 0.299493 0.388218
+-0.013427 -0.006320 -0.020900 0.0 0.0
+0.005159 0.001722 -0.017378 0.541972 0.328988
+0.006008 -0.003221 -0.013176 0.0 0.0
+0.044680 -0.002154 0.004195 0.212358 0.092100
+-0.000907 0.002148 0.009167 0.474108 0.650299
+0.018182 0.005343 0.015586 0.301651 0.485315
+0.043927 0.001725 0.025605 0.0 0.0
+-0.012854 0.002261 -0.012050 0.666436 0.627285
+-0.014517 0.001574 -0.010230 0.670831 0.667926
+-0.011668 -0.001006 -0.012914 0.648374 0.577399
+-0.008316 0.002150 -0.020675 0.670180 0.448525
+-0.010664 0.002390 -0.018678 0.676647 0.483901
+-0.021013 -0.010291 0.007951 0.0 0.0
+-0.014146 -0.001754 -0.047689 0.871625 0.380651
+-0.008422 -0.002312 -0.044596 0.811809 0.335078
+-0.015684 0.000035 -0.039467 0.823954 0.423325
+-0.034331 -0.001228 -0.022520 0.867926 0.743772
+-0.031662 -0.007124 -0.023091 0.0 0.0
+-0.031224 -0.014703 0.002956 0.0 0.0
+-0.022628 -0.011995 -0.002126 0.0 0.0
+-0.025924 -0.004114 -0.023568 0.0 0.0
+0.027872 -0.000761 0.013734 0.287809 0.354222
+-0.034591 -0.000064 -0.031338 0.914569 0.689654
+-0.029145 0.000494 -0.024321 0.845319 0.655410
+-0.028017 -0.005001 -0.028032 0.0 0.0
+0.027902 0.001681 0.011326 0.276857 0.326315
+0.025087 -0.001165 0.011230 0.300071 0.358218
+-0.021752 0.000419 -0.042010 0.892791 0.514352
+-0.026528 0.000997 -0.038365 0.897612 0.549181
+-0.024140 -0.004708 -0.040188 0.0 0.0
+-0.020978 0.001288 -0.033343 0.828966 0.502825
+-0.040551 -0.026143 -0.010199 0.0 0.0
+-0.023197 -0.010713 -0.012877 0.0 0.0
+-0.040551 -0.012883 -0.010199 0.0 0.0
+0.004485 -0.000007 -0.025484 0.601465 0.304044
+0.010626 0.000849 0.002651 0.410322 0.457472
+-0.028255 0.000295 -0.019377 0.812070 0.694379
+0.013332 0.003514 -0.005048 0.413228 0.317771
+0.012798 0.000333 -0.005951 0.0 0.0
+-0.015803 0.002187 -0.014369 0.700775 0.636061
+-0.013341 -0.003209 -0.016395 0.673887 0.556538
+0.004675 -0.002400 -0.004481 0.483440 0.456956
+0.002779 0.000374 -0.003048 0.492235 0.501557
+0.026117 -0.007505 -0.017965 0.0 0.0
+0.028859 -0.001994 -0.015040 0.401776 0.121209
+-0.018952 -0.009957 -0.016014 0.0 0.0
+-0.017184 -0.003861 -0.014796 0.0 0.0
+0.020301 0.003439 -0.001538 0.351704 0.269778
+0.046716 -0.013576 0.023724 0.0 0.0
+0.055055 -0.021059 0.018567 0.0 0.0
+0.055055 -0.007800 0.018567 0.0 0.0
+-0.007396 0.002390 -0.007873 0.599971 0.601408
+0.001875 -0.002670 -0.006494 0.512456 0.471953
+0.038019 -0.008659 0.029998 0.0 0.0
+-0.008029 -0.002626 -0.003881 0.0 0.0
+-0.005500 -0.000144 -0.009081 0.582441 0.547991
+-0.016373 -0.003539 -0.018074 0.693376 0.563980
+-0.010554 -0.001307 0.012690 0.542110 0.813150
+0.049300 -0.002253 0.010554 0.156641 0.074718
+0.038759 -0.002166 0.014632 0.0 0.0
+0.005684 -0.003039 -0.035812 0.664057 0.231011
+0.038019 0.004600 0.029998 0.0 0.0
+0.038019 0.010016 0.029998 0.114875 0.333565
+-0.026286 -0.002491 -0.003954 0.729150 0.852638
+0.017414 0.003222 -0.004081 0.382513 0.280278
+0.044485 0.002423 0.015009 0.154850 0.173882
+0.040181 0.004389 0.016007 0.169553 0.218078
+-0.000017 0.000235 -0.005153 0.522576 0.519037
+-0.033970 -0.003806 -0.010656 0.813819 0.897478
+0.009082 0.001920 -0.014465 0.496261 0.303552
+0.017757 0.000902 -0.014705 0.442129 0.218874
+-0.030192 -0.001153 -0.014581 0.804471 0.778885
+0.025315 0.003889 0.003730 0.298239 0.254431
+0.024009 0.004399 0.004815 0.299684 0.276964
+-0.000443 0.002808 -0.014194 0.567019 0.401750
+0.005274 0.003135 -0.010676 0.501002 0.361565
+0.006826 0.002728 -0.011961 0.497165 0.333554
+0.008126 0.003261 -0.008819 0.469528 0.344355
+0.003879 -0.000228 -0.008173 0.508530 0.439124
+0.032342 0.006310 0.016942 0.206620 0.322798
+0.023610 0.001317 -0.009141 0.378376 0.199234
+0.028043 0.004252 0.006706 0.268835 0.252118
+0.028016 -0.000536 0.005381 0.0 0.0
+0.017259 0.006519 0.029649 0.274881 0.613415
+-0.032441 -0.018179 -0.033098 0.0 0.0
+0.022512 0.003646 0.000819 0.327614 0.262242
+0.027741 0.001325 0.014450 0.267345 0.359882
+0.028451 0.005830 0.015287 0.234742 0.367594
+-0.017863 -0.003577 0.009446 0.606336 0.873144
+0.021215 0.004148 0.001990 0.329253 0.284285
+0.025358 0.002875 -0.000357 0.319055 0.238054
+-0.025580 0.001252 -0.027579 0.834418 0.591281
+-0.032441 -0.004919 -0.033098 0.0 0.0
+-0.038228 -0.001462 -0.027638 0.921967 0.762041
+-0.019666 0.002058 -0.021535 0.760948 0.564549
+0.028581 -0.008715 0.037841 0.0 0.0
+0.010757 0.003389 -0.006914 0.440443 0.329599
+0.019618 0.002475 -0.006065 0.380279 0.255089
+-0.031224 -0.009287 0.002956 0.726653 0.983908
+-0.031224 -0.027962 0.002956 0.0 0.0
+0.034589 -0.002055 -0.008644 0.340553 0.106507
+-0.002831 -0.000029 -0.007070 0.552248 0.532828
+-0.005830 0.001763 -0.004361 0.571170 0.614866
+-0.038103 -0.007971 -0.006212 0.816889 0.989345
+0.031060 0.005869 0.013961 0.222658 0.302436
+-0.030292 0.001057 -0.034857 0.906371 0.609867
+-0.024162 -0.006173 0.006457 0.662433 0.924856
+-0.042999 -0.006964 -0.014187 0.888500 0.981682
+0.001834 0.002718 0.019362 0.427865 0.719550
+0.049505 -0.002359 0.021843 0.0 0.0
+0.011040 0.003784 0.006750 0.391457 0.513336
+-0.001834 0.002801 -0.003863 0.535555 0.568767
+0.001083 0.003102 -0.001764 0.503470 0.551983
+0.022848 0.008014 0.033651 0.219229 0.581941
+0.023375 -0.002185 -0.020891 0.468122 0.139723
+0.028581 0.009961 0.037841 0.135591 0.524833
+0.006652 0.003669 0.002024 0.444507 0.517603
+0.009164 0.003881 0.003676 0.417615 0.501784
+0.008207 0.003356 0.004901 0.421793 0.528731
+0.049505 0.003057 0.021843 0.084272 0.136500
+0.055055 -0.002384 0.018567 0.061146 0.038651
+0.001013 -0.000353 -0.009972 0.538504 0.457627
+0.029329 0.005348 0.010717 0.244768 0.281415
+-0.004550 0.002514 -0.005831 0.565049 0.584900
+-0.007071 0.002482 -0.017312 0.640794 0.454937
+0.046366 0.005522 0.023902 0.092927 0.186502
+-0.045621 -0.024839 -0.019740 0.0 0.0
+0.041488 0.008760 0.027307 0.105299 0.272334
+0.033206 0.010046 0.034162 0.125908 0.426442
+0.026563 0.004882 0.007661 0.273634 0.279815
+3 43 45 44
+3 65 67 66
+3 69 71 70
+3 99 101 100
+3 61 63 138
+3 65 143 142
+3 147 149 148
+3 154 156 155
+3 159 160 45
+3 168 48 169
+3 173 175 174
+3 43 44 176
+3 187 188 77
+3 67 196 195
+3 66 67 195
+3 150 179 152
+3 203 204 205
+3 206 208 207
+3 176 44 209
+3 232 233 234
+3 235 127 236
+3 48 49 1
+3 239 242 241
+3 253 143 65
+3 87 235 59
+3 152 179 177
+3 41 39 125
+3 179 272 271
+3 274 276 275
+3 283 277 284
+3 266 107 173
+3 288 289 291
+3 48 168 49
+3 208 262 207
+3 156 262 295
+3 297 169 284
+3 142 67 65
+3 188 298 75
+3 268 300 269
+3 300 0 269
+3 312 232 234
+3 1 49 0
+3 236 127 149
+3 233 63 61
+3 265 24 318
+3 319 264 236
+3 321 126 125
+3 79 222 323
+3 276 288 291
+3 107 71 69
+3 149 327 326
+3 269 63 233
+3 49 244 0
+3 77 331 330
+3 332 168 297
+3 268 269 233
+3 142 20 266
+3 325 54 55
+3 262 330 205
+3 0 244 63
+3 195 136 222
+3 336 338 337
+3 79 340 194
+3 246 174 265
+3 195 222 194
+3 26 138 349
+3 333 346 350
+3 351 333 127
+3 169 352 284
+3 325 55 308
+3 244 332 324
+3 241 242 277
+3 354 241 277
+3 39 271 155
+3 59 235 264
+3 169 355 238
+3 205 204 295
+3 20 108 107
+3 335 154 214
+3 87 341 358
+3 358 351 87
+3 107 69 173
+3 359 324 329
+3 41 177 39
+3 326 319 236
+3 100 173 174
+3 359 329 364
+3 214 154 155
+3 154 335 207
+3 283 354 277
+3 142 143 21
+3 289 366 291
+3 348 253 66
+3 138 367 349
+3 368 329 242
+3 361 305 341
+3 77 341 331
+3 232 268 233
+3 338 187 337
+3 174 369 100
+3 100 369 99
+3 43 265 174
+3 265 43 176
+3 127 333 342
+3 154 207 156
+3 196 200 344
+3 188 187 357
+3 188 75 77
+3 173 371 175
+3 340 348 194
+3 372 44 373
+3 147 340 79
+3 333 350 334
+3 126 41 125
+3 330 262 208
+3 291 366 352
+3 374 376 136
+3 351 346 333
+3 377 379 378
+3 331 375 205
+3 355 152 238
+3 380 381 326
+3 234 233 61
+3 367 324 359
+3 196 101 200
+3 342 128 127
+3 188 357 298
+3 379 371 378
+3 358 325 308
+3 383 148 128
+3 177 291 238
+3 138 324 367
+3 136 344 374
+3 173 101 266
+3 168 169 297
+3 149 326 236
+3 209 372 384
+3 108 71 107
+3 337 187 330
+3 149 127 128
+3 357 187 338
+3 373 45 160
+3 173 100 101
+3 265 318 246
+3 101 99 200
+3 21 108 20
+3 24 265 176
+3 55 346 308
+3 371 70 378
+3 152 177 238
+3 351 127 235
+3 168 244 49
+3 26 61 138
+3 305 375 331
+3 266 101 196
+3 156 295 125
+3 168 332 244
+3 308 346 351
+3 366 283 284
+3 329 332 242
+3 244 138 63
+3 177 179 39
+3 136 196 344
+3 361 87 59
+3 77 358 341
+3 373 44 45
+3 327 79 386
+3 207 262 156
+3 148 149 128
+3 55 350 346
+3 222 79 194
+3 332 297 277
+3 39 155 156
+3 147 148 365
+3 242 332 277
+3 187 77 330
+3 1 0 300
+3 372 209 44
+3 246 369 174
+3 26 349 318
+3 26 318 24
+3 332 329 324
+3 155 271 385
+3 156 125 39
+3 271 272 385
+3 364 329 368
+3 175 379 159
+3 348 66 194
+3 383 365 148
+3 136 389 222
+3 342 390 383
+3 326 327 386
+3 126 275 41
+3 323 222 389
+3 277 297 284
+3 142 266 67
+3 173 69 371
+3 75 298 54
+3 380 326 386
+3 381 319 326
+3 388 79 323
+3 308 351 358
+3 352 169 238
+3 386 79 388
+3 136 376 389
+3 239 368 242
+3 208 337 330
+3 358 75 325
+3 77 75 358
+3 253 65 66
+3 0 63 269
+3 361 341 87
+3 204 321 295
+3 266 20 107
+3 147 365 340
+3 43 174 175
+3 75 54 325
+3 128 342 383
+3 150 272 179
+3 266 196 67
+3 175 159 43
+3 312 363 384
+3 351 235 87
+3 355 150 152
+3 206 336 208
+3 214 155 385
+3 147 79 327
+3 208 336 337
+3 291 177 41
+3 363 234 61
+3 363 312 234
+3 371 69 70
+3 274 288 276
+3 159 377 160
+3 66 195 194
+3 330 331 205
+3 321 125 295
+3 342 334 390
+3 176 363 61
+3 26 176 61
+3 375 203 205
+3 379 175 371
+3 341 305 331
+3 291 41 276
+3 24 176 26
+3 262 205 295
+3 209 363 176
+3 276 41 275
+3 159 45 43
+3 335 206 207
+3 20 142 21
+3 366 284 352
+3 147 327 149
+3 39 179 271
+3 209 384 363
+3 379 377 159
+3 264 235 236
+3 333 334 342
+3 195 196 136
+3 244 324 138
+3 291 352 238
+3 1 2 3
+3 21 22 23
+3 47 48 3
+3 108 23 90
+3 48 1 3
+3 253 141 143
+3 268 270 2
+3 2 300 268
+3 97 232 312
+3 97 312 316
+3 23 22 167
+3 48 47 169
+3 6 90 258
+3 317 47 3
+3 193 4 216
+3 95 97 316
+3 4 193 296
+3 4 89 6
+3 282 347 93
+3 348 347 253
+3 316 250 248
+3 89 90 6
+3 372 373 303
+3 2 230 301
+3 143 141 260
+3 70 89 296
+3 377 378 296
+3 301 317 2
+3 282 141 347
+3 328 302 313
+3 141 253 347
+3 71 90 89
+3 328 384 372
+3 108 90 71
+3 328 303 302
+3 217 158 216
+3 373 160 158
+3 21 23 108
+3 378 70 296
+3 22 287 167
+3 312 362 316
+3 89 4 296
+3 143 260 21
+3 328 313 362
+3 1 300 2
+3 270 268 232
+3 22 260 287
+3 316 362 250
+3 84 90 23
+3 71 89 70
+3 141 282 287
+3 230 2 270
+3 193 216 158
+3 362 313 250
+3 270 232 97
+3 348 340 93
+3 84 258 90
+3 312 384 362
+3 377 296 193
+3 193 158 160
+3 373 158 303
+3 377 193 160
+3 317 3 2
+3 95 230 97
+3 230 270 97
+3 372 303 328
+3 217 303 158
+3 328 362 384
+3 248 95 316
+3 217 302 303
+3 84 23 167
+3 260 22 21
+3 260 141 287
+3 347 348 93
+3 54 56 55
+3 83 85 84
+3 56 54 109
+3 212 214 213
+3 216 218 217
+3 219 83 167
+3 230 213 231
+3 248 250 249
+3 56 220 55
+3 286 219 287
+3 85 220 258
+3 185 317 183
+3 150 183 272
+3 83 334 85
+3 336 339 338
+3 301 230 231
+3 95 248 212
+3 183 301 231
+3 109 216 4
+3 282 286 287
+3 169 47 355
+3 338 339 36
+3 335 214 212
+3 4 220 56
+3 339 313 302
+3 85 334 350
+3 47 317 185
+3 357 36 218
+3 382 206 335
+3 250 313 382
+3 334 83 219
+3 231 213 272
+3 218 36 217
+3 219 286 383
+3 220 4 6
+3 286 282 93
+3 183 317 301
+3 213 214 385
+3 167 83 84
+3 357 109 298
+3 36 357 338
+3 272 213 385
+3 55 85 350
+3 85 258 84
+3 336 382 339
+3 336 206 382
+3 249 250 382
+3 230 95 213
+3 47 185 355
+3 340 365 93
+3 248 249 212
+3 383 286 365
+3 219 383 390
+3 249 382 335
+3 218 216 109
+3 219 167 287
+3 286 93 365
+3 217 36 302
+3 54 298 109
+3 357 218 109
+3 185 150 355
+3 183 231 272
+3 36 339 302
+3 313 339 382
+3 334 219 390
+3 185 183 150
+3 85 55 220
+3 213 95 212
+3 258 220 6
+3 56 109 4
+3 335 212 249
+3 14 310 309
+4 0 1 2 3
+4 4 5 6 7
+4 8 9 10 11
+4 12 13 14 15
+4 16 17 18 19
+4 20 21 22 23
+4 24 25 26 27
+4 28 29 30 31
+4 32 33 34 35
+4 36 37 33 38
+4 39 40 41 42
+4 43 44 45 46
+4 47 3 48 49
+4 50 51 52 53
+4 54 55 56 57
+4 58 59 60 13
+4 61 62 63 64
+4 65 66 67 68
+4 69 70 71 72
+4 73 8 30 74
+4 75 76 77 78
+4 79 80 81 82
+4 83 84 85 86
+4 87 88 60 13
+4 89 7 90 72
+4 91 78 88 92
+4 93 82 94 17
+4 95 96 97 98
+4 99 100 101 102
+4 103 104 105 106
+4 107 108 23 90
+4 56 109 54 110
+4 111 7 112 113
+4 114 115 116 117
+4 91 118 110 119
+4 120 121 122 123
+4 124 125 126 123
+4 127 18 128 129
+4 130 131 132 133
+4 134 135 136 81
+4 105 27 137 74
+4 61 138 63 62
+4 122 139 140 123
+4 65 141 142 143
+4 47 144 130 133
+4 27 145 96 146
+4 147 148 149 82
+4 150 151 152 153
+4 154 155 156 157
+4 158 159 160 45
+4 161 162 163 164
+4 165 145 27 166
+4 167 12 84 86
+4 168 169 48 170
+4 5 171 166 172
+4 173 174 175 112
+4 43 176 44 46
+4 177 178 179 163
+4 180 181 182 139
+4 183 184 185 153
+4 8 186 157 11
+4 187 77 188 37
+4 173 116 112 72
+4 189 190 191 92
+4 192 186 125 42
+4 159 158 193 46
+4 79 81 194 82
+4 67 195 196 197
+4 122 198 140 182
+4 66 195 67 199
+4 150 152 179 153
+4 200 50 99 102
+4 12 201 52 53
+4 15 14 58 13
+4 202 120 140 123
+4 191 203 204 205
+4 152 151 163 153
+4 206 207 208 10
+4 176 209 44 210
+4 137 35 211 74
+4 212 213 214 215
+4 216 217 218 119
+4 219 167 83 86
+4 118 220 221 56
+4 195 199 222 197
+4 174 223 224 225
+4 137 226 165 27
+4 47 132 130 227
+4 88 228 221 86
+4 131 170 132 133
+4 106 31 137 229
+4 230 231 213 29
+4 232 233 97 234
+4 235 236 127 18
+4 48 3 1 49
+4 214 154 212 157
+4 130 237 182 161
+4 114 68 141 117
+4 125 123 192 42
+4 238 181 177 144
+4 239 240 241 242
+4 103 243 244 245
+4 193 111 159 46
+4 246 224 25 225
+4 184 161 247 151
+4 248 249 250 251
+4 94 199 93 252
+4 253 141 65 143
+4 254 255 240 256
+4 257 210 176 46
+4 258 221 12 259
+4 22 115 260 117
+4 260 115 114 117
+4 94 82 81 17
+4 96 146 145 98
+4 178 163 261 153
+4 262 263 208 10
+4 264 13 58 18
+4 24 265 25 225
+4 87 235 60 59
+4 152 177 179 163
+4 116 101 102 266
+4 34 35 33 38
+4 24 257 176 225
+4 41 125 39 42
+4 261 164 40 229
+4 94 17 16 19
+4 267 81 79 80
+4 268 269 2 270
+4 95 74 248 98
+4 179 271 272 178
+4 178 184 261 29
+4 25 27 273 62
+4 274 121 275 276
+4 277 256 242 278
+4 273 226 279 280
+4 210 145 250 166
+4 181 133 47 281
+4 282 17 94 19
+4 56 55 220 57
+4 283 284 277 285
+4 182 162 161 164
+4 286 287 219 129
+4 266 173 107 116
+4 174 224 246 225
+4 282 18 93 17
+4 51 114 94 19
+4 288 289 290 291
+4 223 257 165 225
+4 205 34 191 92
+4 47 48 168 49
+4 292 293 256 294
+4 208 207 262 10
+4 156 295 262 11
+4 149 80 147 82
+4 223 46 257 225
+4 296 111 193 113
+4 165 257 223 5
+4 297 284 169 281
+4 117 142 67 65
+4 188 75 298 299
+4 85 258 220 221
+4 170 133 180 281
+4 178 215 30 42
+4 270 104 230 64
+4 2 268 300 269
+4 184 106 301 227
+4 302 210 172 303
+4 106 237 261 229
+4 221 118 91 259
+4 300 0 2 269
+4 304 88 305 78
+4 182 131 198 306
+4 3 307 49 245
+4 55 308 220 57
+4 132 243 103 245
+4 14 309 310 311
+4 145 74 96 98
+4 178 29 183 153
+4 44 210 303 172
+4 97 312 232 234
+4 302 166 313 38
+4 1 49 3 0
+4 174 111 223 225
+4 94 81 314 17
+4 73 31 30 229
+4 177 139 291 123
+4 140 198 137 106
+4 211 192 73 186
+4 236 149 127 18
+4 100 224 174 112
+4 176 210 24 315
+4 261 29 184 237
+4 269 104 270 64
+4 292 294 280 293
+4 97 316 312 234
+4 250 145 248 251
+4 23 167 22 115
+4 135 51 50 197
+4 61 315 26 62
+4 48 169 47 170
+4 234 315 61 64
+4 178 229 40 42
+4 137 145 165 35
+4 233 61 63 64
+4 185 183 317 184
+4 46 113 5 172
+4 177 181 291 139
+4 6 258 90 7
+4 135 81 51 197
+4 265 24 25 318
+4 319 236 264 320
+4 181 162 177 144
+4 321 126 124 125
+4 8 10 263 11
+4 322 79 222 323
+4 244 103 324 243
+4 54 110 325 57
+4 73 34 211 186
+4 114 116 53 102
+4 189 211 191 192
+4 51 68 94 252
+4 317 3 47 132
+4 290 276 288 291
+4 282 94 93 252
+4 106 227 237 306
+4 91 299 110 76
+4 261 247 184 161
+4 294 292 280 279
+4 179 178 272 153
+4 150 272 183 153
+4 193 216 4 113
+4 107 90 69 71
+4 12 53 84 7
+4 33 171 302 38
+4 198 293 137 306
+4 149 326 327 320
+4 269 233 63 64
+4 49 0 244 245
+4 6 221 258 259
+4 166 171 35 38
+4 47 247 317 227
+4 291 139 276 123
+4 44 209 328 210
+4 168 170 47 245
+4 191 190 304 92
+4 181 285 291 139
+4 329 243 293 256
+4 95 316 97 96
+4 96 62 315 64
+4 60 88 15 13
+4 77 330 331 76
+4 293 103 105 306
+4 182 181 180 133
+4 137 27 165 145
+4 50 197 51 102
+4 332 297 168 170
+4 106 29 301 31
+4 34 76 33 92
+4 40 162 140 164
+4 320 80 149 17
+4 137 106 198 306
+4 30 157 73 42
+4 140 182 198 306
+4 268 233 269 64
+4 106 307 301 227
+4 142 266 20 117
+4 83 333 85 334
+4 47 170 169 281
+4 313 166 35 38
+4 325 55 54 57
+4 30 8 73 157
+4 262 205 330 263
+4 0 63 244 103
+4 125 186 156 42
+4 3 132 317 307
+4 12 112 53 7
+4 250 35 313 166
+4 94 114 51 252
+4 195 222 136 197
+4 335 10 249 11
+4 18 17 129 19
+4 111 46 223 225
+4 336 337 338 339
+4 266 68 101 102
+4 156 186 157 42
+4 13 86 12 19
+4 18 129 16 19
+4 294 255 254 256
+4 293 243 180 256
+4 137 73 140 229
+4 4 296 193 113
+4 79 194 340 82
+4 295 186 262 11
+4 305 78 341 92
+4 157 9 8 11
+4 342 286 219 343
+4 140 306 106 229
+4 196 135 344 197
+4 93 199 94 82
+4 198 182 345 180
+4 329 293 294 256
+4 5 46 111 113
+4 163 161 261 153
+4 242 254 240 256
+4 293 131 180 170
+4 216 259 4 113
+4 346 333 85 228
+4 207 10 335 11
+4 40 164 140 229
+4 246 265 174 225
+4 195 194 222 199
+4 66 347 348 199
+4 26 349 138 62
+4 333 346 85 350
+4 301 231 230 29
+4 347 199 68 252
+4 351 127 333 343
+4 95 212 248 9
+4 140 164 182 229
+4 73 192 40 42
+4 240 242 239 254
+4 114 68 51 252
+4 213 29 231 215
+4 248 145 250 146
+4 182 237 261 161
+4 52 51 12 53
+4 2 270 269 104
+4 258 6 5 7
+4 184 151 185 153
+4 81 199 94 197
+4 219 129 287 86
+4 97 96 316 64
+4 248 74 95 9
+4 183 231 301 29
+4 169 284 352 281
+4 314 80 320 17
+4 104 64 96 98
+4 217 5 216 172
+4 286 343 127 129
+4 292 198 255 256
+4 33 37 36 119
+4 121 276 290 139
+4 290 285 353 139
+4 130 131 237 227
+4 349 273 138 62
+4 169 181 238 144
+4 325 308 55 57
+4 33 76 37 119
+4 152 163 179 153
+4 111 46 193 113
+4 8 263 34 186
+4 244 324 332 243
+4 104 106 103 307
+4 109 4 216 118
+4 295 192 204 186
+4 241 242 240 277
+4 240 354 241 277
+4 204 192 191 186
+4 39 155 271 215
+4 36 37 218 119
+4 110 118 91 57
+4 353 122 290 139
+4 88 78 304 92
+4 282 287 286 129
+4 303 158 45 46
+4 314 322 267 81
+4 201 223 165 225
+4 230 31 28 98
+4 84 12 258 86
+4 28 29 213 215
+4 59 264 235 18
+4 4 6 89 7
+4 132 170 47 133
+4 51 197 68 102
+4 169 355 47 238
+4 141 68 114 252
+4 356 314 52 51
+4 35 251 8 74
+4 93 18 282 129
+4 218 37 357 299
+4 191 205 204 295
+4 20 23 107 108
+4 149 18 236 320
+4 39 215 178 42
+4 267 309 314 320
+4 47 181 169 144
+4 47 170 132 245
+4 338 337 36 339
+4 216 5 217 259
+4 335 154 212 214
+4 212 157 249 9
+4 173 112 175 72
+4 20 116 22 117
+4 87 358 341 57
+4 2 269 0 104
+4 358 87 351 57
+4 107 173 69 72
+4 217 113 158 172
+4 25 226 273 27
+4 359 329 324 280
+4 41 39 177 40
+4 360 226 137 280
+4 112 7 5 113
+4 4 220 118 56
+4 60 87 361 88
+4 257 5 165 166
+4 224 50 52 53
+4 326 236 319 320
+4 249 251 248 9
+4 218 299 109 118
+4 305 304 361 88
+4 47 133 170 281
+4 100 174 173 112
+4 121 139 122 123
+4 198 293 292 280
+4 316 362 363 146
+4 32 34 211 35
+4 226 280 273 62
+4 294 359 329 364
+4 85 228 83 86
+4 244 105 324 103
+4 40 178 177 163
+4 214 155 154 157
+4 161 151 184 153
+4 169 181 47 281
+4 302 33 36 171
+4 154 207 335 11
+4 302 171 217 172
+4 20 23 22 116
+4 293 103 324 280
+4 283 277 354 285
+4 255 180 345 278
+4 339 302 313 38
+4 154 249 212 157
+4 27 145 210 166
+4 365 93 148 82
+4 28 31 74 98
+4 282 93 347 252
+4 121 275 276 123
+4 261 178 40 164
+4 53 112 173 116
+4 142 21 143 117
+4 81 82 80 17
+4 289 291 366 285
+4 140 137 211 73
+4 360 137 198 280
+4 348 66 253 347
+4 137 27 105 280
+4 101 68 196 102
+4 138 349 367 273
+4 316 248 250 146
+4 51 81 94 197
+4 53 116 100 102
+4 140 73 211 192
+4 368 242 329 256
+4 63 104 269 64
+4 361 341 305 88
+4 182 164 237 229
+4 337 37 339 263
+4 180 243 293 170
+4 77 331 341 78
+4 140 162 182 164
+4 154 157 156 11
+4 44 46 210 172
+4 232 233 268 64
+4 263 10 262 11
+4 51 68 114 102
+4 363 315 316 146
+4 66 68 347 199
+4 140 162 40 123
+4 338 337 187 37
+4 327 320 326 80
+4 174 224 100 369
+4 100 224 99 369
+4 339 37 36 38
+4 43 111 174 265
+4 250 145 210 146
+4 265 176 43 225
+4 180 281 285 278
+4 127 342 333 343
+4 154 156 207 11
+4 69 71 90 72
+4 196 344 200 50
+4 209 210 176 315
+4 140 40 73 192
+4 165 5 12 259
+4 223 112 12 5
+4 220 118 221 6
+4 188 357 187 37
+4 188 77 75 299
+4 304 60 361 88
+4 220 228 221 57
+4 272 178 231 153
+4 182 161 261 164
+4 327 326 267 80
+4 319 58 370 320
+4 58 309 14 311
+4 211 34 73 35
+4 259 171 217 119
+4 238 163 152 144
+4 173 175 371 72
+4 12 53 51 19
+4 89 6 90 7
+4 83 343 219 86
+4 159 111 43 46
+4 182 180 198 131
+4 340 194 348 199
+4 177 162 181 139
+4 88 13 87 228
+4 303 46 44 172
+4 137 106 105 31
+4 69 90 107 72
+4 74 31 105 98
+4 372 303 373 44
+4 329 293 324 280
+4 258 12 84 7
+4 224 53 100 102
+4 47 238 355 144
+4 47 49 168 245
+4 314 51 356 81
+4 304 78 305 92
+4 136 134 374 135
+4 141 114 287 19
+4 165 27 226 257
+4 2 301 230 104
+4 90 7 23 72
+4 27 145 137 74
+4 263 186 8 11
+4 147 79 340 82
+4 359 273 279 280
+4 375 304 305 92
+4 5 7 4 113
+4 223 53 12 112
+4 137 105 293 280
+4 333 85 334 350
+4 148 93 149 17
+4 137 31 73 229
+4 34 33 32 92
+4 91 118 221 57
+4 126 125 41 123
+4 143 260 141 117
+4 330 208 262 263
+4 370 309 267 320
+4 47 185 317 247
+4 83 228 343 86
+4 304 203 191 92
+4 317 307 132 227
+4 30 29 178 229
+4 291 352 366 285
+4 91 57 221 78
+4 184 247 261 237
+4 374 134 136 376
+4 294 254 368 256
+4 371 70 89 296
+4 261 237 247 161
+4 357 218 36 37
+4 347 68 141 252
+4 59 13 264 18
+4 351 333 346 228
+4 124 120 202 123
+4 231 178 272 215
+4 316 315 96 146
+4 36 339 337 37
+4 152 144 163 151
+4 377 296 378 379
+4 255 198 345 180
+4 251 74 248 9
+4 324 103 105 280
+4 88 57 341 78
+4 354 277 240 278
+4 13 228 88 86
+4 192 123 40 42
+4 105 104 96 98
+4 100 53 173 116
+4 331 205 375 92
+4 352 181 169 281
+4 240 255 345 278
+4 355 238 152 144
+4 370 380 381 326
+4 234 61 233 64
+4 105 62 96 64
+4 30 229 178 42
+4 67 117 68 266
+4 63 103 0 104
+4 93 199 347 252
+4 301 2 317 307
+4 132 227 103 306
+4 110 57 91 78
+4 367 359 324 280
+4 293 243 132 170
+4 258 221 85 228
+4 332 256 297 170
+4 382 335 206 10
+4 250 382 313 38
+4 178 29 30 215
+4 333 334 83 219
+4 65 141 253 68
+4 53 116 114 115
+4 97 234 233 64
+4 237 164 261 229
+4 282 347 141 252
+4 326 320 267 80
+4 341 88 87 57
+4 210 328 302 313
+4 169 238 47 144
+4 34 263 8 35
+4 32 88 190 92
+4 40 162 177 123
+4 290 276 291 139
+4 231 272 213 215
+4 25 201 226 225
+4 196 200 101 50
+4 96 315 27 146
+4 342 286 127 128
+4 4 118 6 259
+4 317 247 184 227
+4 12 13 88 86
+4 188 298 357 299
+4 141 347 253 68
+4 290 274 276 121
+4 379 378 371 296
+4 63 62 105 64
+4 110 118 299 119
+4 166 171 302 172
+4 290 289 353 285
+4 358 308 325 57
+4 103 106 105 306
+4 193 46 158 113
+4 165 201 12 223
+4 383 148 286 128
+4 106 29 261 237
+4 12 5 112 7
+4 67 199 195 197
+4 177 238 291 181
+4 58 13 16 18
+4 138 367 324 280
+4 26 315 27 62
+4 37 263 330 76
+4 136 374 344 135
+4 116 173 101 266
+4 237 131 182 306
+4 35 251 250 38
+4 77 76 331 78
+4 168 297 169 170
+4 149 236 326 320
+4 71 89 90 72
+4 105 103 293 280
+4 209 328 384 372
+4 279 226 360 280
+4 354 285 277 278
+4 87 228 351 57
+4 68 199 94 252
+4 93 18 149 17
+4 368 254 242 256
+4 108 90 107 71
+4 196 50 101 102
+4 337 330 187 37
+4 328 210 302 303
+4 265 111 174 225
+4 341 78 331 92
+4 217 216 158 113
+4 345 285 180 139
+4 149 128 127 18
+4 226 257 27 225
+4 249 10 382 251
+4 105 106 137 306
+4 357 338 187 37
+4 130 144 47 151
+4 249 9 157 11
+4 373 158 160 45
+4 10 9 249 11
+4 114 115 287 19
+4 222 322 136 81
+4 173 116 101 100
+4 132 131 293 170
+4 165 223 12 5
+4 331 78 76 92
+4 345 182 122 139
+4 261 29 106 229
+4 265 25 246 318
+4 88 221 12 86
+4 101 200 99 102
+4 145 146 248 74
+4 32 33 91 92
+4 211 35 73 74
+4 177 162 163 144
+4 157 186 156 11
+4 218 217 36 119
+4 287 19 86 129
+4 145 35 137 74
+4 63 105 244 103
+4 6 118 221 259
+4 21 23 20 108
+4 321 125 124 192
+4 24 176 265 225
+4 283 354 353 285
+4 149 93 148 18
+4 165 166 5 171
+4 73 35 8 74
+4 25 27 24 225
+4 55 346 85 308
+4 36 171 33 119
+4 217 171 36 119
+4 103 307 132 245
+4 219 342 383 286
+4 371 378 70 296
+4 316 315 234 64
+4 152 238 177 163
+4 168 243 332 170
+4 138 280 105 62
+4 34 190 92 32
+4 223 5 257 172
+4 33 35 32 171
+4 18 343 13 129
+4 351 235 127 343
+4 13 12 14 16
+4 371 296 89 72
+4 97 233 232 64
+4 220 4 118 6
+4 16 14 58 309
+4 5 113 216 172
+4 73 229 30 42
+4 168 49 244 245
+4 26 138 61 62
+4 96 145 27 74
+4 210 145 27 146
+4 80 82 149 17
+4 305 331 375 92
+4 346 85 308 228
+4 0 307 103 245
+4 230 64 104 98
+4 266 196 101 68
+4 333 343 83 228
+4 226 27 25 225
+4 286 93 282 129
+4 116 53 84 115
+4 161 144 130 151
+4 6 258 5 259
+4 198 131 293 306
+4 50 224 99 102
+4 162 139 177 123
+4 220 221 56 57
+4 181 285 180 281
+4 168 48 47 170
+4 356 51 135 81
+4 103 227 106 306
+4 147 80 79 82
+4 76 78 91 92
+4 221 57 88 78
+4 156 125 295 186
+4 224 201 25 225
+4 183 301 317 184
+4 173 53 100 112
+4 226 201 165 225
+4 58 319 264 320
+4 250 35 145 251
+4 198 180 255 256
+4 230 104 301 31
+4 184 29 106 237
+4 208 263 337 10
+4 168 244 332 243
+4 236 18 264 320
+4 182 306 140 229
+4 283 353 366 285
+4 182 237 130 131
+4 284 285 352 281
+4 178 163 40 164
+4 181 144 47 133
+4 266 117 68 102
+4 308 351 346 228
+4 213 28 230 29
+4 261 237 182 164
+4 122 182 140 139
+4 22 167 287 115
+4 213 385 214 215
+4 267 380 370 326
+4 379 296 371 72
+4 297 281 170 278
+4 366 285 284 283
+4 210 46 257 172
+4 218 118 216 119
+4 146 74 145 98
+4 312 363 316 362
+4 260 114 141 117
+4 141 114 260 115
+4 329 242 332 256
+4 105 244 138 63
+4 262 186 263 11
+4 177 39 179 178
+4 94 199 68 197
+4 325 110 358 57
+4 167 84 83 86
+4 163 162 40 164
+4 136 344 196 135
+4 132 307 3 245
+4 263 251 382 10
+4 361 87 60 59
+4 60 235 87 13
+4 375 203 304 92
+4 321 124 191 192
+4 267 320 314 80
+4 47 132 3 245
+4 222 199 81 197
+4 176 210 44 46
+4 177 163 238 144
+4 299 357 109 298
+4 110 76 75 78
+4 77 341 358 78
+4 357 37 188 299
+4 224 223 201 225
+4 91 171 259 119
+4 34 263 33 76
+4 157 28 8 9
+4 36 338 357 37
+4 344 135 50 197
+4 271 178 39 215
+4 373 45 44 303
+4 105 31 104 98
+4 58 309 370 320
+4 250 210 362 146
+4 267 327 79 386
+4 211 34 190 189
+4 293 131 132 306
+4 8 35 263 251
+4 207 156 262 11
+4 40 192 140 123
+4 189 124 211 192
+4 105 104 63 64
+4 264 18 58 320
+4 272 385 213 215
+4 142 143 141 117
+4 205 295 191 186
+4 50 53 224 102
+4 148 128 149 18
+4 99 224 100 102
+4 261 184 178 153
+4 180 285 345 278
+4 4 7 89 113
+4 231 178 183 153
+4 55 346 350 85
+4 104 31 230 98
+4 222 194 79 81
+4 269 270 268 64
+4 258 5 12 7
+4 110 299 75 76
+4 5 166 257 172
+4 3 49 47 245
+4 93 94 282 17
+4 16 13 12 19
+4 292 255 294 256
+4 27 210 257 166
+4 337 263 336 10
+4 16 320 18 17
+4 332 277 297 256
+4 105 106 104 31
+4 41 40 177 123
+4 39 156 155 157
+4 161 162 182 144
+4 16 18 13 129
+4 131 227 132 306
+4 73 8 34 186
+4 89 296 4 113
+4 210 315 363 146
+4 132 307 103 227
+4 147 82 365 148
+4 339 263 382 10
+4 32 211 165 35
+4 345 180 182 139
+4 289 285 290 139
+4 106 306 237 229
+4 104 106 301 31
+4 242 277 332 256
+4 85 84 258 86
+4 103 132 293 243
+4 224 223 174 112
+4 143 21 260 117
+4 109 110 56 118
+4 336 339 382 10
+4 187 330 77 37
+4 328 362 313 210
+4 196 68 67 197
+4 58 311 370 309
+4 298 110 109 299
+4 1 0 2 300
+4 336 382 206 10
+4 372 44 209 328
+4 137 74 73 31
+4 272 178 271 215
+4 293 105 137 306
+4 145 35 250 166
+4 26 27 25 62
+4 301 106 184 29
+4 240 277 242 278
+4 263 251 35 38
+4 34 211 190 32
+4 249 382 250 251
+4 176 46 43 225
+4 294 293 329 280
+4 148 18 286 128
+4 39 178 40 42
+4 122 121 290 139
+4 261 161 163 164
+4 230 213 95 28
+4 12 223 201 53
+4 36 337 338 37
+4 75 299 77 76
+4 363 210 209 315
+4 216 118 4 259
+4 379 193 296 111
+4 163 144 161 151
+4 270 232 268 64
+4 335 249 154 11
+4 249 157 154 11
+4 130 237 247 227
+4 230 29 28 31
+4 294 255 292 387
+4 285 281 284 278
+4 263 10 8 251
+4 287 114 141 115
+4 282 252 141 19
+4 246 224 174 369
+4 47 247 130 151
+4 26 349 25 318
+4 26 25 24 318
+4 316 234 97 64
+4 182 162 181 144
+4 243 170 168 245
+4 279 329 359 280
+4 286 18 93 129
+4 84 12 167 115
+4 8 28 30 74
+4 32 15 190 88
+4 5 259 165 171
+4 22 287 260 115
+4 190 34 92 189
+4 221 228 88 57
+4 23 116 107 72
+4 353 354 240 278
+4 138 273 367 280
+4 222 81 136 197
+4 258 228 85 86
+4 107 116 173 72
+4 27 257 165 166
+4 254 255 294 387
+4 332 324 329 243
+4 230 28 95 98
+4 244 243 168 245
+4 47 355 185 151
+4 316 250 362 146
+4 40 163 177 162
+4 155 385 271 215
+4 180 256 243 170
+4 156 39 125 42
+4 248 146 95 98
+4 271 385 272 215
+4 84 23 90 7
+4 221 118 56 57
+4 364 368 329 294
+4 8 157 30 28
+4 175 159 379 111
+4 136 81 135 197
+4 204 321 191 192
+4 243 256 332 170
+4 217 259 5 171
+4 340 93 365 82
+4 180 285 181 139
+4 71 70 89 72
+4 84 53 12 115
+4 222 79 322 81
+4 58 14 16 13
+4 141 287 282 19
+4 303 45 44 46
+4 230 270 2 104
+4 33 263 37 76
+4 100 53 224 112
+4 112 116 53 7
+4 2 0 3 307
+4 105 280 27 62
+4 319 370 311 381
+4 35 171 33 38
+4 216 259 217 119
+4 337 339 336 263
+4 33 76 91 92
+4 188 37 77 299
+4 132 170 243 245
+4 248 212 249 9
+4 140 120 122 123
+4 181 162 182 139
+4 382 263 339 38
+4 22 116 23 115
+4 267 79 322 388
+4 41 123 125 42
+4 308 85 220 228
+4 284 281 297 278
+4 279 360 292 280
+4 175 112 174 111
+4 348 194 66 199
+4 18 320 149 17
+4 165 257 226 225
+4 77 299 37 76
+4 137 293 198 280
+4 51 53 50 102
+4 351 228 308 57
+4 32 165 12 259
+4 101 50 200 102
+4 184 29 178 153
+4 49 307 0 245
+4 97 64 230 98
+4 211 34 191 186
+4 282 129 18 17
+4 29 31 106 229
+4 136 135 196 197
+4 111 43 225 265
+4 205 263 34 76
+4 79 327 267 80
+4 291 181 352 285
+4 40 123 41 42
+4 67 68 66 199
+4 34 8 73 35
+4 352 285 181 281
+4 40 178 261 229
+4 43 111 225 46
+4 329 294 368 256
+4 383 365 286 148
+4 103 307 106 227
+4 193 158 216 113
+4 191 34 189 92
+4 293 132 103 306
+4 358 57 110 78
+4 140 106 137 229
+4 30 31 29 229
+4 301 106 104 307
+4 358 110 75 78
+4 52 201 224 53
+4 136 322 222 389
+4 342 219 383 390
+4 277 285 284 278
+4 16 51 314 94
+4 267 326 327 386
+4 23 7 116 72
+4 94 252 282 19
+4 356 135 134 81
+4 130 144 182 133
+4 126 41 275 123
+4 112 5 111 113
+4 134 322 136 389
+4 261 163 178 164
+4 203 205 191 92
+4 296 113 89 72
+4 53 114 51 19
+4 322 323 222 389
+4 257 166 210 172
+4 297 170 256 278
+4 105 74 137 31
+4 343 129 219 86
+4 0 103 244 245
+4 301 29 230 31
+4 88 228 87 57
+4 277 284 297 278
+4 81 199 194 82
+4 125 192 295 186
+4 142 117 67 266
+4 91 76 33 119
+4 173 371 69 72
+4 75 54 298 110
+4 166 302 210 172
+4 351 343 333 228
+4 183 29 184 153
+4 25 265 246 225
+4 324 105 138 280
+4 331 76 205 92
+4 53 116 84 7
+4 249 335 382 10
+4 380 326 267 386
+4 209 362 328 210
+4 370 381 319 326
+4 91 76 110 78
+4 218 109 216 118
+4 322 388 79 323
+4 13 343 87 228
+4 15 88 12 13
+4 353 285 354 278
+4 219 287 167 86
+4 240 256 255 278
+4 262 10 207 11
+4 308 358 351 57
+4 32 91 88 92
+4 170 281 180 278
+4 158 46 303 172
+4 352 238 169 181
+4 191 34 205 186
+4 27 96 105 62
+4 191 295 204 186
+4 267 386 79 388
+4 322 79 267 81
+4 73 74 30 31
+4 367 273 359 280
+4 324 103 293 243
+4 114 252 94 19
+4 136 376 134 389
+4 315 62 61 64
+4 8 74 251 9
+4 261 161 184 153
+4 198 180 293 131
+4 326 319 370 320
+4 214 157 212 215
+4 333 83 85 228
+4 33 171 91 119
+4 85 221 220 228
+4 159 193 379 111
+4 73 186 192 42
+4 353 289 366 285
+4 286 148 365 93
+4 10 251 249 9
+4 169 170 297 281
+4 95 146 96 98
+4 182 144 181 133
+4 182 131 130 133
+4 129 86 13 19
+4 141 252 114 19
+4 130 132 47 133
+4 148 82 93 17
+4 127 128 286 129
+4 128 18 286 129
+4 12 51 16 19
+4 314 320 16 17
+4 12 86 167 19
+4 140 192 124 123
+4 8 251 10 9
+4 239 242 368 254
+4 333 219 83 343
+4 217 302 36 171
+4 13 18 235 343
+4 208 330 337 263
+4 257 46 176 225
+4 118 259 216 119
+4 12 5 258 259
+4 304 190 60 88
+4 358 325 75 110
+4 116 7 112 72
+4 23 116 84 115
+4 32 35 165 171
+4 135 52 50 51
+4 51 114 53 102
+4 77 358 75 78
+4 27 315 96 62
+4 326 370 267 320
+4 253 66 65 68
+4 255 256 180 278
+4 209 363 362 210
+4 212 28 157 9
+4 16 129 13 19
+4 32 91 171 259
+4 149 320 327 80
+4 263 35 34 38
+4 0 269 63 104
+4 355 144 152 151
+4 361 87 341 88
+4 75 110 298 299
+4 290 291 289 139
+4 204 295 321 192
+4 297 256 277 278
+4 256 170 180 278
+4 223 111 112 5
+4 266 107 20 116
+4 28 74 95 98
+4 141 65 142 117
+4 362 250 313 210
+4 177 40 39 178
+4 211 137 165 35
+4 82 147 365 340
+4 191 211 189 34
+4 190 15 60 88
+4 111 43 174 175
+4 89 70 371 72
+4 163 151 161 153
+4 37 76 299 119
+4 330 37 337 263
+4 13 129 343 86
+4 54 109 298 110
+4 75 325 54 110
+4 356 52 135 51
+4 180 170 131 133
+4 273 27 226 62
+4 127 343 18 129
+4 27 257 24 225
+4 35 166 165 171
+4 52 314 16 51
+4 39 157 155 215
+4 345 285 353 278
+4 12 52 16 51
+4 198 106 140 306
+4 12 221 258 86
+4 251 263 382 38
+4 308 228 220 57
+4 286 128 342 383
+4 117 67 68 65
+4 73 157 8 186
+4 28 74 8 9
+4 3 0 49 307
+4 68 197 196 102
+4 293 292 256 198
+4 357 299 109 218
+4 179 163 178 153
+4 150 179 272 153
+4 257 46 223 172
+4 176 257 24 210
+4 266 67 196 68
+4 175 43 159 111
+4 235 13 59 18
+4 270 97 232 64
+4 124 121 120 123
+4 299 76 91 119
+4 348 93 340 199
+4 84 90 258 7
+4 91 259 118 119
+4 185 355 150 151
+4 313 35 250 38
+4 37 263 33 38
+4 301 184 183 29
+4 339 263 37 38
+4 231 29 178 215
+4 136 322 134 81
+4 312 362 384 363
+4 293 180 198 256
+4 68 117 114 102
+4 24 210 27 315
+4 158 113 46 172
+4 12 115 53 19
+4 53 115 114 19
+4 30 74 28 31
+4 185 184 317 247
+4 351 87 235 343
+4 124 192 125 123
+4 355 152 150 151
+4 301 104 2 307
+4 12 221 88 259
+4 212 28 213 215
+4 345 198 122 182
+4 211 73 137 74
+4 292 360 198 280
+4 138 105 63 62
+4 219 343 286 129
+4 206 208 336 10
+4 4 259 5 113
+4 377 296 379 193
+4 214 385 155 215
+4 12 15 32 88
+4 147 327 79 80
+4 217 171 5 172
+4 208 337 336 10
+4 183 272 231 153
+4 291 41 177 123
+4 73 40 140 229
+4 363 61 234 315
+4 36 302 339 38
+4 89 113 7 72
+4 30 215 157 42
+4 194 199 340 82
+4 193 160 158 159
+4 91 32 171 33
+4 60 59 235 13
+4 273 280 138 62
+4 363 234 312 316
+4 24 257 27 210
+4 235 18 127 343
+4 373 158 45 303
+4 16 309 58 320
+4 105 103 63 104
+4 287 115 167 19
+4 353 345 122 139
+4 247 161 130 151
+4 291 285 289 139
+4 313 210 250 166
+4 116 115 22 117
+4 0 104 103 307
+4 134 322 314 81
+4 248 251 145 74
+4 94 68 51 197
+4 60 15 58 13
+4 371 70 69 72
+4 248 74 146 98
+4 156 186 295 11
+4 330 263 205 76
+4 36 33 302 38
+4 18 148 286 93
+4 130 161 182 144
+4 94 199 81 82
+4 155 157 214 215
+4 129 17 282 19
+4 189 140 124 202
+4 290 274 288 276
+4 159 377 193 160
+4 182 162 140 139
+4 194 81 222 199
+4 22 21 20 117
+4 58 264 59 13
+4 379 111 296 72
+4 116 117 266 102
+4 66 194 195 199
+4 330 205 331 76
+4 16 94 314 17
+4 253 347 66 68
+4 185 247 47 151
+4 30 28 157 215
+4 2 104 0 307
+4 101 116 102 100
+4 88 91 32 259
+4 124 202 140 123
+4 84 116 23 7
+4 317 2 3 307
+4 321 295 125 192
+4 27 280 226 62
+4 313 382 339 38
+4 96 104 105 64
+4 342 334 219 390
+4 176 61 363 315
+4 26 61 176 315
+4 165 259 32 171
+4 47 144 355 151
+4 30 29 28 215
+4 95 97 230 98
+4 6 5 4 259
+4 157 186 73 42
+4 156 157 39 42
+4 149 82 148 17
+4 191 192 211 186
+4 185 151 150 153
+4 185 150 183 153
+4 96 315 316 64
+4 180 133 181 281
+4 230 97 270 64
+4 40 229 73 42
+4 157 28 212 215
+4 112 111 175 72
+4 88 221 91 259
+4 184 237 106 227
+4 353 285 345 139
+4 105 96 27 74
+4 301 307 317 227
+4 85 55 308 220
+4 77 37 330 76
+4 226 27 137 280
+4 247 237 130 161
+4 174 112 223 111
+4 126 275 124 123
+4 111 113 296 72
+4 329 279 294 280
+4 276 139 121 123
+4 12 88 32 259
+4 112 7 111 72
+4 110 299 91 119
+4 145 251 35 74
+4 332 243 329 256
+4 124 275 121 123
+4 375 205 203 92
+4 178 29 261 229
+4 379 175 72 371
+4 33 263 34 38
+4 56 110 54 57
+4 372 303 44 328
+4 302 166 210 313
+4 250 251 382 38
+4 341 331 305 92
+4 5 259 216 113
+4 68 199 67 197
+4 107 90 23 72
+4 7 113 111 72
+4 172 217 303 158
+4 184 247 185 151
+4 291 276 41 123
+4 141 68 65 117
+4 56 118 110 57
+4 24 26 176 315
+4 87 13 235 343
+4 262 295 205 186
+4 134 314 356 81
+4 279 294 359 329
+4 328 209 384 362
+4 165 35 145 166
+4 237 227 131 306
+4 24 27 26 315
+4 209 176 363 315
+4 27 315 210 146
+4 276 275 41 123
+4 266 116 20 117
+4 248 316 95 146
+4 340 199 93 82
+4 25 349 26 62
+4 96 74 105 98
+4 190 88 304 92
+4 58 18 16 320
+4 316 96 95 146
+4 159 43 45 46
+4 336 263 339 10
+4 217 172 303 302
+4 240 345 353 278
+4 213 212 95 28
+4 309 16 314 320
+4 84 167 23 115
+4 335 207 206 10
+4 159 45 158 46
+4 20 21 142 117
+4 314 94 51 81
+4 196 197 50 102
+4 285 366 284 352
+4 343 228 13 86
+4 34 263 205 186
+4 96 64 97 98
+4 260 21 22 117
+4 258 6 220 221
+4 333 342 219 343
+4 114 117 116 102
+4 180 131 182 133
+4 305 88 341 78
+4 56 4 109 118
+4 107 23 20 116
+4 247 237 184 227
+4 370 58 319 311
+4 147 149 327 80
+4 362 210 363 146
+4 260 287 141 115
+4 303 44 328 210
+4 335 212 154 249
+4 81 80 314 17
+4 39 271 179 178
+4 191 124 189 192
+4 209 362 363 384
+4 19 287 86 167
+4 91 221 88 78
+4 347 93 348 199
+4 205 263 262 186
+4 316 363 234 315
+4 237 306 182 229
+4 342 127 286 343
+4 317 132 47 227
+4 193 379 377 159
+4 221 228 258 86
+4 167 115 12 19
+4 140 139 162 123
+4 344 50 196 197
+4 242 256 240 278
+4 25 273 349 62
+4 341 57 358 78
+4 205 76 34 92
+4 216 113 217 172
+4 95 28 212 9
+4 189 140 211 124
+4 264 236 235 18
+4 183 178 231 29
+4 175 379 72 111
+4 132 131 130 227
+4 302 171 166 38
+4 317 184 301 227
+4 111 5 223 46
+4 333 334 219 342
+4 195 136 196 197
+4 109 299 110 118
+4 157 215 39 42
+4 130 247 47 227
+4 201 223 224 53
+4 244 105 138 324
+4 299 118 218 119
+4 224 53 223 112
+4 163 162 161 144
+4 287 129 282 19
+4 87 343 351 228
+4 124 140 211 192
+4 51 94 16 19
+4 324 293 329 243
+4 314 81 267 80
+4 95 74 28 9
+4 223 46 5 172
+4 291 238 352 181
+4 37 299 218 119
+389
+323
+388
+386
+380
+368
+364
+359
+274
+366
+283
+241
+239
+367
+349
+318
+369
+99
+200
+344
+374
+381
+319
+264
+59
+361
+305
+375
+204
+321
+126
+275
+203
+322
+267
+246
+309
+289
+288
+354
+376
+314
+370
+294
+279
+121
+353
+240
+254
+273
+25
+224
+50
+135
+311
+58
+60
+304
+191
+124
+290
+134
+292
+360
+120
+345
+255
+387
+226
+201
+52
+310
+14
+15
+190
+189
+202
+122
+356
+1000.0 0.45 7.5e4
diff --git a/Examples/Stapling/Data/StaplingDemo.yaml b/Examples/Stapling/Data/StaplingDemo.yaml
new file mode 100644
index 0000000..8f59ade
--- /dev/null
+++ b/Examples/Stapling/Data/StaplingDemo.yaml
@@ -0,0 +1,463 @@
+SurgSim::Framework::Scene:
+ SceneElements:
+ - SurgSim::Framework::BasicSceneElement:
+ Name: StaplingDemoView
+ Components:
+ - SurgSim::Graphics::OsgCamera:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ RenderGroupReference: __OssDefault__
+ Id: 169b057e-e2d7-4f6d-8628-e1b5cf70835d
+ Name: StaplingDemoView Camera
+ ProjectionMatrix: [[2.414213562373095, 0, 0, 0], [0, 2.414213562373095, 0, 0], [0, 0, -1.002002002002002, -0.02002002002002002], [0, 0, -1, 0]]
+ GroupReferences:
+ []
+ DrawAsWireFrame: false
+ AmbientColor: [0, 0, 0, 0]
+ - SurgSim::Framework::PoseComponent:
+ Pose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Name: Pose
+ Id: b7ea8b3d-ef64-4ade-ab89-664390816842
+ - SurgSim::Graphics::OsgView:
+ Position:
+ - 0
+ - 0
+ FullScreen: false
+ Dimensions:
+ - 1024
+ - 768
+ EyeSeparation: 0.06
+ ScreenDistance: 1
+ Camera:
+ SurgSim::Graphics::OsgCamera:
+ Id: 169b057e-e2d7-4f6d-8628-e1b5cf70835d
+ Name: StaplingDemoView Camera
+ WindowBorder: true
+ DisplayType: 0
+ TargetScreen: 0
+ KeyboardDeviceEnabled: true
+ StereoMode: -1
+ ScreenWidth: 0
+ ScreenHeight: 0
+ CameraManipulatorEnabled: true
+ CameraPosition: [0, 0.5, 0.5]
+ CameraLookAt: [0, 0, 0]
+ OsgMapUniforms: false
+ MouseDeviceEnabled: false
+ Id: f79e6280-9905-40b2-bb97-887bb689e605
+ Name: StaplingDemoView View
+ - SurgSim::Framework::BasicSceneElement:
+ Name: arm
+ Components:
+ - SurgSim::Framework::PoseComponent:
+ Pose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, -0.2, 0]
+ Id: 24879435-329d-47c6-a4d5-8f50c0c3e35c
+ Name: Pose
+ - SurgSim::Physics::RigidCollisionRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Name: Collision
+ Id: 16149947-44fa-404f-bfaa-096e7732ee1d
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: false
+ FileName: Geometry/upperarm.osgb
+ Id: 58b991bb-16dc-4fb0-9f0e-b153da9a641d
+ Name: upperarm
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ DrawAsWireFrame: false
+ FileName: Geometry/forearm.osgb
+ Id: b17ff057-4f16-49d2-95c7-ebaacda9ffc0
+ Name: forearm
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: true
+ Filename: Geometry/arm_collision.ply
+ UpdateOptions: 1
+ Id: 4ea6a631-6289-4886-98d2-aa91580e8250
+ Name: ArmOsgMesh
+ - SurgSim::Physics::FixedRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ NumDof: 0
+ IsGravityEnabled: true
+ IsLocalActive: true
+ CollisionRepresentation:
+ SurgSim::Physics::RigidCollisionRepresentation:
+ Id: 16149947-44fa-404f-bfaa-096e7732ee1d
+ Name: Collision
+ RigidRepresentationState:
+ SurgSim::Physics::RigidRepresentationState:
+ LinearVelocity: [0, 0, 0]
+ AngularVelocity: [0, 0, 0]
+ Pose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Id: 0ee73b63-b022-49cf-91b9-465e7da60a1d
+ Name: Physics
+ Density: 0
+ LinearDamping: 0
+ AngularDamping: 0
+ Shape:
+ SurgSim::Math::MeshShape:
+ FileName: Geometry/arm_collision.ply
+ IsDrivingSceneElementPose: true
+ - SurgSim::Framework::BasicSceneElement:
+ Components:
+ - SurgSim::Physics::VirtualToolCoupler:
+ LinearStiffness:
+ HasValue: false
+ Value: Not set
+ LinearDamping:
+ HasValue: false
+ Value: Not set
+ AngularStiffness:
+ HasValue: false
+ Value: Not set
+ AngularDamping:
+ HasValue: false
+ Value: Not set
+ OutputTorqueScaling: 1
+ Input:
+ SurgSim::Input::InputComponent:
+ Id: a0686eb4-795b-4c66-8ecd-3a8b9b72eba4
+ Name: InputComponent
+ OutputForceScaling: 1
+ Representation:
+ SurgSim::Physics::RigidRepresentation:
+ Id: 4e069c9f-4684-4b31-b08e-9e43c48af422
+ Name: Physics
+ Id: 0f65703b-00aa-40d7-82c7-d9e27bc7021b
+ Name: VTC
+ - SurgSim::Framework::PoseComponent:
+ Pose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Id: 886ced11-abc7-4c4d-8086-9f53247c5f83
+ Name: Pose
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: true
+ Filename: Geometry/stapler_collision.ply
+ UpdateOptions: 1
+ Id: b24b8dd6-714f-4cdb-9db4-66ac958076c2
+ Name: StaplerOsgMesh
+ - SurgSim::Physics::RigidRepresentation:
+ Density: 8050
+ LinearDamping: 0
+ AngularDamping: 0
+ Shape:
+ SurgSim::Math::MeshShape:
+ FileName: Geometry/stapler_collision.ply
+ IsDrivingSceneElementPose: true
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ NumDof: 6
+ IsGravityEnabled: false
+ IsLocalActive: true
+ CollisionRepresentation:
+ SurgSim::Physics::RigidCollisionRepresentation:
+ Id: 104ef7db-95fb-4745-b2fd-b81e00abddea
+ Name: Collision
+ RigidRepresentationState:
+ SurgSim::Physics::RigidRepresentationState:
+ LinearVelocity: [0, 0, 0]
+ AngularVelocity: [0, 0, 0]
+ Pose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Id: 4e069c9f-4684-4b31-b08e-9e43c48af422
+ Name: Physics
+ - StaplerBehavior:
+ StapleEnabledSceneElements:
+ - armSceneElement
+ InputComponent:
+ SurgSim::Input::InputComponent:
+ Id: a0686eb4-795b-4c66-8ecd-3a8b9b72eba4
+ Name: InputComponent
+ Representation:
+ SurgSim::Physics::RigidRepresentation:
+ Id: 4e069c9f-4684-4b31-b08e-9e43c48af422
+ Name: Physics
+ VirtualTeeth:
+ - SurgSim::Collision::ShapeCollisionRepresentation:
+ Id: 56a34e38-2875-4f2b-971d-840d2579e902
+ Name: VirtualToothCollision0
+ - SurgSim::Collision::ShapeCollisionRepresentation:
+ Id: ae77a6b2-8735-465d-ace0-c7cb0953ddc7
+ Name: VirtualToothCollision1
+ Id: 8714973d-9d4e-47c5-a0fd-1c607dd63bb8
+ Name: Behavior
+ - SurgSim::Physics::RigidCollisionRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Id: 104ef7db-95fb-4745-b2fd-b81e00abddea
+ Name: Collision
+ - SurgSim::Blocks::VisualizeContactsBehavior:
+ CollisionRepresentation:
+ SurgSim::Physics::RigidCollisionRepresentation:
+ Id: 104ef7db-95fb-4745-b2fd-b81e00abddea
+ Name: Collision
+ VectorFieldScale: 200
+ Id: a7f9ffba-b788-4241-85ff-71e18826e80e
+ Name: VisualizeContactsBehavior
+ - SurgSim::Input::InputComponent:
+ DeviceName: MultiAxisDevice
+ Id: a0686eb4-795b-4c66-8ecd-3a8b9b72eba4
+ Name: InputComponent
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: false
+ FileName: Geometry/stapler_handle.obj
+ Id: 18def2b5-a733-4021-b778-311c47ac5d7e
+ Name: Handle
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: false
+ FileName: Geometry/stapler_indicator.obj
+ Id: fde00dc7-50de-433c-b596-8ee80da75aaa
+ Name: Indicator
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: false
+ FileName: Geometry/stapler_markings.obj
+ Id: 81eae131-70c9-4752-82cf-9debaf12bcf2
+ Name: Markings
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: false
+ FileName: Geometry/stapler_trigger.obj
+ Id: cbc68446-17a6-4d23-aa5f-803b05a779f9
+ Name: Trigger
+ - SurgSim::Collision::ShapeCollisionRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Id: 56a34e38-2875-4f2b-971d-840d2579e902
+ Shape:
+ SurgSim::Math::MeshShape:
+ FileName: Geometry/virtual_staple_1.ply
+ Name: VirtualToothCollision0
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: true
+ Filename: Geometry/virtual_staple_1.ply
+ UpdateOptions: 1
+ Id: 849d2267-4ce4-4580-9cec-b22584d7f05b
+ Name: virtualToothMesh0
+ - SurgSim::Collision::ShapeCollisionRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Shape:
+ SurgSim::Math::MeshShape:
+ FileName: Geometry/virtual_staple_2.ply
+ Id: ae77a6b2-8735-465d-ace0-c7cb0953ddc7
+ Name: VirtualToothCollision1
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: true
+ Filename: Geometry/virtual_staple_2.ply
+ UpdateOptions: 1
+ Id: 156a4707-830e-445b-842c-f65900d6a621
+ Name: virtualToothMesh1
+ Name: stapler
+ - SurgSim::Framework::BasicSceneElement:
+ Name: wound
+ Components:
+ - SurgSim::Framework::PoseComponent:
+ Pose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, -0.2, 0]
+ Id: b147706a-ea62-4e8b-8ebf-fecd478750d6
+ Name: Pose
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: false
+ Filename: Geometry/wound_deformable.ply
+ UpdateOptions: 1
+ Id: ef06d974-74a8-4339-8885-7d42d281b6be
+ Name: Triangle mesh
+ - SurgSim::Physics::Fem3DRepresentation:
+ IsDrivingSceneElementPose: true
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Filename: Geometry/wound_deformable.ply
+ NumDof: 1173
+ IsGravityEnabled: true
+ IsLocalActive: true
+ CollisionRepresentation:
+ SurgSim::Physics::DeformableCollisionRepresentation:
+ Id: 46070a73-9e34-4a9a-bd83-7d4dbf4cfa26
+ Name: Collision
+ IntegrationScheme:
+ SurgSim::Math::IntegrationScheme: INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER
+ Id: 5b21c6c5-9e7a-4b4c-a1dd-521a83bd3658
+ Name: Physics
+ - SurgSim::Physics::DeformableCollisionRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Shape:
+ SurgSim::Math::MeshShape:
+ FileName: Geometry/wound_deformable.ply
+ Id: 46070a73-9e34-4a9a-bd83-7d4dbf4cfa26
+ Name: Collision
+ - SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior:
+ Target:
+ SurgSim::Graphics::OsgMeshRepresentation:
+ Id: ef06d974-74a8-4339-8885-7d42d281b6be
+ Name: Triangle mesh
+ Source:
+ SurgSim::Physics::Fem3DRepresentation:
+ Id: 5b21c6c5-9e7a-4b4c-a1dd-521a83bd3658
+ Name: Physics
+ Id: 2710b639-f40c-4145-a1d6-c59790edc811
+ Name: PhysicsToGraphicalFem
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Visible: true
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: true
+ Filename: Geometry/wound_deformable.ply
+ UpdateOptions: 1
+ Id: dbafb504-0f09-4f13-9713-7d83b41d4329
+ Name: Wire frame
+ - SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior:
+ Target:
+ SurgSim::Graphics::OsgMeshRepresentation:
+ Id: dbafb504-0f09-4f13-9713-7d83b41d4329
+ Name: Wire frame
+ Source:
+ SurgSim::Physics::Fem3DRepresentation:
+ Id: 5b21c6c5-9e7a-4b4c-a1dd-521a83bd3658
+ Name: Physics
+ Id: 4d0d3a03-3846-4619-ae86-823435531c0e
+ Name: PhysicsToWireFrameFem
+ - SurgSim::Framework::BasicSceneElement:
+ Name: SceneElement
+ Components:
+ - SurgSim::Framework::PoseComponent:
+ Pose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Id: c382ef16-f2b5-4c5d-a533-28566efcce7d
+ Name: Pose
+ - SurgSim::Input::InputComponent:
+ DeviceName: Keyboard
+ Id: 616d866d-cc25-443a-a57f-1fcb43e7f376
+ Name: KeyboardInputComponent
+ - SurgSim::Blocks::KeyboardTogglesComponentBehavior:
+ InputComponent:
+ SurgSim::Input::InputComponent:
+ Id: 616d866d-cc25-443a-a57f-1fcb43e7f376
+ Name: KeyboardInputComponent
+ Id: 81660851-2655-4891-9606-7462e3447b40
+ KeyboardRegistry:
+ 97:
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ Id: 81eae131-70c9-4752-82cf-9debaf12bcf2
+ Name: Markings
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ Id: 18def2b5-a733-4021-b778-311c47ac5d7e
+ Name: Handle
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ Id: cbc68446-17a6-4d23-aa5f-803b05a779f9
+ Name: Trigger
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ Id: fde00dc7-50de-433c-b596-8ee80da75aaa
+ Name: Indicator
+ 98:
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ Id: b24b8dd6-714f-4cdb-9db4-66ac958076c2
+ Name: StaplerOsgMesh
+ 99:
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ Id: b17ff057-4f16-49d2-95c7-ebaacda9ffc0
+ Name: forearm
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ Id: 58b991bb-16dc-4fb0-9f0e-b153da9a641d
+ Name: upperarm
+ 100:
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ Id: 4ea6a631-6289-4886-98d2-aa91580e8250
+ Name: ArmOsgMesh
+ 101:
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ Id: ef06d974-74a8-4339-8885-7d42d281b6be
+ Name: Triangle mesh
+ 102:
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ Id: dbafb504-0f09-4f13-9713-7d83b41d4329
+ Name: Wire frame
+ Name: KeyboardBehavior
diff --git a/Examples/Stapling/SerializedStapling.cpp b/Examples/Stapling/SerializedStapling.cpp
new file mode 100644
index 0000000..8cf4974
--- /dev/null
+++ b/Examples/Stapling/SerializedStapling.cpp
@@ -0,0 +1,121 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "Examples/Stapling/StaplerBehavior.h"
+#include "SurgSim/Blocks/KeyboardTogglesComponentBehavior.h"
+#include "SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h"
+#include "SurgSim/Blocks/VisualizeContactsBehavior.h"
+#include "SurgSim/Devices/MultiAxis/MultiAxisDevice.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Input/InputManager.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/VirtualToolCoupler.h"
+
+using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Device::MultiAxisDevice;
+using SurgSim::Framework::BehaviorManager;
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Graphics::OsgManager;
+using SurgSim::Graphics::OsgView;
+using SurgSim::Graphics::OsgViewElement;
+using SurgSim::Input::InputManager;
+using SurgSim::Math::Vector3d;
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Physics::PhysicsManager;
+
+template <typename Type>
+std::shared_ptr<Type> getComponentChecked(std::shared_ptr<SurgSim::Framework::SceneElement> sceneElement,
+ const std::string& name)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component = sceneElement->getComponent(name);
+ SURGSIM_ASSERT(component != nullptr) << "Failed to get Component named '" << name << "'.";
+
+ std::shared_ptr<Type> result = std::dynamic_pointer_cast<Type>(component);
+ SURGSIM_ASSERT(result != nullptr) << "Failed to convert Component to requested type.";
+
+ return result;
+}
+
+int main(int argc, char* argv[])
+{
+ const std::string deviceName = "MultiAxisDevice";
+
+ std::shared_ptr<BehaviorManager> behaviorManager = std::make_shared<BehaviorManager>();
+ std::shared_ptr<OsgManager> graphicsManager = std::make_shared<OsgManager>();
+ std::shared_ptr<InputManager> inputManager = std::make_shared<InputManager>();
+ std::shared_ptr<PhysicsManager> physicsManager = std::make_shared<PhysicsManager>();
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>("config.txt");
+ runtime->addManager(behaviorManager);
+ runtime->addManager(graphicsManager);
+ runtime->addManager(inputManager);
+ runtime->addManager(physicsManager);
+
+ std::shared_ptr<DeviceInterface> device;
+ device = std::make_shared<MultiAxisDevice>(deviceName);
+ if (!device->initialize())
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Could not initialize device " << device->getName() << " for the tool.";
+
+ device = std::make_shared<IdentityPoseDevice>(deviceName);
+ }
+ inputManager->addDevice(device);
+
+ YAML::Node node = YAML::LoadFile("Data/StaplingDemo.yaml");
+
+ runtime->getScene()->decode(node);
+
+ std::shared_ptr<SceneElement> arm = runtime->getScene()->getSceneElement("arm");
+ std::shared_ptr<SceneElement> wound = runtime->getScene()->getSceneElement("wound");
+ std::shared_ptr<SceneElement> stapler = runtime->getScene()->getSceneElement("stapler");
+ std::shared_ptr<SceneElement> view = runtime->getScene()->getSceneElement("StaplingDemoView");
+ auto osgView = std::dynamic_pointer_cast<OsgView>(view->getComponent("StaplingDemoView View"));
+ SURGSIM_ASSERT(nullptr != osgView) << "No OsgView held by SceneElement StaplingDemoView.";
+ inputManager->addDevice(osgView->getKeyboardDevice());
+
+ // Exclude collision between certain Collision::Representations
+ physicsManager->addExcludedCollisionPair(
+ getComponentChecked<SurgSim::Collision::Representation>(stapler, "Collision"),
+ getComponentChecked<SurgSim::Collision::Representation>(stapler, "VirtualToothCollision0"));
+
+ physicsManager->addExcludedCollisionPair(
+ getComponentChecked<SurgSim::Collision::Representation>(stapler, "Collision"),
+ getComponentChecked<SurgSim::Collision::Representation>(stapler, "VirtualToothCollision1"));
+
+ physicsManager->addExcludedCollisionPair(
+ getComponentChecked<SurgSim::Collision::Representation>(wound, "Collision"),
+ getComponentChecked<SurgSim::Collision::Representation>(arm, "Collision"));
+
+ runtime->execute();
+
+ return 0;
+}
diff --git a/Examples/Stapling/StapleElement.cpp b/Examples/Stapling/StapleElement.cpp
new file mode 100644
index 0000000..58936fa
--- /dev/null
+++ b/Examples/Stapling/StapleElement.cpp
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "Examples/Stapling/StapleElement.h"
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+using SurgSim::Graphics::SceneryRepresentation;
+using SurgSim::Graphics::OsgSceneryRepresentation;
+using SurgSim::Math::MeshShape;
+using SurgSim::Physics::RigidCollisionRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+
+StapleElement::StapleElement(const std::string& name) :
+ SurgSim::Framework::BasicSceneElement(name),
+ m_hasCollisionRepresentation(true)
+{
+}
+
+void StapleElement::setHasCollisionRepresentation(bool flag)
+{
+ m_hasCollisionRepresentation = flag;
+}
+
+bool StapleElement::doInitialize()
+{
+ auto meshShape = std::make_shared<MeshShape>();
+ const std::string file = "/Geometry/staple_collision.ply";
+ meshShape->load(file);
+
+ auto physicsRepresentation = std::make_shared<RigidRepresentation>("Physics");
+ physicsRepresentation->setDensity(8050); // Stainless steel (in Kg.m-3)
+ physicsRepresentation->setShape(meshShape);
+ physicsRepresentation->setLinearDamping(1e-2);
+ physicsRepresentation->setAngularDamping(1e-4);
+
+ std::shared_ptr<SceneryRepresentation> graphicsRepresentation =
+ std::make_shared<OsgSceneryRepresentation>("Graphics");
+ graphicsRepresentation->setFileName("Geometry/staple.obj");
+
+ addComponent(physicsRepresentation);
+ addComponent(graphicsRepresentation);
+
+ if (m_hasCollisionRepresentation)
+ {
+ auto collisionRepresentation = std::make_shared<RigidCollisionRepresentation>("Collision");
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+
+ addComponent(collisionRepresentation);
+ }
+
+ return true;
+}
diff --git a/Examples/Stapling/StapleElement.h b/Examples/Stapling/StapleElement.h
new file mode 100644
index 0000000..6bd8bdd
--- /dev/null
+++ b/Examples/Stapling/StapleElement.h
@@ -0,0 +1,44 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef EXAMPLES_STAPLING_STAPLEELEMENT_H
+#define EXAMPLES_STAPLING_STAPLEELEMENT_H
+
+#include <string>
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+
+class StapleElement : public SurgSim::Framework::BasicSceneElement
+{
+public:
+ /// Constructor
+ /// \param name Name of the staple element.
+ explicit StapleElement(const std::string& name);
+
+ /// Specify whether the staple was created with a collision representation.
+ /// \param flag Flag to specify whether the staple was created with a collision representation.
+ void setHasCollisionRepresentation(bool flag);
+
+protected:
+ /// Initialize this scene element
+ /// \return True on success, otherwise false.
+ virtual bool doInitialize() override;
+
+private:
+ /// Flag to specify if the stapleElement was created with a collision representation.
+ bool m_hasCollisionRepresentation;
+};
+
+#endif //EXAMPLES_STAPLING_STAPLEELEMENT_H
diff --git a/Examples/Stapling/StaplerBehavior.cpp b/Examples/Stapling/StaplerBehavior.cpp
new file mode 100644
index 0000000..512579b
--- /dev/null
+++ b/Examples/Stapling/StaplerBehavior.cpp
@@ -0,0 +1,394 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "Examples/Stapling/StaplerBehavior.h"
+
+#include <boost/exception/to_string.hpp>
+
+#include "Examples/Stapling/StapleElement.h"
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/SceneryRepresentation.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintComponent.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+#include "SurgSim/Physics/DeformableRepresentation.h"
+#include "SurgSim/Physics/FixedRepresentationBilateral3D.h"
+#include "SurgSim/Physics/Fem3DRepresentationBilateral3D.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationBilateral3D.h"
+
+using SurgSim::Collision::ContactMapType;
+using SurgSim::Physics::ConstraintImplementation;
+using SurgSim::Physics::FixedRepresentationBilateral3D;
+using SurgSim::Physics::RigidRepresentationBilateral3D;
+using SurgSim::Physics::Fem3DRepresentationBilateral3D;
+using SurgSim::Physics::Localization;
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, StaplerBehavior, StaplerBehavior);
+
+StaplerBehavior::StaplerBehavior(const std::string& name):
+ SurgSim::Framework::Behavior(name),
+ m_numElements(0),
+ m_button1Index(-1),
+ m_button1IndexCached(false),
+ m_buttonPreviouslyPressed(false)
+{
+ typedef std::array<std::shared_ptr<SurgSim::Collision::Representation>, 2> VirtualTeethArray;
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(StaplerBehavior, std::shared_ptr<SurgSim::Framework::Component>,
+ InputComponent, getInputComponent, setInputComponent);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(StaplerBehavior, std::shared_ptr<SurgSim::Framework::Component>,
+ Representation, getRepresentation, setRepresentation);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(StaplerBehavior, VirtualTeethArray, VirtualTeeth,
+ getVirtualTeeth, setVirtualTeeth);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(StaplerBehavior, std::list<std::string>, StapleEnabledSceneElements,
+ getStapleEnabledSceneElements, setStapleEnabledSceneElements);
+}
+
+void StaplerBehavior::setInputComponent(std::shared_ptr<SurgSim::Framework::Component> inputComponent)
+{
+ SURGSIM_ASSERT(nullptr != inputComponent) << "'inputComponent' cannot be set to 'nullptr'";
+
+ m_from = std::dynamic_pointer_cast<SurgSim::Input::InputComponent>(inputComponent);
+
+ SURGSIM_ASSERT(nullptr != m_from) << "'inputComponent' must derive from SurgSim::Input::InputComponent";
+}
+
+std::shared_ptr<SurgSim::Input::InputComponent> StaplerBehavior::getInputComponent()
+{
+ return m_from;
+}
+
+void StaplerBehavior::setRepresentation(std::shared_ptr<SurgSim::Framework::Component> staplerRepresentation)
+{
+ SURGSIM_ASSERT(nullptr != staplerRepresentation) << "'staplerRepresentation' cannot be set to 'nullptr'";
+
+ m_representation = std::dynamic_pointer_cast<SurgSim::Framework::Representation>(staplerRepresentation);
+
+ SURGSIM_ASSERT(nullptr != m_representation)
+ << "'staplerRepresentation' must derive from SurgSim::Framework::Representation";
+}
+
+std::shared_ptr<SurgSim::Framework::Representation> StaplerBehavior::getRepresentation()
+{
+ return m_representation;
+}
+
+void StaplerBehavior::setVirtualTeeth(
+ const std::array<std::shared_ptr<SurgSim::Collision::Representation>, 2>& virtualTeeth)
+{
+ m_virtualTeeth = virtualTeeth;
+}
+
+const std::array<std::shared_ptr<SurgSim::Collision::Representation>, 2>& StaplerBehavior::getVirtualTeeth()
+{
+ return m_virtualTeeth;
+}
+
+void StaplerBehavior::enableStaplingForSceneElement(std::string sceneElementName)
+{
+ m_stapleEnabledSceneElements.push_back(sceneElementName);
+}
+
+void StaplerBehavior::setStapleEnabledSceneElements(const std::list<std::string>& stapleEnabledSceneElements)
+{
+ m_stapleEnabledSceneElements = stapleEnabledSceneElements;
+}
+
+const std::list<std::string>& StaplerBehavior::getStapleEnabledSceneElements()
+{
+ return m_stapleEnabledSceneElements;
+}
+
+void StaplerBehavior::filterCollisionMapForStapleEnabledRepresentations(ContactMapType* collisionsMap)
+{
+ for (auto it = collisionsMap->begin(); it != collisionsMap->end();)
+ {
+ if (std::find(m_stapleEnabledSceneElements.begin(),
+ m_stapleEnabledSceneElements.end(),
+ (*it).first->getSceneElement()->getName()) == m_stapleEnabledSceneElements.end())
+ {
+ // Representation's scene element is not in the m_stapleEnabledSceneElements.
+ it = collisionsMap->erase(it);
+ }
+ else
+ {
+ ++it;
+ }
+ }
+}
+
+std::shared_ptr<SurgSim::Physics::Representation> StaplerBehavior::findCorrespondingPhysicsRepresentation(
+ std::shared_ptr<SurgSim::Collision::Representation> collisionRepresentation)
+{
+ std::shared_ptr<SurgSim::Physics::Representation> physicsRepresentation = nullptr;
+
+ // Check if the collisionRepresenation is for a Rigid body.
+ std::shared_ptr<SurgSim::Physics::RigidCollisionRepresentation> rigidCollisionRepresentation =
+ std::dynamic_pointer_cast<SurgSim::Physics::RigidCollisionRepresentation>(collisionRepresentation);
+
+ if (rigidCollisionRepresentation != nullptr)
+ {
+ physicsRepresentation = rigidCollisionRepresentation->getRigidRepresentation();
+ }
+ else
+ {
+ // Check if the collisionRepresenation is for a deformable body.
+ std::shared_ptr<SurgSim::Physics::DeformableCollisionRepresentation> deformableCollisionRepresentation =
+ std::dynamic_pointer_cast<SurgSim::Physics::DeformableCollisionRepresentation>(collisionRepresentation);
+
+ if (deformableCollisionRepresentation != nullptr)
+ {
+ physicsRepresentation = deformableCollisionRepresentation->getDeformableRepresentation();
+ }
+ }
+
+ return physicsRepresentation;
+}
+
+void StaplerBehavior::filterCollisionMapForSupportedRepresentationTypes(ContactMapType* collisionsMap)
+{
+ for (auto it = collisionsMap->begin(); it != collisionsMap->end();)
+ {
+ if (findCorrespondingPhysicsRepresentation((*it).first) == nullptr)
+ {
+ // Representation type is not supported to be stapled.
+ it = collisionsMap->erase(it);
+ }
+ else
+ {
+ ++it;
+ }
+ }
+}
+
+std::shared_ptr<SurgSim::Physics::Constraint> StaplerBehavior::createBilateral3DConstraint(
+ std::shared_ptr<SurgSim::Physics::Representation> stapleRep,
+ std::shared_ptr<SurgSim::Physics::Representation> otherRep,
+ SurgSim::DataStructures::Location stapleConstraintLocation,
+ SurgSim::DataStructures::Location otherConstraintLocation)
+{
+ // Create a bilateral constraint between the physicsRepresentation and staple.
+ // First find the points where the constraint is going to be applied.
+ std::shared_ptr<Localization> stapleRepLocalization
+ = stapleRep->createLocalization(stapleConstraintLocation);
+ stapleRepLocalization->setRepresentation(stapleRep);
+
+ std::shared_ptr<Localization> otherRepLocatization
+ = otherRep->createLocalization(otherConstraintLocation);
+ otherRepLocatization->setRepresentation(otherRep);
+
+ std::shared_ptr<SurgSim::Physics::Constraint> constraint = nullptr;
+
+ // Create the Constraint with appropriate constraint implementation.
+ switch (otherRep->getType())
+ {
+ case SurgSim::Physics::REPRESENTATION_TYPE_FIXED:
+ constraint = std::make_shared<SurgSim::Physics::Constraint>(
+ std::make_shared<SurgSim::Physics::ConstraintData>(),
+ std::make_shared<RigidRepresentationBilateral3D>(),
+ stapleRepLocalization,
+ std::make_shared<FixedRepresentationBilateral3D>(),
+ otherRepLocatization);
+ break;
+
+ case SurgSim::Physics::REPRESENTATION_TYPE_RIGID:
+ constraint = std::make_shared<SurgSim::Physics::Constraint>(
+ std::make_shared<SurgSim::Physics::ConstraintData>(),
+ std::make_shared<RigidRepresentationBilateral3D>(),
+ stapleRepLocalization,
+ std::make_shared<RigidRepresentationBilateral3D>(),
+ otherRepLocatization);
+ break;
+
+ case SurgSim::Physics::REPRESENTATION_TYPE_FEM3D:
+ constraint = std::make_shared<SurgSim::Physics::Constraint>(
+ std::make_shared<SurgSim::Physics::ConstraintData>(),
+ std::make_shared<RigidRepresentationBilateral3D>(),
+ stapleRepLocalization,
+ std::make_shared<Fem3DRepresentationBilateral3D>(),
+ otherRepLocatization);
+ break;
+
+ default:
+ SURGSIM_FAILURE() << "Stapling constraint not supported for this representation type";
+ break;
+ }
+
+ return constraint;
+}
+
+void StaplerBehavior::createStaple()
+{
+ // Create the staple (not added to the scene right now).
+ auto staple = std::make_shared<StapleElement>("staple_" + boost::to_string(m_numElements++));
+ staple->setPose(m_representation->getPose());
+
+ int toothId = 0;
+ bool stapleAdded = false;
+ for (auto virtualTooth = m_virtualTeeth.cbegin(); virtualTooth != m_virtualTeeth.cend(); ++virtualTooth)
+ {
+ // The virtual tooth could be in contact with any number of objects in the scene.
+ // Get its collisionMap.
+ ContactMapType collisionsMap = *((*virtualTooth)->getCollisions().safeGet());
+
+ // If the virtualTooth has no collision, continue to next loop iteration.
+ if (collisionsMap.empty())
+ {
+ continue;
+ }
+
+ // Remove representations from the collision map that are not enabled to be stapled.
+ filterCollisionMapForStapleEnabledRepresentations(&collisionsMap);
+
+ // If the collision map is emptied after filtering, continue to next loop iteration.
+ if (collisionsMap.empty())
+ {
+ continue;
+ }
+
+ // Filter the map based on supported Physics::Represention types.
+ filterCollisionMapForSupportedRepresentationTypes(&collisionsMap);
+
+ // If the collision map is emptied after filtering, continue to next loop iteration.
+ if (collisionsMap.empty())
+ {
+ continue;
+ }
+
+ // Find the row (representation, list of contacts) in the map that the virtualTooth has most
+ // collision pairs with.
+ ContactMapType::value_type targetRepresentationContacts
+ = *std::max_element(collisionsMap.begin(), collisionsMap.end(),
+ [](const ContactMapType::value_type& lhs, const ContactMapType::value_type& rhs)
+ { return lhs.second.size() < rhs.second.size(); });
+
+ // Iterate through the list of collision pairs to find a contact with the deepest penetration.
+ std::shared_ptr<SurgSim::Collision::Contact> targetContact
+ = *std::max_element(targetRepresentationContacts.second.begin(), targetRepresentationContacts.second.end(),
+ [](const std::shared_ptr<SurgSim::Collision::Contact>& lhs,
+ const std::shared_ptr<SurgSim::Collision::Contact>& rhs)
+ { return lhs->depth < rhs->depth; });
+
+ // Create the staple, before creating the constraint with the staple.
+ // The staple is created with no collision representation, because it is going to be constrained.
+ if (!stapleAdded)
+ {
+ staple->setHasCollisionRepresentation(false);
+ getScene()->addSceneElement(staple);
+ // The gravity of the staple is disabled to prevent it from rotating about the line
+ // connecting the two points of constraints on the staple.
+ staple->getComponents<SurgSim::Physics::Representation>()[0]->setIsGravityEnabled(false);
+ stapleAdded = true;
+ }
+
+ // Find the corresponding Phsyics::Representation for the target Collision::Representation.
+ // Note that the targetPhysicsRepresentation will NOT be a nullptr, because the
+ // collisionsMap was filtered earlier to remove Representations that returned nullptr
+ // when the function findCorrespondingPhysicsRepresentation was called.
+ // (see filterCollisionMapForStapleEnabledRepresentations above).
+ std::shared_ptr<SurgSim::Physics::Representation> targetPhysicsRepresentation =
+ findCorrespondingPhysicsRepresentation(targetRepresentationContacts.first);
+
+ // The constraint is created at the contact point in targetContact->penetrationPoints.second.
+ // Convert this location to stapleRepresentation.
+ auto stapleRepresentation = staple->getComponents<SurgSim::Physics::Representation>()[0];
+ SurgSim::DataStructures::Location stapleConstraintLocation(SurgSim::Math::Vector3d(
+ stapleRepresentation->getPose().inverse() * targetPhysicsRepresentation->getPose() *
+ targetContact->penetrationPoints.second.rigidLocalPosition.getValue()));
+
+ // Create a bilateral constraint between the targetPhysicsRepresentation and the staple.
+ std::shared_ptr<SurgSim::Physics::Constraint> constraint =
+ createBilateral3DConstraint(staple->getComponents<SurgSim::Physics::Representation>()[0],
+ targetPhysicsRepresentation, stapleConstraintLocation,
+ targetContact->penetrationPoints.second);
+
+ if (constraint == nullptr)
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Failed to create constraint between staple and "
+ << targetRepresentationContacts.first->getSceneElement()->getName()
+ << ". This might be because the createBilateral3DConstraint is not supporting the Physics Type: "
+ << targetPhysicsRepresentation->getType();
+ continue;
+ }
+
+ // Create a component to store this constraint.
+ std::shared_ptr<SurgSim::Physics::ConstraintComponent> constraintComponent =
+ std::make_shared<SurgSim::Physics::ConstraintComponent>(
+ "Bilateral3DConstraint" + boost::to_string(toothId++));
+
+ constraintComponent->setConstraint(constraint);
+ staple->addComponent(constraintComponent);
+ }
+
+ if (!stapleAdded)
+ {
+ // Create the staple element.
+ staple->setHasCollisionRepresentation(true);
+ getScene()->addSceneElement(staple);
+ }
+}
+
+void StaplerBehavior::update(double dt)
+{
+ SurgSim::DataStructures::DataGroup dataGroup;
+ m_from->getData(&dataGroup);
+
+ // Get the button1 index.
+ if (!m_button1IndexCached)
+ {
+ m_button1Index = dataGroup.booleans().getIndex("button1");
+ m_button1IndexCached = true;
+ }
+
+ // Check if the stapler is being pressed.
+ bool button1 = false;
+ dataGroup.booleans().get(m_button1Index, &button1);
+
+ if (button1 && !m_buttonPreviouslyPressed)
+ {
+ createStaple();
+ }
+
+ m_buttonPreviouslyPressed = button1;
+}
+
+int StaplerBehavior::getTargetManagerType() const
+{
+ return SurgSim::Framework::MANAGER_TYPE_PHYSICS;
+}
+
+bool StaplerBehavior::doInitialize()
+{
+ SURGSIM_ASSERT(m_from) << "StaplerBehavior: no InputComponent held.";
+ SURGSIM_ASSERT((m_virtualTeeth[0] != nullptr) && (m_virtualTeeth[1] != nullptr)) <<
+ "StaplerBehavior: setVirtualStaple was not called, or it was passed nullptr for a Collision Representation.";
+ return true;
+}
+
+bool StaplerBehavior::doWakeUp()
+{
+ return true;
+}
diff --git a/Examples/Stapling/StaplerBehavior.h b/Examples/Stapling/StaplerBehavior.h
new file mode 100644
index 0000000..549ad78
--- /dev/null
+++ b/Examples/Stapling/StaplerBehavior.h
@@ -0,0 +1,181 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef EXAMPLES_STAPLING_STAPLERBEHAVIOR_H
+#define EXAMPLES_STAPLING_STAPLERBEHAVIOR_H
+
+#include <array>
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Collision/Representation.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+struct Location;
+}
+
+namespace Framework
+{
+class Representation;
+}
+
+namespace Graphics
+{
+class SceneryRepresentation;
+}
+
+namespace Input
+{
+class InputComponent;
+}
+
+namespace Physics
+{
+class Constraint;
+}
+
+}
+
+SURGSIM_STATIC_REGISTRATION(StaplerBehavior);
+
+/// This behavior is used to add staples.
+/// The stapler is controlled by an input device and when
+/// the user pushes a button on the device, a stapler will be deployed from the stapler.
+class StaplerBehavior: public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit StaplerBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(StaplerBehavior);
+
+ /// Set the input component from which to get the pose
+ /// \param inputComponent The input component which sends the pose.
+ void setInputComponent(std::shared_ptr<SurgSim::Framework::Component> inputComponent);
+
+ /// \return The input component which sends the pose.
+ std::shared_ptr<SurgSim::Input::InputComponent> getInputComponent();
+
+ /// Set the representation of the stapler
+ /// \param staplerRepresentation The representation of a stapler
+ void setRepresentation(std::shared_ptr<SurgSim::Framework::Component> staplerRepresentation);
+
+ /// \return The representation of a stapler
+ std::shared_ptr<SurgSim::Framework::Representation> getRepresentation();
+
+ /// Update the behavior
+ /// \param dt The length of time (seconds) between update calls.
+ virtual void update(double dt) override;
+
+ /// Return the type of manager that should be responsible for this behavior
+ /// \return An integer indicating which manger should be responsible for this behavior.
+ virtual int getTargetManagerType() const override;
+
+ /// Sets the virtual teeth for the virtual staple
+ /// \param virtualTeeth Array of collision representations for the virtual stapler teeth.
+ void setVirtualTeeth(const std::array<std::shared_ptr<SurgSim::Collision::Representation>, 2>& virtualTeeth);
+
+ /// \return Array of collision representations for the virtual stapler teeth.
+ const std::array<std::shared_ptr<SurgSim::Collision::Representation>, 2>& getVirtualTeeth();
+
+ /// Add a scene element (name) for which stapling is enabled within this behaviour.
+ /// \param sceneElementName The name of the scene element that this behaviour can staple.
+ void enableStaplingForSceneElement(std::string sceneElementName);
+
+ /// Sets the list of scene element names that this behaviour can staple
+ /// \param stapleEnabledSceneElements List of scene element names that this behaviour can staple.
+ void setStapleEnabledSceneElements(const std::list<std::string>& stapleEnabledSceneElements);
+
+ /// \return List of scene element names that this behaviour can staple.
+ const std::list<std::string>& getStapleEnabledSceneElements();
+
+protected:
+ /// Initialize this behavior
+ /// \return True on success, otherwise false.
+ /// \note: In current implementation, this method always returns "true".
+ virtual bool doInitialize() override;
+
+ /// Wakeup this behavior
+ /// \return True on success, otherwise false.
+ /// \note: In current implementation, this method always returns "true".
+ virtual bool doWakeUp() override;
+
+private:
+ /// Given a collision map, remove entries whose representations are not part of
+ /// enabled scene element lists.
+ /// \param [in,out] collisionsMap The collision map to be filtered.
+ void filterCollisionMapForStapleEnabledRepresentations(SurgSim::Collision::ContactMapType *collisionsMap);
+
+ /// Given a Collision::Representation, get the corresponding Physics::Representation.
+ /// \param collisionRepresentation shared_ptr to the collision representation.
+ /// \return The shared_ptr of the Physics::Representation. Can be nullptr.
+ std::shared_ptr<SurgSim::Physics::Representation> findCorrespondingPhysicsRepresentation(
+ std::shared_ptr<SurgSim::Collision::Representation> collisionRepresentation);
+
+ /// Given a collision map, remove entries whose representations are not supported to be stapled to.
+ /// \param [in,out] collisionsMap The collision map to be filtered.
+ void filterCollisionMapForSupportedRepresentationTypes(SurgSim::Collision::ContactMapType* collisionsMap);
+
+ /// Create a bilateral constraint given two Physics::Representation and a constraint (global) location.
+ /// \param stapleRep The physics representation of the staple element. This is known to be RigidRepresentation.
+ /// \param otherRep The physics representation of the object stapled to. This could be Rigid, Fixed or Fem3D.
+ /// \param stapleConstraintLocation The location where the constraint is created on the staple.
+ /// \param otherConstraintLocation The location where the constraint is created on the object staples to.
+ /// \return The shared_ptr of the constraint created.
+ std::shared_ptr<SurgSim::Physics::Constraint> createBilateral3DConstraint(
+ std::shared_ptr<SurgSim::Physics::Representation> stapleRep,
+ std::shared_ptr<SurgSim::Physics::Representation> otherRep,
+ SurgSim::DataStructures::Location stapleConstraintLocation,
+ SurgSim::DataStructures::Location otherConstraintLocation);
+
+ /// Function to create the staple element.
+ /// \note This function also checks for collision with stapling enabled objects in the scene to create
+ /// bilateral constraint between the staple element and the object.
+ void createStaple();
+
+ /// Input component from which to get the pose.
+ std::shared_ptr<SurgSim::Input::InputComponent> m_from;
+
+ /// The representation of the stapler.
+ std::shared_ptr<SurgSim::Framework::Representation> m_representation;
+
+ /// The number of staples added
+ int m_numElements;
+
+ /// The NamedData index for the button1.
+ int m_button1Index;
+
+ /// Flag for caching the the NamedData button1Index.
+ bool m_button1IndexCached;
+
+ /// Used to record if a button was previously pressed
+ bool m_buttonPreviouslyPressed;
+
+ /// Contains the teeth for detecting collisions
+ std::array<std::shared_ptr<SurgSim::Collision::Representation>, 2> m_virtualTeeth;
+
+ /// The list of scene element names that this behavior can staple.
+ std::list<std::string> m_stapleEnabledSceneElements;
+};
+
+#endif // EXAMPLES_STAPLING_STAPLERBEHAVIOR_H
diff --git a/Examples/Stapling/Stapling.cpp b/Examples/Stapling/Stapling.cpp
new file mode 100644
index 0000000..6b8fcd7
--- /dev/null
+++ b/Examples/Stapling/Stapling.cpp
@@ -0,0 +1,486 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+
+#include "Examples/Stapling/StaplerBehavior.h"
+#include "SurgSim/Blocks/KeyboardTogglesComponentBehavior.h"
+#include "SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h"
+#include "SurgSim/Blocks/VisualizeContactsBehavior.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+#include "SurgSim/Devices/Keyboard/KeyCode.h"
+#include "SurgSim/Devices/MultiAxis/MultiAxisDevice.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Graphics/MeshRepresentation.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Input/InputManager.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/VirtualToolCoupler.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Graphics/MeshPlyReaderDelegate.h"
+
+using SurgSim::Blocks::KeyboardTogglesComponentBehavior;
+using SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior;
+using SurgSim::Blocks::VisualizeContactsBehavior;
+using SurgSim::Collision::ShapeCollisionRepresentation;
+using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Device::MultiAxisDevice;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Framework::BehaviorManager;
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Graphics::MeshRepresentation;
+using SurgSim::Graphics::SceneryRepresentation;
+using SurgSim::Graphics::OsgManager;
+using SurgSim::Graphics::OsgMeshRepresentation;
+using SurgSim::Graphics::OsgViewElement;
+using SurgSim::Graphics::OsgSceneryRepresentation;
+using SurgSim::Math::MeshShape;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationMatrix;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Vector4f;
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Input::InputComponent;
+using SurgSim::Input::InputManager;
+using SurgSim::Physics::DeformableCollisionRepresentation;
+using SurgSim::Physics::Fem3DRepresentation;
+using SurgSim::Physics::FixedRepresentation;
+using SurgSim::Physics::PhysicsManager;
+using SurgSim::Physics::RigidCollisionRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+using SurgSim::Physics::VirtualToolCoupler;
+
+static std::shared_ptr<SurgSim::Framework::SceneElement> createFemSceneElement(
+ const std::string& name,
+ const std::string& filename,
+ SurgSim::Math::IntegrationScheme integrationScheme,
+ std::shared_ptr<SurgSim::Graphics::OsgMaterial> material)
+{
+ // Create a SceneElement that bundles the pieces associated with the finite element model
+ std::shared_ptr<SceneElement> sceneElement = std::make_shared<BasicSceneElement>(name);
+
+ // Set the file name which contains the tetrahedral mesh. File will be loaded by 'doInitialize()' call.
+ std::shared_ptr<Fem3DRepresentation> physicsRepresentation = std::make_shared<Fem3DRepresentation>("Physics");
+ physicsRepresentation->setFilename(filename);
+ physicsRepresentation->setIntegrationScheme(integrationScheme);
+ sceneElement->addComponent(physicsRepresentation);
+
+ // Load the surface triangle mesh of the finite element model
+ auto meshShape = std::make_shared<MeshShape>();
+ meshShape->load(filename);
+
+ // Create a triangle mesh for visualizing the surface of the finite element model
+ auto graphicalFem = std::make_shared<OsgMeshRepresentation>("Graphics");
+ graphicalFem->setFilename(filename);
+ sceneElement->addComponent(graphicalFem);
+
+ // Create material to transport the Textures
+ graphicalFem->setMaterial(material);
+ sceneElement->addComponent(material);
+
+ // Create the collision mesh for the surface of the finite element model
+ auto collisionRepresentation = std::make_shared<DeformableCollisionRepresentation>("Collision");
+ collisionRepresentation->setShape(meshShape);
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+ sceneElement->addComponent(collisionRepresentation);
+
+ // Create a behavior which transfers the position of the vertices in the FEM to locations in the triangle mesh
+ auto physicsToGraphicalFem = std::make_shared<TransferPhysicsToGraphicsMeshBehavior>("PhysicsToGraphicalFem");
+ physicsToGraphicalFem->setSource(physicsRepresentation);
+ physicsToGraphicalFem->setTarget(graphicalFem);
+ sceneElement->addComponent(physicsToGraphicalFem);
+
+ // WireFrame of the finite element model
+ std::shared_ptr<SurgSim::Graphics::MeshRepresentation> wireFrameFem
+ = std::make_shared<SurgSim::Graphics::OsgMeshRepresentation>("Wire Frame");
+ wireFrameFem->setFilename(filename);
+ wireFrameFem->setDrawAsWireFrame(true);
+ wireFrameFem->setLocalActive(false);
+ sceneElement->addComponent(wireFrameFem);
+
+ // Behavior transfers the position of the physics representation to wire frame representation of the fem.
+ auto physicsToWireFrameFem = std::make_shared<TransferPhysicsToGraphicsMeshBehavior>("PhysicsToWireFrameFem");
+ physicsToWireFrameFem->setSource(physicsRepresentation);
+ physicsToWireFrameFem->setTarget(wireFrameFem);
+ sceneElement->addComponent(physicsToWireFrameFem);
+
+ return sceneElement;
+}
+
+/// Load scenery object from file
+/// \param name Name of this scenery representation.
+/// \param fileName Name of the file from which the scenery representation will be loaded.
+/// \return A scenery representation.
+std::shared_ptr<SceneryRepresentation> createSceneryObject(const std::string& name, const std::string& fileName)
+{
+ std::shared_ptr<SceneryRepresentation> sceneryRepresentation = std::make_shared<OsgSceneryRepresentation>(name);
+ sceneryRepresentation->setFileName(fileName);
+ return sceneryRepresentation;
+}
+
+std::shared_ptr<SceneElement> createStaplerSceneElement(const std::string& staplerName, const std::string& deviceName)
+{
+ const std::string filename = std::string("Geometry/stapler_collision.ply");
+
+ // Stapler collision mesh
+ auto meshShapeForCollision = std::make_shared<MeshShape>();
+ meshShapeForCollision->load(filename);
+
+ std::shared_ptr<MeshRepresentation> meshShapeVisualization =
+ std::make_shared<OsgMeshRepresentation>("Collision Mesh");
+ meshShapeVisualization->setFilename(filename);
+ meshShapeVisualization->setDrawAsWireFrame(true);
+ meshShapeVisualization->setLocalActive(false);
+
+ std::shared_ptr<RigidRepresentation> physicsRepresentation = std::make_shared<RigidRepresentation>("Physics");
+ physicsRepresentation->setIsGravityEnabled(false);
+ physicsRepresentation->setDensity(8050); // Stainless steel (in Kg.m-3)
+ physicsRepresentation->setShape(meshShapeForCollision);
+
+
+ std::shared_ptr<RigidCollisionRepresentation> collisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("Collision");
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+
+ std::shared_ptr<InputComponent> inputComponent = std::make_shared<InputComponent>("InputComponent");
+ inputComponent->setDeviceName(deviceName);
+
+ std::shared_ptr<VirtualToolCoupler> inputVTC = std::make_shared<VirtualToolCoupler>("VTC");
+ inputVTC->setInput(inputComponent);
+ inputVTC->setRepresentation(physicsRepresentation);
+ inputVTC->overrideAttachmentPoint(Vector3d::Zero());
+ inputVTC->setCalculateInertialTorques(false);
+
+ // A stapler behavior controls the release of stale when a button is pushed on the device.
+ // Also, it is aware of collisions of the stapler.
+ std::shared_ptr<StaplerBehavior> staplerBehavior = std::make_shared<StaplerBehavior>("Behavior");
+ staplerBehavior->setInputComponent(inputComponent);
+ staplerBehavior->setRepresentation(physicsRepresentation);
+ staplerBehavior->enableStaplingForSceneElement("wound");
+
+ std::shared_ptr<VisualizeContactsBehavior> visualizeContactsBehavior =
+ std::make_shared<VisualizeContactsBehavior>("Contacts");
+ visualizeContactsBehavior->setCollisionRepresentation(collisionRepresentation);
+ // Note: Since usually the penetration depth of a collision is so small (at the magnitude of mm),
+ // if we use the depth as the length of vector, the vector field will be too small to be seen on the screen.
+ // Thus, we enlarge the vector field by 200 times.
+ visualizeContactsBehavior->setVectorFieldScale(200);
+ visualizeContactsBehavior->setLocalActive(false);
+
+ std::shared_ptr<SceneElement> sceneElement = std::make_shared<BasicSceneElement>(staplerName);
+ sceneElement->addComponent(physicsRepresentation);
+ sceneElement->addComponent(collisionRepresentation);
+ sceneElement->addComponent(meshShapeVisualization);
+ sceneElement->addComponent(inputComponent);
+ sceneElement->addComponent(inputVTC);
+ sceneElement->addComponent(staplerBehavior);
+ sceneElement->addComponent(visualizeContactsBehavior);
+
+ // Load the graphical parts of a stapler.
+ sceneElement->addComponent(createSceneryObject("Handle", "Geometry/stapler_handle.obj"));
+ sceneElement->addComponent(createSceneryObject("Indicator", "Geometry/stapler_indicator.obj"));
+ sceneElement->addComponent(createSceneryObject("Markings", "Geometry/stapler_markings.obj"));
+ sceneElement->addComponent(createSceneryObject("Trigger", "Geometry/stapler_trigger.obj"));
+
+ auto meshShapeForVirtualStaple1 = std::make_shared<MeshShape>();
+ auto meshShapeForVirtualStaple2 = std::make_shared<MeshShape>();
+ meshShapeForVirtualStaple1->load("Geometry/virtual_staple_1.ply");
+ meshShapeForVirtualStaple2->load("Geometry/virtual_staple_2.ply");
+
+ std::vector<std::shared_ptr<MeshShape>> virtualTeethShapes;
+ virtualTeethShapes.push_back(meshShapeForVirtualStaple1);
+ virtualTeethShapes.push_back(meshShapeForVirtualStaple2);
+
+ int i = 0;
+ std::array<std::shared_ptr<SurgSim::Collision::Representation>, 2> virtualTeeth;
+ for (auto it = std::begin(virtualTeethShapes); it != std::end(virtualTeethShapes); ++it, ++i)
+ {
+ std::shared_ptr<ShapeCollisionRepresentation> virtualToothCollision =
+ std::make_shared<ShapeCollisionRepresentation>("VirtualToothCollision" + std::to_string(i));
+ virtualToothCollision->setShape(*it);
+ virtualToothCollision->setLocalPose(RigidTransform3d::Identity());
+
+ virtualTeeth[i] = virtualToothCollision;
+ sceneElement->addComponent(virtualToothCollision);
+ }
+
+ staplerBehavior->setVirtualTeeth(virtualTeeth);
+
+ return sceneElement;
+}
+
+std::shared_ptr<SceneElement> createArmSceneElement(
+ const std::string& armName,
+ std::shared_ptr<SurgSim::Graphics::OsgMaterial> material)
+{
+ const std::string filename = std::string("Geometry/arm_collision.ply");
+
+ // Graphic representation for arm
+ std::shared_ptr<SceneryRepresentation> forearmSceneryRepresentation =
+ createSceneryObject("Forearm", "Geometry/forearm.osgb");
+ forearmSceneryRepresentation->setMaterial(material);
+ std::shared_ptr<SceneryRepresentation> upperarmSceneryRepresentation =
+ createSceneryObject("Upperarm", "Geometry/upperarm.osgb");
+ upperarmSceneryRepresentation->setMaterial(material);
+
+ // Arm collision mesh
+ std::shared_ptr<MeshShape> meshShape = std::make_shared<MeshShape>();
+ meshShape->load(filename);
+
+ // Visualization of arm collision mesh
+ std::shared_ptr<MeshRepresentation> meshShapeVisualization =
+ std::make_shared<OsgMeshRepresentation>("Collision Mesh");
+ meshShapeVisualization->setFilename(filename);
+ meshShapeVisualization->setDrawAsWireFrame(true);
+ meshShapeVisualization->setLocalActive(false);
+
+ std::shared_ptr<FixedRepresentation> physicsRepresentation = std::make_shared<FixedRepresentation>("Physics");
+ physicsRepresentation->setShape(meshShape);
+
+ std::shared_ptr<RigidCollisionRepresentation> collisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("Collision");
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+
+ std::shared_ptr<SceneElement> armSceneElement = std::make_shared<BasicSceneElement>(armName);
+ armSceneElement->addComponent(forearmSceneryRepresentation);
+ armSceneElement->addComponent(meshShapeVisualization);
+ armSceneElement->addComponent(upperarmSceneryRepresentation);
+ armSceneElement->addComponent(collisionRepresentation);
+ armSceneElement->addComponent(physicsRepresentation);
+ armSceneElement->addComponent(material);
+
+ return armSceneElement;
+}
+
+template <typename Type>
+std::shared_ptr<Type> getComponentChecked(std::shared_ptr<SurgSim::Framework::SceneElement> sceneElement,
+ const std::string& name)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component = sceneElement->getComponent(name);
+ SURGSIM_ASSERT(component != nullptr) << "Failed to get Component named '" << name << "'.";
+
+ std::shared_ptr<Type> result = std::dynamic_pointer_cast<Type>(component);
+ SURGSIM_ASSERT(result != nullptr) << "Failed to convert Component to requested type.";
+
+ return result;
+}
+
+std::shared_ptr<OsgViewElement> createViewElement()
+{
+ auto result = std::make_shared<OsgViewElement>("StaplingDemoView");
+ result->enableManipulator(true);
+ result->setManipulatorParameters(Vector3d(0.0, 0.5, 0.5), Vector3d::Zero());
+ result->enableKeyboardDevice(true);
+
+ auto light = std::make_shared<SurgSim::Graphics::OsgLight>("Light");
+ light->setDiffuseColor(Vector4d(1.0, 1.0, 1.0, 1.0));
+ light->setSpecularColor(Vector4d(1.0, 1.0, 1.0, 1.0));
+ result->addComponent(light);
+
+ result->getCamera()->setAmbientColor(Vector4d(0.2, 0.2, 0.2, 1.0));
+
+ return result;
+}
+
+std::shared_ptr<SurgSim::Graphics::OsgMaterial> createShinyMaterial(
+ const SurgSim::Framework::ApplicationData& data,
+ std::shared_ptr<SurgSim::Graphics::OsgShader> shader,
+ std::string defaultTextureName = "Textures/checkered.png")
+{
+ // Default Material with shader
+ // using scopes to keep from having to introduce new variables with different types
+ auto material = std::make_shared<SurgSim::Graphics::OsgMaterial>("shiny");
+ material->setShader(shader);
+
+ {
+ auto uniform = std::make_shared<SurgSim::Graphics::OsgUniform<Vector4f>>("diffuseColor");
+ material->addUniform(uniform);
+ material->setValue("diffuseColor", SurgSim::Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+ }
+
+ {
+ auto uniform = std::make_shared<SurgSim::Graphics::OsgUniform<Vector4f>>("specularColor");
+ material->addUniform(uniform);
+ material->setValue("specularColor", SurgSim::Math::Vector4f(0.01, 0.01, 0.01, 1.0));
+ }
+
+ {
+ auto uniform = std::make_shared<SurgSim::Graphics::OsgUniform<float>>("shininess");
+ material->addUniform(uniform);
+ material->setValue("shininess", 32.0f);
+ }
+
+ std::string blackTexture;
+ SURGSIM_ASSERT(data.tryFindFile("Textures/black.png", &blackTexture));
+
+ std::string defaultTexture;
+ SURGSIM_ASSERT(data.tryFindFile(defaultTextureName, &defaultTexture));
+
+ {
+ // As a default color for the texture map use white
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ texture->loadImage(defaultTexture);
+ auto uniform =
+ std::make_shared<SurgSim::Graphics::OsgTextureUniform<SurgSim::Graphics::OsgTexture2d>>("diffuseMap");
+ uniform->set(texture);
+ material->addUniform(uniform);
+ }
+
+ {
+ // The neutral color for the shadow map is black
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ texture->loadImage(blackTexture);
+ auto uniform =
+ std::make_shared<SurgSim::Graphics::OsgTextureUniform<SurgSim::Graphics::OsgTexture2d>>("shadowMap");
+ uniform->set(texture);
+ uniform->setMinimumTextureUnit(8);
+ material->addUniform(uniform);
+ }
+
+ return material;
+}
+
+int main(int argc, char* argv[])
+{
+ const std::string deviceName = "MultiAxisDevice";
+
+ std::shared_ptr<BehaviorManager> behaviorManager = std::make_shared<BehaviorManager>();
+ std::shared_ptr<OsgManager> graphicsManager = std::make_shared<OsgManager>();
+ std::shared_ptr<InputManager> inputManager = std::make_shared<InputManager>();
+ std::shared_ptr<PhysicsManager> physicsManager = std::make_shared<PhysicsManager>();
+ physicsManager->setRate(100.0);
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>("config.txt");
+ runtime->addManager(behaviorManager);
+ runtime->addManager(graphicsManager);
+ runtime->addManager(inputManager);
+ runtime->addManager(physicsManager);
+
+ std::shared_ptr<DeviceInterface> device;
+ device = std::make_shared<MultiAxisDevice>(deviceName);
+ if (!device->initialize())
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Could not initialize device " << device->getName() << " for the tool.";
+
+ device = std::make_shared<IdentityPoseDevice>(deviceName);
+ }
+ inputManager->addDevice(device);
+
+ std::shared_ptr<OsgViewElement> view = createViewElement();
+ inputManager->addDevice(view->getKeyboardDevice());
+
+ // Shader should be shared between all materials using the same shader
+ auto shader = SurgSim::Graphics::loadShader(*runtime->getApplicationData(), "Shaders/ds_mapping_material");
+ SURGSIM_ASSERT(shader != nullptr) << "Shader could not be loaded.";
+
+ RigidTransform3d armPose = makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, -0.2, 0.0));
+ auto material = createShinyMaterial(*runtime->getApplicationData(), shader);
+ std::shared_ptr<SceneElement> arm = createArmSceneElement("arm", material);
+ arm->setPose(armPose);
+
+ std::shared_ptr<SceneElement> stapler = createStaplerSceneElement("stapler", deviceName);
+ stapler->setPose(RigidTransform3d::Identity());
+
+ std::string woundFilename = std::string("Geometry/wound_deformable.ply");
+ // Mechanical properties are based on Liang and Boppart, "Biomechanical Properties of In Vivo Human Skin From
+ // Dynamic Optical Coherence Elastography", IEEE Transactions on Biomedical Engineering, Vol 57, No 4.
+
+
+ // Material for the wound
+ material = createShinyMaterial(*runtime->getApplicationData(), shader, "Geometry/wound.png");
+
+ std::shared_ptr<SceneElement> wound =
+ createFemSceneElement("wound",
+ woundFilename,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER,
+ material);
+ wound->setPose(armPose);
+
+ std::shared_ptr<InputComponent> keyboardComponent = std::make_shared<InputComponent>("KeyboardInputComponent");
+ keyboardComponent->setDeviceName("Keyboard"); // Name of device is case sensitive.
+ std::shared_ptr<KeyboardTogglesComponentBehavior> keyboardBehavior =
+ std::make_shared<KeyboardTogglesComponentBehavior>("KeyboardBehavior");
+ keyboardBehavior->setInputComponent(keyboardComponent);
+
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_A, stapler->getComponent("Handle"));
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_A, stapler->getComponent("Indicator"));
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_A, stapler->getComponent("Markings"));
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_A, stapler->getComponent("Trigger"));
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_B, stapler->getComponent("Contacts"));
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_C, stapler->getComponent("Collision Mesh"));
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_D, arm->getComponent("Forearm"));
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_D, arm->getComponent("Upperarm"));
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_E, arm->getComponent("Collision Mesh"));
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_F, wound->getComponent("Graphics"));
+ keyboardBehavior->registerKey(SurgSim::Device::KeyCode::KEY_G, wound->getComponent("Wire Frame"));
+
+ std::shared_ptr<SceneElement> keyboard = std::make_shared<BasicSceneElement>("SceneElement");
+ keyboard->addComponent(keyboardComponent);
+ keyboard->addComponent(keyboardBehavior);
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+ scene->addSceneElement(view);
+ scene->addSceneElement(arm);
+ scene->addSceneElement(stapler);
+ scene->addSceneElement(wound);
+ scene->addSceneElement(keyboard);
+
+ // Exclude collision between certain Collision::Representations
+ physicsManager->addExcludedCollisionPair(
+ getComponentChecked<SurgSim::Collision::Representation>(stapler, "Collision"),
+ getComponentChecked<SurgSim::Collision::Representation>(stapler, "VirtualToothCollision0"));
+
+ physicsManager->addExcludedCollisionPair(
+ getComponentChecked<SurgSim::Collision::Representation>(stapler, "Collision"),
+ getComponentChecked<SurgSim::Collision::Representation>(stapler, "VirtualToothCollision1"));
+
+ physicsManager->addExcludedCollisionPair(
+ getComponentChecked<SurgSim::Collision::Representation>(wound, "Collision"),
+ getComponentChecked<SurgSim::Collision::Representation>(arm, "Collision"));
+
+ runtime->execute();
+ return 0;
+}
diff --git a/Examples/Stapling/config.txt.in b/Examples/Stapling/config.txt.in
new file mode 100644
index 0000000..11a8132
--- /dev/null
+++ b/Examples/Stapling/config.txt.in
@@ -0,0 +1,2 @@
+${CMAKE_CURRENT_BINARY_DIR}/Data/
+${SURGSIM_SOURCE_DIR}/Data/
\ No newline at end of file
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..d645695
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,202 @@
+
+ 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:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) 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
+
+ (d) 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/NOTICE b/NOTICE
new file mode 100644
index 0000000..8813a1e
--- /dev/null
+++ b/NOTICE
@@ -0,0 +1,48 @@
+OpenSurgSim simulation framework
+Copyright 2012-2013, SimQuest Solutions Inc.
+
+This product includes software developed by SimQuest Solutions
+Inc. (http://www.simquest.com/).
+
+The calculations of various physical properties from an arbitrary surface
+mesh, implemented in the MeshShape class, are based on research and a
+public-domain implementation by Brian Mirtich.
+ (http://www.cs.berkeley.edu/~jfc/mirtich/massProps.html)
+
+This product uses software developed by The MathJax Consortium and
+licensed under the Apache License, Version 2.0. The OpenSurgSim
+documentation displays equations using MathJax (http://www.mathjax.org/),
+which is an open-source JavaScript display engine for LaTeX, MathML,
+and AsciiMath notation.
+
+------------------------------------------------------------
+
+Contains code based on Boost Threads
+Copyright (C) 2002-2003 David Moore, William E. Kempf
+Copyright (C) 2007-8 Anthony Williams
+which is subject to the following license:
+Boost Software License - Version 1.0 - August 17th, 2003
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+
+------------------------------------------------------------
\ No newline at end of file
diff --git a/README b/README
new file mode 100644
index 0000000..8082eb4
--- /dev/null
+++ b/README
@@ -0,0 +1,143 @@
+Welcome to the OpenSurgSim Project
+
+OpenSurgSim is an open-source project dedicated to real-time surgical
+simulation. It offers an open framework that includes the necessary building
+blocks for surgical simulations, such as native device support, haptic feedback,
+graphics, discrete collision detection and physics simulation. OpenSurgSim is
+flexible. Developers can refactor the physics engine, swap models, ODE solvers,
+or linear system solvers. For current information and documentation about the
+project, please visit our website:
+
+ http://www.opensurgsim.org/
+
+Support can also be found on our public mailing list at
+opensurgsim at simquest.com. An archive of the mailing list is at:
+
+ http://groups.google.com/a/simquest.com/group/opensurgsim/
+
+To help get started using OpenSurgSim, use the following quick start guide:
+
+ 1. Getting OpenSurgSim
+ 2. Compiling on GNU/Linux
+ 3. Compiling on Microsoft Windows
+ Appendix: Dependencies
+
+
+1. Getting OpenSurgSim
+======================
+
+OpenSurgSim uses Git for source control, and this is the easiest way to obtain
+the most up to date version. Resources for installing and using Git can be found
+on Git's website (See Appendix). To obtain OpenSurgSim, run the following
+command:
+
+ git clone git://git.assembla.com/OpenSurgSim.git
+
+This will download the source code for OpenSurgSim and place it in the
+OpenSurgSim directory.
+
+
+2. Compiling on GNU/Linux
+=========================
+
+OpenSurgSim has been tested extensively on Debian Testing (Jessie). If you are
+using another GNU/Linux distribution, please check your package management
+system for the dependencies (see Appendix). For Debian/Ubuntu based systems, the
+dependencies are easily installed through apt-get:
+
+ sudo apt-get install libboost-all-dev cmake doxygen
+ sudo apt-get install libeigen3-dev google-mock libopenscenegraph-dev
+
+To build OpenSurgSim, issue the following commands from the same directory used
+to obtain OpenSurgSim (see Section 1)
+
+ mkdir OpenSurgSim/Build
+ cd OpenSurgSim/Build
+ cmake ../
+ make
+
+That's it!
+
+
+3. Compiling on Microsoft Windows
+=================================
+
+OpenSurgSim has been developed and tested on Microsoft Windows 7, however, other
+versions of Windows are likely to work. At least Microsoft Visual Studio 2012 is
+required to build OpenSurgSim.
+
+First obtain the source code (Section 1), and install the required dependencies
+(Appendix). In addition, the following environment variables will help CMake
+automatically find the dependencies, especially if they are not installed to the
+default locations.
+
+ BOOST_ROOT <Path to Boost>
+ EIGEN_DIR <Path to Eigen>
+ OSG_ROOT <Path to OSG>
+
+Also, add %OSG_ROOT%/bin to your PATH environment variable.
+
+Generating the Visual Studio solution file is done with the CMake graphical user
+interface. Open CMake, and enter the OpenSurgSim folder (from Section 1) into
+the "Where is the source code" field. Then enter a new directory into the "Where
+to build the binaries" field. This folder is referred to as BUILD folder for
+the purpose of the following steps.
+
+Next, click on the 'Configure' button. This will allow you to select the
+compiler (Visual Studio 11 for Visual Studio 2012). If CMake reports “library
+not found”, it is likely that your system variables BOOST_ROOT, EIGEN_DIR or
+OSG_ROOT are incorrect. Once the configuration is complete without error, click
+on the 'Generate' button.
+
+Finally, go to the BUILD folder, open the OpenSurgSim solution file, and build
+the ALL_BUILD project.
+
+
+Appendix: Dependencies
+======================
+
+To compile OpenSurgSim, you will need the following dependencies. Many GNU/Linux
+distributions already provide this software in their software repositories. If
+not, or using Microsoft Windows, please refer to the installation instructions
+found on each dependency's homepage.
+
+Required Dependencies
+---------------------
+
+* Boost
+ Homepage: http://www.boost.org/
+ Modules: chrono, date_time, filesystem, system, thread
+ Minimum Version: 1.54
+
+* CMake
+ Homepage: http://www.cmake.org/
+ Minimum Version: 2.8
+
+* Eigen
+ Homepage: http://eigen.tuxfamily.org/
+ Minimum Version: 3.2.0
+
+* Git
+ Homepage: http://www.git-scm.com/
+ Minimum Version: 1.7.9
+
+* OpenSceneGraph
+ Homepage: http://www.openscenegraph.org/
+ Modules: osg, osgViewer, osgText, osgUtil, osgDB, osgGA, osgAnimation
+ Minimum Version: 3.2.0
+
+Optional Dependencies
+---------------------
+
+* Doxygen
+ Homepage: http://doxygen.org/
+ Minimum Version: 1.8
+
+* FreeGlut
+ Homepage: http://freeglut.sourceforge.net/
+ Minimum Version: 2.6.0
+
+* Google Mock
+ Homepage: https://code.google.com/p/googlemock/
+ Minimum Version: 1.7.0
+
diff --git a/SurgSim/Blocks/CMakeLists.txt b/SurgSim/Blocks/CMakeLists.txt
new file mode 100644
index 0000000..8d84195
--- /dev/null
+++ b/SurgSim/Blocks/CMakeLists.txt
@@ -0,0 +1,70 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ ${OSG_INCLUDE_DIR}
+ ${OPENTHREADS_INCLUDE_DIR}
+)
+
+set(SURGSIM_BLOCKS_SOURCES
+ DriveElementFromInputBehavior.cpp
+ KeyboardTogglesComponentBehavior.cpp
+ MassSpring1DRepresentation.cpp
+ MassSpring2DRepresentation.cpp
+ MassSpring3DRepresentation.cpp
+ MassSpringNDRepresentationUtils.cpp
+ PoseInterpolator.cpp
+ SphereElement.cpp
+ TransferPhysicsToGraphicsMeshBehavior.cpp
+ TransferPhysicsToPointCloudBehavior.cpp
+ VisualizeContactsBehavior.cpp
+)
+
+set(SURGSIM_BLOCKS_HEADERS
+ DriveElementFromInputBehavior.h
+ KeyboardTogglesComponentBehavior.h
+ MassSpring1DRepresentation.h
+ MassSpring2DRepresentation.h
+ MassSpring3DRepresentation.h
+ MassSpringNDRepresentationUtils.h
+ PoseInterpolator.h
+ SphereElement.h
+ TransferPhysicsToGraphicsMeshBehavior.h
+ TransferPhysicsToPointCloudBehavior.h
+ VisualizeContactsBehavior.h
+)
+
+surgsim_add_library(
+ SurgSimBlocks
+ "${SURGSIM_BLOCKS_SOURCES}"
+ "${SURGSIM_BLOCKS_HEADERS}"
+ "SurgSim/Blocks"
+)
+
+SET(LIBS
+ SurgSimFramework
+ SurgSimPhysics
+ ${Boost_LIBRARIES}
+)
+
+target_link_libraries(SurgSimBlocks ${LIBS}
+)
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+endif()
+
+# Put SurgSimBlocks into folder "Blocks"
+set_target_properties(SurgSimBlocks PROPERTIES FOLDER "Blocks")
diff --git a/SurgSim/Blocks/DriveElementFromInputBehavior.cpp b/SurgSim/Blocks/DriveElementFromInputBehavior.cpp
new file mode 100644
index 0000000..fdef48a
--- /dev/null
+++ b/SurgSim/Blocks/DriveElementFromInputBehavior.cpp
@@ -0,0 +1,102 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Blocks/DriveElementFromInputBehavior.h"
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Math::RigidTransform3d;
+
+namespace SurgSim
+{
+namespace Blocks
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::DriveElementFromInputBehavior,
+ DriveElementFromInputBehavior);
+
+DriveElementFromInputBehavior::DriveElementFromInputBehavior(const std::string& name) :
+ SurgSim::Framework::Behavior(name),
+ m_poseName("pose")
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(DriveElementFromInputBehavior, std::shared_ptr<SurgSim::Framework::Component>,
+ Source, getSource, setSource);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(DriveElementFromInputBehavior, std::string, PoseName, getPoseName, setPoseName);
+}
+
+void DriveElementFromInputBehavior::setSource(std::shared_ptr<SurgSim::Framework::Component> source)
+{
+ m_source = std::dynamic_pointer_cast<SurgSim::Input::InputComponent>(source);
+ SURGSIM_ASSERT(m_source != nullptr) << " setSource for " << getClassName()
+ << " requires a SurgSim::Input::InputComponent";
+}
+
+std::shared_ptr<SurgSim::Framework::Component> DriveElementFromInputBehavior::getSource()
+{
+ return m_source;
+}
+
+void DriveElementFromInputBehavior::setPoseName(const std::string& poseName)
+{
+ m_poseName = poseName;
+}
+
+std::string DriveElementFromInputBehavior::getPoseName()
+{
+ return m_poseName;
+}
+
+void DriveElementFromInputBehavior::update(double dt)
+{
+ SurgSim::DataStructures::DataGroup dataGroup;
+ m_source->getData(&dataGroup);
+ RigidTransform3d pose;
+ if (dataGroup.poses().get(m_poseName, &pose))
+ {
+ getPoseComponent()->setPose(pose);
+ }
+}
+
+bool DriveElementFromInputBehavior::doInitialize()
+{
+ return true;
+}
+
+bool DriveElementFromInputBehavior::doWakeUp()
+{
+ bool result = true;
+ if (m_source == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << getClassName() << " named '" +
+ getName() + "' must have a source to do anything.";
+ result = false;
+ }
+
+ if (getPoseComponent() == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << getClassName() << " named '" +
+ getName() + "' must belong to a SceneElement with a PoseComponent.";
+ result = false;
+ }
+
+ return result;
+}
+
+}; //namespace Blocks
+}; //namespace SurgSim
diff --git a/SurgSim/Blocks/DriveElementFromInputBehavior.h b/SurgSim/Blocks/DriveElementFromInputBehavior.h
new file mode 100644
index 0000000..d005062
--- /dev/null
+++ b/SurgSim/Blocks/DriveElementFromInputBehavior.h
@@ -0,0 +1,87 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_DRIVEELEMENTFROMINPUTBEHAVIOR_H
+#define SURGSIM_BLOCKS_DRIVEELEMENTFROMINPUTBEHAVIOR_H
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+namespace SurgSim
+{
+
+namespace Input
+{
+class InputComponent;
+}
+
+namespace Blocks
+{
+
+SURGSIM_STATIC_REGISTRATION(DriveElementFromInputBehavior);
+
+/// Behavior to copy a pose from an input component to a SceneElement
+/// By adding this behavior to a SceneElement, that SceneElement will be moved
+/// in correspondance to the input.
+class DriveElementFromInputBehavior : public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit DriveElementFromInputBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::DriveElementFromInputBehavior);
+
+ /// Set the InputComponent that provides the pose
+ /// \param source A SurgSim::Input::InputComponent
+ void setSource(std::shared_ptr<SurgSim::Framework::Component> source);
+
+ /// Get the InputComponent which is being used by this behavior
+ /// \return A SurgSim::Component::InputComponent
+ std::shared_ptr<SurgSim::Framework::Component> getSource();
+
+ /// Set name of the pose.
+ /// \param poseName The name of the pose.
+ void setPoseName(const std::string& poseName);
+
+ /// Get name of the pose.
+ /// \return The name of the pose.
+ std::string getPoseName();
+
+ /// Update the behavior
+ /// \param dt The length of time (seconds) between update calls.
+ virtual void update(double dt);
+
+protected:
+ /// Initialize the behavior
+ virtual bool doInitialize();
+
+ /// Wakeup the behavior, which copies the initial pose
+ virtual bool doWakeUp();
+
+private:
+ /// InputComponent to get the pose
+ std::shared_ptr<SurgSim::Input::InputComponent> m_source;
+
+ std::string m_poseName;
+};
+
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
+
+
+#endif // SURGSIM_BLOCKS_DRIVEELEMENTFROMINPUTBEHAVIOR_H
diff --git a/SurgSim/Blocks/KeyboardTogglesComponentBehavior.cpp b/SurgSim/Blocks/KeyboardTogglesComponentBehavior.cpp
new file mode 100644
index 0000000..7954c53
--- /dev/null
+++ b/SurgSim/Blocks/KeyboardTogglesComponentBehavior.cpp
@@ -0,0 +1,109 @@
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Blocks/KeyboardTogglesComponentBehavior.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Input/InputComponent.h"
+
+namespace SurgSim
+{
+namespace Blocks
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::KeyboardTogglesComponentBehavior,
+ KeyboardTogglesComponentBehavior);
+
+KeyboardTogglesComponentBehavior::KeyboardTogglesComponentBehavior(const std::string& name) :
+ SurgSim::Framework::Behavior(name),
+ m_keyPressedLastUpdate(false)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(KeyboardTogglesComponentBehavior, std::shared_ptr<SurgSim::Framework::Component>,
+ InputComponent, getInputComponent, setInputComponent);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(KeyboardTogglesComponentBehavior, KeyboardRegistryType,
+ KeyboardRegistry, getKeyboardRegistry, setKeyboardRegistry);
+}
+
+void KeyboardTogglesComponentBehavior::setInputComponent(std::shared_ptr<SurgSim::Framework::Component> inputComponent)
+{
+ SURGSIM_ASSERT(nullptr != inputComponent) << "'inputComponent' cannot be 'nullptr'";
+
+ m_inputComponent = std::dynamic_pointer_cast<SurgSim::Input::InputComponent>(inputComponent);
+
+ SURGSIM_ASSERT(nullptr != m_inputComponent) << "'inputComponent' must derive from SurgSim::Input::InputComponent";
+}
+
+std::shared_ptr<SurgSim::Input::InputComponent> KeyboardTogglesComponentBehavior::getInputComponent() const
+{
+ return m_inputComponent;
+}
+
+void KeyboardTogglesComponentBehavior::registerKey(SurgSim::Device::KeyCode key,
+ std::shared_ptr<SurgSim::Framework::Component> component)
+{
+ m_registry[static_cast<int>(key)].insert(component);
+}
+
+void KeyboardTogglesComponentBehavior::update(double dt)
+{
+ SurgSim::DataStructures::DataGroup dataGroup;
+ m_inputComponent->getData(&dataGroup);
+
+ int key;
+ if (dataGroup.integers().get("key", &key))
+ {
+ auto match = m_registry.find(key);
+ if (match != m_registry.end() && !m_keyPressedLastUpdate)
+ {
+ for (auto it = std::begin(match->second); it != std::end(match->second); ++it)
+ {
+ (*it)->setLocalActive(!(*it)->isLocalActive());
+ };
+ }
+ m_keyPressedLastUpdate = (SurgSim::Device::KeyCode::NONE != key);
+ }
+}
+
+bool KeyboardTogglesComponentBehavior::doInitialize()
+{
+ return true;
+}
+
+bool KeyboardTogglesComponentBehavior::doWakeUp()
+{
+ bool result = true;
+ if (nullptr == m_inputComponent)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__ <<
+ "KeyboardTogglesComponentBehavior " << getName() << " does not have an Input Component.";
+ result = false;
+ }
+ return result;
+}
+
+void KeyboardTogglesComponentBehavior::setKeyboardRegistry(const KeyboardRegistryType& map)
+{
+ m_registry = map;
+}
+
+const KeyboardTogglesComponentBehavior::KeyboardRegistryType&
+ KeyboardTogglesComponentBehavior::getKeyboardRegistry() const
+{
+ return m_registry;
+}
+
+}; // namespace Blocks
+}; // namespace SurgSim
diff --git a/SurgSim/Blocks/KeyboardTogglesComponentBehavior.h b/SurgSim/Blocks/KeyboardTogglesComponentBehavior.h
new file mode 100644
index 0000000..6017e35
--- /dev/null
+++ b/SurgSim/Blocks/KeyboardTogglesComponentBehavior.h
@@ -0,0 +1,107 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_KEYBOARDTOGGLESCOMPONENTBEHAVIOR_H
+#define SURGSIM_BLOCKS_KEYBOARDTOGGLESCOMPONENTBEHAVIOR_H
+
+#include <unordered_map>
+#include <unordered_set>
+
+#include "SurgSim/Devices/Keyboard/KeyCode.h"
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/Macros.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Component;
+}
+
+namespace Input
+{
+class InputComponent;
+}
+
+namespace Blocks
+{
+SURGSIM_STATIC_REGISTRATION(KeyboardTogglesComponentBehavior);
+
+/// This behavior is used to control the visibility of registered graphical representation(s)
+class KeyboardTogglesComponentBehavior : public SurgSim::Framework::Behavior
+{
+public:
+ typedef std::unordered_map<int, std::unordered_set<std::shared_ptr<SurgSim::Framework::Component>>>
+ KeyboardRegistryType;
+
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit KeyboardTogglesComponentBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::KeyboardTogglesComponentBehavior);
+
+ /// Set the input component from which pressed keys come.
+ /// \param inputComponent The input component which contains the pressed key(s).
+ void setInputComponent(std::shared_ptr<SurgSim::Framework::Component> inputComponent);
+
+ /// Get the input component of this behavior
+ /// \return The input component which sends signals to this behavior.
+ std::shared_ptr<SurgSim::Input::InputComponent> getInputComponent() const;
+
+ /// Register a key with a component in this behavior.
+ /// \param key A key used to control the component.
+ /// \param component The component being controlled by the key.
+ /// \note A key can be registered several times, so can a component.
+ void registerKey(SurgSim::Device::KeyCode key, std::shared_ptr<SurgSim::Framework::Component> component);
+
+ /// Set the register map of this behavior
+ /// \param map The register map.
+ void setKeyboardRegistry(const KeyboardRegistryType& map);
+
+ /// Get the register map of this behavior
+ /// \return The register map of this behavior
+ const KeyboardRegistryType& getKeyboardRegistry() const;
+
+ /// Update the behavior
+ /// \param dt The length of time (seconds) between update calls.
+ virtual void update(double dt) override;
+
+protected:
+ /// Initialize this behavior
+ /// \return True on success, otherwise false.
+ /// \note In current implementation, this method always returns "true".
+ virtual bool doInitialize() override;
+
+ /// Wakeup this behavior
+ /// \return True on success, otherwise false.
+ /// \note In current implementation, this method always returns "true".
+ virtual bool doWakeUp() override;
+
+private:
+ /// Record if any key is pressed in last update() call.
+ bool m_keyPressedLastUpdate;
+
+ /// Input component from which pressed keys come.
+ std::shared_ptr<SurgSim::Input::InputComponent> m_inputComponent;
+
+ /// A mapping between key and the graphical representation(s) it controls.
+ KeyboardRegistryType m_registry;
+};
+
+}; // namespace Blocks
+}; // namespace SurgSim
+
+#endif //SURGSIM_BLOCKS_KEYBOARDTOGGLESCOMPONENTBEHAVIOR_H
diff --git a/SurgSim/Blocks/MassSpring1DRepresentation.cpp b/SurgSim/Blocks/MassSpring1DRepresentation.cpp
new file mode 100644
index 0000000..d9bb88d
--- /dev/null
+++ b/SurgSim/Blocks/MassSpring1DRepresentation.cpp
@@ -0,0 +1,142 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Blocks/MassSpring1DRepresentation.h"
+#include "SurgSim/Blocks/MassSpringNDRepresentationUtils.h"
+#include "SurgSim/Math/LinearSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::Mass;
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+void MassSpring1DRepresentation::init1D(
+ const std::vector<Vector3d> nodes,
+ std::vector<size_t> nodeBoundaryConditions,
+ double totalMass,
+ double stiffnessStretching, double dampingStretching,
+ double stiffnessBending, double dampingBending)
+{
+ std::shared_ptr<SurgSim::Math::OdeState> state;
+ state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(getNumDofPerNode(), nodes.size());
+
+ SURGSIM_ASSERT(nodes.size() > 0) << "Number of nodes incorrect: " << nodes.size();
+
+ // Initialize the nodes position, velocity and mass
+ // Note: no need to apply the initialPose here, initialize will take care of it !
+ for (size_t massId = 0; massId < nodes.size(); massId++)
+ {
+ addMass(std::make_shared<Mass>(totalMass / static_cast<double>(nodes.size())));
+
+ SurgSim::Math::setSubVector(nodes[massId], massId, 3, &state->getPositions());
+ }
+
+ // Initialize the stretching springs
+ if (stiffnessStretching || dampingStretching)
+ {
+ for (size_t massId = 0; massId < nodes.size() - 1; massId++)
+ {
+ addSpring(createLinearSpring(state, massId, massId + 1, stiffnessStretching, dampingStretching));
+ }
+ }
+
+ // Initialize the bending springs
+ if (stiffnessBending || dampingBending)
+ {
+ for (size_t massId = 0; massId < nodes.size() - 2; massId++)
+ {
+ addSpring(createLinearSpring(state, massId, massId + 2, stiffnessBending, dampingBending));
+ }
+ }
+
+ // Sets the boundary conditions
+ for (auto boundaryCondition = std::begin(nodeBoundaryConditions);
+ boundaryCondition != std::end(nodeBoundaryConditions);
+ boundaryCondition++)
+ {
+ state->addBoundaryCondition(*boundaryCondition);
+ }
+
+ // setInitialState: Initialize all the states + apply initialPose if any
+ setInitialState(state);
+}
+
+bool MassSpring1DRepresentation::doWakeUp()
+{
+ using SurgSim::Math::LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix;
+
+ if (!MassSpringRepresentation::doWakeUp())
+ {
+ return false;
+ }
+
+ // For implicit ode solver, we solve (M/dt + D + dt.K).deltaV = F - dt.K.v(t)
+ // with M diagonal, K tri-diagonal block of a given size and D a linear combination of M and K.
+ // So (M/dt + D + dt.K) is a tri-diagonal block matrix of the following size:
+ //
+ // If we only have stretching springs, each node is connected with its direct neighbors
+ // The matrix K has the following structure (each X being a 3x3 matrix):
+ // nodeId 3(i-3) 3(i-2) 3(i-1) 3(i) 3(i+1) 3(i+2) 3(i+3)
+ // 3(i-3) = 0........X X 0 0 0 0 0......0
+ // 3(i-2) = 0........X X X 0 0 0 0......0
+ // 3(i-1) = 0........0 X X X 0 0 0......0
+ // 3(i) = 0........0 0 X X X 0 0......0
+ // 3(i+1) = 0........0 0 0 X X X 0......0
+ // 3(i+2) = 0........0 0 0 0 X X X......0
+ // 3(i+3) = 0........0 0 0 0 0 X X......0
+ // => blockSize = 3
+ //
+ // If we also have bending springs, each nodes is also connected with its second direct neighbors
+ // The matrix K has the following structure (each X being a 3x3 matrix):
+ // nodeId 3(i-3) 3(i-2) 3(i-1) 3(i) 3(i+1) 3(i+2) 3(i+3)
+ // 3(i-3) = 0........X X X 0 0 0 0......0
+ // 3(i-2) = 0........X X X X 0 0 0......0
+ // 3(i-1) = 0........X X X X X 0 0......0
+ // 3(i) = 0........0 X X X X X 0......0
+ // 3(i+1) = 0........0 0 X X X X X......0
+ // 3(i+2) = 0........0 0 0 X X X X......0
+ // 3(i+3) = 0........0 0 0 0 X X X......0
+ // => we define blockSize as 6 (if we have an even number of nodes: 3*2n / 6 = n blocks)
+
+ switch (m_integrationScheme)
+ {
+ case SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER:
+ case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER:
+ if (getInitialState()->getNumNodes() % 2 == 0)
+ {
+ m_odeSolver->setLinearSolver(std::make_shared<LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<6>>());
+ }
+ else
+ {
+ // We should use a band matrix solver here in general when available
+ }
+ break;
+ default:
+ break;
+ }
+
+ return true;
+}
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
diff --git a/SurgSim/Blocks/MassSpring1DRepresentation.h b/SurgSim/Blocks/MassSpring1DRepresentation.h
new file mode 100644
index 0000000..a61d34c
--- /dev/null
+++ b/SurgSim/Blocks/MassSpring1DRepresentation.h
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_MASSSPRING1DREPRESENTATION_H
+#define SURGSIM_BLOCKS_MASSSPRING1DREPRESENTATION_H
+
+#include <array>
+#include <vector>
+
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+// This class defines a simple MassSpring 1D structures
+class MassSpring1DRepresentation : public SurgSim::Physics::MassSpringRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The model name
+ explicit MassSpring1DRepresentation(const std::string& name) :
+ SurgSim::Physics::MassSpringRepresentation(name)
+ {
+ }
+
+ /// Initializes a 1D model from a given list of nodes
+ /// \param nodes List of nodes to define the 1D model with
+ /// \param nodeBoundaryConditions The list of all nodeId being boundary conditions (fixed node)
+ /// \param totalMass The total mass of the mass spring (evenly spread out on the masses)
+ /// \param stiffnessStretching, dampingStretching The spring param for all stretching springs (edges)
+ /// \param stiffnessBending, dampingBending The spring param for all bending springs (edges)
+ /// \note Stretching springs are connecting neighbors, bending springs are connecting 1 node
+ /// \note to its 2nd degree neighbors, creating a bending force around the middle node.
+ void init1D(const std::vector<SurgSim::Math::Vector3d> nodes,
+ std::vector<size_t> nodeBoundaryConditions,
+ double totalMass,
+ double stiffnessStretching, double dampingStretching,
+ double stiffnessBending, double dampingBending);
+
+protected:
+
+ virtual bool doWakeUp() override;
+};
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_MASSSPRING1DREPRESENTATION_H
diff --git a/SurgSim/Blocks/MassSpring2DRepresentation.cpp b/SurgSim/Blocks/MassSpring2DRepresentation.cpp
new file mode 100644
index 0000000..ed242d9
--- /dev/null
+++ b/SurgSim/Blocks/MassSpring2DRepresentation.cpp
@@ -0,0 +1,176 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Blocks/MassSpring2DRepresentation.h"
+#include "SurgSim/Blocks/MassSpringNDRepresentationUtils.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::Mass;
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+void MassSpring2DRepresentation::init2DStretchingSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[2], double stiffness, double damping)
+{
+ const size_t rowOffset = numNodesPerDim[0];
+ const size_t colOffset = 1;
+
+ // Initialize the stretching springs
+ if (stiffness || damping)
+ {
+ // ...along X
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ size_t nodeId = row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + colOffset, stiffness, damping));
+ }
+ }
+ // ...along Y
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ size_t nodeId = row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + rowOffset, stiffness, damping));
+ }
+ }
+ }
+}
+
+void MassSpring2DRepresentation::init2DBendingSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[2], double stiffness, double damping)
+{
+ const size_t rowOffset = numNodesPerDim[0];
+ const size_t colOffset = 1;
+
+ // Initialize the bending springs
+ if (stiffness || damping)
+ {
+ // ... along X
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 2; col++)
+ {
+ size_t nodeId = row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + 2 * colOffset, stiffness, damping));
+ }
+ }
+ // ... along Y
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 2; row++)
+ {
+ size_t nodeId = row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + 2 * rowOffset, stiffness, damping));
+ }
+ }
+ }
+}
+
+void MassSpring2DRepresentation::init2DFaceDiagonalSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[2], double stiffness, double damping)
+{
+ const size_t rowOffset = numNodesPerDim[0];
+ const size_t colOffset = 1;
+
+ // Initialize the face diagonal springs
+ if (stiffness || damping)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ size_t nodeId = row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + rowOffset + colOffset, stiffness, damping));
+ addSpring(createLinearSpring(state, nodeId + colOffset, nodeId + rowOffset, stiffness, damping));
+ }
+ }
+ }
+}
+
+void MassSpring2DRepresentation::init2D(
+ const std::array<std::array<Vector3d, 2>, 2> extremities,
+ size_t numNodesPerDim[2],
+ std::vector<size_t> nodeBoundaryConditions,
+ double totalMass,
+ double stiffnessStretching, double dampingStretching,
+ double stiffnessBending, double dampingBending,
+ double stiffnessFaceDiagonal, double dampingFaceDiagonal)
+{
+ std::shared_ptr<SurgSim::Math::OdeState> state;
+ state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(getNumDofPerNode(), numNodesPerDim[0] * numNodesPerDim[1]);
+
+ SURGSIM_ASSERT(numNodesPerDim[0] > 0) << "Number of nodes for dimension 1 is incorrect: " << numNodesPerDim[0];
+ SURGSIM_ASSERT(numNodesPerDim[1] > 0) << "Number of nodes for dimension 2 is incorrect: " << numNodesPerDim[1];
+
+ // Initialize the nodes position, velocity and mass
+ // Note: no need to apply the initialPose here, initialize will take care of it !
+ Vector3d rowExtremititiesDelta[2] =
+ {
+ (extremities[0][1] - extremities[0][0]) / static_cast<double>(numNodesPerDim[1] - 1) ,
+ (extremities[1][1] - extremities[1][0]) / static_cast<double>(numNodesPerDim[1] - 1)
+ };
+ size_t nodeId = 0;
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ Vector3d rowExtremities[2];
+ rowExtremities[0] = extremities[0][0] + rowExtremititiesDelta[0] * static_cast<double>(row);
+ rowExtremities[1] = extremities[1][0] + rowExtremititiesDelta[1] * static_cast<double>(row);
+
+ Vector3d delta = (rowExtremities[1] - rowExtremities[0]) / static_cast<double>(numNodesPerDim[0] - 1);
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ addMass(std::make_shared<Mass>(totalMass / static_cast<double>(numNodesPerDim[0] * numNodesPerDim[1])));
+
+ SurgSim::Math::Vector3d position(rowExtremities[0] + static_cast<double>(col) * delta);
+ SurgSim::Math::setSubVector(position, nodeId, 3, &state->getPositions());
+
+ nodeId++;
+ }
+ }
+
+ // Initialize all the stretching springs
+ init2DStretchingSprings(state, numNodesPerDim, stiffnessStretching, dampingStretching);
+
+ // Initialize all the bending springs
+ init2DBendingSprings(state, numNodesPerDim, stiffnessBending, dampingBending);
+
+ // Initialize all the face diagonal springs
+ init2DFaceDiagonalSprings(state, numNodesPerDim, stiffnessFaceDiagonal, dampingFaceDiagonal);
+
+ // Sets the boundary conditions
+ for (auto boundaryCondition = std::begin(nodeBoundaryConditions);
+ boundaryCondition != std::end(nodeBoundaryConditions);
+ boundaryCondition++)
+ {
+ state->addBoundaryCondition(*boundaryCondition);
+ }
+
+ // setInitialState: Initialize all the states + apply initialPose if any
+ setInitialState(state);
+}
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
diff --git a/SurgSim/Blocks/MassSpring2DRepresentation.h b/SurgSim/Blocks/MassSpring2DRepresentation.h
new file mode 100644
index 0000000..6503dda
--- /dev/null
+++ b/SurgSim/Blocks/MassSpring2DRepresentation.h
@@ -0,0 +1,89 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_MASSSPRING2DREPRESENTATION_H
+#define SURGSIM_BLOCKS_MASSSPRING2DREPRESENTATION_H
+
+#include <array>
+#include <vector>
+
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+// This class defines a simple MassSpring 2D structures
+class MassSpring2DRepresentation : public SurgSim::Physics::MassSpringRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name to assign to the model
+ explicit MassSpring2DRepresentation(const std::string& name) :
+ SurgSim::Physics::MassSpringRepresentation(name)
+ {
+ }
+
+ /// Initializes a 2D MassSpring
+ /// \param extremities 4 positions forming the extremities of the 2D regular model (4 corners)
+ /// \param numNodesPerDim The number of nodes to be created for each dimension (here 2)
+ /// \param nodeBoundaryConditions The list of all nodeId being boundary conditions (fixed node)
+ /// \param totalMass The total mass of the mass spring (evenly spread out on the masses)
+ /// \param stiffnessStretching, dampingStretching The spring param for all stretching springs (edges)
+ /// \param stiffnessBending, dampingBending The spring param for all bending springs (edges)
+ /// \param stiffnessFaceDiagonal, dampingFaceDiagonal The spring param for all face diagonal springs (faces)
+ /// \note Stretching springs are connecting neighbors, bending springs are connecting 1 node
+ /// \note to its 2nd degree neighbors, creating a bending force around the middle node.
+ /// \note Face diagonal springs aim at maintaining the area of a square
+ /// \note extremities are organized as follow:
+ /// \note [0][1] *---* [1][1]
+ /// \note | |
+ /// \note [0][0] *---* [1][0]
+ void init2D(const std::array<std::array<SurgSim::Math::Vector3d, 2>, 2> extremities,
+ size_t numNodesPerDim[2],
+ std::vector<size_t> nodeBoundaryConditions,
+ double totalMass,
+ double stiffnessStretching, double dampingStretching,
+ double stiffnessBending, double dampingBending,
+ double stiffnessFaceDiagonal, double dampingFaceDiagonal);
+
+private:
+ /// Helper method to initialize/add all stretching springs on a 2D structure
+ /// \param state The state to initialize the springs with (rest lengths calculation)
+ /// \param numNodesPerDim The number of nodes on the 2 dimensions
+ /// \param stiffness, damping The spring parameters
+ void init2DStretchingSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[2], double stiffness, double damping);
+ /// Helper method to initialize/add all bending springs on a 2D structure
+ /// \param state The state to initialize the springs with (rest lengths calculation)
+ /// \param numNodesPerDim The number of nodes on the 2 dimensions
+ /// \param stiffness, damping The spring parameters
+ void init2DBendingSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[2], double stiffness, double damping);
+ /// Helper method to initialize/add all face diagonal springs on a 2D structure
+ /// \param state The state to initialize the springs with (rest lengths calculation)
+ /// \param numNodesPerDim The number of nodes on the 2 dimensions
+ /// \param stiffness, damping The spring parameters
+ void init2DFaceDiagonalSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[2], double stiffness, double damping);
+};
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_MASSSPRING2DREPRESENTATION_H
diff --git a/SurgSim/Blocks/MassSpring3DRepresentation.cpp b/SurgSim/Blocks/MassSpring3DRepresentation.cpp
new file mode 100644
index 0000000..3698726
--- /dev/null
+++ b/SurgSim/Blocks/MassSpring3DRepresentation.cpp
@@ -0,0 +1,302 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Blocks/MassSpring3DRepresentation.h"
+#include "SurgSim/Blocks/MassSpringNDRepresentationUtils.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+using SurgSim::Physics::Mass;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+void MassSpring3DRepresentation::init3DStretchingSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[3], double stiffness, double damping)
+{
+ const size_t depthOffset = numNodesPerDim[0] * numNodesPerDim[1];
+ const size_t rowOffset = numNodesPerDim[0];
+ const size_t colOffset = 1;
+
+ // Initialize the stretching springs
+ if (stiffness|| damping)
+ {
+ // ... along X
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + colOffset, stiffness, damping));
+ }
+ }
+ }
+ // ... along Y
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + rowOffset, stiffness, damping));
+ }
+ }
+ }
+ // ... along Z
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t depth = 0; depth < numNodesPerDim[2] - 1; depth++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + depthOffset, stiffness, damping));
+ }
+ }
+ }
+ }
+}
+
+void MassSpring3DRepresentation::init3DBendingSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[3], double stiffness, double damping)
+{
+ const size_t depthOffset = numNodesPerDim[0] * numNodesPerDim[1];
+ const size_t rowOffset = numNodesPerDim[0];
+ const size_t colOffset = 1;
+
+ // Initialize the bending springs
+ if (stiffness || damping)
+ {
+ // ... along X
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 2; col++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + 2 * colOffset, stiffness, damping));
+ }
+ }
+ }
+ // ... along Y
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 2; row++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + 2 * rowOffset, stiffness, damping));
+ }
+ }
+ }
+ // ... along Z
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t depth = 0; depth < numNodesPerDim[2] - 2; depth++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + 2 * depthOffset, stiffness, damping));
+ }
+ }
+ }
+ }
+}
+
+void MassSpring3DRepresentation::init3DFaceDiagonalSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[3], double stiffness, double damping)
+{
+ const size_t depthOffset = numNodesPerDim[0] * numNodesPerDim[1];
+ const size_t rowOffset = numNodesPerDim[0];
+ const size_t colOffset = 1;
+
+ // Initialize the face diagonal springs
+ if (stiffness || damping)
+ {
+ // ... faces orthogonal to Z
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + rowOffset + colOffset, stiffness, damping));
+ addSpring(createLinearSpring(state, nodeId + colOffset, nodeId + rowOffset, stiffness, damping));
+ }
+ }
+ }
+ // ... faces orthogonal to Y
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t depth = 0; depth < numNodesPerDim[2] - 1; depth++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + depthOffset + colOffset, stiffness, damping));
+ addSpring(createLinearSpring(state, nodeId + colOffset, nodeId + depthOffset, stiffness, damping));
+ }
+ }
+ }
+ // ... faces orthogonal to X
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ for (size_t depth = 0; depth < numNodesPerDim[2] - 1; depth++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + depthOffset + rowOffset, stiffness, damping));
+ addSpring(createLinearSpring(state, nodeId + rowOffset, nodeId + depthOffset, stiffness, damping));
+ }
+ }
+ }
+ }
+}
+
+void MassSpring3DRepresentation::init3DVolumeDiagonalSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[3], double stiffness, double damping)
+{
+ const size_t depthOffset = numNodesPerDim[0] * numNodesPerDim[1];
+ const size_t rowOffset = numNodesPerDim[0];
+ const size_t colOffset = 1;
+
+ // For convenience
+ double &s = stiffness;
+ double &d = damping;
+
+ // Initialize the volume diagonal springs
+ if (stiffness || damping)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ for (size_t depth = 0; depth < numNodesPerDim[2] - 1; depth++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+ addSpring(createLinearSpring(state, nodeId, nodeId + depthOffset + rowOffset + colOffset, s, d));
+ addSpring(createLinearSpring(state, nodeId + colOffset, nodeId + depthOffset + rowOffset, s, d));
+ addSpring(createLinearSpring(state, nodeId + rowOffset, nodeId + depthOffset + colOffset, s, d));
+ addSpring(createLinearSpring(state, nodeId + rowOffset + colOffset, nodeId + depthOffset, s, d));
+ }
+ }
+ }
+ }
+}
+
+void MassSpring3DRepresentation::init3D(
+ const std::array<std::array<std::array<SurgSim::Math::Vector3d, 2>, 2>, 2> extremities,
+ size_t numNodesPerDim[3],
+ std::vector<size_t> nodeBoundaryConditions,
+ double totalMass,
+ double stiffnessStretching, double dampingStretching,
+ double stiffnessBending, double dampingBending,
+ double stiffnessFaceDiagonal, double dampingFaceDiagonal,
+ double stiffnessVolumeDiagonal, double dampingVolumeDiagonal)
+{
+ std::shared_ptr<SurgSim::Math::OdeState> state;
+ state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(getNumDofPerNode(), numNodesPerDim[0] * numNodesPerDim[1] * numNodesPerDim[2]);
+
+ // Nodes distribution is done by column 1st, row 2nd, depth 3rd
+ // Example: given a nodeId
+ // Its neighbor on the next column (colOffset) is nodeId + 1
+ // Its neighbor on the next row (rowOffset) is nodeId + numNodesPerDim[0]
+ // Its neighbor on the next depth (depthOffset) is nodeId + numNodesPerDim[0] * numNodesPerDim[1]
+ SURGSIM_ASSERT(numNodesPerDim[0] > 0) << "Number of nodes for dimension 1 is incorrect: " << numNodesPerDim[0];
+ SURGSIM_ASSERT(numNodesPerDim[1] > 0) << "Number of nodes for dimension 2 is incorrect: " << numNodesPerDim[1];
+ SURGSIM_ASSERT(numNodesPerDim[2] > 0) << "Number of nodes for dimension 3 is incorrect: " << numNodesPerDim[2];
+
+ const size_t numNodes = numNodesPerDim[0] * numNodesPerDim[1] * numNodesPerDim[2];
+
+ // Initialize the nodes position, velocity and mass
+ // Note: no need to apply the initialPose here, initialize will take care of it !
+ Vector3d depthExtremitiesDelta[2][2] =
+ {{(extremities[0][0][1] - extremities[0][0][0]) / static_cast<double>(numNodesPerDim[2] - 1) ,
+ (extremities[1][0][1] - extremities[1][0][0]) / static_cast<double>(numNodesPerDim[2] - 1)}
+ ,
+ {(extremities[0][1][1] - extremities[0][1][0]) / static_cast<double>(numNodesPerDim[2] - 1) ,
+ (extremities[1][1][1] - extremities[1][1][0]) / static_cast<double>(numNodesPerDim[2] - 1)}};
+
+ size_t nodeId = 0;
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ Vector3d depthExtremities[2][2];
+ depthExtremities[0][0] = extremities[0][0][0] + depthExtremitiesDelta[0][0] * static_cast<double>(depth);
+ depthExtremities[1][0] = extremities[1][0][0] + depthExtremitiesDelta[0][1] * static_cast<double>(depth);
+ depthExtremities[0][1] = extremities[0][1][0] + depthExtremitiesDelta[1][0] * static_cast<double>(depth);
+ depthExtremities[1][1] = extremities[1][1][0] + depthExtremitiesDelta[1][1] * static_cast<double>(depth);
+
+ Vector3d rowExtremitiesDelta[2] =
+ {(depthExtremities[0][1] - depthExtremities[0][0]) / static_cast<double>(numNodesPerDim[1] - 1) ,
+ (depthExtremities[1][1] - depthExtremities[1][0]) / static_cast<double>(numNodesPerDim[1] - 1)};
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ Vector3d rowExtremities[2];
+ rowExtremities[0] = depthExtremities[0][0] + rowExtremitiesDelta[0] * static_cast<double>(row);
+ rowExtremities[1] = depthExtremities[1][0] + rowExtremitiesDelta[1] * static_cast<double>(row);
+
+ Vector3d delta = (rowExtremities[1] - rowExtremities[0]) / static_cast<double>(numNodesPerDim[0] - 1);
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ addMass(std::make_shared<Mass>(totalMass / static_cast<double>(numNodes)));
+
+ Vector3d position(rowExtremities[0] + static_cast<double>(col) * delta);
+ SurgSim::Math::setSubVector(position, nodeId, 3, &state->getPositions());
+
+ nodeId++;
+ }
+ }
+ }
+
+ // Initialize all the stretching springs
+ init3DStretchingSprings(state, numNodesPerDim, stiffnessStretching, dampingStretching);
+
+ // Initialize all the bending springs
+ init3DBendingSprings(state, numNodesPerDim, stiffnessBending, dampingBending);
+
+ // Initialize all the face diagonal springs
+ init3DFaceDiagonalSprings(state, numNodesPerDim, stiffnessFaceDiagonal, dampingFaceDiagonal);
+
+ // Initialize all the volume diagonal springs
+ init3DVolumeDiagonalSprings(state, numNodesPerDim, stiffnessVolumeDiagonal, dampingVolumeDiagonal);
+
+ // Sets the boundary conditions
+ for (auto boundaryCondition = std::begin(nodeBoundaryConditions);
+ boundaryCondition != std::end(nodeBoundaryConditions);
+ boundaryCondition++)
+ {
+ state->addBoundaryCondition(*boundaryCondition);
+ }
+
+ // setInitialState: Initialize all the states + apply initialPose if any
+ setInitialState(state);
+}
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
diff --git a/SurgSim/Blocks/MassSpring3DRepresentation.h b/SurgSim/Blocks/MassSpring3DRepresentation.h
new file mode 100644
index 0000000..0554dc9
--- /dev/null
+++ b/SurgSim/Blocks/MassSpring3DRepresentation.h
@@ -0,0 +1,100 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_MASSSPRING3DREPRESENTATION_H
+#define SURGSIM_BLOCKS_MASSSPRING3DREPRESENTATION_H
+
+#include <array>
+#include <vector>
+
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+// This class defines a simple MassSpring 3D structures
+class MassSpring3DRepresentation : public SurgSim::Physics::MassSpringRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The model name
+ explicit MassSpring3DRepresentation(const std::string& name) :
+ SurgSim::Physics::MassSpringRepresentation(name)
+ {
+ }
+
+ /// Initializes a 3D model
+ /// \param extremities 8 positions forming the extremities of the 3D regular model (8 corners)
+ /// \param numNodesPerDim The number of nodes to be created for each dimension (here 3)
+ /// \param nodeBoundaryConditions The list of all nodeId being boundary conditions (fixed node)
+ /// \param totalMass The total mass of the mass spring (evenly spread out on the masses)
+ /// \param stiffnessStretching, dampingStretching The spring param for all stretching springs (edges)
+ /// \param stiffnessBending, dampingBending The spring param for all bending springs (edges)
+ /// \param stiffnessFaceDiagonal, dampingFaceDiagonal The spring param for all face diagonal springs (faces)
+ /// \param stiffnessVolumeDiagonal, dampingVolumeDiagonal The spring param for all volume diagonal springs (volume)
+ /// \note Stretching springs are connecting neighbors, bending springs are connecting 1 node
+ /// \note to its 2nd degree neighbors, creating a bending force around the middle node.
+ /// \note Face diagonal springs aim at maintaining the area of a square
+ /// \note Volume diagonal springs aim at maintaining the volume of a cube
+ /// \note extremities are organized as follow:
+ /// \note [0][1][0] *---* [1][1][0]
+ /// \note [0][1][1] *---* [1][1][1]
+ /// \note [0][0][0] <- | | * -> [1][0][0]
+ /// \note [0][0][1] *---* [1][0][1]
+ void init3D(
+ const std::array<std::array<std::array<SurgSim::Math::Vector3d, 2>, 2>, 2> extremities,
+ size_t numNodesPerDim[3],
+ std::vector<size_t> nodeBoundaryConditions,
+ double totalMass,
+ double stiffnessStretching, double dampingStretching,
+ double stiffnessBending, double dampingBending,
+ double stiffnessFaceDiagonal, double dampingFaceDiagonal,
+ double stiffnessVolumeDiagonal, double dampingVolumeDiagonal);
+
+private:
+ /// Helper method to initialize/add all stretching springs on a 3D structure
+ /// \param state The state to initialize the springs with (rest lengths calculation)
+ /// \param numNodesPerDim The number of nodes on the 3 dimensions
+ /// \param stiffness, damping The spring parameters
+ void init3DStretchingSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[3], double stiffness, double damping);
+ /// Helper method to initialize/add all bending springs on a 3D structure
+ /// \param state The state to initialize the springs with (rest lengths calculation)
+ /// \param numNodesPerDim The number of nodes on the 3 dimensions
+ /// \param stiffness, damping The spring parameters
+ void init3DBendingSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[3], double stiffness, double damping);
+ /// Helper method to initialize/add all face diagonal springs on a 3D structure
+ /// \param state The state to initialize the springs with (rest lengths calculation)
+ /// \param numNodesPerDim The number of nodes on the 3 dimensions
+ /// \param stiffness, damping The spring parameters
+ void init3DFaceDiagonalSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[3], double stiffness, double damping);
+ /// Helper method to initialize/add all volume diagonal springs on a 3D structure
+ /// \param state The state to initialize the springs with (rest lengths calculation)
+ /// \param numNodesPerDim The number of nodes on the 3 dimensions
+ /// \param stiffness, damping The spring parameters
+ void init3DVolumeDiagonalSprings(const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t numNodesPerDim[3], double stiffness, double damping);
+};
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_MASSSPRING3DREPRESENTATION_H
diff --git a/SurgSim/Blocks/MassSpringNDRepresentationUtils.cpp b/SurgSim/Blocks/MassSpringNDRepresentationUtils.cpp
new file mode 100644
index 0000000..33541e0
--- /dev/null
+++ b/SurgSim/Blocks/MassSpringNDRepresentationUtils.cpp
@@ -0,0 +1,47 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Blocks/MassSpringNDRepresentationUtils.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+std::shared_ptr<SurgSim::Physics::LinearSpring> createLinearSpring(
+ const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t nodeId0, size_t nodeId1,
+ double stiffness, double damping)
+{
+ using SurgSim::Math::Vector3d;
+
+ std::shared_ptr<SurgSim::Physics::LinearSpring> spring;
+ spring = std::make_shared<SurgSim::Physics::LinearSpring>(nodeId0, nodeId1);
+
+ const Vector3d& A = SurgSim::Math::getSubVector(state->getPositions(), nodeId0, 3);
+ const Vector3d& B = SurgSim::Math::getSubVector(state->getPositions(), nodeId1, 3);
+ spring->setStiffness(stiffness);
+ spring->setDamping(damping);
+ spring->setRestLength((B-A).norm());
+
+ return spring;
+}
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
diff --git a/SurgSim/Blocks/MassSpringNDRepresentationUtils.h b/SurgSim/Blocks/MassSpringNDRepresentationUtils.h
new file mode 100644
index 0000000..66b2a72
--- /dev/null
+++ b/SurgSim/Blocks/MassSpringNDRepresentationUtils.h
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_MASSSPRINGNDREPRESENTATIONUTILS_H
+#define SURGSIM_BLOCKS_MASSSPRINGNDREPRESENTATIONUTILS_H
+
+#include <memory>
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class OdeState;
+}
+
+namespace Physics
+{
+class LinearSpring;
+}
+
+namespace Blocks
+{
+
+/// Helper method to create a LinearSpring
+/// \param state The state to initialize the spring with (rest length calculation)
+/// \param nodeId0, nodeId1 Node ids of the 2 connected masses
+/// \param stiffness, damping The spring parameters
+/// \return The newly create spring
+std::shared_ptr<SurgSim::Physics::LinearSpring> createLinearSpring(
+ const std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t nodeId0, size_t nodeId1,
+ double stiffness, double damping);
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_MASSSPRINGNDREPRESENTATIONUTILS_H
diff --git a/SurgSim/Blocks/PoseInterpolator.cpp b/SurgSim/Blocks/PoseInterpolator.cpp
new file mode 100644
index 0000000..952e38c
--- /dev/null
+++ b/SurgSim/Blocks/PoseInterpolator.cpp
@@ -0,0 +1,149 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Blocks/PoseInterpolator.h"
+
+#include "SurgSim/Framework/SceneElement.h"
+
+#include <memory>
+
+using SurgSim::Math::RigidTransform3d;
+namespace SurgSim
+{
+namespace Blocks
+{
+
+PoseInterpolator::PoseInterpolator(const std::string& name) :
+ Behavior(name),
+ m_startingPose(RigidTransform3d::Identity()),
+ m_endingPose(RigidTransform3d::Identity()),
+ m_duration(1.0),
+ m_currentTime(0.0)
+{
+
+}
+
+void PoseInterpolator::setStartingPose(const SurgSim::Math::RigidTransform3d& transform)
+{
+ if (!isInitialized())
+ {
+ m_optionalStartPose.setValue(transform);
+ }
+}
+
+void PoseInterpolator::setEndingPose(const SurgSim::Math::RigidTransform3d& transform)
+{
+ if (!isInitialized())
+ {
+ m_endingPose = transform;
+ }
+}
+
+void PoseInterpolator::setTarget(std::shared_ptr<SurgSim::Framework::SceneElement> target)
+{
+ if (!isInitialized())
+ {
+ m_target = target;
+ }
+}
+
+void PoseInterpolator::setDuration(double t)
+{
+ if (!isInitialized())
+ {
+ m_duration = t;
+ }
+}
+
+double PoseInterpolator::getDuration() const
+{
+ return m_duration;
+}
+
+bool PoseInterpolator::doInitialize()
+{
+ return true;
+}
+
+bool PoseInterpolator::doWakeUp()
+{
+ bool result = false;
+ if (m_target == nullptr)
+ {
+ m_target = getSceneElement();
+ }
+ if (m_target != nullptr)
+ {
+ m_startingPose = (m_optionalStartPose.hasValue()) ? m_optionalStartPose.getValue() : m_target->getPose();
+ result = true;
+ }
+ return result;
+}
+
+void PoseInterpolator::update(double dt)
+{
+ m_currentTime += dt;
+
+ if (m_currentTime >= m_duration)
+ {
+ if (isLoop())
+ {
+ m_currentTime = m_currentTime - m_duration;
+ }
+ else if (isPingPong())
+ {
+ m_currentTime = m_currentTime - m_duration;
+ std::swap(m_endingPose, m_startingPose);
+ }
+ else
+ {
+ m_currentTime = m_duration;
+ getSceneElement()->removeComponent(getName());
+ }
+ }
+
+ m_target->setPose(SurgSim::Math::interpolate(m_startingPose, m_endingPose, m_currentTime/m_duration));
+}
+
+void PoseInterpolator::setLoop(bool val)
+{
+ m_loop = val;
+ if (m_loop)
+ {
+ m_pingpong = false;
+ }
+}
+
+bool PoseInterpolator::isLoop() const
+{
+ return m_loop;
+}
+
+void PoseInterpolator::setPingPong(bool val)
+{
+ m_pingpong = val;
+ if (m_pingpong)
+ {
+ m_loop = false;
+ }
+}
+
+bool PoseInterpolator::isPingPong() const
+{
+ return m_pingpong;
+}
+
+}; // Blocks
+}; // Surgsim
diff --git a/SurgSim/Blocks/PoseInterpolator.h b/SurgSim/Blocks/PoseInterpolator.h
new file mode 100644
index 0000000..8ee4bc1
--- /dev/null
+++ b/SurgSim/Blocks/PoseInterpolator.h
@@ -0,0 +1,125 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_POSEINTERPOLATOR_H
+#define SURGSIM_BLOCKS_POSEINTERPOLATOR_H
+
+#include <memory>
+#include <string>
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+ class SceneElement;
+}
+}
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+/// Perform linear interpolation on two poses
+class PoseInterpolator : public SurgSim::Framework::Behavior
+{
+public:
+
+ /// Constructor
+ explicit PoseInterpolator(const std::string& name);
+
+ /// Set the starting pose. This is optional, if not set the target's pose
+ /// will be used as the starting pose.
+ /// \param transform The starting pose.
+ void setStartingPose(const SurgSim::Math::RigidTransform3d& transform);
+
+ /// Set the end pose.
+ /// \param transform The end pose.
+ void setEndingPose(const SurgSim::Math::RigidTransform3d& transform);
+
+ /// Set the target of the interpolation, this is where the interpolated transform
+ /// will be applied to. If this value is not set, the Scene Element that contains
+ /// this PoseInterpolator will be used. If no starting pose is set, the pose of
+ /// this scene element will be used as the starting pose
+ /// \param target The target that will use the interpolated pose.
+ void setTarget(std::shared_ptr<SurgSim::Framework::SceneElement> target);
+
+ /// Set the duration of the interpolation.
+ /// \param t The duration in seconds.
+ void setDuration(double t);
+
+ /// Get the duration.
+ /// \return The duration in seconds.
+ double getDuration() const;
+
+ /// Sets the interpolation to looping, pingpong and loop cannot be used together.
+ /// \param val If true the interpolation will loop.
+ void setLoop(bool val);
+
+ /// \return true If the interpolation is looping.
+ bool isLoop() const;
+
+ /// Sets the interpolation to ping pong back and forth between the starting and ending poses.
+ /// pingpong and loop cannot be used together.
+ /// \param val If true the interpolation will ping pong.
+ void setPingPong(bool val);
+
+ /// \return true If the interpolation is doing ping pong.
+ bool isPingPong() const;
+
+ /// Overridden from Behavior
+ virtual void update(double dt) override;
+
+private:
+
+ /// Optional value to take the from rigid transform
+ SurgSim::DataStructures::OptionalValue<SurgSim::Math::RigidTransform3d> m_optionalStartPose;
+
+ /// Target of the interpolation
+ SurgSim::Math::RigidTransform3d m_startingPose;
+
+ /// Start of the interpolation
+ SurgSim::Math::RigidTransform3d m_endingPose;
+
+ /// Target for the interpolated RigidTransform
+ std::shared_ptr<SurgSim::Framework::SceneElement> m_target;
+
+ /// Duration of the interpolation
+ double m_duration;
+
+ /// How far through the interpolation we are
+ double m_currentTime;
+
+ /// Whether to pingpong
+ bool m_pingpong;
+
+ /// Whether to loop
+ bool m_loop;
+
+ /// Overridden from Component
+ virtual bool doWakeUp() override;
+
+ /// Overridden from Component
+ virtual bool doInitialize() override;
+};
+
+
+}; // Blocks
+}; // Surgsim
+
+#endif
diff --git a/SurgSim/Blocks/SphereElement.cpp b/SurgSim/Blocks/SphereElement.cpp
new file mode 100644
index 0000000..f4d1b70
--- /dev/null
+++ b/SurgSim/Blocks/SphereElement.cpp
@@ -0,0 +1,93 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+
+#include "SurgSim/Blocks/SphereElement.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+
+
+using SurgSim::Blocks::SphereElement;
+using SurgSim::Graphics::OsgMaterial;
+using SurgSim::Graphics::OsgShader;
+using SurgSim::Graphics::OsgSphereRepresentation;
+using SurgSim::Math::SphereShape;
+using SurgSim::Physics::RigidCollisionRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+
+SphereElement::SphereElement(const std::string& name) :
+ SurgSim::Framework::SceneElement(name), m_name(name)
+{
+}
+
+
+SphereElement::~SphereElement()
+{
+}
+
+bool SphereElement::doInitialize()
+{
+ std::shared_ptr<RigidRepresentation> physicsRepresentation =
+ std::make_shared<RigidRepresentation>(m_name + " Physics");
+
+ physicsRepresentation->setDensity(700.0); // Wood
+ physicsRepresentation->setLinearDamping(0.1);
+
+ std::shared_ptr<SphereShape> shape = std::make_shared<SphereShape>(0.1); // 1cm Sphere
+ physicsRepresentation->setShape(shape);
+
+ std::shared_ptr<OsgSphereRepresentation> graphicsRepresentation =
+ std::make_shared<OsgSphereRepresentation>(m_name + " Graphics");
+ graphicsRepresentation->setRadius(shape->getRadius());
+
+ std::shared_ptr<OsgMaterial> material = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<OsgShader> shader = std::make_shared<OsgShader>();
+
+ shader->setVertexShaderSource(
+ "varying vec4 color;\n"
+ "void main(void)\n"
+ "{\n"
+ " gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;\n"
+ " color.rgb = gl_Normal;\n"
+ " color.a = 1.0;\n"
+ "}");
+ shader->setFragmentShaderSource(
+ "varying vec4 color;\n"
+ "void main(void)\n"
+ "{\n"
+ " gl_FragColor = color;\n"
+ "}");
+ material->setShader(shader);
+ graphicsRepresentation->setMaterial(material);
+
+ addComponent(physicsRepresentation);
+ addComponent(graphicsRepresentation);
+
+ auto rigidCollision = std::make_shared<RigidCollisionRepresentation>("Sphere Collision Representation");
+ physicsRepresentation->setCollisionRepresentation(rigidCollision);
+ addComponent(rigidCollision);
+
+ return true;
+}
+
+bool SphereElement::doWakeUp()
+{
+ return true;
+}
diff --git a/SurgSim/Blocks/SphereElement.h b/SurgSim/Blocks/SphereElement.h
new file mode 100644
index 0000000..ce812cd
--- /dev/null
+++ b/SurgSim/Blocks/SphereElement.h
@@ -0,0 +1,54 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_SPHEREELEMENT_H
+#define SURGSIM_BLOCKS_SPHEREELEMENT_H
+
+#include "SurgSim/Framework/SceneElement.h"
+
+#include "SurgSim/Math/RigidTransform.h"
+
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+class SphereElement : public SurgSim::Framework::SceneElement
+{
+public:
+
+ explicit SphereElement(const std::string& name);
+
+ ~SphereElement();
+
+
+protected:
+ virtual bool doInitialize();
+
+ virtual bool doWakeUp();
+
+
+private:
+ std::string m_name;
+
+};
+
+
+};
+};
+
+#endif
diff --git a/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.cpp b/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.cpp
new file mode 100644
index 0000000..24ac92e
--- /dev/null
+++ b/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.cpp
@@ -0,0 +1,107 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h"
+
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/DeformableRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior,
+ TransferPhysicsToGraphicsMeshBehavior);
+
+TransferPhysicsToGraphicsMeshBehavior::TransferPhysicsToGraphicsMeshBehavior(const std::string& name) :
+ SurgSim::Framework::Behavior(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TransferPhysicsToGraphicsMeshBehavior,
+ std::shared_ptr<SurgSim::Framework::Component>, Source, getSource, setSource);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TransferPhysicsToGraphicsMeshBehavior,
+ std::shared_ptr<SurgSim::Framework::Component>, Target, getTarget, setTarget);
+}
+
+void TransferPhysicsToGraphicsMeshBehavior::setSource(const std::shared_ptr<SurgSim::Framework::Component>& source)
+{
+ SURGSIM_ASSERT(nullptr != source) << " 'source' can not be nullptr.";
+
+ auto deformable = std::dynamic_pointer_cast<SurgSim::Physics::DeformableRepresentation>(source);
+ SURGSIM_ASSERT(nullptr != deformable) << " 'source' is not a SurgSim::Physics::DeformableRepresentation.";
+
+ m_source = deformable;
+}
+
+void TransferPhysicsToGraphicsMeshBehavior::setTarget(const std::shared_ptr<SurgSim::Framework::Component>& target)
+{
+ SURGSIM_ASSERT(nullptr != target) << " 'target' can not be nullptr.";
+
+ auto mesh = std::dynamic_pointer_cast<SurgSim::Graphics::MeshRepresentation>(target);
+ SURGSIM_ASSERT(nullptr != mesh) << " 'target' is not a SurgSim::Graphics::MeshRepresentation.";
+
+ m_target = mesh;
+}
+
+std::shared_ptr<SurgSim::Physics::DeformableRepresentation> TransferPhysicsToGraphicsMeshBehavior::getSource() const
+{
+ return m_source;
+}
+
+std::shared_ptr<SurgSim::Graphics::MeshRepresentation> TransferPhysicsToGraphicsMeshBehavior::getTarget() const
+{
+ return m_target;
+}
+
+void TransferPhysicsToGraphicsMeshBehavior::update(double dt)
+{
+ auto state = m_source->getFinalState();
+
+ for (size_t nodeId = 0; nodeId < state->getNumNodes(); ++nodeId)
+ {
+ m_target->getMesh()->setVertexPosition(nodeId, state->getPosition(nodeId));
+ }
+}
+
+bool TransferPhysicsToGraphicsMeshBehavior::doInitialize()
+{
+ return true;
+}
+
+bool TransferPhysicsToGraphicsMeshBehavior::doWakeUp()
+{
+ auto state = m_source->getFinalState();
+ auto target = m_target->getMesh();
+
+ if (target->getNumVertices() == 0)
+ {
+ for (size_t nodeId = 0; nodeId < state->getNumNodes(); ++nodeId)
+ {
+ SurgSim::Graphics::Mesh::VertexType vertex(state->getPosition(nodeId));
+ target->addVertex(vertex);
+ }
+ }
+
+ return true;
+}
+
+}; //namespace Blocks
+}; //namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h b/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h
new file mode 100644
index 0000000..bf521cb
--- /dev/null
+++ b/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h
@@ -0,0 +1,86 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_TRANSFERPHYSICSTOGRAPHICSMESHBEHAVIOR_H
+#define SURGSIM_BLOCKS_TRANSFERPHYSICSTOGRAPHICSMESHBEHAVIOR_H
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/Macros.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Component;
+}
+
+namespace Graphics
+{
+class MeshRepresentation;
+}
+
+namespace Physics
+{
+class DeformableRepresentation;
+}
+
+namespace Blocks
+{
+SURGSIM_STATIC_REGISTRATION(TransferPhysicsToGraphicsMeshBehavior);
+
+/// Behavior to copy positions of a PhysicsRepresentation to a GraphicsMesh.
+class TransferPhysicsToGraphicsMeshBehavior : public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit TransferPhysicsToGraphicsMeshBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior);
+
+ /// Set the representation from which the positions are from
+ /// \param source The physics representation
+ void setSource(const std::shared_ptr<SurgSim::Framework::Component>& source);
+
+ /// Set the representation which will receive the positions
+ /// \param target The Graphics Mesh representation
+ void setTarget(const std::shared_ptr<SurgSim::Framework::Component>& target);
+
+ /// Get the Physics representation which sends the positions
+ /// \return The Physics representation which produces positions.
+ std::shared_ptr<SurgSim::Physics::DeformableRepresentation> getSource() const;
+
+ /// Get the Graphics representation which receives the positions
+ /// \return The Graphics Mesh representation which receives positions.
+ std::shared_ptr<SurgSim::Graphics::MeshRepresentation> getTarget() const;
+
+ virtual void update(double dt) override;
+
+private:
+ virtual bool doInitialize() override;
+ virtual bool doWakeUp() override;
+
+ /// The DeformableRepresentation from which the Ode state comes.
+ std::shared_ptr<SurgSim::Physics::DeformableRepresentation> m_source;
+
+ /// The Graphics Mesh Representation to which the vertices' positions are set.
+ std::shared_ptr<SurgSim::Graphics::MeshRepresentation> m_target;
+};
+
+}; // namespace Blocks
+}; // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_TRANSFERPHYSICSTOGRAPHICSMESHBEHAVIOR_H
\ No newline at end of file
diff --git a/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.cpp b/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.cpp
new file mode 100644
index 0000000..cc15390
--- /dev/null
+++ b/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.cpp
@@ -0,0 +1,108 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h"
+
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/DataStructures/Vertex.h"
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/DeformableRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::TransferPhysicsToPointCloudBehavior,
+ TransferPhysicsToPointCloudBehavior);
+
+TransferPhysicsToPointCloudBehavior::TransferPhysicsToPointCloudBehavior(const std::string& name) :
+ SurgSim::Framework::Behavior(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TransferPhysicsToPointCloudBehavior,
+ std::shared_ptr<SurgSim::Framework::Component>, Source, getSource, setSource);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TransferPhysicsToPointCloudBehavior,
+ std::shared_ptr<SurgSim::Framework::Component>, Target, getTarget, setTarget);
+}
+
+void TransferPhysicsToPointCloudBehavior::setSource(const std::shared_ptr<SurgSim::Framework::Component>& source)
+{
+ SURGSIM_ASSERT(nullptr != source) << "'source' can not be nullptr.";
+
+ auto deformable = std::dynamic_pointer_cast<SurgSim::Physics::DeformableRepresentation>(source);
+ SURGSIM_ASSERT(nullptr != deformable) << "'source' is not a SurgSim::Physics::DeformableRepresentation.";
+
+ m_source = deformable;
+}
+
+void TransferPhysicsToPointCloudBehavior::setTarget(const std::shared_ptr<SurgSim::Framework::Component>& target)
+{
+ SURGSIM_ASSERT(nullptr != target) << "'target' can not be nullptr.";
+
+ auto pointCloud = std::dynamic_pointer_cast<SurgSim::Graphics::PointCloudRepresentation>(target);
+ SURGSIM_ASSERT(nullptr != pointCloud) << " 'target' is not a SurgSim::Graphics::PointCloudRepresentation.";
+
+ m_target = pointCloud;
+}
+
+std::shared_ptr<SurgSim::Physics::DeformableRepresentation> TransferPhysicsToPointCloudBehavior::getSource() const
+{
+ return m_source;
+}
+
+std::shared_ptr<SurgSim::Graphics::PointCloudRepresentation> TransferPhysicsToPointCloudBehavior::getTarget() const
+{
+ return m_target;
+}
+
+void TransferPhysicsToPointCloudBehavior::update(double dt)
+{
+ auto state = m_source->getFinalState();
+
+ auto target = m_target->getVertices();
+ for (size_t nodeId = 0; nodeId < state->getNumNodes(); ++nodeId)
+ {
+ target->setVertexPosition(nodeId, state->getPosition(nodeId));
+ }
+}
+
+bool TransferPhysicsToPointCloudBehavior::doInitialize()
+{
+ return true;
+}
+
+bool TransferPhysicsToPointCloudBehavior::doWakeUp()
+{
+ auto state = m_source->getFinalState();
+ auto target = m_target->getVertices();
+
+ if (target->getNumVertices() == 0)
+ {
+ for (size_t nodeId = 0; nodeId < state->getNumNodes(); ++nodeId)
+ {
+ SurgSim::Graphics::PointCloud::VertexType vertex(state->getPosition(nodeId));
+ target->addVertex(vertex);
+ }
+ }
+ return true;
+}
+
+}; //namespace Blocks
+}; //namespace SurgSim
diff --git a/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h b/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h
new file mode 100644
index 0000000..b993055
--- /dev/null
+++ b/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h
@@ -0,0 +1,86 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_TRANSFERPHYSICSTOPOINTCLOUDBEHAVIOR_H
+#define SURGSIM_BLOCKS_TRANSFERPHYSICSTOPOINTCLOUDBEHAVIOR_H
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/Macros.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Component;
+}
+
+namespace Graphics
+{
+class PointCloudRepresentation;
+}
+
+namespace Physics
+{
+class DeformableRepresentation;
+}
+
+namespace Blocks
+{
+SURGSIM_STATIC_REGISTRATION(TransferPhysicsToPointCloudBehavior);
+
+/// Behavior to copy positions of a PhysicsRepresentation to a PointCloud.
+class TransferPhysicsToPointCloudBehavior : public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit TransferPhysicsToPointCloudBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::TransferPhysicsToPointCloudBehavior);
+
+ /// Set the representation from which the positions are from
+ /// \param source The physics representation
+ void setSource(const std::shared_ptr<SurgSim::Framework::Component>& source);
+
+ /// Set the point cloud representation which will receive the positions
+ /// \param target The Graphics PointCloud representation
+ void setTarget(const std::shared_ptr<SurgSim::Framework::Component>& target);
+
+ /// Get the Physics representation which sends the positions
+ /// \return The Physics representation which produces positions.
+ std::shared_ptr<SurgSim::Physics::DeformableRepresentation> getSource() const;
+
+ /// Get the point cloud representation which receives the positions
+ /// \return The Graphics PointCloud representation which receives positions.
+ std::shared_ptr<SurgSim::Graphics::PointCloudRepresentation> getTarget() const;
+
+ virtual void update(double dt) override;
+
+private:
+ virtual bool doInitialize() override;
+ virtual bool doWakeUp() override;
+
+ /// The DeformableRepresentation from which the Ode state comes.
+ std::shared_ptr<SurgSim::Physics::DeformableRepresentation> m_source;
+
+ /// The Graphics PointCloud Representation to which the vertices' positions are set.
+ std::shared_ptr<SurgSim::Graphics::PointCloudRepresentation> m_target;
+};
+
+}; // namespace Blocks
+}; // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_TRANSFERPHYSICSTOPOINTCLOUDBEHAVIOR_H
\ No newline at end of file
diff --git a/SurgSim/Blocks/UnitTests/CMakeLists.txt b/SurgSim/Blocks/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..ef906a1
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/CMakeLists.txt
@@ -0,0 +1,53 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories (
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ KeyboardTogglesComponentBehaviorTests.cpp
+ MassSpring1DRepresentationTests.cpp
+ MassSpring2DRepresentationTests.cpp
+ MassSpring3DRepresentationTests.cpp
+ MassSpringNDRepresentationUtilsTests.cpp
+ PoseInterpolatorTests.cpp
+ SpringTestUtils.cpp
+ TransferPhysicsToGraphicsMeshBehaviorTests.cpp
+ TransferPhysicsToPointCloudBehaviorTests.cpp
+ VisualizeContactsBehaviorTests.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ SpringTestUtils.h
+)
+
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+set(LIBS
+ SurgSimBlocks
+ SurgSimGraphics
+)
+
+surgsim_add_unit_tests(SurgSimBlocksTest)
+
+
+set_target_properties(SurgSimBlocksTest PROPERTIES FOLDER "Blocks")
diff --git a/SurgSim/Blocks/UnitTests/Data/Geometry/wound_deformable.ply b/SurgSim/Blocks/UnitTests/Data/Geometry/wound_deformable.ply
new file mode 100644
index 0000000..b4de671
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/Data/Geometry/wound_deformable.ply
@@ -0,0 +1,2389 @@
+ply
+format ascii 1.0
+comment This file is a part of the OpenSurgSim project.
+comment Copyright 2013, SimQuest Solutions Inc.
+comment
+comment Licensed under the Apache License, Version 2.0 (the "License");
+comment you may not use this file except in compliance with the License.
+comment You may obtain a copy of the License at
+comment
+comment http://www.apache.org/licenses/LICENSE-2.0
+comment
+comment Unless required by applicable law or agreed to in writing, software
+comment distributed under the License is distributed on an "AS IS" BASIS,
+comment WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+comment See the License for the specific language governing permissions and
+comment limitations under the License.
+comment
+comment This file provides the geometry which describes a tetrahedral volume
+comment mesh and triangular surface mesh of an arm wound.
+comment
+element vertex 391
+property double x
+property double y
+property double z
+element face 407
+property list uint uint vertex_indices
+element 3d_element 1476
+property list uint uint vertex_indices
+element boundary_condition 79
+property uint vertex_index
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+-0.017638 0.001427 -0.012363
+-0.018984 0.002069 -0.016867
+-0.014913 -0.001358 -0.015090
+-0.018090 -0.001832 -0.017359
+0.012643 -0.002455 0.001226
+0.009376 -0.006324 0.003409
+0.015479 -0.002289 0.003265
+0.014119 -0.002367 0.007046
+-0.000236 -0.006438 -0.015323
+-0.003732 -0.004010 -0.013609
+0.002155 -0.000546 -0.013612
+0.001262 -0.001523 -0.018049
+0.019667 -0.017785 0.008674
+0.029299 -0.005262 0.004546
+0.046990 -0.020879 0.007375
+0.037332 -0.020768 -0.005328
+0.033586 -0.016523 0.018450
+0.034101 -0.002031 0.016309
+0.033392 -0.002474 0.009623
+0.025349 -0.003774 0.014192
+0.019145 0.004850 0.012264
+0.020216 0.004879 0.011113
+0.020968 0.001399 0.010219
+0.018687 0.001314 0.008476
+-0.007352 0.001018 0.005860
+-0.007333 -0.005722 0.014291
+-0.013652 -0.000173 0.002866
+-0.007113 -0.008223 -0.002577
+-0.006477 -0.004144 -0.015932
+-0.010957 -0.004967 -0.018490
+-0.007629 -0.006377 -0.018801
+-0.009906 -0.007928 -0.014976
+0.014105 -0.020914 -0.009294
+0.009112 -0.006286 -0.008604
+0.006268 -0.009189 -0.014543
+0.002300 -0.008261 -0.009596
+0.006635 0.000026 -0.006193
+0.010115 -0.000309 -0.007852
+0.005102 -0.004418 -0.008996
+-0.006609 0.001420 -0.025080
+-0.011057 -0.007486 -0.026293
+-0.008811 0.000184 -0.034044
+-0.005338 -0.005470 -0.024596
+0.001259 0.002603 0.004418
+0.000123 0.002377 -0.000362
+0.002998 0.002713 0.001375
+0.002769 -0.002124 0.003090
+-0.020005 -0.002711 -0.020006
+-0.021803 0.001971 -0.019803
+-0.021059 0.001058 -0.014582
+0.014631 0.000524 0.027783
+0.022184 -0.002620 0.021520
+0.018739 -0.011699 0.030717
+0.015337 -0.003630 0.015467
+0.016049 0.003671 -0.002811
+0.018898 0.003898 -0.000351
+0.014287 0.000683 -0.000350
+0.019319 -0.000205 -0.003032
+0.046990 -0.007619 0.007375
+0.040075 -0.002131 -0.002012
+0.037332 -0.007509 -0.005328
+-0.011140 0.001039 -0.002776
+-0.014033 -0.006140 -0.001762
+-0.016684 0.000633 -0.005670
+-0.011809 -0.002190 -0.009170
+0.024204 0.005929 0.015670
+0.026606 0.006410 0.017058
+0.023576 0.006924 0.019679
+0.024566 0.001831 0.017759
+0.013743 0.004124 0.008547
+0.011935 0.004109 0.005502
+0.014670 0.004333 0.007344
+0.012704 0.000753 0.008033
+-0.003034 -0.009956 -0.020825
+-0.006038 -0.007999 -0.012282
+0.014736 0.003042 -0.006304
+0.014286 -0.003192 -0.009813
+0.014065 0.002151 -0.010793
+0.019818 -0.002939 -0.008113
+0.036755 0.006575 0.019271
+0.036907 0.001084 0.018946
+0.033263 0.001097 0.023070
+0.033376 0.002597 0.017918
+0.022543 0.001069 0.006601
+0.020245 -0.001947 0.007184
+0.019727 0.001041 0.004292
+0.023642 -0.002413 0.005541
+0.029219 0.001517 -0.003210
+0.022506 -0.007309 -0.002980
+0.013584 0.001132 0.004599
+0.015737 0.001218 0.006218
+0.015374 -0.005687 -0.005108
+0.019027 -0.006775 -0.014839
+0.030475 0.001257 0.015552
+0.027297 -0.001544 0.019319
+-0.006877 -0.002740 -0.012407
+-0.009683 -0.005474 -0.007552
+-0.008963 -0.000571 -0.011342
+-0.009123 -0.005772 -0.011405
+0.006753 0.004254 0.022359
+0.010408 0.004267 0.016270
+0.015903 0.005679 0.020105
+0.017492 0.000471 0.018668
+-0.018916 -0.006283 -0.013171
+-0.013552 -0.004660 -0.011679
+-0.015191 -0.008193 -0.008642
+-0.014454 -0.008852 -0.015804
+0.016241 0.004334 0.010184
+0.017210 0.004543 0.009049
+0.012207 0.000507 -0.002006
+0.015357 0.000245 -0.003791
+0.007154 -0.001854 0.005752
+0.011373 -0.002515 0.009976
+0.009600 -0.002702 0.003200
+0.022251 -0.002063 0.015960
+0.021468 -0.001789 0.012242
+0.017030 0.000365 0.013057
+0.020308 0.001792 0.013974
+0.013639 -0.003166 -0.002562
+0.011924 -0.003711 -0.006304
+-0.014146 -0.020429 -0.047689
+-0.014146 -0.007170 -0.047689
+-0.024140 -0.017967 -0.040188
+-0.012259 -0.006676 -0.033836
+0.002537 -0.008403 -0.037874
+-0.001499 -0.000057 -0.029561
+-0.000610 -0.002936 -0.039936
+0.031048 0.003384 0.005728
+0.031021 0.004616 0.009841
+0.029058 -0.002228 0.009819
+-0.017817 -0.005609 -0.020805
+-0.021077 -0.009144 -0.020053
+-0.019782 -0.005521 -0.017072
+-0.021431 -0.005952 -0.022335
+0.028581 0.004545 0.037841
+0.022848 0.002598 0.033651
+0.026518 0.008415 0.028378
+-0.008942 -0.024739 -0.008176
+-0.020167 -0.001455 -0.000035
+-0.019937 -0.006669 -0.033633
+-0.011533 -0.022757 -0.029013
+0.025681 0.001454 0.013298
+0.021352 0.005330 0.013823
+0.022555 0.005159 0.012575
+-0.018697 -0.003565 -0.023866
+-0.003741 -0.007976 -0.007476
+-0.004960 -0.004029 -0.006795
+0.035352 0.005531 0.016102
+0.033038 0.004995 0.012719
+0.036201 0.003833 0.011247
+-0.014781 0.002213 -0.020373
+-0.016880 -0.002824 -0.021921
+-0.016398 0.001977 -0.024375
+-0.013707 -0.002037 -0.021513
+-0.001967 0.002359 -0.017593
+-0.005030 0.002252 -0.019030
+-0.000406 0.001476 -0.021158
+-0.003015 -0.003057 -0.018944
+0.005527 0.000425 -0.000868
+0.005663 0.003045 0.003152
+0.003921 0.003399 0.000160
+-0.015527 -0.005152 -0.022539
+-0.015560 -0.006666 -0.026900
+-0.014996 -0.002896 -0.024296
+-0.013917 -0.007956 -0.024021
+0.004887 -0.021029 0.001820
+0.002057 -0.006870 -0.003212
+0.022552 -0.001675 0.009108
+-0.024489 0.000741 -0.016918
+-0.024391 0.001936 -0.022406
+-0.024601 -0.004739 -0.018778
+0.006850 -0.006797 -0.004277
+0.004032 -0.004089 -0.000551
+0.012333 0.004106 0.011509
+0.005130 0.003275 0.012820
+0.007317 0.003436 0.008224
+-0.004058 0.001878 0.001264
+-0.014203 0.001575 -0.029189
+-0.010570 -0.002455 -0.021860
+-0.012199 0.002106 -0.022621
+-0.025827 -0.009187 -0.021112
+-0.021601 -0.004637 -0.026044
+-0.017290 -0.008529 -0.023047
+-0.013967 -0.002130 -0.018341
+-0.014398 -0.004396 -0.019489
+-0.016250 -0.002623 -0.019238
+0.002921 -0.006003 -0.021793
+0.009600 0.002834 -0.010048
+0.012195 0.002939 -0.008156
+0.014492 -0.021349 -0.029300
+0.026117 -0.020765 -0.017965
+0.014492 -0.008090 -0.029300
+0.002621 -0.007830 -0.029082
+0.008423 0.000639 0.001153
+0.030678 0.007333 0.019744
+0.026656 0.007758 0.022058
+0.020949 0.007122 0.023900
+0.024451 0.002704 0.023354
+-0.023157 -0.024020 -0.019768
+0.029177 0.003608 0.018712
+0.012003 0.005360 0.025917
+0.004293 -0.015189 0.020860
+0.002537 -0.021663 -0.037874
+0.017564 -0.002515 -0.026912
+0.011421 -0.002833 -0.031689
+0.011215 0.000322 -0.020415
+0.002635 0.003021 -0.012333
+0.001235 0.002490 -0.015739
+0.004272 0.002632 -0.013690
+-0.002997 0.002057 -0.002458
+-0.002614 -0.004314 -0.002919
+0.003125 -0.022994 -0.021280
+-0.005395 -0.000823 -0.013764
+-0.008954 -0.001258 -0.015629
+-0.003834 0.002652 -0.016057
+-0.007047 -0.000971 -0.018908
+0.010433 -0.002408 -0.000327
+0.007502 -0.002301 -0.002415
+0.009372 0.000321 -0.004108
+0.025152 0.001195 0.009053
+0.016960 0.000956 0.001995
+0.019875 -0.003369 -0.000014
+0.030751 0.008190 0.022981
+0.006170 -0.006701 0.007879
+0.004293 -0.001930 0.020860
+-0.000653 -0.004919 0.008637
+-0.014173 -0.021266 0.011121
+-0.017277 -0.006019 -0.017796
+0.023544 -0.000939 0.002037
+-0.009768 -0.009134 -0.022027
+-0.009580 -0.002948 -0.014062
+-0.011797 -0.001691 -0.017176
+-0.010204 0.002309 -0.009891
+-0.011598 0.001574 -0.008061
+-0.008708 0.001627 -0.006285
+0.034635 0.001794 0.002947
+0.039472 0.002062 0.008665
+-0.015907 -0.007981 -0.018944
+-0.020598 0.001813 -0.025718
+-0.045621 -0.006163 -0.019740
+-0.040582 -0.008149 -0.025424
+-0.042936 -0.004004 -0.023210
+-0.038274 -0.003561 -0.017712
+-0.025030 -0.004697 -0.013625
+-0.023215 -0.000003 -0.009663
+-0.021118 -0.002997 -0.014161
+-0.004112 0.000695 0.015891
+-0.016389 -0.004557 -0.019826
+-0.003547 -0.002715 -0.010257
+-0.001840 -0.000524 -0.011726
+-0.000903 -0.002832 -0.008443
+-0.001161 -0.006198 -0.011121
+0.026165 -0.000632 0.016453
+0.025651 0.005503 0.014190
+-0.045621 -0.011579 -0.019740
+-0.040582 -0.021408 -0.025424
+-0.032217 -0.008366 -0.016728
+0.000124 -0.006312 0.001727
+0.017517 -0.002166 0.004909
+0.011949 -0.006639 -0.000263
+0.023286 0.001530 0.011808
+-0.013427 -0.006320 -0.020900
+0.005159 0.001722 -0.017378
+0.006008 -0.003221 -0.013176
+0.044680 -0.002154 0.004195
+-0.000907 0.002148 0.009167
+0.018182 0.005343 0.015586
+0.043927 0.001725 0.025605
+-0.012854 0.002261 -0.012050
+-0.014517 0.001574 -0.010230
+-0.011668 -0.001006 -0.012914
+-0.008316 0.002150 -0.020675
+-0.010664 0.002390 -0.018678
+-0.021013 -0.010291 0.007951
+-0.014146 -0.001754 -0.047689
+-0.008422 -0.002312 -0.044596
+-0.015684 0.000035 -0.039467
+-0.034331 -0.001228 -0.022520
+-0.031662 -0.007124 -0.023091
+-0.031224 -0.014703 0.002956
+-0.022628 -0.011995 -0.002126
+-0.025924 -0.004114 -0.023568
+0.027872 -0.000761 0.013734
+-0.034591 -0.000064 -0.031338
+-0.029145 0.000494 -0.024321
+-0.028017 -0.005001 -0.028032
+0.027902 0.001681 0.011326
+0.025087 -0.001165 0.011230
+-0.021752 0.000419 -0.042010
+-0.026528 0.000997 -0.038365
+-0.024140 -0.004708 -0.040188
+-0.020978 0.001288 -0.033343
+-0.040551 -0.026143 -0.010199
+-0.023197 -0.010713 -0.012877
+-0.040551 -0.012883 -0.010199
+0.004485 -0.000007 -0.025484
+0.010626 0.000849 0.002651
+-0.028255 0.000295 -0.019377
+0.013332 0.003514 -0.005048
+0.012798 0.000333 -0.005951
+-0.015803 0.002187 -0.014369
+-0.013341 -0.003209 -0.016395
+0.004675 -0.002400 -0.004481
+0.002779 0.000374 -0.003048
+0.026117 -0.007505 -0.017965
+0.028859 -0.001994 -0.015040
+-0.018952 -0.009957 -0.016014
+-0.017184 -0.003861 -0.014796
+0.020301 0.003439 -0.001538
+0.046716 -0.013576 0.023724
+0.055055 -0.021059 0.018567
+0.055055 -0.007800 0.018567
+-0.007396 0.002390 -0.007873
+0.001875 -0.002670 -0.006494
+0.038019 -0.008659 0.029998
+-0.008029 -0.002626 -0.003881
+-0.005500 -0.000144 -0.009081
+-0.016373 -0.003539 -0.018074
+-0.010554 -0.001307 0.012690
+0.049300 -0.002253 0.010554
+0.038759 -0.002166 0.014632
+0.005684 -0.003039 -0.035812
+0.038019 0.004600 0.029998
+0.038019 0.010016 0.029998
+-0.026286 -0.002491 -0.003954
+0.017414 0.003222 -0.004081
+0.044485 0.002423 0.015009
+0.040181 0.004389 0.016007
+-0.000017 0.000235 -0.005153
+-0.033970 -0.003806 -0.010656
+0.009082 0.001920 -0.014465
+0.017757 0.000902 -0.014705
+-0.030192 -0.001153 -0.014581
+0.025315 0.003889 0.003730
+0.024009 0.004399 0.004815
+-0.000443 0.002808 -0.014194
+0.005274 0.003135 -0.010676
+0.006826 0.002728 -0.011961
+0.008126 0.003261 -0.008819
+0.003879 -0.000228 -0.008173
+0.032342 0.006310 0.016942
+0.023610 0.001317 -0.009141
+0.028043 0.004252 0.006706
+0.028016 -0.000536 0.005381
+0.017259 0.006519 0.029649
+-0.032441 -0.018179 -0.033098
+0.022512 0.003646 0.000819
+0.027741 0.001325 0.014450
+0.028451 0.005830 0.015287
+-0.017863 -0.003577 0.009446
+0.021215 0.004148 0.001990
+0.025358 0.002875 -0.000357
+-0.025580 0.001252 -0.027579
+-0.032441 -0.004919 -0.033098
+-0.038228 -0.001462 -0.027638
+-0.019666 0.002058 -0.021535
+0.028581 -0.008715 0.037841
+0.010757 0.003389 -0.006914
+0.019618 0.002475 -0.006065
+-0.031224 -0.009287 0.002956
+-0.031224 -0.027962 0.002956
+0.034589 -0.002055 -0.008644
+-0.002831 -0.000029 -0.007070
+-0.005830 0.001763 -0.004361
+-0.038103 -0.007971 -0.006212
+0.031060 0.005869 0.013961
+-0.030292 0.001057 -0.034857
+-0.024162 -0.006173 0.006457
+-0.042999 -0.006964 -0.014187
+0.001834 0.002718 0.019362
+0.049505 -0.002359 0.021843
+0.011040 0.003784 0.006750
+-0.001834 0.002801 -0.003863
+0.001083 0.003102 -0.001764
+0.022848 0.008014 0.033651
+0.023375 -0.002185 -0.020891
+0.028581 0.009961 0.037841
+0.006652 0.003669 0.002024
+0.009164 0.003881 0.003676
+0.008207 0.003356 0.004901
+0.049505 0.003057 0.021843
+0.055055 -0.002384 0.018567
+0.001013 -0.000353 -0.009972
+0.029329 0.005348 0.010717
+-0.004550 0.002514 -0.005831
+-0.007071 0.002482 -0.017312
+0.046366 0.005522 0.023902
+-0.045621 -0.024839 -0.019740
+0.041488 0.008760 0.027307
+0.033206 0.010046 0.034162
+0.026563 0.004882 0.007661
+3 43 45 44
+3 65 67 66
+3 69 71 70
+3 99 101 100
+3 61 63 138
+3 65 143 142
+3 147 149 148
+3 154 156 155
+3 159 160 45
+3 168 48 169
+3 173 175 174
+3 43 44 176
+3 187 188 77
+3 67 196 195
+3 66 67 195
+3 150 179 152
+3 203 204 205
+3 206 208 207
+3 176 44 209
+3 232 233 234
+3 235 127 236
+3 48 49 1
+3 239 242 241
+3 253 143 65
+3 87 235 59
+3 152 179 177
+3 41 39 125
+3 179 272 271
+3 274 276 275
+3 283 277 284
+3 266 107 173
+3 288 289 291
+3 48 168 49
+3 208 262 207
+3 156 262 295
+3 297 169 284
+3 142 67 65
+3 188 298 75
+3 268 300 269
+3 300 0 269
+3 312 232 234
+3 1 49 0
+3 236 127 149
+3 233 63 61
+3 265 24 318
+3 319 264 236
+3 321 126 125
+3 79 222 323
+3 276 288 291
+3 107 71 69
+3 149 327 326
+3 269 63 233
+3 49 244 0
+3 77 331 330
+3 332 168 297
+3 268 269 233
+3 142 20 266
+3 325 54 55
+3 262 330 205
+3 0 244 63
+3 195 136 222
+3 336 338 337
+3 79 340 194
+3 246 174 265
+3 195 222 194
+3 26 138 349
+3 333 346 350
+3 351 333 127
+3 169 352 284
+3 325 55 308
+3 244 332 324
+3 241 242 277
+3 354 241 277
+3 39 271 155
+3 59 235 264
+3 169 355 238
+3 205 204 295
+3 20 108 107
+3 335 154 214
+3 87 341 358
+3 358 351 87
+3 107 69 173
+3 359 324 329
+3 41 177 39
+3 326 319 236
+3 100 173 174
+3 359 329 364
+3 214 154 155
+3 154 335 207
+3 283 354 277
+3 142 143 21
+3 289 366 291
+3 348 253 66
+3 138 367 349
+3 368 329 242
+3 361 305 341
+3 77 341 331
+3 232 268 233
+3 338 187 337
+3 174 369 100
+3 100 369 99
+3 43 265 174
+3 265 43 176
+3 127 333 342
+3 154 207 156
+3 196 200 344
+3 188 187 357
+3 188 75 77
+3 173 371 175
+3 340 348 194
+3 372 44 373
+3 147 340 79
+3 333 350 334
+3 126 41 125
+3 330 262 208
+3 291 366 352
+3 374 376 136
+3 351 346 333
+3 377 379 378
+3 331 375 205
+3 355 152 238
+3 380 381 326
+3 234 233 61
+3 367 324 359
+3 196 101 200
+3 342 128 127
+3 188 357 298
+3 379 371 378
+3 358 325 308
+3 383 148 128
+3 177 291 238
+3 138 324 367
+3 136 344 374
+3 173 101 266
+3 168 169 297
+3 149 326 236
+3 209 372 384
+3 108 71 107
+3 337 187 330
+3 149 127 128
+3 357 187 338
+3 373 45 160
+3 173 100 101
+3 265 318 246
+3 101 99 200
+3 21 108 20
+3 24 265 176
+3 55 346 308
+3 371 70 378
+3 152 177 238
+3 351 127 235
+3 168 244 49
+3 26 61 138
+3 305 375 331
+3 266 101 196
+3 156 295 125
+3 168 332 244
+3 308 346 351
+3 366 283 284
+3 329 332 242
+3 244 138 63
+3 177 179 39
+3 136 196 344
+3 361 87 59
+3 77 358 341
+3 373 44 45
+3 327 79 386
+3 207 262 156
+3 148 149 128
+3 55 350 346
+3 222 79 194
+3 332 297 277
+3 39 155 156
+3 147 148 365
+3 242 332 277
+3 187 77 330
+3 1 0 300
+3 372 209 44
+3 246 369 174
+3 26 349 318
+3 26 318 24
+3 332 329 324
+3 155 271 385
+3 156 125 39
+3 271 272 385
+3 364 329 368
+3 175 379 159
+3 348 66 194
+3 383 365 148
+3 136 389 222
+3 342 390 383
+3 326 327 386
+3 126 275 41
+3 323 222 389
+3 277 297 284
+3 142 266 67
+3 173 69 371
+3 75 298 54
+3 380 326 386
+3 381 319 326
+3 388 79 323
+3 308 351 358
+3 352 169 238
+3 386 79 388
+3 136 376 389
+3 239 368 242
+3 208 337 330
+3 358 75 325
+3 77 75 358
+3 253 65 66
+3 0 63 269
+3 361 341 87
+3 204 321 295
+3 266 20 107
+3 147 365 340
+3 43 174 175
+3 75 54 325
+3 128 342 383
+3 150 272 179
+3 266 196 67
+3 175 159 43
+3 312 363 384
+3 351 235 87
+3 355 150 152
+3 206 336 208
+3 214 155 385
+3 147 79 327
+3 208 336 337
+3 291 177 41
+3 363 234 61
+3 363 312 234
+3 371 69 70
+3 274 288 276
+3 159 377 160
+3 66 195 194
+3 330 331 205
+3 321 125 295
+3 342 334 390
+3 176 363 61
+3 26 176 61
+3 375 203 205
+3 379 175 371
+3 341 305 331
+3 291 41 276
+3 24 176 26
+3 262 205 295
+3 209 363 176
+3 276 41 275
+3 159 45 43
+3 335 206 207
+3 20 142 21
+3 366 284 352
+3 147 327 149
+3 39 179 271
+3 209 384 363
+3 379 377 159
+3 264 235 236
+3 333 334 342
+3 195 196 136
+3 244 324 138
+3 291 352 238
+3 1 2 3
+3 21 22 23
+3 47 48 3
+3 108 23 90
+3 48 1 3
+3 253 141 143
+3 268 270 2
+3 2 300 268
+3 97 232 312
+3 97 312 316
+3 23 22 167
+3 48 47 169
+3 6 90 258
+3 317 47 3
+3 193 4 216
+3 95 97 316
+3 4 193 296
+3 4 89 6
+3 282 347 93
+3 348 347 253
+3 316 250 248
+3 89 90 6
+3 372 373 303
+3 2 230 301
+3 143 141 260
+3 70 89 296
+3 377 378 296
+3 301 317 2
+3 282 141 347
+3 328 302 313
+3 141 253 347
+3 71 90 89
+3 328 384 372
+3 108 90 71
+3 328 303 302
+3 217 158 216
+3 373 160 158
+3 21 23 108
+3 378 70 296
+3 22 287 167
+3 312 362 316
+3 89 4 296
+3 143 260 21
+3 328 313 362
+3 1 300 2
+3 270 268 232
+3 22 260 287
+3 316 362 250
+3 84 90 23
+3 71 89 70
+3 141 282 287
+3 230 2 270
+3 193 216 158
+3 362 313 250
+3 270 232 97
+3 348 340 93
+3 84 258 90
+3 312 384 362
+3 377 296 193
+3 193 158 160
+3 373 158 303
+3 377 193 160
+3 317 3 2
+3 95 230 97
+3 230 270 97
+3 372 303 328
+3 217 303 158
+3 328 362 384
+3 248 95 316
+3 217 302 303
+3 84 23 167
+3 260 22 21
+3 260 141 287
+3 347 348 93
+3 54 56 55
+3 83 85 84
+3 56 54 109
+3 212 214 213
+3 216 218 217
+3 219 83 167
+3 230 213 231
+3 248 250 249
+3 56 220 55
+3 286 219 287
+3 85 220 258
+3 185 317 183
+3 150 183 272
+3 83 334 85
+3 336 339 338
+3 301 230 231
+3 95 248 212
+3 183 301 231
+3 109 216 4
+3 282 286 287
+3 169 47 355
+3 338 339 36
+3 335 214 212
+3 4 220 56
+3 339 313 302
+3 85 334 350
+3 47 317 185
+3 357 36 218
+3 382 206 335
+3 250 313 382
+3 334 83 219
+3 231 213 272
+3 218 36 217
+3 219 286 383
+3 220 4 6
+3 286 282 93
+3 183 317 301
+3 213 214 385
+3 167 83 84
+3 357 109 298
+3 36 357 338
+3 272 213 385
+3 55 85 350
+3 85 258 84
+3 336 382 339
+3 336 206 382
+3 249 250 382
+3 230 95 213
+3 47 185 355
+3 340 365 93
+3 248 249 212
+3 383 286 365
+3 219 383 390
+3 249 382 335
+3 218 216 109
+3 219 167 287
+3 286 93 365
+3 217 36 302
+3 54 298 109
+3 357 218 109
+3 185 150 355
+3 183 231 272
+3 36 339 302
+3 313 339 382
+3 334 219 390
+3 185 183 150
+3 85 55 220
+3 213 95 212
+3 258 220 6
+3 56 109 4
+3 335 212 249
+3 14 310 309
+4 0 1 2 3
+4 4 5 6 7
+4 8 9 10 11
+4 12 13 14 15
+4 16 17 18 19
+4 20 21 22 23
+4 24 25 26 27
+4 28 29 30 31
+4 32 33 34 35
+4 36 37 33 38
+4 39 40 41 42
+4 43 44 45 46
+4 47 3 48 49
+4 50 51 52 53
+4 54 55 56 57
+4 58 59 60 13
+4 61 62 63 64
+4 65 66 67 68
+4 69 70 71 72
+4 73 8 30 74
+4 75 76 77 78
+4 79 80 81 82
+4 83 84 85 86
+4 87 88 60 13
+4 89 7 90 72
+4 91 78 88 92
+4 93 82 94 17
+4 95 96 97 98
+4 99 100 101 102
+4 103 104 105 106
+4 107 108 23 90
+4 56 109 54 110
+4 111 7 112 113
+4 114 115 116 117
+4 91 118 110 119
+4 120 121 122 123
+4 124 125 126 123
+4 127 18 128 129
+4 130 131 132 133
+4 134 135 136 81
+4 105 27 137 74
+4 61 138 63 62
+4 122 139 140 123
+4 65 141 142 143
+4 47 144 130 133
+4 27 145 96 146
+4 147 148 149 82
+4 150 151 152 153
+4 154 155 156 157
+4 158 159 160 45
+4 161 162 163 164
+4 165 145 27 166
+4 167 12 84 86
+4 168 169 48 170
+4 5 171 166 172
+4 173 174 175 112
+4 43 176 44 46
+4 177 178 179 163
+4 180 181 182 139
+4 183 184 185 153
+4 8 186 157 11
+4 187 77 188 37
+4 173 116 112 72
+4 189 190 191 92
+4 192 186 125 42
+4 159 158 193 46
+4 79 81 194 82
+4 67 195 196 197
+4 122 198 140 182
+4 66 195 67 199
+4 150 152 179 153
+4 200 50 99 102
+4 12 201 52 53
+4 15 14 58 13
+4 202 120 140 123
+4 191 203 204 205
+4 152 151 163 153
+4 206 207 208 10
+4 176 209 44 210
+4 137 35 211 74
+4 212 213 214 215
+4 216 217 218 119
+4 219 167 83 86
+4 118 220 221 56
+4 195 199 222 197
+4 174 223 224 225
+4 137 226 165 27
+4 47 132 130 227
+4 88 228 221 86
+4 131 170 132 133
+4 106 31 137 229
+4 230 231 213 29
+4 232 233 97 234
+4 235 236 127 18
+4 48 3 1 49
+4 214 154 212 157
+4 130 237 182 161
+4 114 68 141 117
+4 125 123 192 42
+4 238 181 177 144
+4 239 240 241 242
+4 103 243 244 245
+4 193 111 159 46
+4 246 224 25 225
+4 184 161 247 151
+4 248 249 250 251
+4 94 199 93 252
+4 253 141 65 143
+4 254 255 240 256
+4 257 210 176 46
+4 258 221 12 259
+4 22 115 260 117
+4 260 115 114 117
+4 94 82 81 17
+4 96 146 145 98
+4 178 163 261 153
+4 262 263 208 10
+4 264 13 58 18
+4 24 265 25 225
+4 87 235 60 59
+4 152 177 179 163
+4 116 101 102 266
+4 34 35 33 38
+4 24 257 176 225
+4 41 125 39 42
+4 261 164 40 229
+4 94 17 16 19
+4 267 81 79 80
+4 268 269 2 270
+4 95 74 248 98
+4 179 271 272 178
+4 178 184 261 29
+4 25 27 273 62
+4 274 121 275 276
+4 277 256 242 278
+4 273 226 279 280
+4 210 145 250 166
+4 181 133 47 281
+4 282 17 94 19
+4 56 55 220 57
+4 283 284 277 285
+4 182 162 161 164
+4 286 287 219 129
+4 266 173 107 116
+4 174 224 246 225
+4 282 18 93 17
+4 51 114 94 19
+4 288 289 290 291
+4 223 257 165 225
+4 205 34 191 92
+4 47 48 168 49
+4 292 293 256 294
+4 208 207 262 10
+4 156 295 262 11
+4 149 80 147 82
+4 223 46 257 225
+4 296 111 193 113
+4 165 257 223 5
+4 297 284 169 281
+4 117 142 67 65
+4 188 75 298 299
+4 85 258 220 221
+4 170 133 180 281
+4 178 215 30 42
+4 270 104 230 64
+4 2 268 300 269
+4 184 106 301 227
+4 302 210 172 303
+4 106 237 261 229
+4 221 118 91 259
+4 300 0 2 269
+4 304 88 305 78
+4 182 131 198 306
+4 3 307 49 245
+4 55 308 220 57
+4 132 243 103 245
+4 14 309 310 311
+4 145 74 96 98
+4 178 29 183 153
+4 44 210 303 172
+4 97 312 232 234
+4 302 166 313 38
+4 1 49 3 0
+4 174 111 223 225
+4 94 81 314 17
+4 73 31 30 229
+4 177 139 291 123
+4 140 198 137 106
+4 211 192 73 186
+4 236 149 127 18
+4 100 224 174 112
+4 176 210 24 315
+4 261 29 184 237
+4 269 104 270 64
+4 292 294 280 293
+4 97 316 312 234
+4 250 145 248 251
+4 23 167 22 115
+4 135 51 50 197
+4 61 315 26 62
+4 48 169 47 170
+4 234 315 61 64
+4 178 229 40 42
+4 137 145 165 35
+4 233 61 63 64
+4 185 183 317 184
+4 46 113 5 172
+4 177 181 291 139
+4 6 258 90 7
+4 135 81 51 197
+4 265 24 25 318
+4 319 236 264 320
+4 181 162 177 144
+4 321 126 124 125
+4 8 10 263 11
+4 322 79 222 323
+4 244 103 324 243
+4 54 110 325 57
+4 73 34 211 186
+4 114 116 53 102
+4 189 211 191 192
+4 51 68 94 252
+4 317 3 47 132
+4 290 276 288 291
+4 282 94 93 252
+4 106 227 237 306
+4 91 299 110 76
+4 261 247 184 161
+4 294 292 280 279
+4 179 178 272 153
+4 150 272 183 153
+4 193 216 4 113
+4 107 90 69 71
+4 12 53 84 7
+4 33 171 302 38
+4 198 293 137 306
+4 149 326 327 320
+4 269 233 63 64
+4 49 0 244 245
+4 6 221 258 259
+4 166 171 35 38
+4 47 247 317 227
+4 291 139 276 123
+4 44 209 328 210
+4 168 170 47 245
+4 191 190 304 92
+4 181 285 291 139
+4 329 243 293 256
+4 95 316 97 96
+4 96 62 315 64
+4 60 88 15 13
+4 77 330 331 76
+4 293 103 105 306
+4 182 181 180 133
+4 137 27 165 145
+4 50 197 51 102
+4 332 297 168 170
+4 106 29 301 31
+4 34 76 33 92
+4 40 162 140 164
+4 320 80 149 17
+4 137 106 198 306
+4 30 157 73 42
+4 140 182 198 306
+4 268 233 269 64
+4 106 307 301 227
+4 142 266 20 117
+4 83 333 85 334
+4 47 170 169 281
+4 313 166 35 38
+4 325 55 54 57
+4 30 8 73 157
+4 262 205 330 263
+4 0 63 244 103
+4 125 186 156 42
+4 3 132 317 307
+4 12 112 53 7
+4 250 35 313 166
+4 94 114 51 252
+4 195 222 136 197
+4 335 10 249 11
+4 18 17 129 19
+4 111 46 223 225
+4 336 337 338 339
+4 266 68 101 102
+4 156 186 157 42
+4 13 86 12 19
+4 18 129 16 19
+4 294 255 254 256
+4 293 243 180 256
+4 137 73 140 229
+4 4 296 193 113
+4 79 194 340 82
+4 295 186 262 11
+4 305 78 341 92
+4 157 9 8 11
+4 342 286 219 343
+4 140 306 106 229
+4 196 135 344 197
+4 93 199 94 82
+4 198 182 345 180
+4 329 293 294 256
+4 5 46 111 113
+4 163 161 261 153
+4 242 254 240 256
+4 293 131 180 170
+4 216 259 4 113
+4 346 333 85 228
+4 207 10 335 11
+4 40 164 140 229
+4 246 265 174 225
+4 195 194 222 199
+4 66 347 348 199
+4 26 349 138 62
+4 333 346 85 350
+4 301 231 230 29
+4 347 199 68 252
+4 351 127 333 343
+4 95 212 248 9
+4 140 164 182 229
+4 73 192 40 42
+4 240 242 239 254
+4 114 68 51 252
+4 213 29 231 215
+4 248 145 250 146
+4 182 237 261 161
+4 52 51 12 53
+4 2 270 269 104
+4 258 6 5 7
+4 184 151 185 153
+4 81 199 94 197
+4 219 129 287 86
+4 97 96 316 64
+4 248 74 95 9
+4 183 231 301 29
+4 169 284 352 281
+4 314 80 320 17
+4 104 64 96 98
+4 217 5 216 172
+4 286 343 127 129
+4 292 198 255 256
+4 33 37 36 119
+4 121 276 290 139
+4 290 285 353 139
+4 130 131 237 227
+4 349 273 138 62
+4 169 181 238 144
+4 325 308 55 57
+4 33 76 37 119
+4 152 163 179 153
+4 111 46 193 113
+4 8 263 34 186
+4 244 324 332 243
+4 104 106 103 307
+4 109 4 216 118
+4 295 192 204 186
+4 241 242 240 277
+4 240 354 241 277
+4 204 192 191 186
+4 39 155 271 215
+4 36 37 218 119
+4 110 118 91 57
+4 353 122 290 139
+4 88 78 304 92
+4 282 287 286 129
+4 303 158 45 46
+4 314 322 267 81
+4 201 223 165 225
+4 230 31 28 98
+4 84 12 258 86
+4 28 29 213 215
+4 59 264 235 18
+4 4 6 89 7
+4 132 170 47 133
+4 51 197 68 102
+4 169 355 47 238
+4 141 68 114 252
+4 356 314 52 51
+4 35 251 8 74
+4 93 18 282 129
+4 218 37 357 299
+4 191 205 204 295
+4 20 23 107 108
+4 149 18 236 320
+4 39 215 178 42
+4 267 309 314 320
+4 47 181 169 144
+4 47 170 132 245
+4 338 337 36 339
+4 216 5 217 259
+4 335 154 212 214
+4 212 157 249 9
+4 173 112 175 72
+4 20 116 22 117
+4 87 358 341 57
+4 2 269 0 104
+4 358 87 351 57
+4 107 173 69 72
+4 217 113 158 172
+4 25 226 273 27
+4 359 329 324 280
+4 41 39 177 40
+4 360 226 137 280
+4 112 7 5 113
+4 4 220 118 56
+4 60 87 361 88
+4 257 5 165 166
+4 224 50 52 53
+4 326 236 319 320
+4 249 251 248 9
+4 218 299 109 118
+4 305 304 361 88
+4 47 133 170 281
+4 100 174 173 112
+4 121 139 122 123
+4 198 293 292 280
+4 316 362 363 146
+4 32 34 211 35
+4 226 280 273 62
+4 294 359 329 364
+4 85 228 83 86
+4 244 105 324 103
+4 40 178 177 163
+4 214 155 154 157
+4 161 151 184 153
+4 169 181 47 281
+4 302 33 36 171
+4 154 207 335 11
+4 302 171 217 172
+4 20 23 22 116
+4 293 103 324 280
+4 283 277 354 285
+4 255 180 345 278
+4 339 302 313 38
+4 154 249 212 157
+4 27 145 210 166
+4 365 93 148 82
+4 28 31 74 98
+4 282 93 347 252
+4 121 275 276 123
+4 261 178 40 164
+4 53 112 173 116
+4 142 21 143 117
+4 81 82 80 17
+4 289 291 366 285
+4 140 137 211 73
+4 360 137 198 280
+4 348 66 253 347
+4 137 27 105 280
+4 101 68 196 102
+4 138 349 367 273
+4 316 248 250 146
+4 51 81 94 197
+4 53 116 100 102
+4 140 73 211 192
+4 368 242 329 256
+4 63 104 269 64
+4 361 341 305 88
+4 182 164 237 229
+4 337 37 339 263
+4 180 243 293 170
+4 77 331 341 78
+4 140 162 182 164
+4 154 157 156 11
+4 44 46 210 172
+4 232 233 268 64
+4 263 10 262 11
+4 51 68 114 102
+4 363 315 316 146
+4 66 68 347 199
+4 140 162 40 123
+4 338 337 187 37
+4 327 320 326 80
+4 174 224 100 369
+4 100 224 99 369
+4 339 37 36 38
+4 43 111 174 265
+4 250 145 210 146
+4 265 176 43 225
+4 180 281 285 278
+4 127 342 333 343
+4 154 156 207 11
+4 69 71 90 72
+4 196 344 200 50
+4 209 210 176 315
+4 140 40 73 192
+4 165 5 12 259
+4 223 112 12 5
+4 220 118 221 6
+4 188 357 187 37
+4 188 77 75 299
+4 304 60 361 88
+4 220 228 221 57
+4 272 178 231 153
+4 182 161 261 164
+4 327 326 267 80
+4 319 58 370 320
+4 58 309 14 311
+4 211 34 73 35
+4 259 171 217 119
+4 238 163 152 144
+4 173 175 371 72
+4 12 53 51 19
+4 89 6 90 7
+4 83 343 219 86
+4 159 111 43 46
+4 182 180 198 131
+4 340 194 348 199
+4 177 162 181 139
+4 88 13 87 228
+4 303 46 44 172
+4 137 106 105 31
+4 69 90 107 72
+4 74 31 105 98
+4 372 303 373 44
+4 329 293 324 280
+4 258 12 84 7
+4 224 53 100 102
+4 47 238 355 144
+4 47 49 168 245
+4 314 51 356 81
+4 304 78 305 92
+4 136 134 374 135
+4 141 114 287 19
+4 165 27 226 257
+4 2 301 230 104
+4 90 7 23 72
+4 27 145 137 74
+4 263 186 8 11
+4 147 79 340 82
+4 359 273 279 280
+4 375 304 305 92
+4 5 7 4 113
+4 223 53 12 112
+4 137 105 293 280
+4 333 85 334 350
+4 148 93 149 17
+4 137 31 73 229
+4 34 33 32 92
+4 91 118 221 57
+4 126 125 41 123
+4 143 260 141 117
+4 330 208 262 263
+4 370 309 267 320
+4 47 185 317 247
+4 83 228 343 86
+4 304 203 191 92
+4 317 307 132 227
+4 30 29 178 229
+4 291 352 366 285
+4 91 57 221 78
+4 184 247 261 237
+4 374 134 136 376
+4 294 254 368 256
+4 371 70 89 296
+4 261 237 247 161
+4 357 218 36 37
+4 347 68 141 252
+4 59 13 264 18
+4 351 333 346 228
+4 124 120 202 123
+4 231 178 272 215
+4 316 315 96 146
+4 36 339 337 37
+4 152 144 163 151
+4 377 296 378 379
+4 255 198 345 180
+4 251 74 248 9
+4 324 103 105 280
+4 88 57 341 78
+4 354 277 240 278
+4 13 228 88 86
+4 192 123 40 42
+4 105 104 96 98
+4 100 53 173 116
+4 331 205 375 92
+4 352 181 169 281
+4 240 255 345 278
+4 355 238 152 144
+4 370 380 381 326
+4 234 61 233 64
+4 105 62 96 64
+4 30 229 178 42
+4 67 117 68 266
+4 63 103 0 104
+4 93 199 347 252
+4 301 2 317 307
+4 132 227 103 306
+4 110 57 91 78
+4 367 359 324 280
+4 293 243 132 170
+4 258 221 85 228
+4 332 256 297 170
+4 382 335 206 10
+4 250 382 313 38
+4 178 29 30 215
+4 333 334 83 219
+4 65 141 253 68
+4 53 116 114 115
+4 97 234 233 64
+4 237 164 261 229
+4 282 347 141 252
+4 326 320 267 80
+4 341 88 87 57
+4 210 328 302 313
+4 169 238 47 144
+4 34 263 8 35
+4 32 88 190 92
+4 40 162 177 123
+4 290 276 291 139
+4 231 272 213 215
+4 25 201 226 225
+4 196 200 101 50
+4 96 315 27 146
+4 342 286 127 128
+4 4 118 6 259
+4 317 247 184 227
+4 12 13 88 86
+4 188 298 357 299
+4 141 347 253 68
+4 290 274 276 121
+4 379 378 371 296
+4 63 62 105 64
+4 110 118 299 119
+4 166 171 302 172
+4 290 289 353 285
+4 358 308 325 57
+4 103 106 105 306
+4 193 46 158 113
+4 165 201 12 223
+4 383 148 286 128
+4 106 29 261 237
+4 12 5 112 7
+4 67 199 195 197
+4 177 238 291 181
+4 58 13 16 18
+4 138 367 324 280
+4 26 315 27 62
+4 37 263 330 76
+4 136 374 344 135
+4 116 173 101 266
+4 237 131 182 306
+4 35 251 250 38
+4 77 76 331 78
+4 168 297 169 170
+4 149 236 326 320
+4 71 89 90 72
+4 105 103 293 280
+4 209 328 384 372
+4 279 226 360 280
+4 354 285 277 278
+4 87 228 351 57
+4 68 199 94 252
+4 93 18 149 17
+4 368 254 242 256
+4 108 90 107 71
+4 196 50 101 102
+4 337 330 187 37
+4 328 210 302 303
+4 265 111 174 225
+4 341 78 331 92
+4 217 216 158 113
+4 345 285 180 139
+4 149 128 127 18
+4 226 257 27 225
+4 249 10 382 251
+4 105 106 137 306
+4 357 338 187 37
+4 130 144 47 151
+4 249 9 157 11
+4 373 158 160 45
+4 10 9 249 11
+4 114 115 287 19
+4 222 322 136 81
+4 173 116 101 100
+4 132 131 293 170
+4 165 223 12 5
+4 331 78 76 92
+4 345 182 122 139
+4 261 29 106 229
+4 265 25 246 318
+4 88 221 12 86
+4 101 200 99 102
+4 145 146 248 74
+4 32 33 91 92
+4 211 35 73 74
+4 177 162 163 144
+4 157 186 156 11
+4 218 217 36 119
+4 287 19 86 129
+4 145 35 137 74
+4 63 105 244 103
+4 6 118 221 259
+4 21 23 20 108
+4 321 125 124 192
+4 24 176 265 225
+4 283 354 353 285
+4 149 93 148 18
+4 165 166 5 171
+4 73 35 8 74
+4 25 27 24 225
+4 55 346 85 308
+4 36 171 33 119
+4 217 171 36 119
+4 103 307 132 245
+4 219 342 383 286
+4 371 378 70 296
+4 316 315 234 64
+4 152 238 177 163
+4 168 243 332 170
+4 138 280 105 62
+4 34 190 92 32
+4 223 5 257 172
+4 33 35 32 171
+4 18 343 13 129
+4 351 235 127 343
+4 13 12 14 16
+4 371 296 89 72
+4 97 233 232 64
+4 220 4 118 6
+4 16 14 58 309
+4 5 113 216 172
+4 73 229 30 42
+4 168 49 244 245
+4 26 138 61 62
+4 96 145 27 74
+4 210 145 27 146
+4 80 82 149 17
+4 305 331 375 92
+4 346 85 308 228
+4 0 307 103 245
+4 230 64 104 98
+4 266 196 101 68
+4 333 343 83 228
+4 226 27 25 225
+4 286 93 282 129
+4 116 53 84 115
+4 161 144 130 151
+4 6 258 5 259
+4 198 131 293 306
+4 50 224 99 102
+4 162 139 177 123
+4 220 221 56 57
+4 181 285 180 281
+4 168 48 47 170
+4 356 51 135 81
+4 103 227 106 306
+4 147 80 79 82
+4 76 78 91 92
+4 221 57 88 78
+4 156 125 295 186
+4 224 201 25 225
+4 183 301 317 184
+4 173 53 100 112
+4 226 201 165 225
+4 58 319 264 320
+4 250 35 145 251
+4 198 180 255 256
+4 230 104 301 31
+4 184 29 106 237
+4 208 263 337 10
+4 168 244 332 243
+4 236 18 264 320
+4 182 306 140 229
+4 283 353 366 285
+4 182 237 130 131
+4 284 285 352 281
+4 178 163 40 164
+4 181 144 47 133
+4 266 117 68 102
+4 308 351 346 228
+4 213 28 230 29
+4 261 237 182 164
+4 122 182 140 139
+4 22 167 287 115
+4 213 385 214 215
+4 267 380 370 326
+4 379 296 371 72
+4 297 281 170 278
+4 366 285 284 283
+4 210 46 257 172
+4 218 118 216 119
+4 146 74 145 98
+4 312 363 316 362
+4 260 114 141 117
+4 141 114 260 115
+4 329 242 332 256
+4 105 244 138 63
+4 262 186 263 11
+4 177 39 179 178
+4 94 199 68 197
+4 325 110 358 57
+4 167 84 83 86
+4 163 162 40 164
+4 136 344 196 135
+4 132 307 3 245
+4 263 251 382 10
+4 361 87 60 59
+4 60 235 87 13
+4 375 203 304 92
+4 321 124 191 192
+4 267 320 314 80
+4 47 132 3 245
+4 222 199 81 197
+4 176 210 44 46
+4 177 163 238 144
+4 299 357 109 298
+4 110 76 75 78
+4 77 341 358 78
+4 357 37 188 299
+4 224 223 201 225
+4 91 171 259 119
+4 34 263 33 76
+4 157 28 8 9
+4 36 338 357 37
+4 344 135 50 197
+4 271 178 39 215
+4 373 45 44 303
+4 105 31 104 98
+4 58 309 370 320
+4 250 210 362 146
+4 267 327 79 386
+4 211 34 190 189
+4 293 131 132 306
+4 8 35 263 251
+4 207 156 262 11
+4 40 192 140 123
+4 189 124 211 192
+4 105 104 63 64
+4 264 18 58 320
+4 272 385 213 215
+4 142 143 141 117
+4 205 295 191 186
+4 50 53 224 102
+4 148 128 149 18
+4 99 224 100 102
+4 261 184 178 153
+4 180 285 345 278
+4 4 7 89 113
+4 231 178 183 153
+4 55 346 350 85
+4 104 31 230 98
+4 222 194 79 81
+4 269 270 268 64
+4 258 5 12 7
+4 110 299 75 76
+4 5 166 257 172
+4 3 49 47 245
+4 93 94 282 17
+4 16 13 12 19
+4 292 255 294 256
+4 27 210 257 166
+4 337 263 336 10
+4 16 320 18 17
+4 332 277 297 256
+4 105 106 104 31
+4 41 40 177 123
+4 39 156 155 157
+4 161 162 182 144
+4 16 18 13 129
+4 131 227 132 306
+4 73 8 34 186
+4 89 296 4 113
+4 210 315 363 146
+4 132 307 103 227
+4 147 82 365 148
+4 339 263 382 10
+4 32 211 165 35
+4 345 180 182 139
+4 289 285 290 139
+4 106 306 237 229
+4 104 106 301 31
+4 242 277 332 256
+4 85 84 258 86
+4 103 132 293 243
+4 224 223 174 112
+4 143 21 260 117
+4 109 110 56 118
+4 336 339 382 10
+4 187 330 77 37
+4 328 362 313 210
+4 196 68 67 197
+4 58 311 370 309
+4 298 110 109 299
+4 1 0 2 300
+4 336 382 206 10
+4 372 44 209 328
+4 137 74 73 31
+4 272 178 271 215
+4 293 105 137 306
+4 145 35 250 166
+4 26 27 25 62
+4 301 106 184 29
+4 240 277 242 278
+4 263 251 35 38
+4 34 211 190 32
+4 249 382 250 251
+4 176 46 43 225
+4 294 293 329 280
+4 148 18 286 128
+4 39 178 40 42
+4 122 121 290 139
+4 261 161 163 164
+4 230 213 95 28
+4 12 223 201 53
+4 36 337 338 37
+4 75 299 77 76
+4 363 210 209 315
+4 216 118 4 259
+4 379 193 296 111
+4 163 144 161 151
+4 270 232 268 64
+4 335 249 154 11
+4 249 157 154 11
+4 130 237 247 227
+4 230 29 28 31
+4 294 255 292 387
+4 285 281 284 278
+4 263 10 8 251
+4 287 114 141 115
+4 282 252 141 19
+4 246 224 174 369
+4 47 247 130 151
+4 26 349 25 318
+4 26 25 24 318
+4 316 234 97 64
+4 182 162 181 144
+4 243 170 168 245
+4 279 329 359 280
+4 286 18 93 129
+4 84 12 167 115
+4 8 28 30 74
+4 32 15 190 88
+4 5 259 165 171
+4 22 287 260 115
+4 190 34 92 189
+4 221 228 88 57
+4 23 116 107 72
+4 353 354 240 278
+4 138 273 367 280
+4 222 81 136 197
+4 258 228 85 86
+4 107 116 173 72
+4 27 257 165 166
+4 254 255 294 387
+4 332 324 329 243
+4 230 28 95 98
+4 244 243 168 245
+4 47 355 185 151
+4 316 250 362 146
+4 40 163 177 162
+4 155 385 271 215
+4 180 256 243 170
+4 156 39 125 42
+4 248 146 95 98
+4 271 385 272 215
+4 84 23 90 7
+4 221 118 56 57
+4 364 368 329 294
+4 8 157 30 28
+4 175 159 379 111
+4 136 81 135 197
+4 204 321 191 192
+4 243 256 332 170
+4 217 259 5 171
+4 340 93 365 82
+4 180 285 181 139
+4 71 70 89 72
+4 84 53 12 115
+4 222 79 322 81
+4 58 14 16 13
+4 141 287 282 19
+4 303 45 44 46
+4 230 270 2 104
+4 33 263 37 76
+4 100 53 224 112
+4 112 116 53 7
+4 2 0 3 307
+4 105 280 27 62
+4 319 370 311 381
+4 35 171 33 38
+4 216 259 217 119
+4 337 339 336 263
+4 33 76 91 92
+4 188 37 77 299
+4 132 170 243 245
+4 248 212 249 9
+4 140 120 122 123
+4 181 162 182 139
+4 382 263 339 38
+4 22 116 23 115
+4 267 79 322 388
+4 41 123 125 42
+4 308 85 220 228
+4 284 281 297 278
+4 279 360 292 280
+4 175 112 174 111
+4 348 194 66 199
+4 18 320 149 17
+4 165 257 226 225
+4 77 299 37 76
+4 137 293 198 280
+4 51 53 50 102
+4 351 228 308 57
+4 32 165 12 259
+4 101 50 200 102
+4 184 29 178 153
+4 49 307 0 245
+4 97 64 230 98
+4 211 34 191 186
+4 282 129 18 17
+4 29 31 106 229
+4 136 135 196 197
+4 111 43 225 265
+4 205 263 34 76
+4 79 327 267 80
+4 291 181 352 285
+4 40 123 41 42
+4 67 68 66 199
+4 34 8 73 35
+4 352 285 181 281
+4 40 178 261 229
+4 43 111 225 46
+4 329 294 368 256
+4 383 365 286 148
+4 103 307 106 227
+4 193 158 216 113
+4 191 34 189 92
+4 293 132 103 306
+4 358 57 110 78
+4 140 106 137 229
+4 30 31 29 229
+4 301 106 104 307
+4 358 110 75 78
+4 52 201 224 53
+4 136 322 222 389
+4 342 219 383 390
+4 277 285 284 278
+4 16 51 314 94
+4 267 326 327 386
+4 23 7 116 72
+4 94 252 282 19
+4 356 135 134 81
+4 130 144 182 133
+4 126 41 275 123
+4 112 5 111 113
+4 134 322 136 389
+4 261 163 178 164
+4 203 205 191 92
+4 296 113 89 72
+4 53 114 51 19
+4 322 323 222 389
+4 257 166 210 172
+4 297 170 256 278
+4 105 74 137 31
+4 343 129 219 86
+4 0 103 244 245
+4 301 29 230 31
+4 88 228 87 57
+4 277 284 297 278
+4 81 199 194 82
+4 125 192 295 186
+4 142 117 67 266
+4 91 76 33 119
+4 173 371 69 72
+4 75 54 298 110
+4 166 302 210 172
+4 351 343 333 228
+4 183 29 184 153
+4 25 265 246 225
+4 324 105 138 280
+4 331 76 205 92
+4 53 116 84 7
+4 249 335 382 10
+4 380 326 267 386
+4 209 362 328 210
+4 370 381 319 326
+4 91 76 110 78
+4 218 109 216 118
+4 322 388 79 323
+4 13 343 87 228
+4 15 88 12 13
+4 353 285 354 278
+4 219 287 167 86
+4 240 256 255 278
+4 262 10 207 11
+4 308 358 351 57
+4 32 91 88 92
+4 170 281 180 278
+4 158 46 303 172
+4 352 238 169 181
+4 191 34 205 186
+4 27 96 105 62
+4 191 295 204 186
+4 267 386 79 388
+4 322 79 267 81
+4 73 74 30 31
+4 367 273 359 280
+4 324 103 293 243
+4 114 252 94 19
+4 136 376 134 389
+4 315 62 61 64
+4 8 74 251 9
+4 261 161 184 153
+4 198 180 293 131
+4 326 319 370 320
+4 214 157 212 215
+4 333 83 85 228
+4 33 171 91 119
+4 85 221 220 228
+4 159 193 379 111
+4 73 186 192 42
+4 353 289 366 285
+4 286 148 365 93
+4 10 251 249 9
+4 169 170 297 281
+4 95 146 96 98
+4 182 144 181 133
+4 182 131 130 133
+4 129 86 13 19
+4 141 252 114 19
+4 130 132 47 133
+4 148 82 93 17
+4 127 128 286 129
+4 128 18 286 129
+4 12 51 16 19
+4 314 320 16 17
+4 12 86 167 19
+4 140 192 124 123
+4 8 251 10 9
+4 239 242 368 254
+4 333 219 83 343
+4 217 302 36 171
+4 13 18 235 343
+4 208 330 337 263
+4 257 46 176 225
+4 118 259 216 119
+4 12 5 258 259
+4 304 190 60 88
+4 358 325 75 110
+4 116 7 112 72
+4 23 116 84 115
+4 32 35 165 171
+4 135 52 50 51
+4 51 114 53 102
+4 77 358 75 78
+4 27 315 96 62
+4 326 370 267 320
+4 253 66 65 68
+4 255 256 180 278
+4 209 363 362 210
+4 212 28 157 9
+4 16 129 13 19
+4 32 91 171 259
+4 149 320 327 80
+4 263 35 34 38
+4 0 269 63 104
+4 355 144 152 151
+4 361 87 341 88
+4 75 110 298 299
+4 290 291 289 139
+4 204 295 321 192
+4 297 256 277 278
+4 256 170 180 278
+4 223 111 112 5
+4 266 107 20 116
+4 28 74 95 98
+4 141 65 142 117
+4 362 250 313 210
+4 177 40 39 178
+4 211 137 165 35
+4 82 147 365 340
+4 191 211 189 34
+4 190 15 60 88
+4 111 43 174 175
+4 89 70 371 72
+4 163 151 161 153
+4 37 76 299 119
+4 330 37 337 263
+4 13 129 343 86
+4 54 109 298 110
+4 75 325 54 110
+4 356 52 135 51
+4 180 170 131 133
+4 273 27 226 62
+4 127 343 18 129
+4 27 257 24 225
+4 35 166 165 171
+4 52 314 16 51
+4 39 157 155 215
+4 345 285 353 278
+4 12 52 16 51
+4 198 106 140 306
+4 12 221 258 86
+4 251 263 382 38
+4 308 228 220 57
+4 286 128 342 383
+4 117 67 68 65
+4 73 157 8 186
+4 28 74 8 9
+4 3 0 49 307
+4 68 197 196 102
+4 293 292 256 198
+4 357 299 109 218
+4 179 163 178 153
+4 150 179 272 153
+4 257 46 223 172
+4 176 257 24 210
+4 266 67 196 68
+4 175 43 159 111
+4 235 13 59 18
+4 270 97 232 64
+4 124 121 120 123
+4 299 76 91 119
+4 348 93 340 199
+4 84 90 258 7
+4 91 259 118 119
+4 185 355 150 151
+4 313 35 250 38
+4 37 263 33 38
+4 301 184 183 29
+4 339 263 37 38
+4 231 29 178 215
+4 136 322 134 81
+4 312 362 384 363
+4 293 180 198 256
+4 68 117 114 102
+4 24 210 27 315
+4 158 113 46 172
+4 12 115 53 19
+4 53 115 114 19
+4 30 74 28 31
+4 185 184 317 247
+4 351 87 235 343
+4 124 192 125 123
+4 355 152 150 151
+4 301 104 2 307
+4 12 221 88 259
+4 212 28 213 215
+4 345 198 122 182
+4 211 73 137 74
+4 292 360 198 280
+4 138 105 63 62
+4 219 343 286 129
+4 206 208 336 10
+4 4 259 5 113
+4 377 296 379 193
+4 214 385 155 215
+4 12 15 32 88
+4 147 327 79 80
+4 217 171 5 172
+4 208 337 336 10
+4 183 272 231 153
+4 291 41 177 123
+4 73 40 140 229
+4 363 61 234 315
+4 36 302 339 38
+4 89 113 7 72
+4 30 215 157 42
+4 194 199 340 82
+4 193 160 158 159
+4 91 32 171 33
+4 60 59 235 13
+4 273 280 138 62
+4 363 234 312 316
+4 24 257 27 210
+4 235 18 127 343
+4 373 158 45 303
+4 16 309 58 320
+4 105 103 63 104
+4 287 115 167 19
+4 353 345 122 139
+4 247 161 130 151
+4 291 285 289 139
+4 313 210 250 166
+4 116 115 22 117
+4 0 104 103 307
+4 134 322 314 81
+4 248 251 145 74
+4 94 68 51 197
+4 60 15 58 13
+4 371 70 69 72
+4 248 74 146 98
+4 156 186 295 11
+4 330 263 205 76
+4 36 33 302 38
+4 18 148 286 93
+4 130 161 182 144
+4 94 199 81 82
+4 155 157 214 215
+4 129 17 282 19
+4 189 140 124 202
+4 290 274 288 276
+4 159 377 193 160
+4 182 162 140 139
+4 194 81 222 199
+4 22 21 20 117
+4 58 264 59 13
+4 379 111 296 72
+4 116 117 266 102
+4 66 194 195 199
+4 330 205 331 76
+4 16 94 314 17
+4 253 347 66 68
+4 185 247 47 151
+4 30 28 157 215
+4 2 104 0 307
+4 101 116 102 100
+4 88 91 32 259
+4 124 202 140 123
+4 84 116 23 7
+4 317 2 3 307
+4 321 295 125 192
+4 27 280 226 62
+4 313 382 339 38
+4 96 104 105 64
+4 342 334 219 390
+4 176 61 363 315
+4 26 61 176 315
+4 165 259 32 171
+4 47 144 355 151
+4 30 29 28 215
+4 95 97 230 98
+4 6 5 4 259
+4 157 186 73 42
+4 156 157 39 42
+4 149 82 148 17
+4 191 192 211 186
+4 185 151 150 153
+4 185 150 183 153
+4 96 315 316 64
+4 180 133 181 281
+4 230 97 270 64
+4 40 229 73 42
+4 157 28 212 215
+4 112 111 175 72
+4 88 221 91 259
+4 184 237 106 227
+4 353 285 345 139
+4 105 96 27 74
+4 301 307 317 227
+4 85 55 308 220
+4 77 37 330 76
+4 226 27 137 280
+4 247 237 130 161
+4 174 112 223 111
+4 126 275 124 123
+4 111 113 296 72
+4 329 279 294 280
+4 276 139 121 123
+4 12 88 32 259
+4 112 7 111 72
+4 110 299 91 119
+4 145 251 35 74
+4 332 243 329 256
+4 124 275 121 123
+4 375 205 203 92
+4 178 29 261 229
+4 379 175 72 371
+4 33 263 34 38
+4 56 110 54 57
+4 372 303 44 328
+4 302 166 210 313
+4 250 251 382 38
+4 341 331 305 92
+4 5 259 216 113
+4 68 199 67 197
+4 107 90 23 72
+4 7 113 111 72
+4 172 217 303 158
+4 184 247 185 151
+4 291 276 41 123
+4 141 68 65 117
+4 56 118 110 57
+4 24 26 176 315
+4 87 13 235 343
+4 262 295 205 186
+4 134 314 356 81
+4 279 294 359 329
+4 328 209 384 362
+4 165 35 145 166
+4 237 227 131 306
+4 24 27 26 315
+4 209 176 363 315
+4 27 315 210 146
+4 276 275 41 123
+4 266 116 20 117
+4 248 316 95 146
+4 340 199 93 82
+4 25 349 26 62
+4 96 74 105 98
+4 190 88 304 92
+4 58 18 16 320
+4 316 96 95 146
+4 159 43 45 46
+4 336 263 339 10
+4 217 172 303 302
+4 240 345 353 278
+4 213 212 95 28
+4 309 16 314 320
+4 84 167 23 115
+4 335 207 206 10
+4 159 45 158 46
+4 20 21 142 117
+4 314 94 51 81
+4 196 197 50 102
+4 285 366 284 352
+4 343 228 13 86
+4 34 263 205 186
+4 96 64 97 98
+4 260 21 22 117
+4 258 6 220 221
+4 333 342 219 343
+4 114 117 116 102
+4 180 131 182 133
+4 305 88 341 78
+4 56 4 109 118
+4 107 23 20 116
+4 247 237 184 227
+4 370 58 319 311
+4 147 149 327 80
+4 362 210 363 146
+4 260 287 141 115
+4 303 44 328 210
+4 335 212 154 249
+4 81 80 314 17
+4 39 271 179 178
+4 191 124 189 192
+4 209 362 363 384
+4 19 287 86 167
+4 91 221 88 78
+4 347 93 348 199
+4 205 263 262 186
+4 316 363 234 315
+4 237 306 182 229
+4 342 127 286 343
+4 317 132 47 227
+4 193 379 377 159
+4 221 228 258 86
+4 167 115 12 19
+4 140 139 162 123
+4 344 50 196 197
+4 242 256 240 278
+4 25 273 349 62
+4 341 57 358 78
+4 205 76 34 92
+4 216 113 217 172
+4 95 28 212 9
+4 189 140 211 124
+4 264 236 235 18
+4 183 178 231 29
+4 175 379 72 111
+4 132 131 130 227
+4 302 171 166 38
+4 317 184 301 227
+4 111 5 223 46
+4 333 334 219 342
+4 195 136 196 197
+4 109 299 110 118
+4 157 215 39 42
+4 130 247 47 227
+4 201 223 224 53
+4 244 105 138 324
+4 299 118 218 119
+4 224 53 223 112
+4 163 162 161 144
+4 287 129 282 19
+4 87 343 351 228
+4 124 140 211 192
+4 51 94 16 19
+4 324 293 329 243
+4 314 81 267 80
+4 95 74 28 9
+4 223 46 5 172
+4 291 238 352 181
+4 37 299 218 119
+389
+323
+388
+386
+380
+368
+364
+359
+274
+366
+283
+241
+239
+367
+349
+318
+369
+99
+200
+344
+374
+381
+319
+264
+59
+361
+305
+375
+204
+321
+126
+275
+203
+322
+267
+246
+309
+289
+288
+354
+376
+314
+370
+294
+279
+121
+353
+240
+254
+273
+25
+224
+50
+135
+311
+58
+60
+304
+191
+124
+290
+134
+292
+360
+120
+345
+255
+387
+226
+201
+52
+310
+14
+15
+190
+189
+202
+122
+356
+1000.0 0.45 7.5e4
diff --git a/SurgSim/Blocks/UnitTests/KeyboardTogglesComponentBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/KeyboardTogglesComponentBehaviorTests.cpp
new file mode 100644
index 0000000..0924781
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/KeyboardTogglesComponentBehaviorTests.cpp
@@ -0,0 +1,178 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 KeyboardTogglesComponentBehavior class.
+
+#include <algorithm>
+#include <gtest/gtest.h>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Blocks/KeyboardTogglesComponentBehavior.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Input/OutputComponent.h"
+
+using SurgSim::Blocks::KeyboardTogglesComponentBehavior;
+using SurgSim::Framework::Component;
+using SurgSim::Graphics::OsgBoxRepresentation;
+using SurgSim::Graphics::Representation;
+using SurgSim::Input::InputComponent;
+using SurgSim::Input::OutputComponent;
+
+TEST(KeyboardTogglesComponentBehavior, Constructor)
+{
+ EXPECT_NO_THROW(KeyboardTogglesComponentBehavior behavior("KeyboardTogglesComponentBehavior"));
+}
+
+TEST(KeyboardTogglesComponentBehavior, InputComponentTests)
+{
+ auto keyboardTogglesComponentBehavior =
+ std::make_shared<KeyboardTogglesComponentBehavior>("KeyboardTogglesComponentBehavior");
+ {
+ auto invalidInputComponent = std::make_shared<OutputComponent>("InvalidInputComponent");
+ EXPECT_ANY_THROW(keyboardTogglesComponentBehavior->setInputComponent(invalidInputComponent));
+ }
+
+ {
+ auto inputComponent = std::make_shared<InputComponent>("InputComponent");
+
+ EXPECT_NO_THROW(keyboardTogglesComponentBehavior->setInputComponent(inputComponent));
+ EXPECT_EQ(inputComponent, keyboardTogglesComponentBehavior->getInputComponent());
+ }
+}
+
+TEST(KeyboardTogglesComponentBehavior, RegistrationTests)
+{
+ auto keyboardTogglesComponentBehavior =
+ std::make_shared<KeyboardTogglesComponentBehavior>("KeyboardTogglesComponentBehavior");
+ {
+ auto graphics = std::make_shared<OsgBoxRepresentation>("Graphics");
+ auto graphics2 = std::make_shared<OsgBoxRepresentation>("Graphics2");
+ auto graphics3 = std::make_shared<OsgBoxRepresentation>("Graphics3");
+
+ keyboardTogglesComponentBehavior->registerKey(SurgSim::Device::KeyCode::KEY_A, graphics);
+ keyboardTogglesComponentBehavior->registerKey(SurgSim::Device::KeyCode::KEY_A, graphics2);
+ keyboardTogglesComponentBehavior->registerKey(SurgSim::Device::KeyCode::KEY_B, graphics3);
+
+ auto keyMap = keyboardTogglesComponentBehavior->getKeyboardRegistry();
+ auto keyAPair = keyMap.find(SurgSim::Device::KeyCode::KEY_A);
+ auto keyBPair = keyMap.find(SurgSim::Device::KeyCode::KEY_B);
+
+ EXPECT_EQ(2u, keyAPair->second.size());
+ EXPECT_EQ(1u, keyBPair->second.size());
+
+ EXPECT_NE(std::end(keyAPair->second), keyAPair->second.find(graphics));
+ EXPECT_NE(std::end(keyAPair->second), keyAPair->second.find(graphics2));
+ EXPECT_NE(std::end(keyBPair->second), keyBPair->second.find(graphics3));
+ }
+}
+
+TEST(KeyboardTogglesComponentBehavior, SetAndGetKeyboardRegisterTypeTest)
+{
+ auto keyboardTogglesComponentBehavior =
+ std::make_shared<KeyboardTogglesComponentBehavior>("KeyboardTogglesComponentBehavior");
+ std::shared_ptr<Representation> graphics1 = std::make_shared<OsgBoxRepresentation>("graphics1");
+ std::shared_ptr<Representation> graphics2 = std::make_shared<OsgBoxRepresentation>("graphics2");
+ std::shared_ptr<Representation> graphics3 = std::make_shared<OsgBoxRepresentation>("graphics3");
+
+ std::unordered_set<std::shared_ptr<Component>> set1;
+ std::unordered_set<std::shared_ptr<Component>> set2;
+
+ set1.insert(graphics1);
+ set2.insert(graphics2);
+ set2.insert(graphics3);
+
+ KeyboardTogglesComponentBehavior::KeyboardRegistryType keyMap;
+ keyMap[SurgSim::Device::KeyCode::KEY_A] = set1;
+ keyMap[SurgSim::Device::KeyCode::KEY_B] = set2;
+
+ EXPECT_NO_THROW(keyboardTogglesComponentBehavior->setKeyboardRegistry(keyMap));
+
+ auto retrievedKeyMap = keyboardTogglesComponentBehavior->getKeyboardRegistry();
+ EXPECT_EQ(keyMap.size(), retrievedKeyMap.size());
+ for (auto it = std::begin(keyMap); it != std::end(keyMap); ++it)
+ {
+ auto componentSet = retrievedKeyMap.find(it->first)->second;
+ EXPECT_EQ(it->second.size(), componentSet.size());
+ for (auto item = std::begin(it->second); item != std::end(it->second); ++item)
+ {
+ auto match = std::find_if(std::begin(componentSet), std::end(componentSet),
+ [&item](const std::shared_ptr<Component> rep)
+ {
+ return rep->getName() == (*item)->getName();
+ });
+ EXPECT_NE(std::end(componentSet), match);
+ }
+ }
+}
+
+TEST(KeyboardTogglesComponentBehavior, Serialization)
+{
+ auto keyboardTogglesComponentBehavior =
+ std::make_shared<KeyboardTogglesComponentBehavior>("KeyboardTogglesComponentBehavior");
+
+ std::shared_ptr<SurgSim::Framework::Component> inputComponent = std::make_shared<InputComponent>("InputComponent");
+ std::shared_ptr<Representation> graphics1 = std::make_shared<OsgBoxRepresentation>("graphics1");
+ std::shared_ptr<Representation> graphics2 = std::make_shared<OsgBoxRepresentation>("graphics2");
+
+ std::unordered_set<std::shared_ptr<Component>> set1;
+ std::unordered_set<std::shared_ptr<Component>> set2;
+
+ set1.insert(graphics1);
+ set2.insert(graphics2);
+
+ KeyboardTogglesComponentBehavior::KeyboardRegistryType keyMap;
+ keyMap[SurgSim::Device::KeyCode::KEY_A] = set1;
+ keyMap[SurgSim::Device::KeyCode::KEY_B] = set2;
+
+ keyboardTogglesComponentBehavior->setValue("KeyboardRegistry", keyMap);
+ keyboardTogglesComponentBehavior->setValue("InputComponent", inputComponent);
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*keyboardTogglesComponentBehavior));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(5, node[keyboardTogglesComponentBehavior->getClassName()].size());
+
+ std::shared_ptr<KeyboardTogglesComponentBehavior> newKeyboardTogglesComponentBehavior;
+ EXPECT_NO_THROW(newKeyboardTogglesComponentBehavior = std::dynamic_pointer_cast<KeyboardTogglesComponentBehavior>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ ASSERT_NE(nullptr, newKeyboardTogglesComponentBehavior);
+ EXPECT_NE(nullptr, newKeyboardTogglesComponentBehavior->getValue<
+ std::shared_ptr<InputComponent>>("InputComponent"));
+
+ // Make sure every registered representation in the original 'keyMap' is present in the de-serialized keyMap.
+ auto retrievedKeyMap = newKeyboardTogglesComponentBehavior->getValue<
+ KeyboardTogglesComponentBehavior::KeyboardRegistryType>("KeyboardRegistry");
+
+ EXPECT_EQ(keyMap.size(), retrievedKeyMap.size());
+ for (auto it = std::begin(keyMap); it != std::end(keyMap); ++it)
+ {
+ auto componentSet = retrievedKeyMap.find(it->first)->second;
+ EXPECT_EQ(it->second.size(), componentSet.size());
+ for (auto item = std::begin(it->second); item != std::end(it->second); ++item)
+ {
+ auto match = std::find_if(std::begin(componentSet), std::end(componentSet),
+ [&item](const std::shared_ptr<Component> rep)
+ {
+ return rep->getName() == (*item)->getName();
+ });
+ EXPECT_NE(std::end(componentSet), match);
+ }
+ }
+}
diff --git a/SurgSim/Blocks/UnitTests/MassSpring1DRepresentationTests.cpp b/SurgSim/Blocks/UnitTests/MassSpring1DRepresentationTests.cpp
new file mode 100644
index 0000000..01a2e3d
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/MassSpring1DRepresentationTests.cpp
@@ -0,0 +1,110 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MassSpring1DRepresentation class.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/MassSpring1DRepresentation.h"
+#include "SurgSim/Blocks/UnitTests/SpringTestUtils.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::LinearSpring;
+using SurgSim::Blocks::MassSpring1DRepresentation;
+using SurgSim::Blocks::springTest;
+
+TEST(MassSpring1DRepresentationTests, init1DTest)
+{
+ MassSpring1DRepresentation m("MassSpring1D");
+
+ std::array<Vector3d, 2> extremities = {{ Vector3d(1.1, 1.2, 1.3), Vector3d(2.2, 2.3, 2.4) }};
+ size_t numNodesPerDim[1] = {10};
+
+ std::vector<Vector3d> nodes;
+ for (size_t nodeId = 0; nodeId < numNodesPerDim[0]; ++nodeId)
+ {
+ double abscissa = static_cast<double>(nodeId) / static_cast<double>(numNodesPerDim[0] - 1);
+ nodes.push_back(extremities[0] + abscissa * (extremities[1] - extremities[0]));
+ }
+
+ std::vector<size_t> boundaryConditions;
+ double totalMass = 1.1;
+ double stiffnessStretching = 2.2;
+ double dampingStretching = 3.3;
+ double stiffnessBending = 4.4;
+ double dampingBending = 5.5;
+ boundaryConditions.push_back(0);
+ boundaryConditions.push_back(numNodesPerDim[0] - 1);
+ m.init1D(nodes, boundaryConditions,
+ totalMass, stiffnessStretching, dampingStretching, stiffnessBending, dampingBending);
+ m.initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m.wakeUp();
+
+ EXPECT_EQ(numNodesPerDim[0] * 3, m.getNumDof());
+ EXPECT_EQ(numNodesPerDim[0], m.getNumMasses());
+ EXPECT_EQ(numNodesPerDim[0] - 1 + numNodesPerDim[0] - 2, m.getNumSprings());
+
+ for (size_t massId = 0; massId < m.getNumMasses(); massId++)
+ {
+ EXPECT_DOUBLE_EQ(totalMass/numNodesPerDim[0], m.getMass(massId)->getMass());
+ }
+
+ // The 1st springs are the stretching springs
+ size_t springId = 0;
+ for (size_t nodeId = 0; nodeId < m.getNumMasses() - 1; nodeId++)
+ {
+ springTest(std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + 1, stiffnessStretching, dampingStretching);
+ springId++;
+ }
+
+ // Followed by bending springs
+ for (size_t nodeId = 0; nodeId < m.getNumMasses() - 2; nodeId++)
+ {
+ springTest(std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + 2, stiffnessBending, dampingBending);
+ springId++;
+ }
+
+ // States all equals
+ EXPECT_EQ(*m.getPreviousState(), *m.getFinalState());
+ EXPECT_EQ(*m.getCurrentState(), *m.getFinalState());
+ EXPECT_EQ(*m.getInitialState(), *m.getFinalState());
+
+ // States should contains expected values
+ Vector3d delta = (extremities[1] - extremities[0]) / static_cast<double>(numNodesPerDim[0] - 1);
+ EXPECT_TRUE(m.getFinalState()->getVelocities().isZero());
+ EXPECT_FALSE(m.getFinalState()->getPositions().isZero());
+ for (size_t nodeId = 0; nodeId < numNodesPerDim[0]; nodeId++)
+ {
+ Vector3d piExpected = extremities[0] + delta * static_cast<double>(nodeId);
+ SurgSim::Math::Vector& x = m.getFinalState()->getPositions();
+ Eigen::VectorBlock<SurgSim::Math::Vector> pi = SurgSim::Math::getSubVector(x, nodeId, 3);
+ EXPECT_TRUE(pi.isApprox(piExpected));
+ }
+
+ std::vector<size_t> dofBoundaryConditions;
+ for (auto it = boundaryConditions.begin(); it != boundaryConditions.end(); ++it)
+ {
+ dofBoundaryConditions.push_back((*it) * 3);
+ dofBoundaryConditions.push_back((*it) * 3 + 1);
+ dofBoundaryConditions.push_back((*it) * 3 + 2);
+ }
+ EXPECT_EQ(dofBoundaryConditions, m.getFinalState()->getBoundaryConditions());
+}
diff --git a/SurgSim/Blocks/UnitTests/MassSpring2DRepresentationTests.cpp b/SurgSim/Blocks/UnitTests/MassSpring2DRepresentationTests.cpp
new file mode 100644
index 0000000..b246cb4
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/MassSpring2DRepresentationTests.cpp
@@ -0,0 +1,185 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MassSpring2DRepresentation class.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/MassSpring2DRepresentation.h"
+#include "SurgSim/Blocks/UnitTests/SpringTestUtils.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::LinearSpring;
+using SurgSim::Blocks::MassSpring2DRepresentation;
+using SurgSim::Blocks::springTest;
+
+TEST(MassSpring2DRepresentationTests, init2DTest)
+{
+ MassSpring2DRepresentation m("MassSpring2D");
+
+ std::array<std::array<Vector3d, 2>, 2> extremities =
+ {{
+ {{Vector3d(0.1, 0.2, 0.3), Vector3d(1.1, 1.2, 1.3)}},
+ {{Vector3d(10.1, 10.2, 10.4), Vector3d(9.5, 9.6, 9.3)}}
+ }};
+ size_t numNodesPerDim[2] = {10, 5};
+ std::vector<size_t> boundaryConditions;
+ double totalMass = 1.1;
+ double stiffnessStretching = 2.2;
+ double dampingStretching = 3.3;
+ double stiffnessBending = 4.4;
+ double dampingBending = 5.5;
+ double stiffnessFaceDiagonal = 6.6;
+ double dampingFaceDiagonal = 7.7;
+ boundaryConditions.push_back(0);
+ boundaryConditions.push_back(numNodesPerDim[0] * numNodesPerDim[1] - 1);
+ m.init2D(extremities, numNodesPerDim, boundaryConditions,
+ totalMass,
+ stiffnessStretching, dampingStretching,
+ stiffnessBending, dampingBending,
+ stiffnessFaceDiagonal, dampingFaceDiagonal);
+ m.initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m.wakeUp();
+
+
+ const size_t numNodes = numNodesPerDim[0] * numNodesPerDim[1];
+ EXPECT_EQ(numNodes * 3, m.getNumDof());
+ EXPECT_EQ(numNodes, m.getNumMasses());
+ size_t numSpringsExpected = 0;
+ numSpringsExpected += numNodesPerDim[1] * (numNodesPerDim[0] - 1); // Stretching along Y
+ numSpringsExpected += numNodesPerDim[0] * (numNodesPerDim[1] - 1); // Stretching along X
+ numSpringsExpected += numNodesPerDim[1] * (numNodesPerDim[0] - 2); // Bending along Y
+ numSpringsExpected += numNodesPerDim[0] * (numNodesPerDim[1] - 2); // Bending along X
+ numSpringsExpected += (numNodesPerDim[0] - 1) * (numNodesPerDim[1] - 1) * 2; // Face diagonal
+ EXPECT_EQ(numSpringsExpected, m.getNumSprings());
+
+ for (size_t massId = 0; massId < m.getNumMasses(); massId++)
+ {
+ EXPECT_DOUBLE_EQ(totalMass/numNodes, m.getMass(massId)->getMass());
+ }
+
+ const size_t rowOffset = numNodesPerDim[0];
+ const size_t colOffset = 1;
+
+ size_t springId = 0;
+ // The 1st springs are the stretching springs along X
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ size_t nodeId = row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + colOffset, stiffnessStretching, dampingStretching);
+ springId++;
+ }
+ }
+ // Followed by stretching springs along Y
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ size_t nodeId = row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + rowOffset, stiffnessStretching, dampingStretching);
+ springId++;
+ }
+ }
+ // Followed by bending springs along X
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 2; col++)
+ {
+ size_t nodeId = row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + 2 * colOffset, stiffnessBending, dampingBending);
+ springId++;
+ }
+ }
+ // Followed by bending springs along Y
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 2; row++)
+ {
+ size_t nodeId = row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + 2 * rowOffset, stiffnessBending, dampingBending);
+ springId++;
+ }
+ }
+ // Followed by face diagonal springs
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ size_t nodeId = row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + rowOffset + colOffset, stiffnessFaceDiagonal, dampingFaceDiagonal);
+ springId++;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId + colOffset, nodeId + rowOffset, stiffnessFaceDiagonal, dampingFaceDiagonal);
+ springId++;
+ }
+ }
+
+ // States all equals
+ EXPECT_EQ(*m.getPreviousState(), *m.getFinalState());
+ EXPECT_EQ(*m.getCurrentState(), *m.getFinalState());
+ EXPECT_EQ(*m.getInitialState(), *m.getFinalState());
+
+ // States should contains expected values
+ EXPECT_TRUE(m.getFinalState()->getVelocities().isZero());
+ EXPECT_FALSE(m.getFinalState()->getPositions().isZero());
+ Vector3d rowExtremititiesDelta[2] =
+ {
+ (extremities[0][1] - extremities[0][0]) / static_cast<double>(numNodesPerDim[1] - 1) ,
+ (extremities[1][1] - extremities[1][0]) / static_cast<double>(numNodesPerDim[1] - 1)
+ };
+ size_t nodeId = 0;
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ Vector3d rowExtremities[2];
+ rowExtremities[0] = extremities[0][0] + rowExtremititiesDelta[0] * static_cast<double>(row);
+ rowExtremities[1] = extremities[1][0] + rowExtremititiesDelta[1] * static_cast<double>(row);
+
+ Vector3d delta = (rowExtremities[1] - rowExtremities[0]) / static_cast<double>(numNodesPerDim[0] - 1);
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ SurgSim::Math::Vector3d piExpected(rowExtremities[0] + static_cast<double>(col) * delta);
+ SurgSim::Math::Vector& x = m.getFinalState()->getPositions();
+ Eigen::VectorBlock<SurgSim::Math::Vector> pi = SurgSim::Math::getSubVector(x, nodeId, 3);
+ EXPECT_TRUE(pi.isApprox(piExpected));
+ nodeId++;
+ }
+ }
+
+ std::vector<size_t> dofBoundaryConditions;
+ for (auto it = boundaryConditions.begin(); it != boundaryConditions.end(); ++it)
+ {
+ dofBoundaryConditions.push_back((*it) * 3);
+ dofBoundaryConditions.push_back((*it) * 3 + 1);
+ dofBoundaryConditions.push_back((*it) * 3 + 2);
+ }
+ EXPECT_EQ(dofBoundaryConditions, m.getFinalState()->getBoundaryConditions());
+}
diff --git a/SurgSim/Blocks/UnitTests/MassSpring3DRepresentationTests.cpp b/SurgSim/Blocks/UnitTests/MassSpring3DRepresentationTests.cpp
new file mode 100644
index 0000000..24969ce
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/MassSpring3DRepresentationTests.cpp
@@ -0,0 +1,334 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MassSpring3DRepresentation class.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/MassSpring3DRepresentation.h"
+#include "SurgSim/Blocks/UnitTests/SpringTestUtils.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::LinearSpring;
+using SurgSim::Blocks::MassSpring3DRepresentation;
+using SurgSim::Blocks::springTest;
+
+TEST(MassSpring3DRepresentationTests, init3DTest)
+{
+ MassSpring3DRepresentation m("MassSpring3D");
+
+ std::array<std::array<std::array<Vector3d, 2>, 2>, 2> extremities =
+ {{
+ {{
+ {{Vector3d(-0.5, -0.4, -0.3), Vector3d( 0.4, -0.5, -0.3)}},
+ {{Vector3d(-0.4, 0.3, -0.5), Vector3d( 0.5, 0.3, -0.4)}}
+ }},
+ {{
+ {{Vector3d(-0.3, -0.5, 0.4), Vector3d( 0.5, -0.3, 0.4)}},
+ {{Vector3d(-0.4, 0.3, 0.5), Vector3d( 0.4, 0.3, 0.5)}}
+ }}
+ }};
+ size_t numNodesPerDim[3] = {10, 5, 3};
+ std::vector<size_t> boundaryConditions;
+ double totalMass = 1.1;
+ double stiffnessStretching = 2.2;
+ double dampingStretching = 3.3;
+ double stiffnessBending = 4.4;
+ double dampingBending = 5.5;
+ double stiffnessFaceDiagonal = 6.6;
+ double dampingFaceDiagonal = 7.7;
+ double stiffnessVolumeDiagonal = 8.8;
+ double dampingVolumeDiagonal = 9.9;
+ boundaryConditions.push_back(0);
+ boundaryConditions.push_back(numNodesPerDim[0] * numNodesPerDim[1] * numNodesPerDim[2] - 1);
+ m.init3D(extremities, numNodesPerDim, boundaryConditions,
+ totalMass,
+ stiffnessStretching, dampingStretching,
+ stiffnessBending, dampingBending,
+ stiffnessFaceDiagonal, dampingFaceDiagonal,
+ stiffnessVolumeDiagonal, dampingVolumeDiagonal);
+ m.initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m.wakeUp();
+
+ const size_t numNodes = numNodesPerDim[0] * numNodesPerDim[1] * numNodesPerDim[2];
+ EXPECT_EQ(numNodes * 3, m.getNumDof());
+ EXPECT_EQ(numNodes, m.getNumMasses());
+ size_t numSpringsExpected = 0;
+ // Stretching springs
+ numSpringsExpected += numNodesPerDim[0] * numNodesPerDim[2] * (numNodesPerDim[1] - 1); // along X
+ numSpringsExpected += numNodesPerDim[1] * numNodesPerDim[2] * (numNodesPerDim[0] - 1); // along Y
+ numSpringsExpected += numNodesPerDim[0] * numNodesPerDim[1] * (numNodesPerDim[2] - 1); // along Z
+ // Bending springs
+ numSpringsExpected += numNodesPerDim[0] * numNodesPerDim[2] * (numNodesPerDim[1] - 2); // along X
+ numSpringsExpected += numNodesPerDim[1] * numNodesPerDim[2] * (numNodesPerDim[0] - 2); // along Y
+ numSpringsExpected += numNodesPerDim[0] * numNodesPerDim[1] * (numNodesPerDim[2] - 2); // along Z
+ // Face diagonal springs
+ numSpringsExpected += numNodesPerDim[1] * (numNodesPerDim[2] - 1) * (numNodesPerDim[0] - 1) * 2; // facing X
+ numSpringsExpected += numNodesPerDim[0] * (numNodesPerDim[2] - 1) * (numNodesPerDim[1] - 1) * 2; // facing Y
+ numSpringsExpected += numNodesPerDim[2] * (numNodesPerDim[1] - 1) * (numNodesPerDim[0] - 1) * 2; // facing Z
+ // Volume diagonal springs
+ numSpringsExpected += (numNodesPerDim[0] - 1) * (numNodesPerDim[1] - 1) * (numNodesPerDim[2] - 1) * 4;
+ EXPECT_EQ(numSpringsExpected, m.getNumSprings());
+
+ for (size_t massId = 0; massId < m.getNumMasses(); massId++)
+ {
+ EXPECT_DOUBLE_EQ(totalMass/numNodes, m.getMass(massId)->getMass());
+ }
+
+ const size_t depthOffset = numNodesPerDim[0] * numNodesPerDim[1];
+ const size_t rowOffset = numNodesPerDim[0];
+ const size_t colOffset = 1;
+
+ size_t springId = 0;
+ // The 1st springs are the stretching springs along X
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + colOffset, stiffnessStretching, dampingStretching);
+ springId++;
+ }
+ }
+ }
+ // Followed by stretching springs along Y
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + rowOffset, stiffnessStretching, dampingStretching);
+ springId++;
+ }
+ }
+ }
+ // Followed by stretching springs along Z
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t depth = 0; depth < numNodesPerDim[2] - 1; depth++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + depthOffset, stiffnessStretching, dampingStretching);
+ springId++;
+ }
+ }
+ }
+
+ // Followed by bending springs along X
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 2; col++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + 2 * colOffset, stiffnessBending, dampingBending);
+ springId++;
+ }
+ }
+ }
+ // Followed by bending springs along Y
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 2; row++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + 2 * rowOffset, stiffnessBending, dampingBending);
+ springId++;
+ }
+ }
+ }
+ // Followed by bending springs along Z
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t depth = 0; depth < numNodesPerDim[2] - 2; depth++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + 2 * depthOffset, stiffnessBending, dampingBending);
+ springId++;
+ }
+ }
+ }
+
+ // Followed by face diagonal springs orthogonal to Z
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + rowOffset + colOffset, stiffnessFaceDiagonal, dampingFaceDiagonal);
+ springId++;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId + colOffset, nodeId + rowOffset, stiffnessFaceDiagonal, dampingFaceDiagonal);
+ springId++;
+ }
+ }
+ }
+ // Followed by face diagonal springs orthogonal to Y
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ for (size_t depth = 0; depth < numNodesPerDim[2] - 1; depth++)
+ {
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + depthOffset + colOffset, stiffnessFaceDiagonal, dampingFaceDiagonal);
+ springId++;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId + colOffset, nodeId + depthOffset, stiffnessFaceDiagonal, dampingFaceDiagonal);
+ springId++;
+ }
+ }
+ }
+ // Followed by face diagonal springs orthogonal to X
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ for (size_t depth = 0; depth < numNodesPerDim[2] - 1; depth++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + depthOffset + rowOffset, stiffnessFaceDiagonal, dampingFaceDiagonal);
+ springId++;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId + rowOffset, nodeId + depthOffset, stiffnessFaceDiagonal, dampingFaceDiagonal);
+ springId++;
+ }
+ }
+ }
+
+ // Followed by volume diagonal springs
+ for (size_t col = 0; col < numNodesPerDim[0] - 1; col++)
+ {
+ for (size_t row = 0; row < numNodesPerDim[1] - 1; row++)
+ {
+ for (size_t depth = 0; depth < numNodesPerDim[2] - 1; depth++)
+ {
+ size_t nodeId = depth * depthOffset + row * rowOffset + col * colOffset;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId, nodeId + depthOffset + rowOffset + colOffset,
+ stiffnessVolumeDiagonal, dampingVolumeDiagonal);
+ springId++;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId + colOffset, nodeId + depthOffset + rowOffset,
+ stiffnessVolumeDiagonal, dampingVolumeDiagonal);
+ springId++;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId + rowOffset, nodeId + depthOffset + colOffset,
+ stiffnessVolumeDiagonal, dampingVolumeDiagonal);
+ springId++;
+
+ springTest( std::dynamic_pointer_cast<LinearSpring>(m.getSpring(springId)), m.getFinalState(),
+ nodeId + rowOffset + colOffset, nodeId + depthOffset,
+ stiffnessVolumeDiagonal, dampingVolumeDiagonal);
+ springId++;
+ }
+ }
+ }
+
+ // States all equals
+ EXPECT_EQ(*m.getPreviousState(), *m.getFinalState());
+ EXPECT_EQ(*m.getCurrentState(), *m.getFinalState());
+ EXPECT_EQ(*m.getInitialState(), *m.getFinalState());
+
+ // States should contains expected values
+ EXPECT_TRUE(m.getFinalState()->getVelocities().isZero());
+ EXPECT_FALSE(m.getFinalState()->getPositions().isZero());
+ Vector3d depthExtremitiesDelta[2][2] =
+ {{(extremities[0][0][1] - extremities[0][0][0]) / static_cast<double>(numNodesPerDim[2] - 1) ,
+ (extremities[1][0][1] - extremities[1][0][0]) / static_cast<double>(numNodesPerDim[2] - 1)}
+ ,
+ {(extremities[0][1][1] - extremities[0][1][0]) / static_cast<double>(numNodesPerDim[2] - 1) ,
+ (extremities[1][1][1] - extremities[1][1][0]) / static_cast<double>(numNodesPerDim[2] - 1)}};
+
+ size_t nodeId = 0;
+ for (size_t depth = 0; depth < numNodesPerDim[2]; depth++)
+ {
+ Vector3d depthExtremities[2][2];
+ depthExtremities[0][0] = extremities[0][0][0] + depthExtremitiesDelta[0][0] * static_cast<double>(depth);
+ depthExtremities[1][0] = extremities[1][0][0] + depthExtremitiesDelta[0][1] * static_cast<double>(depth);
+ depthExtremities[0][1] = extremities[0][1][0] + depthExtremitiesDelta[1][0] * static_cast<double>(depth);
+ depthExtremities[1][1] = extremities[1][1][0] + depthExtremitiesDelta[1][1] * static_cast<double>(depth);
+
+ Vector3d rowExtremitiesDelta[2] =
+ {(depthExtremities[0][1] - depthExtremities[0][0]) / static_cast<double>(numNodesPerDim[1] - 1) ,
+ (depthExtremities[1][1] - depthExtremities[1][0]) / static_cast<double>(numNodesPerDim[1] - 1)};
+ for (size_t row = 0; row < numNodesPerDim[1]; row++)
+ {
+ Vector3d rowExtremities[2];
+ rowExtremities[0] = depthExtremities[0][0] + rowExtremitiesDelta[0] * static_cast<double>(row);
+ rowExtremities[1] = depthExtremities[1][0] + rowExtremitiesDelta[1] * static_cast<double>(row);
+
+ Vector3d delta = (rowExtremities[1] - rowExtremities[0]) / static_cast<double>(numNodesPerDim[0] - 1);
+ for (size_t col = 0; col < numNodesPerDim[0]; col++)
+ {
+ Vector3d xiExpected(rowExtremities[0] + static_cast<double>(col) * delta);
+ SurgSim::Math::Vector& x = m.getFinalState()->getPositions();
+ Eigen::VectorBlock<SurgSim::Math::Vector> xi = SurgSim::Math::getSubVector(x, nodeId, 3);
+ EXPECT_TRUE(xi.isApprox(xiExpected));
+ nodeId++;
+ }
+ }
+ }
+
+ std::vector<size_t> dofBoundaryConditions;
+ for (auto it = boundaryConditions.begin(); it != boundaryConditions.end(); ++it)
+ {
+ dofBoundaryConditions.push_back((*it) * 3);
+ dofBoundaryConditions.push_back((*it) * 3 + 1);
+ dofBoundaryConditions.push_back((*it) * 3 + 2);
+ }
+ EXPECT_EQ(dofBoundaryConditions, m.getFinalState()->getBoundaryConditions());
+}
diff --git a/SurgSim/Blocks/UnitTests/MassSpringNDRepresentationUtilsTests.cpp b/SurgSim/Blocks/UnitTests/MassSpringNDRepresentationUtilsTests.cpp
new file mode 100644
index 0000000..e75cf57
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/MassSpringNDRepresentationUtilsTests.cpp
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MassSpring1DRepresentation class.
+
+#include <memory>
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/MassSpringNDRepresentationUtils.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+using SurgSim::Physics::LinearSpring;
+
+TEST(MassSpringNDRepresentationUtilsTests, CreateLinearSpring)
+{
+ const int numDofPerNode = 3;
+ const int numNodes = 10;
+ double stiffness = 14.54;
+ double damping = 4.398;
+
+ {
+ SCOPED_TRACE("Create linear spring with 0 rest-length");
+ std::shared_ptr<SurgSim::Math::OdeState> state;
+ state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(numDofPerNode, numNodes);
+
+ std::shared_ptr<LinearSpring> linearSpring;
+ linearSpring = SurgSim::Blocks::createLinearSpring(state, 3, 7, stiffness, damping);
+ EXPECT_DOUBLE_EQ(damping, linearSpring->getDamping());
+ EXPECT_DOUBLE_EQ(stiffness, linearSpring->getStiffness());
+ EXPECT_DOUBLE_EQ(0.0, linearSpring->getRestLength());
+ EXPECT_EQ(2u, linearSpring->getNumNodes());
+ EXPECT_EQ(3u, linearSpring->getNodeId(0));
+ EXPECT_EQ(7u, linearSpring->getNodeId(1));
+ EXPECT_EQ(2, linearSpring->getNodeIds().size());
+ EXPECT_EQ(3u, linearSpring->getNodeIds()[0]);
+ EXPECT_EQ(7u, linearSpring->getNodeIds()[1]);
+ }
+
+ {
+ SCOPED_TRACE("Create linear spring with non 0 rest-length");
+ std::shared_ptr<SurgSim::Math::OdeState> state;
+ state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(numDofPerNode, numNodes);
+ for (int nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ SurgSim::Math::Vector& x = state->getPositions();
+ SurgSim::Math::getSubVector(x, nodeId, numDofPerNode)[0] = static_cast<double>(nodeId) + 0.45009;
+ SurgSim::Math::getSubVector(x, nodeId, numDofPerNode)[1] = -4.5343;
+ SurgSim::Math::getSubVector(x, nodeId, numDofPerNode)[2] = 0.2325445;
+ }
+
+ std::shared_ptr<LinearSpring> linearSpring;
+ linearSpring = SurgSim::Blocks::createLinearSpring(state, 3, 7, stiffness, damping);
+ EXPECT_DOUBLE_EQ(damping, linearSpring->getDamping());
+ EXPECT_DOUBLE_EQ(stiffness, linearSpring->getStiffness());
+ EXPECT_DOUBLE_EQ(4.0, linearSpring->getRestLength());
+ EXPECT_EQ(2u, linearSpring->getNumNodes());
+ EXPECT_EQ(3u, linearSpring->getNodeId(0));
+ EXPECT_EQ(7u, linearSpring->getNodeId(1));
+ EXPECT_EQ(2, linearSpring->getNodeIds().size());
+ EXPECT_EQ(3u, linearSpring->getNodeIds()[0]);
+ EXPECT_EQ(7u, linearSpring->getNodeIds()[1]);
+ }
+}
diff --git a/SurgSim/Blocks/UnitTests/PoseInterpolatorTests.cpp b/SurgSim/Blocks/UnitTests/PoseInterpolatorTests.cpp
new file mode 100644
index 0000000..f9501d3
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/PoseInterpolatorTests.cpp
@@ -0,0 +1,178 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+
+#include "SurgSim/Blocks/PoseInterpolator.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Representation.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+
+
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::interpolate;
+
+namespace
+{
+ RigidTransform3d startPose = makeRigidTransform(Quaterniond::Identity(), Vector3d(1.0, 2.0, 3.0));
+ RigidTransform3d endPose = makeRigidTransform(Quaterniond::Identity(), Vector3d(3.0, 2.0, 1.0));
+}
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+TEST(PoseInterpolatorTests, InitTest)
+{
+ ASSERT_NO_THROW({auto interpolator = std::make_shared<PoseInterpolator>("test");});
+}
+
+TEST(PoseInterpolatorTests, StartAndEndPose)
+{
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("element");
+ auto representation = std::make_shared<SurgSim::Framework::Representation>("representation");
+ auto interpolator = std::make_shared<PoseInterpolator>("interpolator");
+
+ interpolator->setStartingPose(startPose);
+ interpolator->setEndingPose(endPose);
+
+ element->addComponent(representation);
+ element->addComponent(interpolator);
+ element->initialize();
+
+ interpolator->wakeUp();
+ representation->wakeUp();
+ interpolator->update(0.0);
+
+ EXPECT_TRUE(startPose.isApprox(representation->getPose()));
+ interpolator->update(0.5);
+
+ RigidTransform3d pose = interpolate(startPose, endPose, 0.5);
+ EXPECT_TRUE(pose.matrix().isApprox(representation->getPose().matrix())) << pose.matrix() << std::endl
+ << representation->getPose().matrix();
+}
+
+TEST(PoseInterpolatorTests, UseOptionalStartPose)
+{
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("element");
+ auto representation = std::make_shared<SurgSim::Framework::Representation>("representation");
+ auto interpolator = std::make_shared<PoseInterpolator>("interpolator");
+
+ interpolator->setStartingPose(startPose);
+ interpolator->setEndingPose(endPose);
+ interpolator->setTarget(element);
+
+ element->addComponent(representation);
+ element->addComponent(interpolator);
+ element->initialize();
+
+ interpolator->wakeUp();
+ representation->wakeUp();
+ interpolator->update(0.0);
+
+ EXPECT_TRUE(startPose.isApprox(representation->getPose()));
+ interpolator->update(0.5);
+
+ RigidTransform3d pose = interpolate(startPose, endPose, 0.5);
+ EXPECT_TRUE(pose.matrix().isApprox(representation->getPose().matrix())) << pose.matrix() << std::endl
+ << representation->getPose().matrix();
+}
+
+TEST(PoseInterpolatorTests, UseLoop)
+{
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("element");
+ auto representation = std::make_shared<SurgSim::Framework::Representation>("representation");
+ auto interpolator = std::make_shared<PoseInterpolator>("interpolator");
+
+ interpolator->setStartingPose(startPose);
+ interpolator->setEndingPose(endPose);
+
+ element->addComponent(representation);
+ element->addComponent(interpolator);
+ element->initialize();
+
+ interpolator->setPingPong(true);
+ interpolator->setLoop(true);
+
+ // Enabling loop should disable pingpong
+ EXPECT_TRUE(interpolator->isLoop());
+ EXPECT_FALSE(interpolator->isPingPong());
+
+ interpolator->wakeUp();
+ representation->wakeUp();
+ interpolator->update(0.0);
+
+ EXPECT_TRUE(startPose.isApprox(representation->getPose()));
+ interpolator->update(0.25);
+
+ RigidTransform3d pose = interpolate(startPose, endPose, 0.25);
+ EXPECT_TRUE(pose.matrix().isApprox(representation->getPose().matrix())) << pose.matrix() << std::endl
+ << representation->getPose().matrix();
+
+ // We advance by 1.0, this should wrap around to 0.25 again and the poses should be the same
+ interpolator->update(1.0);
+ EXPECT_TRUE(pose.matrix().isApprox(representation->getPose().matrix())) << pose.matrix() << std::endl
+ << representation->getPose().matrix();
+
+}
+
+TEST(PoseInterpolatorTests, UsePingPong)
+{
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("element");
+ auto representation = std::make_shared<SurgSim::Framework::Representation>("representation");
+ auto interpolator = std::make_shared<PoseInterpolator>("interpolator");
+
+ interpolator->setStartingPose(startPose);
+ interpolator->setEndingPose(endPose);
+
+ element->addComponent(representation);
+ element->addComponent(interpolator);
+ element->initialize();
+
+ interpolator->setLoop(true);
+ interpolator->setPingPong(true);
+
+ // Enabling PingPong should disable Loop
+ EXPECT_FALSE(interpolator->isLoop());
+ EXPECT_TRUE(interpolator->isPingPong());
+
+ interpolator->wakeUp();
+ representation->wakeUp();
+ interpolator->update(0.0);
+
+ EXPECT_TRUE(startPose.isApprox(representation->getPose()));
+ interpolator->update(0.25);
+
+ RigidTransform3d pose = interpolate(startPose, endPose, 0.25);
+ EXPECT_TRUE(pose.matrix().isApprox(representation->getPose().matrix())) << pose.matrix() << std::endl
+ << representation->getPose().matrix();
+
+ // We advance by 1.0, this should wrap around to 0.25 and the poses should be flipped
+ pose = interpolate(endPose, startPose, 0.25);
+ interpolator->update(1.0);
+ EXPECT_TRUE(pose.matrix().isApprox(representation->getPose().matrix())) << pose.matrix() << std::endl
+ << representation->getPose().matrix();
+
+}
+
+};
+};
diff --git a/SurgSim/Blocks/UnitTests/SpringTestUtils.cpp b/SurgSim/Blocks/UnitTests/SpringTestUtils.cpp
new file mode 100644
index 0000000..2f1892d
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/SpringTestUtils.cpp
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Utility function to test a LinearSpring
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/UnitTests/SpringTestUtils.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+void springTest(std::shared_ptr<SurgSim::Physics::LinearSpring> spring,
+ std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t expectedNodeId0, size_t expectedNodeId1,
+ double expectedStiffness, double expectedDamping)
+{
+ EXPECT_DOUBLE_EQ(expectedStiffness, spring->getStiffness());
+ EXPECT_DOUBLE_EQ(expectedDamping, spring->getDamping());
+ EXPECT_EQ(expectedNodeId0, spring->getNodeId(0));
+ EXPECT_EQ(expectedNodeId1, spring->getNodeId(1));
+ SurgSim::Math::Vector& x =state->getPositions();
+ Eigen::VectorBlock<SurgSim::Math::Vector> x0 = SurgSim::Math::getSubVector(x, expectedNodeId0, 3);
+ Eigen::VectorBlock<SurgSim::Math::Vector> x1 = SurgSim::Math::getSubVector(x, expectedNodeId1, 3);
+ EXPECT_DOUBLE_EQ((x1 - x0).norm(), spring->getRestLength());
+}
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
diff --git a/SurgSim/Blocks/UnitTests/SpringTestUtils.h b/SurgSim/Blocks/UnitTests/SpringTestUtils.h
new file mode 100644
index 0000000..d18062c
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/SpringTestUtils.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Utility function to test a LinearSpring
+
+#ifndef SURGSIM_BLOCKS_UNITTESTS_SPRINGTESTUTILS_H
+#define SURGSIM_BLOCKS_UNITTESTS_SPRINGTESTUTILS_H
+
+#include <memory>
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class OdeState;
+};
+
+namespace Physics
+{
+class LinearSpring;
+};
+
+namespace Blocks
+{
+
+void springTest(std::shared_ptr<SurgSim::Physics::LinearSpring> spring,
+ std::shared_ptr<SurgSim::Math::OdeState> state,
+ size_t expectedNodeId0, size_t expectedNodeId1,
+ double expectedStiffness, double expectedDamping);
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_UNITTESTS_SPRINGTESTUTILS_H
diff --git a/SurgSim/Blocks/UnitTests/TransferPhysicsToGraphicsMeshBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/TransferPhysicsToGraphicsMeshBehaviorTests.cpp
new file mode 100644
index 0000000..38873cd
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/TransferPhysicsToGraphicsMeshBehaviorTests.cpp
@@ -0,0 +1,152 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 TransferPhysicsToGraphicsBehavior class.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+using SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Framework::BehaviorManager;
+using SurgSim::Framework::Runtime;
+using SurgSim::Graphics::OsgBoxRepresentation;
+using SurgSim::Graphics::OsgMeshRepresentation;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::Fem3DRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(TransferPhysicsToGraphicsMeshBehavior("TestBehavior"));
+}
+
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SetGetSourceTest)
+{
+ auto physics = std::make_shared<Fem3DRepresentation>("PhysicsDeformable");
+ auto rigid = std::make_shared<RigidRepresentation>("PhysicsRigid");
+ auto behavior = std::make_shared<TransferPhysicsToGraphicsMeshBehavior>("Behavior");
+
+ EXPECT_THROW(behavior->setSource(nullptr), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(behavior->setSource(rigid), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(behavior->setSource(physics));
+ EXPECT_EQ(physics, behavior->getSource());
+}
+
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SetGetTargetTest)
+{
+ auto graphics = std::make_shared<OsgMeshRepresentation>("OsgMesh");
+ auto graphicsBox = std::make_shared<OsgBoxRepresentation>("OsgBox");
+ auto behavior = std::make_shared<TransferPhysicsToGraphicsMeshBehavior>("Behavior");
+
+ EXPECT_THROW(behavior->setTarget(nullptr), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(behavior->setTarget(graphicsBox), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(behavior->setTarget(graphics));
+ EXPECT_EQ(graphics, behavior->getTarget());
+}
+
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, UpdateTest)
+{
+ auto runtime = std::make_shared<Runtime>("config.txt");
+ auto behaviorManager = std::make_shared<BehaviorManager>();
+ runtime->addManager(behaviorManager);
+
+ auto scene = runtime->getScene();
+ auto sceneElement = std::make_shared<BasicSceneElement>("scene element");
+
+ auto physics = std::make_shared<Fem3DRepresentation>("Fem3D");
+ physics->setFilename("Geometry/wound_deformable.ply");
+
+ auto graphics = std::make_shared<OsgMeshRepresentation>("GraphicsMesh");
+ auto behavior = std::make_shared<TransferPhysicsToGraphicsMeshBehavior>("Behavior");
+ behavior->setSource(physics);
+ behavior->setTarget(graphics);
+
+ sceneElement->addComponent(behavior);
+ sceneElement->addComponent(physics);
+ sceneElement->addComponent(graphics);
+ scene->addSceneElement(sceneElement);
+
+ // Test doInitialize(), doWakeUP()
+ EXPECT_NO_THROW(runtime->start());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+
+ auto finalState = physics->getFinalState();
+ auto numNodes = finalState->getNumNodes();
+ auto target = graphics->getMesh();
+ ASSERT_NE(0u, target->getNumVertices());
+ ASSERT_NE(0u, numNodes);
+ ASSERT_EQ(numNodes, target->getNumVertices());
+
+ for (size_t nodeId = 0; nodeId < numNodes; ++nodeId)
+ {
+ EXPECT_TRUE(finalState->getPosition(nodeId).isApprox(target->getVertex(nodeId).position));
+ }
+
+ // Test TransferPhysicsToGraphicsBehavior::update()
+ finalState->reset();
+ behavior->update(1.0);
+
+ for (size_t nodeId = 0; nodeId < numNodes; ++nodeId)
+ {
+ EXPECT_TRUE(target->getVertex(nodeId).position.isApprox(Vector3d::Zero()));
+ }
+
+ runtime->stop();
+}
+
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SerializationTest)
+{
+ std::string filename = std::string("Data/Geometry/wound_deformable.ply");
+
+ std::shared_ptr<SurgSim::Framework::Component> physics = std::make_shared<Fem3DRepresentation>("Fem3D");
+ auto fem3d = std::dynamic_pointer_cast<Fem3DRepresentation>(physics);
+ fem3d->setFilename(filename);
+
+ std::shared_ptr<SurgSim::Framework::Component> graphics =
+ std::make_shared<OsgMeshRepresentation>("GraphicsMesh");
+ auto behavior = std::make_shared<TransferPhysicsToGraphicsMeshBehavior>("Behavior");
+
+ EXPECT_NO_THROW(behavior->setValue("Source", physics));
+ EXPECT_NO_THROW(behavior->setValue("Target", graphics));
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*behavior));
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior"];
+ EXPECT_EQ(5u, data.size());
+
+ std::shared_ptr<TransferPhysicsToGraphicsMeshBehavior> newBehavior;
+ ASSERT_NO_THROW(newBehavior = std::dynamic_pointer_cast<TransferPhysicsToGraphicsMeshBehavior>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_EQ("SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior", newBehavior->getClassName());
+ EXPECT_NE(nullptr, newBehavior->getValue<std::shared_ptr<SurgSim::Physics::DeformableRepresentation>>("Source"));
+ EXPECT_NE(nullptr, newBehavior->getValue<std::shared_ptr<SurgSim::Graphics::MeshRepresentation>>("Target"));
+}
diff --git a/SurgSim/Blocks/UnitTests/TransferPhysicsToPointCloudBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/TransferPhysicsToPointCloudBehaviorTests.cpp
new file mode 100644
index 0000000..8b5d8f9
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/TransferPhysicsToPointCloudBehaviorTests.cpp
@@ -0,0 +1,151 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 TransferPhysicsToGraphicsBehavior class.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+using SurgSim::Blocks::TransferPhysicsToPointCloudBehavior;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Framework::BehaviorManager;
+using SurgSim::Framework::Runtime;
+using SurgSim::Graphics::OsgBoxRepresentation;
+using SurgSim::Graphics::OsgPointCloudRepresentation;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::Fem3DRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+
+TEST(TransferPhysicsToPointCloudBehaviorTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(TransferPhysicsToPointCloudBehavior("TestBehavior"));
+}
+
+TEST(TransferPhysicsToPointCloudBehaviorTests, SetGetSourceTest)
+{
+ auto physics = std::make_shared<Fem3DRepresentation>("PhysicsDeformable");
+ auto rigid = std::make_shared<RigidRepresentation>("PhysicsRigid");
+ auto behavior = std::make_shared<TransferPhysicsToPointCloudBehavior>("Behavior");
+
+ EXPECT_THROW(behavior->setSource(nullptr), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(behavior->setSource(rigid), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(behavior->setSource(physics));
+ EXPECT_EQ(physics, behavior->getSource());
+}
+
+TEST(TransferPhysicsToPointCloudBehaviorTests, SetGetTargetTest)
+{
+ auto pointCloud = std::make_shared<OsgPointCloudRepresentation>("OsgMesh");
+ auto graphicsBox = std::make_shared<OsgBoxRepresentation>("OsgBox");
+ auto behavior = std::make_shared<TransferPhysicsToPointCloudBehavior>("Behavior");
+
+ EXPECT_THROW(behavior->setTarget(nullptr), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(behavior->setTarget(graphicsBox), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(behavior->setTarget(pointCloud));
+ EXPECT_EQ(pointCloud, behavior->getTarget());
+}
+
+TEST(TransferPhysicsToPointCloudBehaviorTests, UpdateTest)
+{
+ auto runtime = std::make_shared<Runtime>("config.txt");
+ auto behaviorManager = std::make_shared<BehaviorManager>();
+ runtime->addManager(behaviorManager);
+
+ auto scene = runtime->getScene();
+ auto sceneElement = std::make_shared<BasicSceneElement>("scene element");
+
+ auto physics = std::make_shared<Fem3DRepresentation>("Fem3D");
+ physics->setFilename("Geometry/wound_deformable.ply");
+
+ auto pointCloud = std::make_shared<OsgPointCloudRepresentation>("GraphicsMesh");
+ auto behavior = std::make_shared<TransferPhysicsToPointCloudBehavior>("Behavior");
+ behavior->setSource(physics);
+ behavior->setTarget(pointCloud);
+
+ sceneElement->addComponent(behavior);
+ sceneElement->addComponent(physics);
+ sceneElement->addComponent(pointCloud);
+ scene->addSceneElement(sceneElement);
+
+ // Test doInitialize(), doWakeUP()
+ EXPECT_NO_THROW(runtime->start());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+
+ auto finalState = physics->getFinalState();
+ auto numNodes = finalState->getNumNodes();
+ auto target = pointCloud->getVertices();
+ ASSERT_NE(0, target->getNumVertices());
+ ASSERT_NE(0, numNodes);
+ ASSERT_EQ(numNodes, target->getNumVertices());
+
+ for (size_t nodeId = 0; nodeId < numNodes; ++nodeId)
+ {
+ EXPECT_TRUE(finalState->getPosition(nodeId).isApprox(target->getVertex(nodeId).position));
+ }
+
+ // Test TransferPhysicsToGraphicsBehavior::update()
+ finalState->reset();
+ behavior->update(1.0);
+
+ for (size_t nodeId = 0; nodeId < numNodes; ++nodeId)
+ {
+ EXPECT_TRUE(target->getVertex(nodeId).position.isApprox(Vector3d::Zero()));
+ }
+
+ runtime->stop();
+}
+
+TEST(TransferPhysicsToPointCloudBehaviorTests, SerializationTest)
+{
+ std::string filename = std::string("Data/Geometry/wound_deformable.ply");
+
+ std::shared_ptr<SurgSim::Framework::Component> physics = std::make_shared<Fem3DRepresentation>("Fem3D");
+ auto fem3d = std::dynamic_pointer_cast<Fem3DRepresentation>(physics);
+ fem3d->setFilename(filename);
+
+ std::shared_ptr<SurgSim::Framework::Component> pointCloud =
+ std::make_shared<OsgPointCloudRepresentation>("GraphicsMesh");
+ auto behavior = std::make_shared<TransferPhysicsToPointCloudBehavior>("Behavior");
+
+ EXPECT_NO_THROW(behavior->setValue("Source", physics));
+ EXPECT_NO_THROW(behavior->setValue("Target", pointCloud));
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*behavior));
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Blocks::TransferPhysicsToPointCloudBehavior"];
+ EXPECT_EQ(5u, data.size());
+
+ std::shared_ptr<TransferPhysicsToPointCloudBehavior> newBehavior;
+ ASSERT_NO_THROW(newBehavior = std::dynamic_pointer_cast<TransferPhysicsToPointCloudBehavior>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_EQ("SurgSim::Blocks::TransferPhysicsToPointCloudBehavior", newBehavior->getClassName());
+ EXPECT_NE(nullptr, newBehavior->getValue<std::shared_ptr<SurgSim::Physics::DeformableRepresentation>>("Source"));
+ EXPECT_NE(nullptr, newBehavior->getValue<std::shared_ptr<SurgSim::Graphics::PointCloudRepresentation>>("Target"));
+}
diff --git a/SurgSim/Blocks/UnitTests/VisualizeContactsBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/VisualizeContactsBehaviorTests.cpp
new file mode 100644
index 0000000..716c488
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/VisualizeContactsBehaviorTests.cpp
@@ -0,0 +1,81 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 VisualizeContactsBehavior class.
+
+#include <gtest/gtest.h>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Blocks/VisualizeContactsBehavior.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+using SurgSim::Blocks::VisualizeContactsBehavior;
+using SurgSim::Physics::RigidCollisionRepresentation;
+using YAML::Node;
+
+TEST(VisualizeContactsBehaviorTests, Constructor)
+{
+ EXPECT_NO_THROW(std::make_shared<VisualizeContactsBehavior>("VisualizeContactsBehavior"));
+ EXPECT_NO_THROW({ VisualizeContactsBehavior visualizeContactsBehavior("VisualizeContactsBehavior"); });
+}
+
+TEST(VisualizeContactsBehaviorTests, SettersGetters)
+{
+ auto visualizeContactsBehavior = std::make_shared<VisualizeContactsBehavior>("VisualizeContactsBehavior");
+
+ // Test collision representation.
+ std::string name = "CollisionRepresentation";
+ auto collisionRepresentaiton = std::make_shared<RigidCollisionRepresentation>(name);
+
+ EXPECT_NO_THROW(visualizeContactsBehavior->setCollisionRepresentation(collisionRepresentaiton));
+ EXPECT_EQ(name, visualizeContactsBehavior->getCollisionRepresentation()->getName());
+
+ // Test vector field scale.
+ EXPECT_ANY_THROW(visualizeContactsBehavior->setVectorFieldScale(-1.023));
+
+ double scale = 1.234;
+ EXPECT_NO_THROW(visualizeContactsBehavior->setVectorFieldScale(scale));
+ EXPECT_EQ(scale, visualizeContactsBehavior->getVectorFieldScale());
+}
+
+TEST(VisualizeContactsBehaviorTests, Serialization)
+{
+ auto visualizeContactsBehavior = std::make_shared<VisualizeContactsBehavior>("VisualizeContactsBehavior");
+ std::string name = "CollisionRepresentation";
+ auto collisionRepresentation = std::make_shared<RigidCollisionRepresentation>(name);
+ EXPECT_NO_THROW(visualizeContactsBehavior->setValue("CollisionRepresentation",
+ std::static_pointer_cast<SurgSim::Framework::Component>(collisionRepresentation)););
+ double scale = 1.234;
+ EXPECT_NO_THROW(visualizeContactsBehavior->setValue("VectorFieldScale", scale));
+ EXPECT_EQ("SurgSim::Blocks::VisualizeContactsBehavior", visualizeContactsBehavior->getClassName());
+
+ // Encode
+ Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*visualizeContactsBehavior));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(5, node[visualizeContactsBehavior->getClassName()].size());
+
+ // Decode
+ std::shared_ptr<VisualizeContactsBehavior> newVisualizeContactsBehavior;
+ EXPECT_NO_THROW(newVisualizeContactsBehavior = std::dynamic_pointer_cast<VisualizeContactsBehavior>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ // Verify
+ EXPECT_EQ(name, SurgSim::Framework::convert<std::shared_ptr<SurgSim::Framework::Component>>(
+ newVisualizeContactsBehavior->getValue("CollisionRepresentation"))->getName());
+ EXPECT_EQ(scale, SurgSim::Framework::convert<double>(newVisualizeContactsBehavior->getValue("VectorFieldScale")));
+}
\ No newline at end of file
diff --git a/SurgSim/Blocks/UnitTests/config.txt.in b/SurgSim/Blocks/UnitTests/config.txt.in
new file mode 100644
index 0000000..9c1cf05
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/config.txt.in
@@ -0,0 +1 @@
+${CMAKE_CURRENT_BINARY_DIR}/Data/
\ No newline at end of file
diff --git a/SurgSim/Blocks/VisualizeContactsBehavior.cpp b/SurgSim/Blocks/VisualizeContactsBehavior.cpp
new file mode 100644
index 0000000..9e91f1e
--- /dev/null
+++ b/SurgSim/Blocks/VisualizeContactsBehavior.cpp
@@ -0,0 +1,141 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <algorithm>
+
+#include "SurgSim/Blocks/VisualizeContactsBehavior.h"
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/DataStructures/BufferedValue.h"
+#include "SurgSim/DataStructures/Vertex.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgVectorFieldRepresentation.h"
+#include "SurgSim/Graphics/VectorField.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Collision::Contact;
+using SurgSim::Collision::Representation;
+using SurgSim::DataStructures::Vertex;
+using SurgSim::Graphics::OsgVectorFieldRepresentation;
+using SurgSim::Graphics::VectorField;
+using SurgSim::Graphics::VectorFieldData;
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::VisualizeContactsBehavior,
+ VisualizeContactsBehavior);
+
+VisualizeContactsBehavior::VisualizeContactsBehavior(const std::string& name):
+ SurgSim::Framework::Behavior(name),
+ m_vectorField(std::make_shared<OsgVectorFieldRepresentation>("VisualizeContacts"))
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VisualizeContactsBehavior, std::shared_ptr<SurgSim::Framework::Component>,
+ CollisionRepresentation, getCollisionRepresentation, setCollisionRepresentation);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VisualizeContactsBehavior, double, VectorFieldScale,
+ getVectorFieldScale, setVectorFieldScale);
+}
+
+std::shared_ptr<SurgSim::Framework::Component> VisualizeContactsBehavior::getCollisionRepresentation()
+{
+ return m_collisionRepresentation;
+}
+
+void VisualizeContactsBehavior::setCollisionRepresentation(
+ std::shared_ptr<SurgSim::Framework::Component> collisionRepresentation)
+{
+ m_collisionRepresentation = std::dynamic_pointer_cast<Representation>(collisionRepresentation);
+}
+
+void VisualizeContactsBehavior::update(double dt)
+{
+ std::shared_ptr<const SurgSim::Collision::ContactMapType> collisions =
+ m_collisionRepresentation->getCollisions().safeGet();
+ if (!collisions->empty())
+ {
+ size_t totalContacts = 0;
+ for (auto collision = collisions->cbegin(); collision != collisions->cend(); ++collision)
+ {
+ totalContacts += collision->second.size();
+ }
+
+ std::shared_ptr<VectorField> vectorField = m_vectorField->getVectorField();
+ vectorField->clear();
+ vectorField->getVertices().reserve(2 * totalContacts);
+
+ SurgSim::Math::RigidTransform3d inverseElementPose = getSceneElement()->getPose().inverse();
+ auto representationPoseFirst = m_collisionRepresentation->getPose();
+ for (auto it = collisions->cbegin(); it != collisions->cend(); ++it)
+ {
+ auto representationPoseSecond = (*it).first->getPose();
+ for (auto iter = (*it).second.cbegin(); iter != (*it).second.cend(); ++iter)
+ {
+ VectorFieldData vectorData1;
+ VectorFieldData vectorData2;
+ vectorData1.direction = -(*iter)->normal * (*iter)->depth;
+ vectorData2.direction = (*iter)->normal * (*iter)->depth;
+
+ Vertex<VectorFieldData> vertex1 = Vertex<VectorFieldData>(
+ (*iter)->penetrationPoints.first.rigidLocalPosition.getValue(), vectorData1);
+ Vertex<VectorFieldData> vertex2 = Vertex<VectorFieldData>(
+ (*iter)->penetrationPoints.second.rigidLocalPosition.getValue(), vectorData2);
+
+ vertex1.position = inverseElementPose * representationPoseFirst * vertex1.position;
+ vertex2.position = inverseElementPose * representationPoseSecond * vertex2.position;
+ vectorField->addVertex(vertex1);
+ vectorField->addVertex(vertex2);
+ }
+ }
+ m_vectorField->setVisible(true);
+ }
+ else
+ {
+ m_vectorField->setVisible(false);
+ }
+}
+
+int VisualizeContactsBehavior::getTargetManagerType() const
+{
+ return SurgSim::Framework::MANAGER_TYPE_GRAPHICS;
+}
+
+bool VisualizeContactsBehavior::doInitialize()
+{
+ SURGSIM_ASSERT(m_collisionRepresentation) << "VisualizeContactsBehavior: no collision representation held.";
+ return true;
+}
+
+bool VisualizeContactsBehavior::doWakeUp()
+{
+ getSceneElement()->addComponent(m_vectorField);
+ return true;
+}
+
+double VisualizeContactsBehavior::getVectorFieldScale()
+{
+ return m_vectorField->getScale();
+}
+
+void VisualizeContactsBehavior::setVectorFieldScale(double scale)
+{
+ SURGSIM_ASSERT(scale > 0.0) << "Scale of vector field must be positive.";
+ m_vectorField->setScale(scale);
+}
+
+} // namespace Blocks
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Blocks/VisualizeContactsBehavior.h b/SurgSim/Blocks/VisualizeContactsBehavior.h
new file mode 100644
index 0000000..bca0911
--- /dev/null
+++ b/SurgSim/Blocks/VisualizeContactsBehavior.h
@@ -0,0 +1,99 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_BLOCKS_VISUALIZECONTACTSBEHAVIOR_H
+#define SURGSIM_BLOCKS_VISUALIZECONTACTSBEHAVIOR_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+class VectorFieldRepresentation;
+}
+
+namespace Collision
+{
+class Representation;
+}
+
+namespace Blocks
+{
+
+SURGSIM_STATIC_REGISTRATION(VisualizeContactsBehavior);
+
+/// This behavior is used to visualize the contacts
+/// on collision representation through vector field
+class VisualizeContactsBehavior: public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit VisualizeContactsBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::VisualizeContactsBehavior);
+
+ /// Used for serialization.
+ /// \return The collision representation whose contacts will be visualized.
+ std::shared_ptr<SurgSim::Framework::Component> getCollisionRepresentation();
+
+ /// Used for serialization.
+ /// \param collisionRepresentation The collision representation whose contacts will be visualized.
+ void setCollisionRepresentation(std::shared_ptr<SurgSim::Framework::Component> collisionRepresentation);
+
+ /// Update the behavior, show vector field for contacts if there is any.
+ /// \param dt The length of time (seconds) between update calls.
+ virtual void update(double dt) override;
+
+ /// Return the type of manager that should be responsible for this behavior
+ /// \return An integer indicating which manger should be responsible for this behavior.
+ virtual int getTargetManagerType() const override;
+
+ /// \return The scale of the vector field.
+ double getVectorFieldScale();
+
+ /// Set the scale of vector field, default 1.0.
+ // \param scale The scale of the vector field.
+ void setVectorFieldScale(double scale);
+
+protected:
+ /// Initialize this behavior
+ /// \return True on success, otherwise false.
+ /// \note In current implementation, this method always returns "true".
+ virtual bool doInitialize() override;
+
+ /// Wakeup this behavior
+ /// \return True on success, otherwise false.
+ /// \note In current implementation, this method always returns "true".
+ virtual bool doWakeUp() override;
+
+private:
+ /// The collision representation to get contacts for visualizing.
+ std::shared_ptr<SurgSim::Collision::Representation> m_collisionRepresentation;
+
+ /// The osg vector field for visualizing contacts on collision representation
+ std::shared_ptr<SurgSim::Graphics::VectorFieldRepresentation> m_vectorField;
+};
+
+} // namespace Blocks
+} // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_VISUALIZECONTACTSBEHAVIOR_H
diff --git a/SurgSim/CMakeLists.txt b/SurgSim/CMakeLists.txt
new file mode 100644
index 0000000..6012d83
--- /dev/null
+++ b/SurgSim/CMakeLists.txt
@@ -0,0 +1,28 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+link_directories(${Boost_LIBRARY_DIRS})
+
+add_subdirectory(Blocks)
+add_subdirectory(Collision)
+add_subdirectory(DataStructures)
+add_subdirectory(Devices)
+add_subdirectory(Framework)
+add_subdirectory(Graphics)
+add_subdirectory(Input)
+add_subdirectory(Math)
+add_subdirectory(Physics)
+add_subdirectory(Testing)
diff --git a/SurgSim/Collision/BoxCapsuleDcdContact.cpp b/SurgSim/Collision/BoxCapsuleDcdContact.cpp
new file mode 100644
index 0000000..cfdaa15
--- /dev/null
+++ b/SurgSim/Collision/BoxCapsuleDcdContact.cpp
@@ -0,0 +1,194 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <Eigen/Core>
+
+#include "SurgSim/Collision/BoxCapsuleDcdContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/CapsuleShape.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::CapsuleShape;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::doesIntersectBoxCapsule;
+using SurgSim::Math::distancePointSegment;
+using SurgSim::Math::intersectionsSegmentBox;
+using SurgSim::Math::Geometry::DistanceEpsilon;
+
+namespace {
+
+typedef Eigen::AlignedBox<double, 3>::CornerType CornerType;
+
+const std::array<std::pair<CornerType, CornerType>, 12> edges = {
+ std::make_pair(CornerType::BottomLeftFloor, CornerType::TopLeftFloor),
+ std::make_pair(CornerType::BottomRightFloor, CornerType::TopRightFloor),
+ std::make_pair(CornerType::BottomLeftCeil, CornerType::TopLeftCeil),
+ std::make_pair(CornerType::BottomRightCeil, CornerType::TopRightCeil),
+
+ std::make_pair(CornerType::BottomLeftFloor, CornerType::BottomRightFloor),
+ std::make_pair(CornerType::BottomLeftCeil, CornerType::BottomRightCeil),
+ std::make_pair(CornerType::TopLeftFloor, CornerType::TopRightFloor),
+ std::make_pair(CornerType::TopLeftCeil, CornerType::TopRightCeil),
+
+ std::make_pair(CornerType::BottomLeftFloor, CornerType::BottomLeftCeil),
+ std::make_pair(CornerType::BottomRightFloor, CornerType::BottomRightCeil),
+ std::make_pair(CornerType::TopLeftFloor, CornerType::TopLeftCeil),
+ std::make_pair(CornerType::TopRightFloor, CornerType::TopRightCeil)
+};
+};
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+BoxCapsuleDcdContact::BoxCapsuleDcdContact()
+{
+}
+
+std::pair<int,int> BoxCapsuleDcdContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_BOX, SurgSim::Math::SHAPE_TYPE_CAPSULE);
+}
+
+void BoxCapsuleDcdContact::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ std::shared_ptr<Representation> boxRepresentation = pair->getFirst();
+ std::shared_ptr<Representation> capsuleRepresentation = pair->getSecond();
+
+ std::shared_ptr<CapsuleShape> capsuleShape =
+ std::static_pointer_cast<CapsuleShape>(capsuleRepresentation->getShape());
+ std::shared_ptr<BoxShape> boxShape = std::static_pointer_cast<BoxShape>(boxRepresentation->getShape());
+
+ RigidTransform3d boxPose = boxRepresentation->getPose();
+ RigidTransform3d capsuleToBoxTransform = boxPose.inverse() * capsuleRepresentation->getPose();
+ Vector3d capsuleBottom = capsuleToBoxTransform * capsuleShape->bottomCenter();
+ Vector3d capsuleTop = capsuleToBoxTransform * capsuleShape->topCenter();
+ double capsuleRadius = capsuleShape->getRadius();
+
+ Vector3d boxRadii = boxShape->getSize() / 2.0;
+ Eigen::AlignedBox<double, 3> box(-boxRadii, boxRadii);
+
+ if (doesIntersectBoxCapsule(capsuleBottom, capsuleTop, capsuleRadius, box))
+ {
+ Vector3d normal, segmentPoint, deepestBoxPoint, deepestCapsulePoint;
+ distancePointSegment(Vector3d::Zero().eval(), capsuleBottom, capsuleTop, &segmentPoint);
+ if (!segmentPoint.isZero(DistanceEpsilon))
+ {
+ // The capsule's segment does not pass through the box center.
+ if (box.contains(segmentPoint))
+ {
+ // The capsule's segment passes through the box.
+ Vector3d::Index closestFace;
+ (boxRadii - segmentPoint.cwiseAbs()).minCoeff(&closestFace);
+ normal.setZero();
+ normal[closestFace] = -segmentPoint[closestFace];
+ normal.normalize();
+ deepestBoxPoint = boxRadii.array() * (1 - 2 * (segmentPoint.array() < 0).cast<double>());
+ deepestCapsulePoint = segmentPoint - capsuleRadius * segmentPoint.normalized();
+ }
+ else
+ {
+ // The closest point on the capsule's segment to the center of the box is outside the box.
+ deepestBoxPoint = segmentPoint.array().min(box.max().array()).max(box.min().array());
+ normal = deepestBoxPoint - segmentPoint;
+ if (normal.norm() > capsuleRadius)
+ {
+ // The closest point to the box center is too far away.
+ // Find the closest point to all 12 box edges.
+ double minDistance = 2.0 * capsuleRadius;
+ for (auto edge : edges)
+ {
+ Vector3d tempSegmentPoint;
+ Vector3d tempBoxPoint;
+ double tempDistance = SurgSim::Math::distanceSegmentSegment(capsuleBottom, capsuleTop,
+ box.corner(edge.first), box.corner(edge.second),
+ &tempSegmentPoint, &tempBoxPoint);
+ if (tempDistance < minDistance)
+ {
+ minDistance = tempDistance;
+ segmentPoint = tempSegmentPoint;
+ deepestBoxPoint = tempBoxPoint;
+ }
+ }
+ normal = deepestBoxPoint - segmentPoint;
+ if (normal.norm() > capsuleRadius)
+ {
+ // The closest point to any edge is too far away.
+ // Check the endpoints.
+ segmentPoint = capsuleTop;
+ deepestBoxPoint = segmentPoint.array().min(box.max().array()).max(box.min().array());
+ normal = deepestBoxPoint - segmentPoint;
+ if (normal.norm() > capsuleRadius)
+ {
+ segmentPoint = capsuleBottom;
+ deepestBoxPoint = segmentPoint.array().min(box.max().array()).max(box.min().array());
+ normal = deepestBoxPoint - segmentPoint;
+ }
+ }
+ }
+ normal.normalize();
+ deepestCapsulePoint = segmentPoint + capsuleRadius * normal;
+ }
+ }
+ else
+ {
+ // The capsule's segment passes through the box center.
+ if (capsuleTop.isZero(DistanceEpsilon) && capsuleBottom.isZero(DistanceEpsilon))
+ {
+ // The capsule's segment has no length and is located at the box center.
+ Vector3d::Index closestFace;
+ boxRadii.minCoeff(&closestFace);
+ normal.setZero();
+ normal[closestFace] = -boxRadii[closestFace];
+ normal.normalize();
+ }
+ else
+ {
+ // The capsule's segment has a length, pick the closest endpoint to the box center.
+ if (capsuleTop.squaredNorm() < capsuleBottom.squaredNorm())
+ {
+ segmentPoint = capsuleTop;
+ normal = -capsuleBottom.normalized();
+ }
+ else
+ {
+ segmentPoint = capsuleBottom;
+ normal = -capsuleTop.normalized();
+ }
+ }
+ deepestBoxPoint = boxRadii.array() * (1 - 2 * (normal.array() > 0).cast<double>());
+ deepestCapsulePoint = segmentPoint + capsuleRadius * normal;
+ }
+
+ double distance = (deepestCapsulePoint - deepestBoxPoint).dot(normal);
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(deepestBoxPoint);
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ capsuleRepresentation->getPose().inverse() * boxPose * deepestCapsulePoint);
+ pair->addContact(distance, boxPose.linear() * normal, penetrationPoints);
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/BoxCapsuleDcdContact.h b/SurgSim/Collision/BoxCapsuleDcdContact.h
new file mode 100644
index 0000000..3805ed6
--- /dev/null
+++ b/SurgSim/Collision/BoxCapsuleDcdContact.h
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_BOXCAPSULEDCDCONTACT_H
+#define SURGSIM_COLLISION_BOXCAPSULEDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between Boxes and Capsules
+class BoxCapsuleDcdContact : public ContactCalculation
+{
+public:
+
+ /// Constructor.
+ BoxCapsuleDcdContact();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int,int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair) override;
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.cpp b/SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.cpp
new file mode 100644
index 0000000..e2d8d16
--- /dev/null
+++ b/SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.cpp
@@ -0,0 +1,166 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+BoxDoubleSidedPlaneDcdContact::BoxDoubleSidedPlaneDcdContact()
+{
+}
+
+std::pair<int,int> BoxDoubleSidedPlaneDcdContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_BOX, SurgSim::Math::SHAPE_TYPE_DOUBLESIDEDPLANE);
+}
+
+void BoxDoubleSidedPlaneDcdContact::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ using SurgSim::Math::Geometry::DistanceEpsilon;
+
+ std::shared_ptr<Representation> representationBox;
+ std::shared_ptr<Representation> representationPlane;
+
+ representationBox = pair->getFirst();
+ representationPlane = pair->getSecond();
+
+ std::shared_ptr<BoxShape> box = std::static_pointer_cast<BoxShape>(representationBox->getShape());
+ std::shared_ptr<DoubleSidedPlaneShape> plane =
+ std::static_pointer_cast<DoubleSidedPlaneShape>(representationPlane->getShape());
+
+ // Transform the plane normal to box co-ordinate system.
+ SurgSim::Math::RigidTransform3d planeLocalToBoxLocal = representationBox->getPose().inverse() *
+ representationPlane->getPose();
+ Vector3d planeNormal = planeLocalToBoxLocal.linear() * plane->getNormal();
+ Vector3d planeNormalScaled = plane->getNormal() * -plane->getD();
+ Vector3d planePoint = planeLocalToBoxLocal * planeNormalScaled;
+ double planeD = -planeNormal.dot(planePoint);
+
+ // Loop through the box vertices (boxVertex) and calculate "d = (planeNormal.dot(boxVertex) + planeD)".
+ // Keep track of max and min of 'd'.
+ // Collision check overview:
+ // - If 'd' values contain both positive and negative values, there is an intersection.
+ // ---- Lower of the abs(maxD) and abs(minD) is the deepest penetration point.
+ // ---- collisionNormal is sign(d) * planeNormal.
+ // - If not, at least one of the 'd' values is zero.
+ // ---- collisionNormal is sign(max(abs(maxD), abs(minD))) * planeNormal.
+ double d[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double maxD = -std::numeric_limits<double>::max();
+ double minD = std::numeric_limits<double>::max();
+ Vector3d boxVertices[8];
+ for (int i = 0; i < 8; ++i)
+ {
+ boxVertices[i] = box->getVertex(i);
+ d[i] = planeNormal.dot(boxVertices[i]) + planeD;
+ maxD = std::max(d[i], maxD);
+ minD = std::min(d[i], minD);
+ }
+
+ if (!(maxD > DistanceEpsilon && minD > DistanceEpsilon) && !(maxD < -DistanceEpsilon && minD < -DistanceEpsilon))
+ {
+ // There is an intersection.
+ // Two cases:
+ // - Vertex touching plane.
+ // - Vertex through plane.
+
+ Vector3d normal;
+ Vector3d boxVertexGlobal;
+
+ enum BoxPlaneIntersectionType
+ {
+ BoxPlaneIntersectionTypeEqualsZero,
+ BoxPlaneIntersectionTypeLessThanZero,
+ BoxPlaneIntersectionTypeGreaterThanZero
+ } boxPlaneIntersectionType;
+
+ if (std::abs(maxD) < DistanceEpsilon)
+ {
+ // Box is touching the "back side" of plane.
+ normal = -(representationPlane->getPose().linear() * plane->getNormal());
+ boxPlaneIntersectionType = BoxPlaneIntersectionTypeEqualsZero;
+ }
+ else if (std::abs(minD) < DistanceEpsilon)
+ {
+ // Box is touching the "front side" of plane.
+ normal = representationPlane->getPose().linear() * plane->getNormal();
+ boxPlaneIntersectionType = BoxPlaneIntersectionTypeEqualsZero;
+ }
+ else
+ {
+ if (std::abs(maxD) >= std::abs(minD))
+ {
+ // Box is penetrating through the "front side" of plane.
+ normal = representationPlane->getPose().linear() * plane->getNormal();
+ boxPlaneIntersectionType = BoxPlaneIntersectionTypeLessThanZero;
+ }
+ else
+ {
+ // Box is penetrating through the "back side" of plane.
+ normal = -(representationPlane->getPose().linear() * plane->getNormal());
+ boxPlaneIntersectionType = BoxPlaneIntersectionTypeGreaterThanZero;
+ }
+ }
+
+ // Loop through vertices and check if a contact point needs to be generated.
+ bool generateContact = false;
+ for (int i = 0; i < 8; ++i)
+ {
+ switch (boxPlaneIntersectionType)
+ {
+ case BoxPlaneIntersectionTypeEqualsZero:
+ generateContact = std::abs(d[i]) < DistanceEpsilon;
+ break;
+ case BoxPlaneIntersectionTypeLessThanZero:
+ generateContact = d[i] < -DistanceEpsilon;
+ break;
+ case BoxPlaneIntersectionTypeGreaterThanZero:
+ generateContact = d[i] > DistanceEpsilon;
+ break;
+ }
+
+ if (generateContact)
+ {
+ std::pair<Location, Location> penetrationPoints;
+ boxVertexGlobal = representationBox->getPose() * boxVertices[i];
+ penetrationPoints.first.rigidLocalPosition.setValue(boxVertices[i]);
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ representationPlane->getPose().inverse() * (boxVertexGlobal + normal * std::abs(d[i])));
+
+ pair->addContact(std::abs(d[i]), normal, penetrationPoints);
+
+ generateContact = false;
+ }
+ }
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.h b/SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.h
new file mode 100644
index 0000000..d8cf457
--- /dev/null
+++ b/SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.h
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_BOXDOUBLESIDEDPLANEDCDCONTACT_H
+#define SURGSIM_COLLISION_BOXDOUBLESIDEDPLANEDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between Boxes and Planes
+class BoxDoubleSidedPlaneDcdContact : public ContactCalculation
+{
+public:
+
+ /// Constructor.
+ BoxDoubleSidedPlaneDcdContact();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int,int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair);
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/BoxPlaneDcdContact.cpp b/SurgSim/Collision/BoxPlaneDcdContact.cpp
new file mode 100644
index 0000000..2ddeac7
--- /dev/null
+++ b/SurgSim/Collision/BoxPlaneDcdContact.cpp
@@ -0,0 +1,88 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/BoxPlaneDcdContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+BoxPlaneDcdContact::BoxPlaneDcdContact()
+{
+}
+
+std::pair<int,int> BoxPlaneDcdContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_BOX, SurgSim::Math::SHAPE_TYPE_PLANE);
+}
+
+void BoxPlaneDcdContact::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ using SurgSim::Math::Geometry::DistanceEpsilon;
+
+ std::shared_ptr<Representation> representationBox;
+ std::shared_ptr<Representation> representationPlane;
+
+ representationBox = pair->getFirst();
+ representationPlane = pair->getSecond();
+
+ std::shared_ptr<BoxShape> box = std::static_pointer_cast<BoxShape>(representationBox->getShape());
+ std::shared_ptr<PlaneShape> plane = std::static_pointer_cast<PlaneShape>(representationPlane->getShape());
+
+ // Transform the plane normal to box co-ordinate system.
+ SurgSim::Math::RigidTransform3d planeLocalToBoxLocal = representationBox->getPose().inverse() *
+ representationPlane->getPose();
+ SurgSim::Math::RigidTransform3d boxLocalToPlaneLocal = representationPlane->getPose().inverse() *
+ representationBox->getPose();
+ Vector3d planeNormal = planeLocalToBoxLocal.linear() * plane->getNormal();
+ Vector3d planeNormalScaled = plane->getNormal() * -plane->getD();
+ Vector3d planePoint = planeLocalToBoxLocal * planeNormalScaled;
+ double planeD = -planeNormal.dot(planePoint);
+
+ // Loop through the box vertices (boxVertex) and check it it is below plane.
+ double d = 0.0;
+ Vector3d boxVertex;
+ for (int i = 0; i < 8; ++i)
+ {
+ boxVertex = box->getVertex(i);
+ d = planeNormal.dot(boxVertex) + planeD;
+ if (d < DistanceEpsilon)
+ {
+ // Add a contact.
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(boxVertex);
+ penetrationPoints.second.rigidLocalPosition.setValue(boxLocalToPlaneLocal * (boxVertex - planeNormal * d));
+
+ pair->addContact(-d, representationPlane->getPose().linear() * plane->getNormal(), penetrationPoints);
+ }
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/BoxPlaneDcdContact.h b/SurgSim/Collision/BoxPlaneDcdContact.h
new file mode 100644
index 0000000..392dafb
--- /dev/null
+++ b/SurgSim/Collision/BoxPlaneDcdContact.h
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_BOXPLANEDCDCONTACT_H
+#define SURGSIM_COLLISION_BOXPLANEDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between Boxes and Planes
+class BoxPlaneDcdContact : public ContactCalculation
+{
+public:
+
+ /// Constructor.
+ BoxPlaneDcdContact();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int,int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair);
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/BoxSphereDcdContact.cpp b/SurgSim/Collision/BoxSphereDcdContact.cpp
new file mode 100644
index 0000000..d050850
--- /dev/null
+++ b/SurgSim/Collision/BoxSphereDcdContact.cpp
@@ -0,0 +1,131 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/BoxSphereDcdContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+BoxSphereDcdContact::BoxSphereDcdContact()
+{
+}
+
+std::pair<int,int> BoxSphereDcdContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_BOX, SurgSim::Math::SHAPE_TYPE_SPHERE);
+}
+
+void BoxSphereDcdContact::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ using SurgSim::Math::Geometry::DistanceEpsilon;
+ using SurgSim::Math::Geometry::SquaredDistanceEpsilon;
+
+ std::shared_ptr<Representation> representationBox;
+ std::shared_ptr<Representation> representationSphere;
+
+ representationBox = pair->getFirst();
+ representationSphere = pair->getSecond();
+
+ std::shared_ptr<BoxShape> box = std::static_pointer_cast<BoxShape>(representationBox->getShape());
+ std::shared_ptr<SphereShape> sphere = std::static_pointer_cast<SphereShape>(representationSphere->getShape());
+
+ // Sphere center...
+ Vector3d sphereCenter = representationSphere->getPose().translation();
+ // ... in Box coordinate system.
+ Vector3d boxLocalSphereCenter = representationBox->getPose().inverse() * sphereCenter;
+
+ // Box half size.
+ Vector3d boxSize(box->getSizeX() * 0.5, box->getSizeY() * 0.5, box->getSizeZ() * 0.5);
+
+ // Determine the closest point to the sphere center in the box
+ Vector3d closestPoint = boxLocalSphereCenter;
+ closestPoint.x() = std::min(boxSize.x(), closestPoint.x());
+ closestPoint.x() = std::max(-boxSize.x(), closestPoint.x());
+ closestPoint.y() = std::min(boxSize.y(), closestPoint.y());
+ closestPoint.y() = std::max(-boxSize.y(), closestPoint.y());
+ closestPoint.z() = std::min(boxSize.z(), closestPoint.z());
+ closestPoint.z() = std::max(-boxSize.z(), closestPoint.z());
+
+ // Distance between the closestPoint and boxLocalSphereCenter. Normal points into first representation, the box.
+ Vector3d normal = closestPoint - boxLocalSphereCenter;
+ double distanceSquared = normal.squaredNorm();
+ if (distanceSquared - (sphere->getRadius() * sphere->getRadius()) > SquaredDistanceEpsilon)
+ {
+ // There is no collision.
+ return;
+ }
+
+ double distance = 0.0;
+
+ // If sphere center is inside box, it is handled differently.
+ if (distanceSquared <= SquaredDistanceEpsilon)
+ {
+ // Sphere center is inside the box.
+ // In this case closestPoint is equal to boxLocalSphereCenter.
+ // Find which face of the box is closest to the closestPoint.
+ // abs(boxSize.x - closestPoint.x) and abs(-boxSize.x - closestPoint.x) are the distances between the
+ // closestPoint and the two faces (along x-axis) of the box.
+ // But since the abs(closestPoint.x) will always <= boxSize.x (because the point is inside box),
+ // (boxSize.x() - abs(closestPoint.x())) gives the distance from the closestPoint to whichever x-axis face is
+ // closest. This value is calculated for all the axes. The axis with the minimum value contains the
+ // colliding face.
+ Vector3d distancesFromFaces = boxSize - closestPoint.cwiseAbs();
+ int minimumDistanceId;
+ distancesFromFaces.minCoeff(&minimumDistanceId);
+ // The mininumDistanceId is the index of the non-zero component of the normal of the closest face.
+ // The normal points toward the first representation, the box. So the sign (or direction) of that entry is +1
+ // if the closestPoint component is negative and vice versa.
+ double direction = closestPoint[minimumDistanceId] > -DistanceEpsilon ? -1.0 : 1.0;
+ normal.setZero();
+ normal[minimumDistanceId] = direction;
+ // The closestPoint should be on the closest box face, so the negative of the normal direction.
+ closestPoint[minimumDistanceId] = boxSize[minimumDistanceId] * (-direction);
+ distance = -std::abs(distancesFromFaces[minimumDistanceId]);
+ }
+ else
+ {
+ // Sphere center is outside box.
+ distance = normal.norm();
+ normal /= distance;
+ }
+
+ // Transform normal into global pose.
+ normal = representationBox->getPose().linear() * normal;
+
+ // Create the contact.
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(closestPoint);
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ representationSphere->getPose().inverse() * (sphereCenter + (normal * sphere->getRadius())));
+
+ pair->addContact(std::abs(distance - sphere->getRadius()), normal, penetrationPoints);
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/BoxSphereDcdContact.h b/SurgSim/Collision/BoxSphereDcdContact.h
new file mode 100644
index 0000000..d029cbb
--- /dev/null
+++ b/SurgSim/Collision/BoxSphereDcdContact.h
@@ -0,0 +1,55 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_BOXSPHEREDCDCONTACT_H
+#define SURGSIM_COLLISION_BOXSPHEREDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between a box and a sphere
+class BoxSphereDcdContact : public ContactCalculation
+{
+public:
+
+ /// Constructor.
+ BoxSphereDcdContact();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int,int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ /// \note If there is a contact, the normal in the local space is always normal to the box face closest to the
+ /// sphere center. That means that if a penetration near the corner increases or decreases in depth the normal may
+ /// switch directions instantly, leading to instabilities.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair) override;
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/CMakeLists.txt b/SurgSim/Collision/CMakeLists.txt
new file mode 100644
index 0000000..cdf20ab
--- /dev/null
+++ b/SurgSim/Collision/CMakeLists.txt
@@ -0,0 +1,76 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+set(SURGSIM_COLLISION_SOURCES
+ BoxCapsuleDcdContact.cpp
+ BoxDoubleSidedPlaneDcdContact.cpp
+ BoxPlaneDcdContact.cpp
+ BoxSphereDcdContact.cpp
+ CapsuleSphereDcdContact.cpp
+ CollisionPair.cpp
+ ContactCalculation.cpp
+ DefaultContactCalculation.cpp
+ OctreeDcdContact.cpp
+ Representation.cpp
+ ShapeCollisionRepresentation.cpp
+ SphereDoubleSidedPlaneDcdContact.cpp
+ SpherePlaneDcdContact.cpp
+ SphereSphereDcdContact.cpp
+ TriangleMeshPlaneDcdContact.cpp
+ TriangleMeshTriangleMeshDcdContact.cpp
+)
+
+set(SURGSIM_COLLISION_HEADERS
+ BoxCapsuleDcdContact.h
+ BoxDoubleSidedPlaneDcdContact.h
+ BoxPlaneDcdContact.h
+ BoxSphereDcdContact.h
+ CapsuleSphereDcdContact.h
+ CollisionPair.h
+ ContactCalculation.h
+ DcdCollision.h
+ DefaultContactCalculation.h
+ OctreeDcdContact.h
+ Representation.h
+ ShapeCollisionRepresentation.h
+ SphereDoubleSidedPlaneDcdContact.h
+ SpherePlaneDcdContact.h
+ SphereSphereDcdContact.h
+ TriangleMeshPlaneDcdContact.h
+ TriangleMeshTriangleMeshDcdContact.h
+)
+
+surgsim_add_library(
+ SurgSimCollision
+ "${SURGSIM_COLLISION_SOURCES}"
+ "${SURGSIM_COLLISION_HEADERS}"
+ "SurgSim/Collision"
+)
+
+SET(LIBS
+ SurgSimFramework
+ SurgSimMath
+ ${Boost_LIBRARIES}
+)
+
+target_link_libraries(SurgSimCollision ${LIBS}
+)
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+endif()
+
+# Put SurgSimCollision into folder "Collision"
+set_target_properties(SurgSimCollision PROPERTIES FOLDER "Collision")
diff --git a/SurgSim/Collision/CapsuleSphereDcdContact.cpp b/SurgSim/Collision/CapsuleSphereDcdContact.cpp
new file mode 100644
index 0000000..cc7b1cf
--- /dev/null
+++ b/SurgSim/Collision/CapsuleSphereDcdContact.cpp
@@ -0,0 +1,78 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/CapsuleSphereDcdContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/CapsuleShape.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/SphereShape.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::CapsuleShape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+CapsuleSphereDcdContact::CapsuleSphereDcdContact()
+{
+}
+
+std::pair<int,int> CapsuleSphereDcdContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_CAPSULE, SurgSim::Math::SHAPE_TYPE_SPHERE);
+}
+
+void CapsuleSphereDcdContact::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ std::shared_ptr<Representation> representationCapsule(pair->getFirst());
+ std::shared_ptr<Representation> representationSphere(pair->getSecond());
+
+ std::shared_ptr<CapsuleShape> capsule(std::static_pointer_cast<CapsuleShape>(representationCapsule->getShape()));
+ std::shared_ptr<SphereShape> sphere(std::static_pointer_cast<SphereShape>(representationSphere->getShape()));
+
+ Vector3d sphereCenter(representationSphere->getPose().translation());
+ Vector3d globalTop(representationCapsule->getPose() * capsule->topCenter());
+ Vector3d globalBottom(representationCapsule->getPose() * capsule->bottomCenter());
+ Vector3d result;
+
+ double dist =
+ SurgSim::Math::distancePointSegment(sphereCenter, globalTop, globalBottom, &result);
+ double distThreshold = capsule->getRadius() + sphere->getRadius();
+
+ if (dist < distThreshold)
+ {
+ double depth = distThreshold - dist;
+
+ // Calculate the normal going from the sphere to the capsule
+ Vector3d normal = (result - sphereCenter).normalized();
+
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ representationCapsule->getPose().inverse() * (result - normal * capsule->getRadius()));
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ representationSphere->getPose().inverse() * (sphereCenter + normal * sphere->getRadius()));
+
+ pair->addContact(depth, normal, penetrationPoints);
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/CapsuleSphereDcdContact.h b/SurgSim/Collision/CapsuleSphereDcdContact.h
new file mode 100644
index 0000000..bb19058
--- /dev/null
+++ b/SurgSim/Collision/CapsuleSphereDcdContact.h
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_CAPSULESPHEREDCDCONTACT_H
+#define SURGSIM_COLLISION_CAPSULESPHEREDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between a capsule and a sphere
+class CapsuleSphereDcdContact : public ContactCalculation
+{
+public:
+
+ /// Constructor.
+ CapsuleSphereDcdContact();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int,int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair) override;
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/CollisionPair.cpp b/SurgSim/Collision/CollisionPair.cpp
new file mode 100644
index 0000000..92c7dd8
--- /dev/null
+++ b/SurgSim/Collision/CollisionPair.cpp
@@ -0,0 +1,132 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <numeric>
+
+#include "SurgSim/Collision/CollisionPair.h"
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Assert.h"
+
+using SurgSim::DataStructures::Location;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+CollisionPair::CollisionPair()
+{
+}
+
+CollisionPair::CollisionPair(const std::shared_ptr<Representation>& first,
+ const std::shared_ptr<Representation>& second) :
+ m_representations(first, second), m_isSwapped(false)
+{
+ SURGSIM_ASSERT(first != second) << "Collision Representation cannot collide with itself";
+ SURGSIM_ASSERT(first != nullptr && second != nullptr) << "Collision Representation cannot be null";
+}
+
+CollisionPair::~CollisionPair()
+{
+
+}
+
+void CollisionPair::setRepresentations(const std::shared_ptr<Representation>& first,
+ const std::shared_ptr<Representation>& second)
+{
+ SURGSIM_ASSERT(first != second) << "Should try to collide with self";
+ SURGSIM_ASSERT(first != nullptr && second != nullptr) << "Collision Representation cannot be null";
+
+ // Invalidate the current contacts
+ clearContacts();
+ m_representations.first = first;
+ m_representations.second = second;
+ m_isSwapped = false;
+}
+
+const std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>>&
+ CollisionPair::getRepresentations() const
+{
+ return m_representations;
+}
+
+std::shared_ptr<Representation> CollisionPair::getFirst() const
+{
+ return m_representations.first;
+}
+
+std::shared_ptr<Representation> CollisionPair::getSecond() const
+{
+ return m_representations.second;
+}
+
+bool CollisionPair::hasContacts() const
+{
+ return !m_contacts.empty();
+}
+
+void CollisionPair::addContact(const double& depth,
+ const SurgSim::Math::Vector3d& contactPoint,
+ const SurgSim::Math::Vector3d& normal,
+ const std::pair<Location, Location>& penetrationPoints)
+{
+ addContact(std::make_shared<Contact>(depth, contactPoint, normal, penetrationPoints));
+}
+
+void CollisionPair::addContact(const double& depth,
+ const SurgSim::Math::Vector3d& normal,
+ const std::pair<Location, Location>& penetrationPoints)
+{
+ addContact(std::make_shared<Contact>(depth, SurgSim::Math::Vector3d::Zero(), normal, penetrationPoints));
+}
+
+void CollisionPair::addContact(const std::shared_ptr<Contact>& contact)
+{
+ m_contacts.push_back(contact);
+ m_representations.first->getCollisions().unsafeGet()[m_representations.second].push_back(contact);
+ std::shared_ptr<Contact> contact2 =
+ std::make_shared<Contact>(contact->depth, contact->contact, -contact->normal,
+ std::pair<Location, Location>(
+ contact->penetrationPoints.second,
+ contact->penetrationPoints.first));
+ m_representations.second->getCollisions().unsafeGet()[m_representations.first].push_back(contact2);
+}
+
+const std::list<std::shared_ptr<Contact>>& CollisionPair::getContacts() const
+{
+ return m_contacts;
+}
+
+void CollisionPair::clearContacts()
+{
+ m_contacts.clear();
+}
+
+void CollisionPair::swapRepresentations()
+{
+ SURGSIM_ASSERT(! hasContacts()) << "Can't swap representations after contacts have already been calculated";
+ m_isSwapped = !m_isSwapped;
+ std::swap(m_representations.first, m_representations.second);
+}
+
+bool CollisionPair::isSwapped() const
+{
+ return m_isSwapped;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
+
diff --git a/SurgSim/Collision/CollisionPair.h b/SurgSim/Collision/CollisionPair.h
new file mode 100644
index 0000000..15751b9
--- /dev/null
+++ b/SurgSim/Collision/CollisionPair.h
@@ -0,0 +1,141 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_COLLISIONPAIR_H
+#define SURGSIM_COLLISION_COLLISIONPAIR_H
+
+#include <memory>
+#include <list>
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class Representation;
+
+/// Contact data structure used when two representations touch each other
+/// The convention is that if body 1 is moved along the normal vector by
+/// a distance depth (or equivalently if body 2 is moved the same distance
+/// in the opposite direction) then the penetration depth will be reduced to
+/// zero. This means that the normal vector points "in" to body 1
+struct Contact {
+ Contact(const double& newDepth,
+ const SurgSim::Math::Vector3d& newContact,
+ const SurgSim::Math::Vector3d& newNormal,
+ const std::pair<SurgSim::DataStructures::Location,
+ SurgSim::DataStructures::Location>& newPenetrationPoints) :
+ depth(newDepth), contact(newContact), normal(newNormal), penetrationPoints(newPenetrationPoints)
+ {
+ };
+ double depth; ///< What is the penetration depth for the representation
+ SurgSim::Math::Vector3d contact; ///< The actual contact point, only used for CCD
+ SurgSim::Math::Vector3d normal; ///< The normal on the contact point (normalized)
+ std::pair<SurgSim::DataStructures::Location,
+ SurgSim::DataStructures::Location> penetrationPoints; ///< The deepest point inside the opposing object
+};
+
+/// Collision Pair class, it signifies a pair of items that should be checked with the
+/// collision algorithm, this structure will be used for input as well as output, as contacts
+/// get appended to the contacts list when found.
+class CollisionPair
+{
+public:
+ /// Default Constructor
+ CollisionPair();
+
+ /// Normal constructor
+ CollisionPair(const std::shared_ptr<Representation>& first,
+ const std::shared_ptr<Representation>& second);
+
+ /// Destructor
+ ~CollisionPair();
+
+ /// Sets the representations in this pair, representations cannot be the same instance and neither can be nullptr.
+ /// \param first The first Collision Representation.
+ /// \param second The second Collision Representation.
+ void setRepresentations(const std::shared_ptr<Representation>& first,
+ const std::shared_ptr<Representation>& second);
+
+ /// Function that returns the pair of representations of the objects that are colliding.
+ /// \return The pair of representations that are colliding.
+ const std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>>&
+ getRepresentations() const;
+
+ /// \return The representation considered to be the first
+ std::shared_ptr<Representation> getFirst() const;
+
+ /// \return The representation considered to be the second
+ std::shared_ptr<Representation> getSecond() const;
+
+ /// \return true if there are any contacts assigned to the pair, false otherwise
+ bool hasContacts() const;
+
+ /// Adds a contact to the collision pair.
+ /// \param depth The depth of the intersection.
+ /// \param contactPoint The contact point, between the two bodies.
+ /// \param normal The normal of the contact pointing into the first representation.
+ /// \param penetrationPoints The points furthest into the opposing object
+ void addContact(const double& depth,
+ const SurgSim::Math::Vector3d& contactPoint,
+ const SurgSim::Math::Vector3d& normal,
+ const std::pair<SurgSim::DataStructures::Location,
+ SurgSim::DataStructures::Location>& penetrationPoints);
+
+ /// Adds a contact to the collision pair.
+ /// \param depth The depth of the intersection.
+ /// \param normal The normal of the contact pointing into the first representation.
+ /// \param penetrationPoints The points furthest into the opposing object
+ void addContact(const double& depth,
+ const SurgSim::Math::Vector3d& normal,
+ const std::pair<SurgSim::DataStructures::Location,
+ SurgSim::DataStructures::Location>& penetrationPoints);
+
+ /// Adds a contact.
+ /// \param contact The contact between the first and the second representation.
+ void addContact(const std::shared_ptr<Contact>& contact);
+
+ /// \return All the contacts.
+ const std::list<std::shared_ptr<Contact>>& getContacts() const;
+
+ /// Reset clear the list of contacts, invalidating all the contacts
+ void clearContacts();
+
+ /// Swap the representation pair so that first becomes second and second becomes first
+ void swapRepresentations();
+
+ /// Query if this the pair has been swapped from when it was constructed.
+ /// \return true if swapped, false if not.
+ bool isSwapped() const;
+
+private:
+
+ /// Pair of objects that are colliding
+ std::pair<std::shared_ptr<Representation>,
+ std::shared_ptr<Representation>> m_representations;
+
+ /// List of current contacts
+ std::list<std::shared_ptr<Contact>> m_contacts;
+
+ bool m_isSwapped;
+};
+
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/ContactCalculation.cpp b/SurgSim/Collision/ContactCalculation.cpp
new file mode 100644
index 0000000..2de9ec0
--- /dev/null
+++ b/SurgSim/Collision/ContactCalculation.cpp
@@ -0,0 +1,63 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+ContactCalculation::ContactCalculation()
+{
+}
+
+ContactCalculation::~ContactCalculation()
+{
+}
+
+void ContactCalculation::calculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ std::pair<int,int> shapeTypes = getShapeTypes();
+ int firstShapeType = pair->getFirst()->getShapeType();
+ int secondShapeType = pair->getSecond()->getShapeType();
+
+ if (firstShapeType != secondShapeType && firstShapeType == shapeTypes.second &&
+ secondShapeType == shapeTypes.first)
+ {
+ pair->swapRepresentations();
+ std::swap(firstShapeType, secondShapeType);
+ }
+
+ if(shapeTypes.first != SurgSim::Math::SHAPE_TYPE_NONE)
+ {
+ SURGSIM_ASSERT(firstShapeType == shapeTypes.first) <<
+ "First Object, wrong type of object" << firstShapeType;
+ }
+
+ if(shapeTypes.second != SurgSim::Math::SHAPE_TYPE_NONE)
+ {
+ SURGSIM_ASSERT(secondShapeType == shapeTypes.second) <<
+ "Second Object, wrong type of object" << secondShapeType;
+ }
+
+ doCalculateContact(pair);
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/ContactCalculation.h b/SurgSim/Collision/ContactCalculation.h
new file mode 100644
index 0000000..39aa6b5
--- /dev/null
+++ b/SurgSim/Collision/ContactCalculation.h
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_CONTACTCALCULATION_H
+#define SURGSIM_COLLISION_CONTACTCALCULATION_H
+
+#include <memory>
+
+#include "SurgSim/Collision/CollisionPair.h"
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+/// Base class responsible for calculating contact data between two given shapes, calculateContact needs to
+/// determine whether the two shapes intersect, and if yes calculate the correct data for this contact, which
+/// consists of, the normal to displace the first shape so that the two shapes just barely touch. And the
+/// penetration point (the point that is furthest inside the other object) for each shape.
+/// This base class also handles the swapping of the shapes if the pair is asymmetric. The sub classes
+/// assume that the pair is always in correct order.
+class ContactCalculation
+{
+public:
+
+ /// Constructor
+ ContactCalculation();
+
+ /// Destructor
+ virtual ~ContactCalculation();
+
+ /// Function that handles asymmetric pair and calls the actual contact calculation routine of the sub class.
+ /// \param pair The pair that is under consideration.
+ void calculateContact(std::shared_ptr<CollisionPair> pair);
+
+ /// Virtual function that returns the shapes that this ContactCalculation class handles.
+ /// \return Return the shape types this class handles.
+ virtual std::pair<int,int> getShapeTypes() = 0;
+
+private:
+
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair) = 0;
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/DcdCollision.h b/SurgSim/Collision/DcdCollision.h
new file mode 100644
index 0000000..5c85039
--- /dev/null
+++ b/SurgSim/Collision/DcdCollision.h
@@ -0,0 +1,33 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_DCDCOLLISION_H
+#define SURGSIM_COLLISION_DCDCOLLISION_H
+
+#include "SurgSim/Collision/BoxCapsuleDcdContact.h"
+#include "SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.h"
+#include "SurgSim/Collision/BoxPlaneDcdContact.h"
+#include "SurgSim/Collision/BoxSphereDcdContact.h"
+#include "SurgSim/Collision/CapsuleSphereDcdContact.h"
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Collision/DefaultContactCalculation.h"
+#include "SurgSim/Collision/OctreeDcdContact.h"
+#include "SurgSim/Collision/SphereSphereDcdContact.h"
+#include "SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.h"
+#include "SurgSim/Collision/SpherePlaneDcdContact.h"
+#include "SurgSim/Collision/TriangleMeshPlaneDcdContact.h"
+#include "SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.h"
+
+#endif // SURGSIM_COLLISION_DCDCOLLISION_H
diff --git a/SurgSim/Collision/DefaultContactCalculation.cpp b/SurgSim/Collision/DefaultContactCalculation.cpp
new file mode 100644
index 0000000..c5d41b6
--- /dev/null
+++ b/SurgSim/Collision/DefaultContactCalculation.cpp
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/DefaultContactCalculation.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+DefaultContactCalculation::DefaultContactCalculation(bool doAssert) :
+ m_doAssert(doAssert)
+{
+}
+
+DefaultContactCalculation::~DefaultContactCalculation()
+{
+}
+
+std::pair<int,int> DefaultContactCalculation::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_NONE, SurgSim::Math::SHAPE_TYPE_NONE);
+}
+
+void DefaultContactCalculation::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ SURGSIM_ASSERT(!m_doAssert) << "Contact calculation not implemented for pairs with types ("<<
+ pair->getFirst()->getShapeType() << ", " << pair->getSecond()->getShapeType() << ").";
+ SURGSIM_LOG_INFO(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "Contact calculation not implemented for pairs with types (" <<
+ pair->getFirst()->getShapeType() << ", " << pair->getSecond()->getShapeType() << ").";
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/DefaultContactCalculation.h b/SurgSim/Collision/DefaultContactCalculation.h
new file mode 100644
index 0000000..c750cb6
--- /dev/null
+++ b/SurgSim/Collision/DefaultContactCalculation.h
@@ -0,0 +1,59 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_DEFAULTCONTACTCALCULATION_H
+#define SURGSIM_COLLISION_DEFAULTCONTACTCALCULATION_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// A default calculation, it does nothing and can be used as a placeholder
+class DefaultContactCalculation : public ContactCalculation
+{
+public:
+
+ /// Constructor
+ /// \param doAssert If set the calculation will throw an exception if it is executed, this
+ /// can be used to detect cases where a contact calculation is being called
+ /// on a pair that should be implemented
+ explicit DefaultContactCalculation(bool doAssert = false);
+
+ /// Destructor
+ virtual ~DefaultContactCalculation();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int,int> getShapeTypes() override;
+
+private:
+ bool m_doAssert;
+
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair) override;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/OctreeDcdContact.cpp b/SurgSim/Collision/OctreeDcdContact.cpp
new file mode 100644
index 0000000..890f1fa
--- /dev/null
+++ b/SurgSim/Collision/OctreeDcdContact.cpp
@@ -0,0 +1,109 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/OctreeDcdContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Math/Vector.h"
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+OctreeDcdContact::OctreeDcdContact(std::shared_ptr<ContactCalculation> calculator) :
+ m_calculator(calculator)
+{
+ SURGSIM_ASSERT(m_calculator->getShapeTypes().first == SurgSim::Math::SHAPE_TYPE_BOX) <<
+ "OctreeDcdContact needs a contact calculator that works with Boxes";
+ m_shapeTypes = m_calculator->getShapeTypes();
+ m_shapeTypes.first = SurgSim::Math::SHAPE_TYPE_OCTREE;
+
+ m_nodeCollisionRepresentation = std::make_shared<ShapeCollisionRepresentation>("Octree Node");
+}
+
+std::pair<int, int> OctreeDcdContact::getShapeTypes()
+{
+ return m_shapeTypes;
+}
+
+void OctreeDcdContact::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ typedef SurgSim::Math::OctreeShape OctreeShapeType;
+ std::shared_ptr<OctreeShapeType> octree = std::static_pointer_cast<OctreeShapeType>(pair->getFirst()->getShape());
+ calculateContactWithNode(octree->getRootNode(), pair, std::make_shared<SurgSim::DataStructures::OctreePath>());
+}
+
+void OctreeDcdContact::calculateContactWithNode(
+ std::shared_ptr<const SurgSim::Math::OctreeShape::NodeType> node,
+ std::shared_ptr<CollisionPair> pair,
+ std::shared_ptr<SurgSim::DataStructures::OctreePath> nodePath)
+{
+ if (! node->isActive())
+ {
+ return;
+ }
+
+ SurgSim::Math::Vector3d nodeSize = node->getBoundingBox().sizes();
+ std::shared_ptr<SurgSim::Math::Shape> nodeShape;
+ nodeShape = std::make_shared<SurgSim::Math::BoxShape>(nodeSize.x(), nodeSize.y(), nodeSize.z());
+ SurgSim::Math::Vector3d nodeCenter = node->getBoundingBox().center();
+ SurgSim::Math::RigidTransform3d nodePose = pair->getFirst()->getPose();
+ nodePose.translation() += nodePose.linear() * nodeCenter;
+
+ m_nodeCollisionRepresentation->setShape(nodeShape);
+ m_nodeCollisionRepresentation->setLocalPose(nodePose);
+
+ std::shared_ptr<CollisionPair> localPair = std::make_shared<CollisionPair>(m_nodeCollisionRepresentation,
+ pair->getSecond());
+ m_calculator->calculateContact(localPair);
+
+ if (localPair->hasContacts())
+ {
+ if (node->hasChildren())
+ {
+ for (size_t i = 0; i < node->getChildren().size(); i++)
+ {
+ nodePath->push_back(i);
+ calculateContactWithNode(node->getChild(i), pair, nodePath);
+ nodePath->pop_back();
+ }
+ }
+ else
+ {
+ const std::list<std::shared_ptr<Contact>>& newContacts = localPair->getContacts();
+ SurgSim::Math::Vector3d contactPosition;
+ for (auto contact = newContacts.cbegin(); contact != newContacts.cend(); ++contact)
+ {
+ (*contact)->penetrationPoints.first.octreeNodePath.setValue(*nodePath);
+
+ contactPosition = (*contact)->penetrationPoints.first.rigidLocalPosition.getValue();
+ contactPosition += nodeCenter;
+ (*contact)->penetrationPoints.first.rigidLocalPosition.setValue(contactPosition);
+
+ pair->addContact(*contact);
+ }
+ }
+ }
+}
+
+};
+};
+
diff --git a/SurgSim/Collision/OctreeDcdContact.h b/SurgSim/Collision/OctreeDcdContact.h
new file mode 100644
index 0000000..a136611
--- /dev/null
+++ b/SurgSim/Collision/OctreeDcdContact.h
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_OCTREEDCDCONTACT_H
+#define SURGSIM_COLLISION_OCTREEDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Math/OctreeShape.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+class ShapeCollisionRepresentation;
+
+/// Class to calculate intersections between an Octree and other shapes
+class OctreeDcdContact : public ContactCalculation
+{
+public:
+
+ /// Constructor.
+ /// \param calculator The contact calculator to use on each octree node
+ explicit OctreeDcdContact(std::shared_ptr<ContactCalculation> calculator);
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return A pair of shape type ids
+ virtual std::pair<int, int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair) override;
+
+ /// Calculate the collision between a specific octree node and a shape
+ /// This function will check for contact between the node and shape. If
+ /// contact is found, this function will be called on each of the
+ /// node's children. Once a leaf node is reached, contacts are added to the
+ /// CollisionPair.
+ /// \param node the octree node to collide with
+ /// \param [in,out] pair the collision pair that is under consideration
+ /// \param nodePath the path of the current node
+ void calculateContactWithNode(std::shared_ptr<const SurgSim::Math::OctreeShape::NodeType> node,
+ std::shared_ptr<CollisionPair> pair,
+ std::shared_ptr<SurgSim::DataStructures::OctreePath> nodePath);
+
+ /// The contact calculator to use on each octree node
+ const std::shared_ptr<ContactCalculation> m_calculator;
+
+ /// The shape types that this contact caculation handles
+ std::pair<int, int> m_shapeTypes;
+
+ /// Collision Representation used to detect contacts with each octree node
+ std::shared_ptr<ShapeCollisionRepresentation> m_nodeCollisionRepresentation;
+};
+
+};
+};
+
+
+
+#endif // SURGSIM_COLLISION_OCTREEDCDCONTACT_H
diff --git a/SurgSim/Collision/Representation.cpp b/SurgSim/Collision/Representation.cpp
new file mode 100644
index 0000000..ead4cd3
--- /dev/null
+++ b/SurgSim/Collision/Representation.cpp
@@ -0,0 +1,44 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+Representation::Representation(const std::string& name) :
+ SurgSim::Framework::Representation(name)
+{
+}
+
+Representation::~Representation()
+{
+
+}
+
+SurgSim::DataStructures::BufferedValue<ContactMapType>& Representation::getCollisions()
+{
+ return m_collisions;
+}
+
+void Representation::update(const double& dt)
+{
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/Representation.h b/SurgSim/Collision/Representation.h
new file mode 100644
index 0000000..972e935
--- /dev/null
+++ b/SurgSim/Collision/Representation.h
@@ -0,0 +1,90 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_REPRESENTATION_H
+#define SURGSIM_COLLISION_REPRESENTATION_H
+
+#include <list>
+#include <memory>
+#include <unordered_map>
+
+#include "SurgSim/DataStructures/BufferedValue.h"
+#include "SurgSim/Framework/Representation.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class Shape;
+};
+
+namespace Physics
+{
+class Representation;
+};
+
+namespace Collision
+{
+struct Contact;
+class Representation;
+
+typedef std::unordered_map<std::shared_ptr<SurgSim::Collision::Representation>,
+ std::list<std::shared_ptr<SurgSim::Collision::Contact>>> ContactMapType;
+
+/// Wrapper class to use for the collision operation, handles its enclosed shaped
+/// and a possible local to global coordinate system transform, if the physics representation
+/// is a nullptr or a has gone out of scope ASSERT's will be triggered.
+/// Collision with other representations will be updated by CollisionPair::addContact() and
+/// be cleared every time DcdCollision::updatePair() makes a new CollisionPair.
+class Representation : public SurgSim::Framework::Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of this collision representation
+ explicit Representation(const std::string& name);
+
+ /// Destructor
+ virtual ~Representation();
+
+ /// Get the shape type id
+ /// \return The unique type of the shape, used to determine which calculation to use.
+ virtual int getShapeType() const = 0;
+
+ /// Get the shape
+ /// \return The actual shape used for collision.
+ virtual const std::shared_ptr<SurgSim::Math::Shape> getShape() const = 0;
+
+ /// A map between collision representations and contacts.
+ /// For each collision representation, it gives the list of contacts registered against this instance.
+ /// \return A map with collision representations as keys and lists of contacts as the associated value.
+ SurgSim::DataStructures::BufferedValue<ContactMapType>& getCollisions();
+
+ /// Update the representation.
+ /// \param dt the time passed from the last update.
+ virtual void update(const double& dt);
+
+protected:
+ /// A map which associates a list of contacts with each collision representation.
+ /// Every contact added to this map follows the convention of pointing the contact normal toward this
+ /// representation. And the first penetration point is on this representation.
+ SurgSim::DataStructures::BufferedValue<ContactMapType> m_collisions;
+};
+
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/ShapeCollisionRepresentation.cpp b/SurgSim/Collision/ShapeCollisionRepresentation.cpp
new file mode 100644
index 0000000..bd5bdef
--- /dev/null
+++ b/SurgSim/Collision/ShapeCollisionRepresentation.cpp
@@ -0,0 +1,87 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Collision::ShapeCollisionRepresentation,
+ ShapeCollisionRepresentation);
+
+ShapeCollisionRepresentation::ShapeCollisionRepresentation(const std::string& name) :
+ Representation(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(ShapeCollisionRepresentation, std::shared_ptr<SurgSim::Math::Shape>, Shape,
+ getShape, setShape);
+}
+
+ShapeCollisionRepresentation::~ShapeCollisionRepresentation()
+{
+}
+
+int ShapeCollisionRepresentation::getShapeType() const
+{
+ return m_shape->getType();
+}
+
+
+void ShapeCollisionRepresentation::setLocalPose(const SurgSim::Math::RigidTransform3d& pose)
+{
+ Representation::setLocalPose(pose);
+ update(0.0);
+}
+
+void ShapeCollisionRepresentation::setShape(const std::shared_ptr<SurgSim::Math::Shape>& shape)
+{
+ SURGSIM_ASSERT(nullptr != shape) << "Can not set a empty shape.";
+ m_shape = shape;
+ update(0.0);
+}
+
+const std::shared_ptr<SurgSim::Math::Shape> ShapeCollisionRepresentation::getShape() const
+{
+ return m_shape;
+}
+
+void ShapeCollisionRepresentation::update(const double& dt)
+{
+ auto meshShape = std::dynamic_pointer_cast<SurgSim::Math::MeshShape>(m_shape);
+ if (nullptr != meshShape)
+ {
+ SURGSIM_LOG_IF(!meshShape->isValid(), SurgSim::Framework::Logger::getDefaultLogger(), WARNING) <<
+ "Try to update an invalid MeshShape.";
+ meshShape->setPose(getPose());
+ }
+}
+
+bool ShapeCollisionRepresentation::doInitialize()
+{
+ if (nullptr != m_shape)
+ {
+ SURGSIM_ASSERT(m_shape->isValid()) <<
+ "An invalid MeshShape is used in this ShapeCollisionRepresentation.";
+ }
+
+ return true;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/ShapeCollisionRepresentation.h b/SurgSim/Collision/ShapeCollisionRepresentation.h
new file mode 100644
index 0000000..ec0101f
--- /dev/null
+++ b/SurgSim/Collision/ShapeCollisionRepresentation.h
@@ -0,0 +1,67 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_SHAPECOLLISIONREPRESENTATION_H
+#define SURGSIM_COLLISION_SHAPECOLLISIONREPRESENTATION_H
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+class Shape;
+}
+
+namespace Collision
+{
+SURGSIM_STATIC_REGISTRATION(ShapeCollisionRepresentation);
+
+/// Use a Shape as a Collision Representation, any SurgSim::Physics::Representation can
+/// be used as a backing representation
+class ShapeCollisionRepresentation : public Representation
+{
+public:
+ /// Constructor
+ explicit ShapeCollisionRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~ShapeCollisionRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Collision::ShapeCollisionRepresentation);
+
+ virtual int getShapeType() const override;
+
+ virtual void setLocalPose(const SurgSim::Math::RigidTransform3d& pose) override;
+
+ // Set the shape to be used in this representation
+ // \param shape Shape to be used in this representation.
+ void setShape(const std::shared_ptr<SurgSim::Math::Shape>& shape);
+ virtual const std::shared_ptr<SurgSim::Math::Shape> getShape() const override;
+
+ virtual void update(const double& dt) override;
+ virtual bool doInitialize() override;
+
+private:
+ // Shape used by this representation
+ std::shared_ptr<SurgSim::Math::Shape> m_shape;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_COLLISION_SHAPECOLLISIONREPRESENTATION_H
\ No newline at end of file
diff --git a/SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.cpp b/SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.cpp
new file mode 100644
index 0000000..4b92af7
--- /dev/null
+++ b/SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.cpp
@@ -0,0 +1,85 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+SphereDoubleSidedPlaneDcdContact::SphereDoubleSidedPlaneDcdContact()
+{
+}
+
+std::pair<int,int> SphereDoubleSidedPlaneDcdContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_SPHERE, SurgSim::Math::SHAPE_TYPE_DOUBLESIDEDPLANE);
+}
+
+void SphereDoubleSidedPlaneDcdContact::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ std::shared_ptr<Representation> representationPlane;
+ std::shared_ptr<Representation> representationSphere;
+
+ representationSphere = pair->getFirst();
+ representationPlane = pair->getSecond();
+
+ std::shared_ptr<SphereShape> sphere = std::static_pointer_cast<SphereShape>(representationSphere->getShape());
+ std::shared_ptr<DoubleSidedPlaneShape> plane =
+ std::static_pointer_cast<DoubleSidedPlaneShape>(representationPlane->getShape());
+
+ Vector3d sphereCenter = representationSphere->getPose().translation();
+
+ // Move into Plane coordinate system
+ Vector3d planeLocalSphereCenter = representationPlane->getPose().inverse() * sphereCenter;
+
+ Vector3d result;
+ double dist = SurgSim::Math::distancePointPlane(planeLocalSphereCenter, plane->getNormal(), plane->getD(),
+ &result);
+ double distAbsolute = std::abs(dist);
+ if (distAbsolute < sphere->getRadius())
+ {
+ double depth = sphere->getRadius() - distAbsolute;
+
+ // Calculate the normal going from the plane to the sphere, it is the plane normal transformed by the
+ // plane pose, flipped if the sphere is behind the plane and normalize it
+ Vector3d normal =
+ (representationPlane->getPose().linear() * plane->getNormal()) * ((dist < 0.0) ? -1.0 : 1.0);
+
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ representationSphere->getPose().inverse() * (sphereCenter - normal * sphere->getRadius()));
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ representationPlane->getPose().inverse() * (sphereCenter - normal * distAbsolute));
+
+ pair->addContact(depth, normal, penetrationPoints);
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.h b/SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.h
new file mode 100644
index 0000000..e7fb713
--- /dev/null
+++ b/SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.h
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_SPHEREDOUBLESIDEDPLANEDCDCONTACT_H
+#define SURGSIM_COLLISION_SPHEREDOUBLESIDEDPLANEDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between Spheres and DoubleSidedPlanes
+class SphereDoubleSidedPlaneDcdContact : public ContactCalculation
+{
+public:
+
+ /// Constructor.
+ SphereDoubleSidedPlaneDcdContact();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int,int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair);
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/SpherePlaneDcdContact.cpp b/SurgSim/Collision/SpherePlaneDcdContact.cpp
new file mode 100644
index 0000000..a5b49a8
--- /dev/null
+++ b/SurgSim/Collision/SpherePlaneDcdContact.cpp
@@ -0,0 +1,82 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/SpherePlaneDcdContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+SpherePlaneDcdContact::SpherePlaneDcdContact()
+{
+}
+
+std::pair<int,int> SpherePlaneDcdContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_SPHERE, SurgSim::Math::SHAPE_TYPE_PLANE);
+}
+
+void SpherePlaneDcdContact::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ std::shared_ptr<Representation> representationSphere;
+ std::shared_ptr<Representation> representationPlane;
+
+ representationSphere = pair->getFirst();
+ representationPlane = pair->getSecond();
+
+ std::shared_ptr<SphereShape> sphere = std::static_pointer_cast<SphereShape>(representationSphere->getShape());
+ std::shared_ptr<PlaneShape> plane = std::static_pointer_cast<PlaneShape>(representationPlane->getShape());
+
+ Vector3d sphereCenter = representationSphere->getPose().translation();
+
+ // Move into Plane coordinate system
+ Vector3d planeLocalSphereCenter = representationPlane->getPose().inverse() * sphereCenter;
+
+ Vector3d result;
+ double dist = SurgSim::Math::distancePointPlane(planeLocalSphereCenter, plane->getNormal(), plane->getD(),
+ &result);
+ if (dist < sphere->getRadius())
+ {
+ double depth = sphere->getRadius() - dist;
+
+ // Calculate the normal going from the plane to the sphere, it is the plane normal transformed by the
+ // plane pose, flipped if the sphere is behind the plane and normalize it
+ Vector3d normal = representationPlane->getPose().linear() * plane->getNormal();
+
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ representationSphere->getPose().inverse() * (sphereCenter - normal * sphere->getRadius()));
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ representationPlane->getPose().inverse() * (sphereCenter - normal * dist));
+
+ pair->addContact(depth, normal, penetrationPoints);
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SpherePlaneDcdContact.h b/SurgSim/Collision/SpherePlaneDcdContact.h
new file mode 100644
index 0000000..15ae6f0
--- /dev/null
+++ b/SurgSim/Collision/SpherePlaneDcdContact.h
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_SPHEREPLANEDCDCONTACT_H
+#define SURGSIM_COLLISION_SPHEREPLANEDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between Spheres and Planes
+class SpherePlaneDcdContact : public ContactCalculation
+{
+public:
+
+ /// Constructor.
+ SpherePlaneDcdContact();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int,int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair);
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/SphereSphereDcdContact.cpp b/SurgSim/Collision/SphereSphereDcdContact.cpp
new file mode 100644
index 0000000..8ad3cfb
--- /dev/null
+++ b/SurgSim/Collision/SphereSphereDcdContact.cpp
@@ -0,0 +1,66 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/SphereSphereDcdContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+SphereSphereDcdContact::SphereSphereDcdContact()
+{
+}
+
+std::pair<int,int> SphereSphereDcdContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_SPHERE, SurgSim::Math::SHAPE_TYPE_SPHERE);
+}
+
+void SphereSphereDcdContact::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ std::shared_ptr<SphereShape> firstSphere = std::static_pointer_cast<SphereShape>(pair->getFirst()->getShape());
+ std::shared_ptr<SphereShape> secondSphere = std::static_pointer_cast<SphereShape>(pair->getSecond()->getShape());
+
+ Vector3d firstCenter = pair->getFirst()->getPose().translation();
+ Vector3d secondCenter = pair->getSecond()->getPose().translation();
+
+ Vector3d normal = firstCenter - secondCenter;
+ double dist = normal.norm();
+ double maxDist = firstSphere->getRadius() + secondSphere->getRadius();
+ if (dist < maxDist)
+ {
+ std::pair<Location, Location> penetrationPoints;
+ normal.normalize();
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ (pair->getFirst()->getPose().linear().inverse() * -normal) * firstSphere->getRadius());
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ (pair->getSecond()->getPose().linear().inverse() * normal) * secondSphere->getRadius());
+
+ pair->addContact(maxDist - dist, normal, penetrationPoints);
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SphereSphereDcdContact.h b/SurgSim/Collision/SphereSphereDcdContact.h
new file mode 100644
index 0000000..ef02ed2
--- /dev/null
+++ b/SurgSim/Collision/SphereSphereDcdContact.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_SPHERESPHEREDCDCONTACT_H
+#define SURGSIM_COLLISION_SPHERESPHEREDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between spheres
+class SphereSphereDcdContact : public ContactCalculation
+{
+public:
+ /// Constructor
+ SphereSphereDcdContact();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int,int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair);
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/TriangleMeshPlaneDcdContact.cpp b/SurgSim/Collision/TriangleMeshPlaneDcdContact.cpp
new file mode 100644
index 0000000..6f20121
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshPlaneDcdContact.cpp
@@ -0,0 +1,91 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/TriangleMeshPlaneDcdContact.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/MeshShape.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::MeshShape;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+TriangleMeshPlaneDcdContact::TriangleMeshPlaneDcdContact()
+{
+}
+
+std::pair<int, int> TriangleMeshPlaneDcdContact::getShapeTypes()
+{
+ return std::pair<int, int> (SurgSim::Math::SHAPE_TYPE_MESH, SurgSim::Math::SHAPE_TYPE_PLANE);
+}
+
+void TriangleMeshPlaneDcdContact::doCalculateContact
+ (std::shared_ptr<CollisionPair> pair)
+{
+ std::shared_ptr<Representation> representationTriangleMesh;
+ std::shared_ptr<Representation> representationPlane;
+
+ representationTriangleMesh = pair->getFirst();
+ representationPlane = pair->getSecond();
+
+ std::shared_ptr<MeshShape> mesh =
+ std::static_pointer_cast<MeshShape>(representationTriangleMesh->getShape());
+
+ std::shared_ptr<PlaneShape> plane(std::static_pointer_cast<PlaneShape>(representationPlane->getShape()));
+
+ // Transform the plane normal to Mesh co-ordinate system.
+ RigidTransform3d planeLocalToMeshLocal = representationPlane->getPose();
+ Vector3d planeNormal = planeLocalToMeshLocal.linear() * plane->getNormal();
+ Vector3d planeNormalScaled = plane->getNormal() * -plane->getD();
+ Vector3d planePoint = planeLocalToMeshLocal * planeNormalScaled;
+ double planeD = -planeNormal.dot(planePoint);
+
+ // Now loop through all the vertices on the Mesh and check if it below the plane
+ size_t totalMeshVertices = mesh->getMesh()->getNumVertices();
+
+ double d;
+ Vector3d normal;
+ Vector3d meshVertex;
+
+ for (size_t i = 0; i < totalMeshVertices; ++i)
+ {
+ meshVertex = mesh->getMesh()->getVertex(i).position;
+ d = planeNormal.dot(meshVertex) + planeD;
+ if (d < SurgSim::Math::Geometry::DistanceEpsilon)
+ {
+ // Create the contact
+ normal = representationPlane->getPose().linear() * plane->getNormal();
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ representationTriangleMesh->getPose().inverse() * meshVertex);
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ representationPlane->getPose().inverse() * (meshVertex - normal * d));
+
+ pair->addContact(-d, normal, penetrationPoints);
+ }
+ }
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Collision/TriangleMeshPlaneDcdContact.h b/SurgSim/Collision/TriangleMeshPlaneDcdContact.h
new file mode 100644
index 0000000..498c666
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshPlaneDcdContact.h
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_TRIANGLEMESHPLANEDCDCONTACT_H
+#define SURGSIM_COLLISION_TRIANGLEMESHPLANEDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/ContactCalculation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+/// Class to calculate intersections between a triangle mesh and a plane
+class TriangleMeshPlaneDcdContact : public ContactCalculation
+{
+public:
+
+ /// Constructor
+ TriangleMeshPlaneDcdContact();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int, int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair) override;
+
+};
+
+};
+};
+
+
+#endif
diff --git a/SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.cpp b/SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.cpp
new file mode 100644
index 0000000..87b8579
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.cpp
@@ -0,0 +1,229 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/AabbTreeNode.h"
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::DataStructures::TriangleMesh;
+using SurgSim::Math::MeshShape;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+TriangleMeshTriangleMeshDcdContact::TriangleMeshTriangleMeshDcdContact()
+{
+}
+
+std::pair<int,int> TriangleMeshTriangleMeshDcdContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_MESH, SurgSim::Math::SHAPE_TYPE_MESH);
+}
+
+#ifdef SURGSIM_DEBUG_TRIANGLETRIANGLECONTACT
+namespace
+{
+
+/// Asserts the points are coplanar, and prints debug output on the failing condition.
+/// \param triangle0, triangle1, triangle2 the vertices of the triangle
+/// \param point the point to compare against
+/// \throws If the points are not coplanar
+void assertIsCoplanar(const Vector3d& triangle0,
+ const Vector3d& triangle1,
+ const Vector3d& triangle2,
+ const Vector3d& point)
+{
+ SURGSIM_ASSERT(SurgSim::Math::isCoplanar(triangle0, triangle1, triangle2, point))
+ << "Coplanar assertion failed with: "
+ "t0 [" << triangle0.transpose() << "], "
+ "t1 [" << triangle1.transpose() << "], "
+ "t2 [" << triangle2.transpose() << "], "
+ "pt [" << point.transpose() << "]";
+}
+
+/// Asserts the point is inside the triangle, and prints debug output on the failing condition.
+/// \param point the point to compare against
+/// \param triangle0, triangle1, triangle2 the vertices of the triangle
+/// \param normal the unit normal of the triangle
+/// \throws If the point is not inside the triangle
+void assertIsPointInsideTriangle(const Vector3d& point,
+ const Vector3d& triangle0,
+ const Vector3d& triangle1,
+ const Vector3d& triangle2,
+ const Vector3d& normal)
+{
+ SURGSIM_ASSERT(SurgSim::Math::isPointInsideTriangle(point, triangle0, triangle1, triangle2, normal))
+ << "Point inside triangle assertion failed with: "
+ "t0 [" << triangle0.transpose() << "], "
+ "t1 [" << triangle1.transpose() << "], "
+ "t2 [" << triangle2.transpose() << "], "
+ "n [" << normal.transpose() << "], "
+ "pt [" << point.transpose() << "]";
+}
+
+/// Asserts the provided normal and depth minimally resolve the interpenetration of the two triangles, and prints debug
+/// output on the failing condition.
+/// \param normal the unit normal in the direction to resolve the penetration
+/// \param penetrationDepth the depth of penetration to check
+/// \param triangleA0, triangleA1, triangleA2 the vertices of the first triangle
+/// \param triangleB0, triangleB1, triangleB2 the vertices of the second triangle
+/// \throws If the normal and depth do not minimally resolve the interpenetration of the two triangles
+void assertIsCorrectNormalAndDepth(const Vector3d& normal,
+ double penetrationDepth,
+ const Vector3d& triangleA0,
+ const Vector3d& triangleA1,
+ const Vector3d& triangleA2,
+ const Vector3d& triangleB0,
+ const Vector3d& triangleB1,
+ const Vector3d& triangleB2)
+{
+ Vector3d correction = normal * (penetrationDepth - SurgSim::Math::Geometry::DistanceEpsilon);
+
+ SURGSIM_ASSERT(SurgSim::Math::doesIntersectTriangleTriangle(
+ (Vector3d)(triangleA0 + correction), (Vector3d)(triangleA1 + correction), (Vector3d)(triangleA2 + correction),
+ triangleB0, triangleB1, triangleB2))
+ << "Correct normal and depth assertion failed with: "
+ "n [" << normal.transpose() << "], "
+ "d [" << penetrationDepth << "], "
+ "a0 [" << triangleA0.transpose() << "], "
+ "a1 [" << triangleA1.transpose() << "], "
+ "a2 [" << triangleA2.transpose() << "], "
+ "b0 [" << triangleB0.transpose() << "], "
+ "b1 [" << triangleB1.transpose() << "], "
+ "b2 [" << triangleB2.transpose() << "]";
+
+ correction = normal * (penetrationDepth + 2.0 * SurgSim::Math::Geometry::DistanceEpsilon);
+
+ SURGSIM_ASSERT(!SurgSim::Math::doesIntersectTriangleTriangle(
+ (Vector3d)(triangleA0 + correction), (Vector3d)(triangleA1 + correction), (Vector3d)(triangleA2 + correction),
+ triangleB0, triangleB1, triangleB2))
+ << "Correct normal and depth assertion failed with: "
+ "n [" << normal.transpose() << "], "
+ "d [" << penetrationDepth << "], "
+ "a0 [" << triangleA0.transpose() << "], "
+ "a1 [" << triangleA1.transpose() << "], "
+ "a2 [" << triangleA2.transpose() << "], "
+ "b0 [" << triangleB0.transpose() << "], "
+ "b1 [" << triangleB1.transpose() << "], "
+ "b2 [" << triangleB2.transpose() << "]";
+}
+
+} // namespace
+#endif //SURGSIM_DEBUG_TRIANGLETRIANGLECONTACT
+
+void TriangleMeshTriangleMeshDcdContact::doCalculateContact(std::shared_ptr<CollisionPair> pair)
+{
+ auto meshShapeA = std::static_pointer_cast<MeshShape>(pair->getFirst()->getShape());
+ auto meshShapeB = std::static_pointer_cast<MeshShape>(pair->getSecond()->getShape());
+
+ std::shared_ptr<TriangleMesh> collisionMeshA = meshShapeA->getMesh();
+ std::shared_ptr<TriangleMesh> collisionMeshB = meshShapeB->getMesh();
+
+ std::list<SurgSim::DataStructures::AabbTree::TreeNodePairType> intersectionList
+ = meshShapeA->getAabbTree()->spatialJoin(*meshShapeB->getAabbTree());
+
+ double depth = 0.0;
+ Vector3d normal;
+ Vector3d penetrationPointA, penetrationPointB;
+
+ for (auto intersection = intersectionList.begin(); intersection != intersectionList.end(); ++intersection)
+ {
+ std::shared_ptr<SurgSim::DataStructures::AabbTreeNode> nodeA = intersection->first;
+ std::shared_ptr<SurgSim::DataStructures::AabbTreeNode> nodeB = intersection->second;
+
+ std::list<size_t> triangleListA;
+ std::list<size_t> triangleListB;
+
+ nodeA->getIntersections(nodeB->getAabb(), &triangleListA);
+ nodeB->getIntersections(nodeA->getAabb(), &triangleListB);
+
+ for (auto i = triangleListA.begin(); i != triangleListA.end(); ++i)
+ {
+ const Vector3d& normalA = collisionMeshA->getNormal(*i);
+ if (normalA.isZero())
+ {
+ continue;
+ }
+
+ auto verticesA = collisionMeshA->getTrianglePositions(*i);
+
+ for (auto j = triangleListB.begin(); j != triangleListB.end(); ++j)
+ {
+ const Vector3d& normalB = collisionMeshB->getNormal(*j);
+ if (normalB.isZero())
+ {
+ continue;
+ }
+
+ auto verticesB = collisionMeshB->getTrianglePositions(*j);
+
+ // Check if the triangles intersect.
+ if (SurgSim::Math::calculateContactTriangleTriangle(verticesA[0], verticesA[1], verticesA[2],
+ verticesB[0], verticesB[1], verticesB[2],
+ normalA, normalB, &depth,
+ &penetrationPointA, &penetrationPointB,
+ &normal))
+ {
+#ifdef SURGSIM_DEBUG_TRIANGLETRIANGLECONTACT
+ assertIsCoplanar(verticesA[0], verticesA[1], verticesA[2], penetrationPointA);
+ assertIsCoplanar(verticesB[0], verticesB[1], verticesB[2], penetrationPointB);
+
+ assertIsPointInsideTriangle(
+ penetrationPointA, verticesA[0], verticesA[1], verticesA[2], normalA);
+ assertIsPointInsideTriangle(penetrationPointB, verticesB[0], verticesB[1], verticesB[2], normalB);
+
+ assertIsCorrectNormalAndDepth(normal, depth, verticesA[0], verticesA[1], verticesA[2],
+ verticesB[0], verticesB[1], verticesB[2]);
+#endif
+
+ // Create the contact.
+ std::pair<Location, Location> penetrationPoints;
+ Vector3d barycentricCoordinate;
+ SurgSim::Math::barycentricCoordinates(penetrationPointA, verticesA[0], verticesA[1], verticesA[2],
+ normalA, &barycentricCoordinate);
+ penetrationPoints.first.meshLocalCoordinate.setValue(
+ SurgSim::DataStructures::IndexedLocalCoordinate(*i, barycentricCoordinate));
+ SurgSim::Math::barycentricCoordinates(penetrationPointB, verticesB[0], verticesB[1], verticesB[2],
+ normalB, &barycentricCoordinate);
+ penetrationPoints.second.meshLocalCoordinate.setValue(
+ SurgSim::DataStructures::IndexedLocalCoordinate(*j, barycentricCoordinate));
+
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ pair->getFirst()->getPose().inverse() * penetrationPointA);
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ pair->getSecond()->getPose().inverse() * penetrationPointB);
+
+ pair->addContact(std::abs(depth), normal, penetrationPoints);
+ }
+ }
+ }
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.h b/SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.h
new file mode 100644
index 0000000..a927f89
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_TRIANGLEMESHTRIANGLEMESHDCDCONTACT_H
+#define SURGSIM_COLLISION_TRIANGLEMESHTRIANGLEMESHDCDCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between a triangle mesh and a triangle mesh
+class TriangleMeshTriangleMeshDcdContact : public ContactCalculation
+{
+public:
+ /// Constructor.
+ TriangleMeshTriangleMeshDcdContact();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ virtual std::pair<int, int> getShapeTypes() override;
+
+private:
+ /// Calculate the actual contact between two shapes of the given CollisionPair.
+ /// \param pair The symmetric pair that is under consideration.
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair) override;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_COLLISION_TRIANGLEMESHTRIANGLEMESHDCDCONTACT_H
diff --git a/SurgSim/Collision/UnitTests/BoxCapsuleContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/BoxCapsuleContactCalculationTests.cpp
new file mode 100644
index 0000000..472f438
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/BoxCapsuleContactCalculationTests.cpp
@@ -0,0 +1,318 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "SurgSim/Collision/BoxCapsuleDcdContact.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/CapsuleShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::CapsuleShape;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+void doBoxCapsuleTest(std::shared_ptr<BoxShape> box,
+ const Quaterniond& boxQuat,
+ const Vector3d& boxTrans,
+ std::shared_ptr<CapsuleShape> capsule,
+ const Quaterniond& capsuleQuat,
+ const Vector3d& capsuleTrans,
+ const bool expectedInContact)
+{
+ std::shared_ptr<ShapeCollisionRepresentation> boxRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Box 0");
+ boxRep->setShape(box);
+ boxRep->setLocalPose(makeRigidTransform(boxQuat, boxTrans));
+
+ std::shared_ptr<ShapeCollisionRepresentation> capsuleRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Capsule 0");
+ capsuleRep->setShape(capsule);
+ capsuleRep->setLocalPose(makeRigidTransform(capsuleQuat, capsuleTrans));
+
+ // Perform collision detection.
+ BoxCapsuleDcdContact calcContact;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(boxRep, capsuleRep);
+ calcContact.calculateContact(pair);
+
+ EXPECT_EQ(expectedInContact, pair->hasContacts());
+
+ if (expectedInContact)
+ {
+ Vector3d capsuleToBox = boxTrans - capsuleTrans;
+
+ double depthMax = box->getSize().norm();
+ depthMax += capsule->getLength() / 2.0 + capsule->getRadius();
+
+ auto contacts = pair->getContacts();
+ for (auto contact=contacts.cbegin(); contact!=contacts.cend(); ++contact)
+ {
+ if (! capsuleToBox.isZero())
+ {
+ // Check that each normal is pointing into the box
+ EXPECT_LT(0.0, (*contact)->normal.dot(capsuleToBox));
+ }
+
+ // Check that the depth is sane
+ EXPECT_LT(0.0, (*contact)->depth);
+ EXPECT_GT(depthMax, (*contact)->depth);
+
+ // Check that the locations are sane
+ Vector3d boxPenetrationPoint =
+ boxQuat * (*contact)->penetrationPoints.first.rigidLocalPosition.getValue() + boxTrans;
+ Vector3d capsulePenetrationPoint =
+ capsuleQuat * (*contact)->penetrationPoints.second.rigidLocalPosition.getValue() + capsuleTrans;
+ EXPECT_GT(0.0, (*contact)->normal.dot(boxPenetrationPoint - boxTrans));
+ EXPECT_LT(0.0, (*contact)->normal.dot(capsulePenetrationPoint - capsuleTrans));
+ }
+ }
+}
+
+TEST(BoxCapsuleContactCalculationTests, UnitTests)
+{
+ std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(1.0, 1.0, 1.0);
+ std::shared_ptr<CapsuleShape> capsule = std::make_shared<CapsuleShape>(4.0, 1.0);
+ Quaterniond boxQuat;
+ Vector3d boxTrans;
+ Quaterniond capsuleQuat;
+ Vector3d capsuleTrans;
+
+ {
+ SCOPED_TRACE("No intersection, box in front of capsule");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(10.6, 0.0, 0.0);
+ capsuleQuat = Quaterniond::Identity();
+ capsuleTrans = Vector3d::Zero();
+ bool expectedInContact = false;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("No intersection, capsule beyond corner of box");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d::Zero();
+ capsuleQuat = Quaterniond::Identity();
+ bool expectedInContact = false;
+ capsuleTrans = Vector3d(1.5, 0.0, 1.5);
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ capsuleTrans = Vector3d(1.5, 0.0, -1.5);
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ capsuleTrans = Vector3d(-1.5, 0.0, 1.5);
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ capsuleTrans = Vector3d(-1.5, 0.0, -1.5);
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("No intersection, box below capsule");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(0.0, -3.6, 0.0);
+ capsuleQuat = Quaterniond::Identity();
+ capsuleTrans = Vector3d::Zero();
+ bool expectedInContact = false;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection, box intersection with capsule side");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(1.0 , 0.0, 0.0);
+ capsuleQuat = Quaterniond::Identity();
+ capsuleTrans = Vector3d::Zero();
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection, box intersection with upside down capsule");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(1.0 , 0.0, 0.0);
+ capsuleQuat = makeRotationQuaternion(M_PI, Vector3d(0.0, 0.0, 1.0));
+ capsuleTrans = Vector3d::Zero();
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection, box intersection with z-axis capsule");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(1.0 , 0.0, 0.0);
+ capsuleQuat = makeRotationQuaternion(M_PI_2, Vector3d(1.0, 0.0, 0.0));
+ capsuleTrans = Vector3d::Zero();
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection, box intersection with x-axis capsule");
+ boxQuat = makeRotationQuaternion(M_PI, Vector3d(0.0, 0.0, 1.0));
+ boxTrans = Vector3d(1.0 , 0.0, 0.0);
+ capsuleQuat = makeRotationQuaternion(M_PI_2, Vector3d(1.0, 0.0, 0.0));
+ capsuleTrans = Vector3d::Zero();
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection, box intersection with capsule cap");
+ boxQuat = makeRotationQuaternion(M_PI_2, Vector3d(0.0, 0.0, 1.0));
+ boxTrans = Vector3d(0.1 , 0.0, 0.1);
+ capsuleQuat = Quaterniond::Identity();
+ capsuleTrans = Vector3d(0.0 , 2.6, 0.0);
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("No intersection, capsule near box corner, but not intersecting");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(0.0 , 0.0, 0.0);
+ capsuleQuat = Quaterniond::Identity();
+ capsuleTrans = Vector3d(1.3 , 0.0, 1.3);
+ bool expectedInContact = false;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection, capsule intersecting with box corner");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(0.0 , 0.0, 0.0);
+ capsuleQuat = Quaterniond::Identity();
+ capsuleTrans = Vector3d(1.2 , 0.0, 1.2);
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+
+ {
+ SCOPED_TRACE("Intersection, box inside capsule");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d::Zero();
+ capsuleQuat = Quaterniond::Identity();
+ capsuleTrans = Vector3d::Zero();
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection, capsule inside box");
+ std::shared_ptr<BoxShape> bigBox = std::make_shared<BoxShape>(10.0, 10.0, 10.0);
+ boxQuat = makeRotationQuaternion(-M_PI_4, Vector3d(0.0, 1.0, 0.0));
+ boxTrans = Vector3d::Zero();
+ capsuleQuat = makeRotationQuaternion(M_PI, Vector3d(1.0, 0.0, 0.0));
+ capsuleTrans = Vector3d(0.0, 0.0, 0.0);
+ bool expectedInContact = true;
+ doBoxCapsuleTest(bigBox, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection, capsule bottom at box center");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d::Zero();
+ capsuleQuat = Quaterniond::Identity();
+ capsuleTrans = Vector3d(0.0, -2.0, 0.0);
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection, capsule top at box center");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d::Zero();
+ capsuleQuat = Quaterniond::Identity();
+ capsuleTrans = Vector3d(0.0, 2.0, 0.0);
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection with box edge, box's point on edge");
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d::Zero();
+ capsuleQuat = makeRotationQuaternion(0.1, Vector3d::UnitZ().eval());
+ capsuleTrans = Vector3d(1.52, 0.0, 0.0);
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection with box edge, box's point on edge, capsule point along vector towards box point");
+ std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(0.0256, 0.0256, 0.0256);
+ std::shared_ptr<CapsuleShape> capsule = std::make_shared<CapsuleShape>(0.01, 0.0063);
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(0.0254, 0.0524, 0.5128);
+ capsuleQuat = Quaterniond::Identity();
+ capsuleTrans = Vector3d(0.0081837091898594016, 0.074665473951012307, 0.50404931721342927);
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection with box corner");
+ std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(0.0032, 0.0032, 0.0032);
+ std::shared_ptr<CapsuleShape> capsule = std::make_shared<CapsuleShape>(0.01, 0.0063);
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(0.011, 0.038, 0.5496);
+ capsuleQuat =
+ Quaterniond(0.99814646292568798, 0.0035271245394549833, -0.023780789153701510, 0.055907709742473284);
+ capsuleTrans = Vector3d(0.0059124370262071749, 0.031538130383304983, 0.54312746745813301);
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Another intersection with box corner");
+ std::shared_ptr<BoxShape> box =
+ std::make_shared<BoxShape>(0.0008, 0.0008, 0.0008);
+ std::shared_ptr<CapsuleShape> capsule = std::make_shared<CapsuleShape>(0.01, 0.0063);
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(0.005, 0.052, 0.5308);
+ capsuleQuat =
+ Quaterniond(0.71851427633922127, 0.00027205941221747750, 0.0021375922639339773, 0.69550887225089708);
+ capsuleTrans = Vector3d(0.010224217835903153, 0.058515488684803690, 0.53177563225691493);
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+
+ {
+ SCOPED_TRACE("Intersection with box face, but closest point to box center is outside dilated box");
+ std::shared_ptr<BoxShape> box =
+ std::make_shared<BoxShape>(0.012800000000000004, 0.012799999999999999, 0.012800000000000034);
+ std::shared_ptr<CapsuleShape> capsule = std::make_shared<CapsuleShape>(0.01, 0.0063);
+ boxQuat = Quaterniond::Identity();
+ boxTrans = Vector3d(0.019000000000000003, 0.045999999999999999, 0.51920000000000011);
+ capsuleQuat =
+ Quaterniond(0.71552146749248391, -0.00014598153123885886, 0.0013667288118696403, 0.69858939320544333);
+ capsuleTrans = Vector3d(0.017905427782122299, 0.058803866737869748, 0.51747490113489192);
+ bool expectedInContact = true;
+ doBoxCapsuleTest(box, boxQuat, boxTrans, capsule, capsuleQuat, capsuleTrans, expectedInContact);
+ }
+}
+
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/BoxDoubleSidedPlaneContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/BoxDoubleSidedPlaneContactCalculationTests.cpp
new file mode 100644
index 0000000..59225d7
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/BoxDoubleSidedPlaneContactCalculationTests.cpp
@@ -0,0 +1,232 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.h"
+
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::DoubleSidedPlaneShape;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+void doBoxDoubleSidedPlaneTest(std::shared_ptr<BoxShape> box,
+ const Quaterniond& boxQuat,
+ const Vector3d& boxTrans,
+ std::shared_ptr<DoubleSidedPlaneShape> plane,
+ const Quaterniond& planeQuat,
+ const Vector3d& planeTrans,
+ const int expectedNumberOfContacts,
+ const int* expectedBoxIndicesInContacts,
+ const bool collisionNormalIsPlaneNormal)
+{
+ std::shared_ptr<ShapeCollisionRepresentation> boxRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Box 0");
+ boxRep->setShape(box);
+ boxRep->setLocalPose(SurgSim::Math::makeRigidTransform(boxQuat, boxTrans));
+
+ std::shared_ptr<ShapeCollisionRepresentation> planeRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Plane 0");
+ planeRep->setShape(plane);
+ planeRep->setLocalPose(SurgSim::Math::makeRigidTransform(planeQuat, planeTrans));
+
+ // First calculate the expected contact info.
+ std::list<std::shared_ptr<Contact>> expectedContacts;
+ if (expectedNumberOfContacts > 0)
+ {
+ generateBoxDoubleSidedPlaneContact(&expectedContacts, expectedNumberOfContacts, expectedBoxIndicesInContacts,
+ box, boxTrans, boxQuat, plane, planeTrans,
+ planeQuat, collisionNormalIsPlaneNormal);
+ }
+
+ // Perform collision detection.
+ BoxDoubleSidedPlaneDcdContact calcContact;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(boxRep, planeRep);
+ calcContact.calculateContact(pair);
+
+ // Compare the contact info.
+ contactsInfoEqualityTest(expectedContacts, pair->getContacts());
+}
+
+TEST(BoxDoubleSidedPlaneContactCalculationTests, UnitTests)
+{
+ std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(1.0, 1.0, 1.0);
+ std::shared_ptr<DoubleSidedPlaneShape> plane = std::make_shared<DoubleSidedPlaneShape>();
+ SurgSim::Math::Quaterniond boxQuat;
+ SurgSim::Math::Vector3d boxTrans;
+ SurgSim::Math::Quaterniond planeQuat;
+ SurgSim::Math::Vector3d planeTrans;
+ SurgSim::Math::Quaterniond globalQuat;
+
+ {
+ SCOPED_TRACE("No intersection, box in front of rotated plane");
+ boxQuat = SurgSim::Math::makeRotationQuaternion(0.5674, Vector3d(0.4332,0.927, 0.13557).normalized());
+ boxTrans = Vector3d(2.5,10.0,350.0);
+ planeQuat = SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = Vector3d::Zero();
+ int expectedNumberOfContacts = 0;
+ int expectedBoxIndicesInContacts[] = {0};
+ bool collisionNormalIsPlaneNormal = true;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+
+ {
+ SCOPED_TRACE("Intersection in front of plane, four contacts, rotated plane");
+ boxQuat = SurgSim::Math::makeRotationQuaternion(1.233469, Vector3d(0.91834,0.39687,0.8271).normalized());
+ boxTrans = Vector3d(0.5,10.0,350.0);
+ planeQuat = boxQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + boxQuat * Vector3d(-0.5,0.0,0.0);
+ int expectedNumberOfContacts = 4;
+ int expectedBoxIndicesInContacts[] = {0, 1, 2, 3};
+ bool collisionNormalIsPlaneNormal = true;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+
+ {
+ SCOPED_TRACE("Intersection inside of plane, four contacts, rotated plane");
+ boxQuat = SurgSim::Math::makeRotationQuaternion(1.233469, Vector3d(0.91834,0.39687,0.8271).normalized());
+ boxTrans = Vector3d(0.5,10.0,350.0);
+ planeQuat = boxQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + boxQuat * Vector3d(-0.4,0.0,0.0);
+ int expectedNumberOfContacts = 4;
+ int expectedBoxIndicesInContacts[] = {0, 1, 2, 3};
+ bool collisionNormalIsPlaneNormal = true;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+
+ {
+ SCOPED_TRACE("Intersection in front of plane, two contacts, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(0.8753, Vector3d(0.235345,0.6754,0.4567).normalized());
+ boxQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_4, Vector3d(0.0,0.0,1.0));
+ boxTrans = Vector3d(std::sqrt(0.5),230.0,540.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(-std::sqrt(0.5),0.0,0.0);
+ int expectedNumberOfContacts = 2;
+ int expectedBoxIndicesInContacts[] = {0, 1};
+ bool collisionNormalIsPlaneNormal = true;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+
+ {
+ SCOPED_TRACE("Intersection inside of plane, two contacts, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(0.8753, Vector3d(0.235345,0.6754,0.4567).normalized());
+ boxQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_4, Vector3d(0.0,0.0,1.0));
+ boxTrans = Vector3d(std::sqrt(0.5),230.0,540.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(-std::sqrt(0.45),0.0,0.0);
+ int expectedNumberOfContacts = 2;
+ int expectedBoxIndicesInContacts[] = {0, 1};
+ bool collisionNormalIsPlaneNormal = true;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+
+ {
+ SCOPED_TRACE("Intersection in front of plane, one contact, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(-0.3257, Vector3d(-0.4575,-0.8563,0.63457).normalized());
+ double angle = -35.264389682754654315377000330019*(M_PI/180.0);
+ boxQuat = globalQuat * Quaterniond(SurgSim::Math::makeRotationMatrix(angle, Vector3d(0.0,1.0,0.0)) *
+ SurgSim::Math::makeRotationMatrix(-M_PI_4, Vector3d(0.0,0.0,1.0)));
+ boxTrans = Vector3d(std::sqrt(0.75),0.0,0.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(-std::sqrt(0.75),0.0,0.0);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {1};
+ bool collisionNormalIsPlaneNormal = true;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+
+ {
+ SCOPED_TRACE("Intersection inside of plane, one contact, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(-0.3257, Vector3d(-0.4575,-0.8563,0.63457).normalized());
+ double angle = -35.264389682754654315377000330019*(M_PI/180.0);
+ boxQuat = globalQuat * Quaterniond(SurgSim::Math::makeRotationMatrix(angle, Vector3d(0.0,1.0,0.0)) *
+ SurgSim::Math::makeRotationMatrix(-M_PI_4, Vector3d(0.0,0.0,1.0)));
+ boxTrans = Vector3d(std::sqrt(0.75),0.0,0.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(-std::sqrt(0.74),0.0,0.0);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {1};
+ bool collisionNormalIsPlaneNormal = true;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+
+ {
+ SCOPED_TRACE("No intersection, box behind rotated plane");
+ boxQuat = SurgSim::Math::makeRotationQuaternion(0.3252, Vector3d(0.5434,0.634,0.13435).normalized());
+ boxTrans = Vector3d(-45.5,10.0,350.0);
+ planeQuat = SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = Vector3d::Zero();
+ int expectedNumberOfContacts = 0;
+ int expectedBoxIndicesInContacts[] = {0};
+ bool collisionNormalIsPlaneNormal = false;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+
+ {
+ SCOPED_TRACE("Intersection behind plane, four contacts, rotated plane");
+ boxQuat = SurgSim::Math::makeRotationQuaternion(0.1436, Vector3d(0.8441,0.3579,0.2168).normalized());
+ boxTrans = Vector3d(-0.5,0.0,0.0);
+ planeQuat = boxQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + boxQuat * Vector3d(0.5,0.0,0.0);
+ int expectedNumberOfContacts = 4;
+ int expectedBoxIndicesInContacts[] = {4, 5, 6, 7};
+ bool collisionNormalIsPlaneNormal = false;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+
+ {
+ SCOPED_TRACE("Intersection behind plane, two contacts, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(-0.2356, Vector3d(0.4542,-0.2356,0.1187).normalized());
+ boxQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_4, Vector3d(0.0,0.0,1.0));
+ boxTrans = Vector3d(-std::sqrt(0.5),0.0,0.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(std::sqrt(0.5),0.0,0.0);
+ int expectedNumberOfContacts = 2;
+ int expectedBoxIndicesInContacts[] = {6, 7};
+ bool collisionNormalIsPlaneNormal = false;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+
+ {
+ SCOPED_TRACE("Intersection behind plane, one contact, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(1.4576, Vector3d(23.45,-98.24,42.46).normalized());
+ double angle = -35.264389682754654315377000330019*(M_PI/180.0);
+ boxQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(angle, Vector3d(0.0,0.0,1.0)) *
+ SurgSim::Math::makeRotationQuaternion(-M_PI_4, Vector3d(0.0,1.0,0.0));
+ boxTrans = Vector3d(-std::sqrt(0.75),0.0,0.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(std::sqrt(0.75),0.0,0.0);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {7};
+ bool collisionNormalIsPlaneNormal = false;
+ doBoxDoubleSidedPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts, collisionNormalIsPlaneNormal);
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/BoxPlaneContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/BoxPlaneContactCalculationTests.cpp
new file mode 100644
index 0000000..22f6045
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/BoxPlaneContactCalculationTests.cpp
@@ -0,0 +1,215 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/Collision/BoxPlaneDcdContact.h"
+#include "SurgSim/Math/Geometry.h"
+
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::Geometry::DistanceEpsilon;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+void doBoxPlaneTest(std::shared_ptr<BoxShape> box,
+ const Quaterniond& boxQuat,
+ const Vector3d& boxTrans,
+ std::shared_ptr<PlaneShape> plane,
+ const Quaterniond& planeQuat,
+ const Vector3d& planeTrans,
+ const int expectedNumberOfContacts,
+ const int* expectedBoxIndicesInContacts)
+{
+ std::shared_ptr<ShapeCollisionRepresentation> boxRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Box 0");
+ boxRep->setShape(box);
+ boxRep->setLocalPose(SurgSim::Math::makeRigidTransform(boxQuat, boxTrans));
+
+ std::shared_ptr<ShapeCollisionRepresentation> planeRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Plane 0");
+ planeRep->setShape(plane);
+ planeRep->setLocalPose(SurgSim::Math::makeRigidTransform(planeQuat, planeTrans));
+
+ // First calculate the expected contact info.
+ std::list<std::shared_ptr<Contact>> expectedContacts;
+ if (expectedNumberOfContacts > 0)
+ {
+ generateBoxPlaneContact(&expectedContacts, expectedNumberOfContacts, expectedBoxIndicesInContacts,
+ box, boxTrans, boxQuat, plane, planeTrans, planeQuat);
+ }
+
+ // Perform collision detection.
+ BoxPlaneDcdContact calcContact;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(boxRep, planeRep);
+ calcContact.calculateContact(pair);
+
+ const Vector3d globalPlaneNormal = planeRep->getPose().linear() * plane->getNormal();
+ const Vector3d planeToBox = boxTrans - planeTrans;
+ Vector3d nearestPointOnPlane;
+ const double distanceBoxPlane = SurgSim::Math::distancePointPlane(planeToBox, globalPlaneNormal,
+ plane->getD(), &nearestPointOnPlane);
+
+ const Vector3d boxRadii = box->getSize() / 2.0;
+ const double minDepth = -distanceBoxPlane - boxRadii.norm();
+ const double maxDepth = -distanceBoxPlane + boxRadii.norm();
+
+ for (auto contact : pair->getContacts())
+ {
+ EXPECT_LT(-DistanceEpsilon, contact->depth);
+ EXPECT_LT(minDepth - DistanceEpsilon, contact->depth);
+ EXPECT_GT(maxDepth + DistanceEpsilon, contact->depth);
+ EXPECT_TRUE(eigenEqual(globalPlaneNormal, contact->normal));
+ }
+
+ // Compare the contact info.
+ contactsInfoEqualityTest(expectedContacts, pair->getContacts());
+}
+
+TEST(BoxPlaneContactCalculationTests, UnitTests)
+{
+ std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(1.0, 1.0, 1.0);
+ std::shared_ptr<PlaneShape> plane = std::make_shared<PlaneShape>();
+ SurgSim::Math::Quaterniond boxQuat;
+ SurgSim::Math::Vector3d boxTrans;
+ SurgSim::Math::Quaterniond planeQuat;
+ SurgSim::Math::Vector3d planeTrans;
+ SurgSim::Math::Quaterniond globalQuat;
+
+ {
+ SCOPED_TRACE("No intersection, box in front of rotated plane");
+ boxQuat = SurgSim::Math::makeRotationQuaternion(0.5674, Vector3d(0.4332,0.927, 0.13557).normalized());
+ boxTrans = Vector3d(3.4535,10.0,350.0);
+ planeQuat = SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = Vector3d::Zero();
+ int expectedNumberOfContacts = 0;
+ int expectedBoxIndicesInContacts[] = {0};
+ doBoxPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("Intersection in front of plane, four contacts, rotated plane");
+ boxQuat = SurgSim::Math::makeRotationQuaternion(1.233469, Vector3d(0.91834,0.39687,0.8271).normalized());
+ boxTrans = Vector3d(0.5,10.0,350.0);
+ planeQuat = boxQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + boxQuat * Vector3d(-0.5,0.0,0.0);
+ int expectedNumberOfContacts = 4;
+ int expectedBoxIndicesInContacts[] = {0, 1, 2, 3};
+ doBoxPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("Intersection in front of plane, two contacts, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(0.8753, Vector3d(0.235345,0.6754,0.4567).normalized());
+ boxQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_4, Vector3d(0.0,0.0,1.0));
+ boxTrans = Vector3d(std::sqrt(0.5),230.0,540.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(-std::sqrt(0.5),0.0,0.0);
+ int expectedNumberOfContacts = 2;
+ int expectedBoxIndicesInContacts[] = {0, 1};
+ doBoxPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("Intersection in front of plane, one contact, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(-0.3257, Vector3d(-0.4575,-0.8563,0.63457).normalized());
+ double angle = -35.264389682754654315377000330019*(M_PI/180.0);
+ boxQuat = globalQuat * Quaterniond(SurgSim::Math::makeRotationMatrix(angle, Vector3d(0.0,1.0,0.0)) *
+ SurgSim::Math::makeRotationMatrix(-M_PI_4, Vector3d(0.0,0.0,1.0)));
+ boxTrans = Vector3d(std::sqrt(0.75),0.0,0.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(-std::sqrt(0.75),0.0,0.0);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {1};
+ doBoxPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("Intersection inside of plane, one contact, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(0.3465, Vector3d(54.4575,76.8563,43.63457).normalized());
+ double angle = -35.264389682754654315377000330019*(M_PI/180.0);
+ boxQuat = globalQuat * Quaterniond(SurgSim::Math::makeRotationMatrix(angle, Vector3d(0.0,1.0,0.0)) *
+ SurgSim::Math::makeRotationMatrix(-M_PI_4, Vector3d(0.0,0.0,1.0)));
+ boxTrans = Vector3d(std::sqrt(0.73),0.0,0.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(-std::sqrt(0.75),0.0,0.0);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {1};
+ doBoxPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("Intersection in front of plane, two contacts, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(-0.8753, Vector3d(-1.235345,1.6754,1.4567).normalized());
+ boxQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_4, Vector3d(0.0,0.0,1.0));
+ boxTrans = Vector3d(std::sqrt(0.45),230.0,540.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(-std::sqrt(0.5),0.0,0.0);
+ int expectedNumberOfContacts = 2;
+ int expectedBoxIndicesInContacts[] = {0, 1};
+ doBoxPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("Intersection inside of plane, four contacts, rotated plane");
+ boxQuat = SurgSim::Math::makeRotationQuaternion(.99763, Vector3d(0.19834,0.93687,0.2871).normalized());
+ boxTrans = Vector3d(0.23,10.0,350.0);
+ planeQuat = boxQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + boxQuat * Vector3d(-0.5,0.0,0.0);
+ int expectedNumberOfContacts = 4;
+ int expectedBoxIndicesInContacts[] = {0, 1, 2, 3};
+ doBoxPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("Intersection inside of plane - case 1, eight contacts, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(-0.8753, Vector3d(-1.235345,1.6754,1.4567).normalized());
+ boxQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_4, Vector3d(0.0,0.0,1.0));
+ boxTrans = Vector3d(0.435,230.0,540.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(9.43523,0.0,0.0);
+ int expectedNumberOfContacts = 8;
+ int expectedBoxIndicesInContacts[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ doBoxPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("Intersection inside of plane - case 2, eight contacts, rotated plane");
+ globalQuat = SurgSim::Math::makeRotationQuaternion(1.4576, Vector3d(23.45,-98.24,42.46).normalized());
+ double angle = -35.264389682754654315377000330019*(M_PI/180.0);
+ boxQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(angle, Vector3d(0.0,0.0,1.0)) *
+ SurgSim::Math::makeRotationQuaternion(-M_PI_4, Vector3d(0.0,1.0,0.0));
+ boxTrans = Vector3d(0.34,0.0,0.0);
+ planeQuat = globalQuat * SurgSim::Math::makeRotationQuaternion(-M_PI_2, Vector3d(0.0,0.0,1.0));
+ planeTrans = boxTrans + globalQuat * Vector3d(5.345,0.0,0.0);
+ int expectedNumberOfContacts = 8;
+ int expectedBoxIndicesInContacts[] = {0, 1, 2, 3, 4, 5, 6, 7};
+ doBoxPlaneTest(box, boxQuat, boxTrans, plane, planeQuat, planeTrans, expectedNumberOfContacts,
+ expectedBoxIndicesInContacts);
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/BoxSphereContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/BoxSphereContactCalculationTests.cpp
new file mode 100644
index 0000000..bbbf512
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/BoxSphereContactCalculationTests.cpp
@@ -0,0 +1,328 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/Collision/BoxSphereDcdContact.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/SphereShape.h"
+
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::SphereShape;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+void doBoxSphereTest(std::shared_ptr<BoxShape> box,
+ const Quaterniond& boxQuat,
+ const Vector3d& boxTrans,
+ std::shared_ptr<SphereShape> sphere,
+ const Quaterniond& sphereQuat,
+ const Vector3d& sphereTrans,
+ bool hasContacts = false,
+ double expectedDepth = 0.0,
+ Vector3d expectedNormal = Vector3d::UnitX(),
+ Vector3d expectedPenetrationPoint0 = Vector3d::Zero(),
+ Vector3d expectedPenetrationPoint1 = Vector3d::Zero())
+{
+ using SurgSim::Math::Geometry::DistanceEpsilon;
+ using SurgSim::Math::Geometry::ScalarEpsilon;
+
+ std::shared_ptr<ShapeCollisionRepresentation> boxRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Box 0");
+ boxRep->setShape(box);
+ boxRep->setLocalPose(SurgSim::Math::makeRigidTransform(boxQuat, boxTrans));
+
+ std::shared_ptr<ShapeCollisionRepresentation> sphereRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Sphere 0");
+ sphereRep->setShape(sphere);
+ sphereRep->setLocalPose(SurgSim::Math::makeRigidTransform(sphereQuat, sphereTrans));
+
+ // Perform collision detection.
+ BoxSphereDcdContact calcContact;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(boxRep, sphereRep);
+ calcContact.calculateContact(pair);
+
+ // Compare contact info.
+ EXPECT_EQ(hasContacts, pair->hasContacts());
+ if (pair->hasContacts())
+ {
+ std::shared_ptr<Contact> contact = pair->getContacts().front();
+ EXPECT_TRUE(eigenEqual(expectedNormal, contact->normal));
+ EXPECT_NEAR(expectedDepth, contact->depth, DistanceEpsilon);
+ EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());
+
+ EXPECT_TRUE(eigenEqual(expectedPenetrationPoint0,
+ contact->penetrationPoints.first.rigidLocalPosition.getValue()));
+ EXPECT_TRUE(eigenEqual(expectedPenetrationPoint1,
+ contact->penetrationPoints.second.rigidLocalPosition.getValue()));
+ }
+}
+
+TEST(BoxSphereContactCalculationTests, UnitTests)
+{
+ std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(1.0, 1.0, 1.0);
+ std::shared_ptr<SphereShape> sphere = std::make_shared<SphereShape>(1.0);
+ SurgSim::Math::Quaterniond boxQuat;
+ SurgSim::Math::Vector3d boxTrans;
+ SurgSim::Math::Quaterniond sphereQuat;
+ SurgSim::Math::Vector3d sphereTrans;
+ SurgSim::Math::Quaterniond globalQuat;
+ SurgSim::Math::Vector3d globalTrans;
+
+ {
+ SCOPED_TRACE("No Intersection");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(100.0,0.0,0.0);
+ sphereQuat.setIdentity();
+ sphereTrans = Vector3d(0.0,0.0,0.0);
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, false);
+ }
+
+ {
+ SCOPED_TRACE("Intersection on top face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(0.0,0.0,0.0);
+ sphereQuat.setIdentity();
+ sphereTrans = Vector3d(0.0,1.0,0.0);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(0.35465, Vector3d(0.3454, 0.78567, 0.234346).normalized());
+ globalTrans = Vector3d(24.6,-32.67,87.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 0.5, // depth
+ boxQuat * Vector3d(0.0,-1.0,0.0), // normal points into first representation of CollisionPair
+ Vector3d(0.0,0.5,0.0), // box penetration point
+ Vector3d(0.0,-1.0,0.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Sphere center inside box, intersection on top face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(0.0,0.0,0.0);
+ sphereQuat.setIdentity();
+ sphereTrans = Vector3d(0.0,0.05,0.0);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(0.35465, Vector3d(0.3454, 0.78567, 0.234346).normalized());
+ globalTrans = Vector3d(24.6,-32.67,87.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 1.45, // depth
+ boxQuat * Vector3d(0.0,-1.0,0.0), // normal points into first representation of CollisionPair
+ Vector3d(0.0,0.5,0.0), // box penetration point
+ Vector3d(0.0,-1.0,0.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Intersection on bottom face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(0.0,0.0,0.0);
+ sphereQuat.setIdentity();
+ sphereTrans = Vector3d(0.3345,-1.2,0.1234);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(-0.35465,
+ Vector3d(18.3454, -27.78567, 23.234346).normalized());
+ globalTrans = Vector3d(234.6,326.67,987.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 0.3, // depth
+ boxQuat * Vector3d(0.0,1.0,0.0), // normal points into first representation of CollisionPair
+ Vector3d(0.3345,-0.5,0.1234), // box penetration point
+ Vector3d(0.0,1.0,0.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Sphere center inside box, intersection on bottom face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(0.0,0.0,0.0);
+ sphereQuat.setIdentity();
+ sphereTrans = Vector3d(0.3345,-0.4,0.1234);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(-0.35465,
+ Vector3d(18.3454, -27.78567, 23.234346).normalized());
+ globalTrans = Vector3d(234.6,326.67,987.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 1.1, // depth
+ boxQuat * Vector3d(0.0,1.0,0.0), // normal points into first representation of CollisionPair
+ Vector3d(0.3345,-0.5,0.1234), // box penetration point
+ Vector3d(0.0,1.0,0.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Intersection on right face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(23.545,3.4321,5.3421);
+ sphereQuat.setIdentity();
+ sphereTrans = boxTrans + Vector3d(1.2324,-0.2354,0.412);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(1.285, Vector3d(23.446, 13.786, 32.254).normalized());
+ globalTrans = Vector3d(-249.6,532.67,977.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 0.2676, // depth
+ boxQuat * Vector3d(-1.0,0.0,0.0), // normal points into first representation of CollisionPair
+ Vector3d(0.5,-0.2354,0.412), // box penetration point
+ Vector3d(-1.0,0.0,0.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Sphere center inside box, intersection on right face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(23.545,3.4321,5.3421);
+ sphereQuat.setIdentity();
+ sphereTrans = boxTrans + Vector3d(0.45,-0.2354,0.412);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(1.285, Vector3d(23.446, 13.786, 32.254).normalized());
+ globalTrans = Vector3d(-249.6,532.67,977.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 1.05, // depth
+ boxQuat * Vector3d(-1.0,0.0,0.0), // normal points into first representation of CollisionPair
+ Vector3d(0.5,-0.2354,0.412), // box penetration point
+ Vector3d(-1.0,0.0,0.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Intersection on left face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(876.324,6754.23,7343.76);
+ sphereQuat.setIdentity();
+ sphereTrans = boxTrans + Vector3d(-1.1223,0.2354,-0.412);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(0.276, Vector3d(0.945, 1.532, 0.896).normalized());
+ globalTrans = Vector3d(-24.6,32.67,97.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 0.3777, // depth
+ boxQuat * Vector3d(1.0,0.0,0.0), // normal points into first representation of CollisionPair
+ Vector3d(-0.5,0.2354,-0.412), // box penetration point
+ Vector3d(1.0,0.0,0.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Sphere center inside box, intersection on left face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(876.324,6754.23,7343.76);
+ sphereQuat.setIdentity();
+ sphereTrans = boxTrans + Vector3d(-0.3,0.2354,-0.012);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(0.276, Vector3d(0.945, 1.532, 0.896).normalized());
+ globalTrans = Vector3d(-24.6,32.67,97.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 1.2, // depth
+ boxQuat * Vector3d(1.0,0.0,0.0), // normal points into first representation of CollisionPair
+ Vector3d(-0.5,0.2354,-0.012), // box penetration point
+ Vector3d(1.0,0.0,0.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Intersection on front face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(0.3252,-0.64564,0.12345);
+ sphereQuat.setIdentity();
+ sphereTrans = boxTrans + Vector3d(0.1564,-0.2987,-0.8986);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(-1.32, Vector3d(235.67, 215.567, 146.345).normalized());
+ globalTrans = Vector3d(224.6,132.67,27.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 0.6014, // depth
+ boxQuat * Vector3d(0.0,0.0,1.0), // normal points into first representation of CollisionPair
+ Vector3d(0.1564,-0.2987,-0.5), // box penetration point
+ Vector3d(0.0,0.0,1.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Sphere center inside box, intersection on front face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(0.3252,-0.64564,0.12345);
+ sphereQuat.setIdentity();
+ sphereTrans = boxTrans + Vector3d(0.1564,-0.2987,-0.3986);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(-1.32, Vector3d(235.67, 215.567, 146.345).normalized());
+ globalTrans = Vector3d(224.6,132.67,27.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 1.1014, // depth
+ boxQuat * Vector3d(0.0,0.0,1.0), // normal points into first representation of CollisionPair
+ Vector3d(0.1564,-0.2987,-0.5), // box penetration point
+ Vector3d(0.0,0.0,1.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Intersection on back face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(24.345,-865.325,46.345);
+ sphereQuat.setIdentity();
+ sphereTrans = boxTrans + Vector3d(-0.2564,-0.4987,0.7986);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(1.2, Vector3d(25.67, -25.567, 16.345).normalized());
+ globalTrans = Vector3d(24.6,3243.67,9762.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 0.7014, // depth
+ boxQuat * Vector3d(0.0,0.0,-1.0), // normal points into first representation of CollisionPair
+ Vector3d(-0.2564,-0.4987,0.5), // box penetration point
+ Vector3d(0.0,0.0,-1.0)); // sphere penetration point
+ }
+
+ {
+ SCOPED_TRACE("Sphere center inside box, intersection on back face");
+ boxQuat.setIdentity();
+ boxTrans = Vector3d(24.345,-865.325,46.345);
+ sphereQuat.setIdentity();
+ sphereTrans = boxTrans + Vector3d(-0.2564,-0.3987,0.48);
+ globalQuat = SurgSim::Math::makeRotationQuaternion(1.2, Vector3d(25.67, -25.567, 16.345).normalized());
+ globalTrans = Vector3d(24.6,3243.67,9762.53);
+ boxQuat = globalQuat * boxQuat;
+ boxTrans = globalQuat * boxTrans + globalTrans;
+ sphereQuat = globalQuat * sphereQuat;
+ sphereTrans = globalQuat * sphereTrans + globalTrans;
+ doBoxSphereTest(box, boxQuat, boxTrans, sphere, sphereQuat, sphereTrans, true,
+ 1.02, // depth
+ boxQuat * Vector3d(0.0,0.0,-1.0), // normal points into first representation of CollisionPair
+ Vector3d(-0.2564,-0.3987,0.5), // box penetration point
+ Vector3d(0.0,0.0,-1.0)); // sphere penetration point
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/CMakeLists.txt b/SurgSim/Collision/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..715a530
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/CMakeLists.txt
@@ -0,0 +1,65 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories (
+ ${gtest_SOURCE_DIR}/include
+ ${gmock_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ BoxCapsuleContactCalculationTests.cpp
+ BoxDoubleSidedPlaneContactCalculationTests.cpp
+ BoxPlaneContactCalculationTests.cpp
+ BoxSphereContactCalculationTests.cpp
+ CapsuleSphereContactCalculationTests.cpp
+ CollisionPairTests.cpp
+ ContactCalculationTests.cpp
+ ContactCalculationTestsCommon.cpp
+ DefaultContactCalculationTests.cpp
+ OctreeContactCalculationTests.cpp
+ RepresentationTest.cpp
+ RepresentationUtilities.cpp
+ ShapeCollisionRepresentationTest.cpp
+ SphereDoubleSidedPlaneContactCalculationTests.cpp
+ SpherePlaneContactCalculationTests.cpp
+ SphereSphereContactCalculationTests.cpp
+ TriangleMeshPlaneContactCalculationTests.cpp
+ TriangleMeshTriangleMeshContactCalculationTests.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ ContactCalculationTestsCommon.h
+ RepresentationUtilities.h
+)
+
+set(LIBS
+ SurgSimCollision
+ SurgSimDataStructures
+ SurgSimMath
+ SurgSimPhysics
+ SurgSimInput
+)
+
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/MeshShapeData DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Data)
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+surgsim_add_unit_tests(SurgSimCollisionTest)
+
+set_target_properties(SurgSimCollisionTest PROPERTIES FOLDER "Collision")
diff --git a/SurgSim/Collision/UnitTests/CapsuleSphereContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/CapsuleSphereContactCalculationTests.cpp
new file mode 100644
index 0000000..704a180
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/CapsuleSphereContactCalculationTests.cpp
@@ -0,0 +1,102 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/Collision/CapsuleSphereDcdContact.h"
+#include "SurgSim/Math/CapsuleShape.h"
+#include "SurgSim/Math/SphereShape.h"
+
+using SurgSim::Math::CapsuleShape;
+using SurgSim::Math::SphereShape;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+void doCapsuleSphereTest(double capsuleHeight, double capsuleRadius,
+ const Vector3d& capsulePosition, const Quaterniond& capsuleQuat,
+ double sphereRadius, const Vector3d& spherePosition, const Quaterniond& sphereQuat,
+ bool hasContacts, double depth,
+ const Vector3d& sphereProjection = Vector3d::Zero(),
+ const Vector3d& expectedNorm = Vector3d::Zero())
+{
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(
+ makeCapsuleRepresentation(capsuleHeight, capsuleRadius, capsuleQuat, capsulePosition),
+ makeSphereRepresentation(sphereRadius, sphereQuat, spherePosition));
+
+ CapsuleSphereDcdContact calc;
+ calc.calculateContact(pair);
+ EXPECT_EQ(hasContacts, pair->hasContacts());
+
+ if (pair->hasContacts())
+ {
+ std::shared_ptr<Contact> contact(pair->getContacts().front());
+
+ EXPECT_TRUE(eigenEqual(expectedNorm, contact->normal));
+ EXPECT_NEAR(depth, contact->depth, SurgSim::Math::Geometry::DistanceEpsilon);
+ EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());
+
+ Vector3d capsuleLocalNormal = capsuleQuat.inverse() * expectedNorm;
+ Vector3d penetrationPoint0(sphereProjection - capsuleLocalNormal * capsuleRadius);
+ Vector3d sphereLocalNormal = sphereQuat.inverse() * expectedNorm;
+ Vector3d penetrationPoint1(sphereLocalNormal * sphereRadius);
+ EXPECT_TRUE(eigenEqual(penetrationPoint0, contact->penetrationPoints.first.rigidLocalPosition.getValue()));
+ EXPECT_TRUE(eigenEqual(penetrationPoint1, contact->penetrationPoints.second.rigidLocalPosition.getValue()));
+ }
+}
+
+TEST(CapsuleSphereContactCalculationTests, UnitTests)
+{
+ {
+ SCOPED_TRACE("No Intersection");
+ doCapsuleSphereTest(0.2, 0.1, Vector3d::Zero(), Quaterniond::Identity(),
+ 0.1, Vector3d(1.0, 1.0, 1.0), Quaterniond::Identity(), false, 0.0);
+ }
+
+ {
+ SCOPED_TRACE("Capsule along Y-axis, intersection with cylindrical part of the capsule");
+ doCapsuleSphereTest(0.8, 0.5, Vector3d::Zero(), Quaterniond::Identity(),
+ 0.3, Vector3d(0.7, 0, 0), Quaterniond::Identity(), true, 0.1,
+ Vector3d::Zero(), Vector3d(-1.0, 0.0, 0.0));
+ }
+
+ {
+ SCOPED_TRACE("Capsule along X-axis, intersection with hemispherical part of the capsule");
+ doCapsuleSphereTest(0.1, 0.2, Vector3d::Zero(),
+ SurgSim::Math::makeRotationQuaternion(M_PI_2, Vector3d(0.0, 0.0, 1.0)),
+ 0.1, Vector3d(-0.2, 0.0, 0.0),
+ Quaterniond::Identity(), true, 0.15,
+ Vector3d(0.0, 0.05, 0.0), Vector3d(1.0, 0.0, 0.0));
+ }
+
+ {
+ SCOPED_TRACE("Intersection, capsule roated along Z-axis clockwise 45 degrees");
+ Vector3d sphereCenter(2.0, 0.0, 0.0);
+ Vector3d sphereProjection(1.0, 1.0, 0.0);
+ Vector3d expectedNormal = (sphereProjection - sphereCenter).normalized();
+
+ doCapsuleSphereTest(2 * M_SQRT2, M_SQRT2, Vector3d::Zero(),
+ SurgSim::Math::makeRotationQuaternion(-M_PI_4, Vector3d(0.0, 0.0, 1.0)),
+ 1.0, sphereCenter,
+ Quaterniond::Identity(), true, 1,
+ Vector3d(0.0, M_SQRT2, 0.0), expectedNormal);
+
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/CollisionPairTests.cpp b/SurgSim/Collision/UnitTests/CollisionPairTests.cpp
new file mode 100644
index 0000000..8526be0
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/CollisionPairTests.cpp
@@ -0,0 +1,159 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+#include "SurgSim/Collision/UnitTests/RepresentationUtilities.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+#include "SurgSim/Physics/RigidRepresentationState.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/DataStructures/BufferedValue.h"
+
+using SurgSim::Collision::ContactMapType;
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+TEST(CollisionPairTests, InitTest)
+{
+ // Default Constructor, needs to work for ReuseFrepresentationy
+ EXPECT_NO_THROW({CollisionPair pair;});
+
+ std::shared_ptr<Representation> rep0 = makeSphereRepresentation(1.0);
+ std::shared_ptr<Representation> rep1 = makeSphereRepresentation(2.0);
+
+ EXPECT_ANY_THROW({CollisionPair pair(rep0, rep0);});
+ EXPECT_ANY_THROW({CollisionPair pair(nullptr, rep0);});
+ EXPECT_ANY_THROW({CollisionPair pair(nullptr, nullptr);});
+ EXPECT_ANY_THROW({CollisionPair pair(rep0, nullptr);});
+
+ ASSERT_NO_THROW({CollisionPair pair(rep0, rep1);});
+ CollisionPair pair(rep0,rep1);
+
+ EXPECT_EQ(rep0, pair.getFirst());
+ EXPECT_EQ(rep1, pair.getSecond());
+ EXPECT_FALSE(pair.hasContacts());
+ EXPECT_FALSE(pair.isSwapped());
+
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(Vector3d(0.1, 0.2, 0.3));
+ penetrationPoints.second.rigidLocalPosition.setValue(Vector3d(0.4, 0.5, 0.6));
+ pair.addContact(1.0, Vector3d(1.0,0.0,0.0),penetrationPoints);
+ EXPECT_TRUE(pair.hasContacts());
+}
+
+TEST(CollisionPairTests, SwapTest)
+{
+ std::shared_ptr<Representation> rep0 = makeSphereRepresentation(1.0);
+ std::shared_ptr<Representation> rep1 = makeSphereRepresentation(2.0);
+
+ CollisionPair pair(rep0,rep1);
+ EXPECT_FALSE(pair.isSwapped());
+ EXPECT_EQ(rep0.get(),pair.getRepresentations().first.get());
+ EXPECT_EQ(rep1.get(),pair.getRepresentations().second.get());
+ pair.swapRepresentations();
+ EXPECT_TRUE(pair.isSwapped());
+ pair.swapRepresentations();
+ EXPECT_FALSE(pair.isSwapped());
+
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(Vector3d(0.1, 0.2, 0.3));
+ penetrationPoints.second.rigidLocalPosition.setValue(Vector3d(0.4, 0.5, 0.6));
+
+ pair.addContact(1.0, Vector3d(1.0,0.0,0.0),penetrationPoints);
+ EXPECT_TRUE(pair.hasContacts());
+
+ EXPECT_ANY_THROW(pair.swapRepresentations());
+}
+
+TEST(CollisionPairTests, setRepresentationsTest)
+{
+ std::shared_ptr<Representation> rep0 = makeSphereRepresentation(1.0);
+ std::shared_ptr<Representation> rep1 = makeSphereRepresentation(2.0);
+ std::shared_ptr<Representation> repA = makeSphereRepresentation(99.0);
+ std::shared_ptr<Representation> repB = makeSphereRepresentation(100.0);
+
+ CollisionPair pair(repA,repB);
+ EXPECT_FALSE(pair.isSwapped());
+ pair.swapRepresentations();
+
+ pair.setRepresentations(rep0,rep1);
+
+ EXPECT_EQ(rep0.get(), pair.getRepresentations().first.get());
+ EXPECT_EQ(rep1.get(), pair.getRepresentations().second.get());
+ EXPECT_FALSE(pair.isSwapped());
+}
+
+TEST(CollisionPairTests, addContactTest)
+{
+ std::shared_ptr<Representation> rep0 = makeSphereRepresentation(1.0);
+ std::shared_ptr<Representation> rep1 = makeSphereRepresentation(2.0);
+
+ ContactMapType& rep0Collisions = rep0->getCollisions().unsafeGet();
+ ContactMapType& rep1Collisions = rep1->getCollisions().unsafeGet();
+
+ EXPECT_TRUE(rep0Collisions.empty());
+ EXPECT_TRUE(rep1Collisions.empty());
+
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(Vector3d(0.1, 0.2, 0.3));
+ penetrationPoints.second.rigidLocalPosition.setValue(Vector3d(0.4, 0.5, 0.6));
+
+ CollisionPair pair(rep0, rep1);
+ pair.addContact(1.0, Vector3d::UnitY(), penetrationPoints);
+
+ rep0->update(0.0);
+ rep1->update(0.0);
+
+ EXPECT_EQ(1u, rep0Collisions.size());
+ auto rep0CollisionContacts = rep0Collisions.find(rep1);
+ EXPECT_NE(rep0Collisions.end(), rep0CollisionContacts);
+ EXPECT_EQ(rep1, rep0CollisionContacts->first);
+ std::shared_ptr<SurgSim::Collision::Contact> rep0FirstContact = rep0CollisionContacts->second.front();
+ EXPECT_EQ(rep0FirstContact->depth, 1.0);
+ EXPECT_TRUE(rep0FirstContact->normal.isApprox(Vector3d::UnitY()));
+ EXPECT_TRUE(rep0FirstContact->penetrationPoints.first.rigidLocalPosition.getValue().isApprox(
+ Vector3d(0.1, 0.2, 0.3)));
+ EXPECT_TRUE(rep0FirstContact->penetrationPoints.second.rigidLocalPosition.getValue().isApprox(
+ Vector3d(0.4, 0.5, 0.6)));
+
+ EXPECT_EQ(1u, rep1Collisions.size());
+ auto rep1CollisionContacts = rep1Collisions.find(rep0);
+ EXPECT_NE(rep1Collisions.end(), rep1CollisionContacts);
+ EXPECT_EQ(rep0, rep1CollisionContacts->first);
+ std::shared_ptr<SurgSim::Collision::Contact> rep1FirstContact = rep1CollisionContacts->second.front();
+ EXPECT_EQ(rep1FirstContact->depth, 1.0);
+ EXPECT_TRUE(rep1FirstContact->normal.isApprox(-Vector3d::UnitY()));
+ EXPECT_TRUE(rep1FirstContact->penetrationPoints.first.rigidLocalPosition.getValue().isApprox(
+ Vector3d(0.4, 0.5, 0.6)));
+ EXPECT_TRUE(rep1FirstContact->penetrationPoints.second.rigidLocalPosition.getValue().isApprox(
+ Vector3d(0.1, 0.2, 0.3)));
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/ContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/ContactCalculationTests.cpp
new file mode 100644
index 0000000..0b80960
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/ContactCalculationTests.cpp
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Collision/SpherePlaneDcdContact.h"
+
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/PlaneShape.h"
+
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::SphereShape;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+// This uses a concrete contact calculation as a placeholder, the current algorithm
+// has the check for the pair in the superclass, therefore a test on one class should
+// be sufficient
+TEST(ContactCalculationTests, SwappedPairTest)
+{
+ std::shared_ptr<PlaneShape> plane = std::make_shared<PlaneShape>();
+ std::shared_ptr<SphereShape> sphere = std::make_shared<SphereShape>(1.0);
+
+ Vector3d trans(0.0,0.0,0.0);
+ Quaterniond quat = Quaterniond::Identity();
+
+ std::shared_ptr<ShapeCollisionRepresentation> planeRep =
+ std::make_shared<ShapeCollisionRepresentation>("Plane Shape");
+ planeRep->setShape(plane);
+ planeRep->setLocalPose(SurgSim::Math::makeRigidTransform(quat, trans));
+
+ std::shared_ptr<ShapeCollisionRepresentation> sphereRep =
+ std::make_shared<ShapeCollisionRepresentation>("Sphere Shape");
+ sphereRep->setShape(sphere);
+ sphereRep->setLocalPose(SurgSim::Math::makeRigidTransform(quat, trans));
+
+ std::shared_ptr<CollisionPair> pair1 = std::make_shared<CollisionPair>(sphereRep, planeRep);
+ std::shared_ptr<CollisionPair> pair2 = std::make_shared<CollisionPair>(planeRep, sphereRep);
+
+ std::shared_ptr<SpherePlaneDcdContact> calc = std::make_shared<SpherePlaneDcdContact>();
+
+ EXPECT_NO_THROW(calc->calculateContact(pair1));
+ EXPECT_NO_THROW(calc->calculateContact(pair2));
+
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.cpp b/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.cpp
new file mode 100644
index 0000000..bde50f1
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.cpp
@@ -0,0 +1,205 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+::testing::AssertionResult eigenEqual(const Vector3d& left, const Vector3d& right)
+{
+ if (std::abs((left - right).norm()) < SurgSim::Math::Geometry::DistanceEpsilon)
+ {
+ return ::testing::AssertionSuccess();
+ }
+ else
+ {
+ return ::testing::AssertionFailure() << std::endl << "Vectors not close, expected: " << left.transpose() <<
+ std::endl << " result: " << right.transpose() << std::endl;
+ }
+}
+
+void checkContactInfo(std::shared_ptr<Contact> contact, double expectedDepth,
+ Vector3d &expectedNormal, Vector3d &expectedPenetrationPointFirst,
+ Vector3d &expectedPenetrationPointSecond)
+{
+ EXPECT_NEAR(expectedDepth, contact->depth, SurgSim::Math::Geometry::DistanceEpsilon);
+ EXPECT_TRUE(eigenEqual(expectedNormal, contact->normal));
+ EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(eigenEqual(expectedPenetrationPointFirst,
+ contact->penetrationPoints.first.rigidLocalPosition.getValue()));
+ EXPECT_TRUE(eigenEqual(expectedPenetrationPointSecond,
+ contact->penetrationPoints.second.rigidLocalPosition.getValue()));
+}
+
+bool checkMeshLocalCoordinate(
+ SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate>& actualLocalCoordinate,
+ const std::array<SurgSim::Math::Vector3d, 3>& vertices,
+ SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate>& expectedLocalCoordinate,
+ const SurgSim::Math::Vector3d& expectedLocalPosition)
+{
+ bool isEqual = true;
+ EXPECT_EQ(expectedLocalCoordinate.hasValue(), actualLocalCoordinate.hasValue());
+ if (expectedLocalCoordinate.hasValue() && actualLocalCoordinate.hasValue())
+ {
+ isEqual &=
+ expectedLocalCoordinate.getValue().index == actualLocalCoordinate.getValue().index;
+ Vector3d barycentricCoordinates = actualLocalCoordinate.getValue().coordinate;
+ isEqual &= eigenEqual(expectedLocalPosition,
+ barycentricCoordinates[0] * vertices[0] +
+ barycentricCoordinates[1] * vertices[1] +
+ barycentricCoordinates[2] * vertices[2]);
+ }
+ return isEqual;
+}
+
+::testing::AssertionResult isContactPresentInList(std::shared_ptr<Contact> expected,
+ const std::list<std::shared_ptr<Contact>>& contactsList,
+ bool expectedHasTriangleContactObject)
+{
+ using SurgSim::Math::Geometry::ScalarEpsilon;
+
+ bool contactPresent = false;
+ for (auto it = contactsList.begin(); it != contactsList.end() && !contactPresent; ++it)
+ {
+ // Compare the normals.
+ contactPresent = eigenEqual(expected->normal, it->get()->normal);
+ // Compare the global position of first object.
+ contactPresent &= eigenEqual(expected->penetrationPoints.first.rigidLocalPosition.getValue(),
+ it->get()->penetrationPoints.first.rigidLocalPosition.getValue());
+ // Compare the global position of second object.
+ contactPresent &= eigenEqual(expected->penetrationPoints.second.rigidLocalPosition.getValue(),
+ it->get()->penetrationPoints.second.rigidLocalPosition.getValue());
+ // Compare the depth.
+ contactPresent &= std::abs(expected->depth - it->get()->depth) <= ScalarEpsilon;
+ // Check if the optional 'meshLocalCoordinate' are the same.
+ std::shared_ptr<SurgSim::Collision::TriangleContact> triangleContact;
+ std::shared_ptr<SurgSim::Collision::Contact> contact;
+ if (expectedHasTriangleContactObject)
+ {
+ triangleContact = std::static_pointer_cast<SurgSim::Collision::TriangleContact>(expected);
+ contact = *it;
+ }
+ else
+ {
+ triangleContact = std::static_pointer_cast<SurgSim::Collision::TriangleContact>(*it);
+ contact = expected;
+ }
+ contactPresent &= checkMeshLocalCoordinate(
+ contact->penetrationPoints.first.meshLocalCoordinate,
+ triangleContact->firstVertices,
+ triangleContact->penetrationPoints.first.meshLocalCoordinate,
+ expected->penetrationPoints.first.rigidLocalPosition.getValue());
+ contactPresent &= checkMeshLocalCoordinate(
+ contact->penetrationPoints.second.meshLocalCoordinate,
+ triangleContact->secondVertices,
+ triangleContact->penetrationPoints.second.meshLocalCoordinate,
+ expected->penetrationPoints.second.rigidLocalPosition.getValue());
+ }
+
+ if (contactPresent)
+ {
+ return ::testing::AssertionSuccess();
+ }
+ else
+ {
+ return ::testing::AssertionFailure() << "Expected contact not found in calculated contacts list:\n" <<
+ "Normal: " << expected->normal << "\n" <<
+ "First objects' contact point: " << expected->penetrationPoints.first.rigidLocalPosition.getValue()
+ << "\n" <<
+ "Second objects' contact point: " << expected->penetrationPoints.second.rigidLocalPosition.getValue()
+ << "\n" <<
+ "Depth of penetration: " << expected->depth << "\n";
+ }
+}
+
+void contactsInfoEqualityTest(const std::list<std::shared_ptr<Contact>>& expectedContacts,
+ const std::list<std::shared_ptr<Contact>>& calculatedContacts,
+ bool expectedHasTriangleContactObject)
+{
+ SCOPED_TRACE("Comparing the contact info.");
+
+ EXPECT_EQ(expectedContacts.size(), calculatedContacts.size());
+
+ for (auto it = expectedContacts.begin(); it != expectedContacts.end(); ++it)
+ {
+ EXPECT_TRUE(isContactPresentInList(*it, calculatedContacts, expectedHasTriangleContactObject));
+ }
+}
+
+void generateBoxPlaneContact(std::list<std::shared_ptr<Contact>>* expectedContacts,
+ const int expectedNumberOfContacts,
+ const int* expectedBoxIndicesInContacts,
+ const std::shared_ptr<BoxShape> box,
+ const Vector3d& boxTrans, const Quaterniond& boxQuat,
+ const std::shared_ptr<PlaneShape> plane,
+ const Vector3d& planeTrans, const Quaterniond& planeQuat)
+{
+ Vector3d vertex;
+ Vector3d boxLocalVertex, planeLocalVertex;
+ Vector3d planeNormalGlobal = planeQuat * plane->getNormal();
+ Vector3d pointOnPlane = planeTrans + (planeNormalGlobal * plane->getD());
+ double depth = 0.0;
+ Vector3d collisionNormal = planeNormalGlobal;
+ RigidTransform3d boxTransform = SurgSim::Math::makeRigidTransform(boxQuat, boxTrans);
+ for (int i = 0; i < expectedNumberOfContacts; ++i)
+ {
+ boxLocalVertex = box->getVertex(expectedBoxIndicesInContacts[i]);
+ vertex = boxTransform * boxLocalVertex;
+ depth = -planeNormalGlobal.dot(vertex - pointOnPlane);
+ planeLocalVertex = planeQuat.inverse() * (vertex + planeNormalGlobal * depth - planeTrans);
+ std::pair<Location, Location> penetrationPoint;
+ penetrationPoint.first.rigidLocalPosition.setValue(boxLocalVertex);
+ penetrationPoint.second.rigidLocalPosition.setValue(planeLocalVertex);
+ expectedContacts->push_back(std::make_shared<Contact>(depth, Vector3d::Zero(),
+ collisionNormal, penetrationPoint));
+ }
+}
+
+void generateBoxDoubleSidedPlaneContact(std::list<std::shared_ptr<Contact>>* expectedContacts,
+ const int expectedNumberOfContacts,
+ const int* expectedBoxIndicesInContacts,
+ const std::shared_ptr<BoxShape> box,
+ const Vector3d& boxTrans, const Quaterniond& boxQuat,
+ const std::shared_ptr<DoubleSidedPlaneShape> plane,
+ const Vector3d& planeTrans, const Quaterniond& planeQuat,
+ const bool collisionNormalIsPlaneNormal)
+{
+ Vector3d vertex;
+ Vector3d boxLocalVertex, planeLocalVertex;
+ Vector3d planeNormalGlobal = planeQuat * plane->getNormal();
+ Vector3d pointOnPlane = planeTrans + (planeNormalGlobal * plane->getD());
+ double depth = 0.0;
+ Vector3d collisionNormal = planeNormalGlobal * (collisionNormalIsPlaneNormal ? 1.0 : -1.0);
+ RigidTransform3d boxTransform = SurgSim::Math::makeRigidTransform(boxQuat, boxTrans);
+ for (int i = 0; i < expectedNumberOfContacts; ++i)
+ {
+ boxLocalVertex = box->getVertex(expectedBoxIndicesInContacts[i]);
+ vertex = boxTransform * boxLocalVertex;
+ depth = planeNormalGlobal.dot(vertex - pointOnPlane);
+ planeLocalVertex = planeQuat.inverse() * (vertex - planeNormalGlobal * depth - planeTrans);
+ std::pair<Location, Location> penetrationPoint;
+ penetrationPoint.first.rigidLocalPosition.setValue(boxLocalVertex);
+ penetrationPoint.second.rigidLocalPosition.setValue(planeLocalVertex);
+ expectedContacts->push_back(std::make_shared<Contact>(std::abs(depth), Vector3d::Zero(),
+ collisionNormal, penetrationPoint));
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h b/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h
new file mode 100644
index 0000000..cdfc782
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h
@@ -0,0 +1,151 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_UNITTESTS_CONTACTCALCULATIONTESTSCOMMON_H
+#define SURGSIM_COLLISION_UNITTESTS_CONTACTCALCULATIONTESTSCOMMON_H
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "SurgSim/Collision/UnitTests/RepresentationUtilities.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/MeshShape.h"
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+
+#include "SurgSim/Math/Geometry.h"
+
+using SurgSim::DataStructures::Location;
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::MeshShape;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+/// Struct to store the triangle vertices along with the Contact struct.
+struct TriangleContact : public Contact
+{
+ TriangleContact(const double& newDepth,
+ const SurgSim::Math::Vector3d& newContact,
+ const SurgSim::Math::Vector3d& newNormal,
+ const std::pair<Location, Location>& newPenetrationPoints)
+ : Contact(newDepth, newContact, newNormal, newPenetrationPoints)
+ {
+ }
+
+ std::array<SurgSim::Math::Vector3d, 3> firstVertices;
+ std::array<SurgSim::Math::Vector3d, 3> secondVertices;
+};
+
+/// Function that compares two 3d vectors and asserts that they are equal.
+/// The tolerance for the numerical values is SurgSim::Math::Geometry::DistanceEpsilon.
+/// \param left First vector.
+/// \param right Second vector.
+::testing::AssertionResult eigenEqual(const Vector3d& left, const Vector3d& right);
+
+/// Function that checks if a given contact contains the given normal, depth and
+/// penetration points.
+/// \param contact The contact object that is being checked.
+/// \param expectedDepth The expected depth.
+/// \param expectedNormal The expected normal.
+/// \param expectedPenetrationPointFirst The expected first penetration point.
+/// \param expectedPenetrationPointSecond The expected second penetration point.
+void checkContactInfo(std::shared_ptr<Contact> contact, double expectedDepth,
+ Vector3d &expectedNormal, Vector3d &expectedPenetrationPointFirst,
+ Vector3d &expectedPenetrationPointSecond);
+
+/// Function that checks if a given contact is present in the list of given contacts.
+/// \param expected The expected contact.
+/// \param contactsList The list of contacts.
+/// \param expectedHasTriangleContactObject True, if the expected pointer points to a TriangleContact object.
+/// False, if contactsList points to TriangleContact objects.
+::testing::AssertionResult isContactPresentInList(std::shared_ptr<Contact> expected,
+ const std::list<std::shared_ptr<Contact>>& contactsList,
+ bool expectedHasTriangleContactObject = false);
+
+/// Function that checks if two given list of contacts are the same.
+/// \param expectedContacts The expected contact lists.
+/// \param calculatedContacts The list of given contact list.
+/// \param expectedHasTriangleContactObject True, if the expectedContacts points to TriangleContact objects.
+/// False, if calculatedContacts points to TriangleContact objects.
+void contactsInfoEqualityTest(const std::list<std::shared_ptr<Contact>>& expectedContacts,
+ const std::list<std::shared_ptr<Contact>>& calculatedContacts,
+ bool expectedHasTriangleContactObject = false);
+
+/// Function that generates (no collision detection performed) the contact information between a box and a plane,
+/// given the box vertex indices that are known to be in contact.
+/// \param expectedContacts The list where the generated contacts are added.
+/// \param expectedNumberOfContacts The number of contacts to be generated.
+/// \param expectedBoxIndicesInContacts The vertex indices of the box which are in collision with the plane.
+/// \param box The box shape.
+/// \param boxTrans The box translation in global co-ordinate system.
+/// \param boxQuat The box orientation in global co-ordinate system.
+/// \param plane The plane shape.
+/// \param planeTrans The plane translation in global co-ordinate system.
+/// \param planeQuat The plane orientation in global co-ordinate system.
+void generateBoxPlaneContact(std::list<std::shared_ptr<Contact>>* expectedContacts,
+ const int expectedNumberOfContacts,
+ const int* expectedBoxIndicesInContacts,
+ const std::shared_ptr<BoxShape> box,
+ const Vector3d& boxTrans, const Quaterniond& boxQuat,
+ const std::shared_ptr<PlaneShape> plane,
+ const Vector3d& planeTrans, const Quaterniond& planeQuat);
+
+/// Function that generates (no collision detection performed) the contact information between a box and
+/// a double-sided plane, given the box vertex indices that are known to be in contact.
+/// \param expectedContacts The list where the generated contacts are added.
+/// \param expectedNumberOfContacts The number of contacts to be generated.
+/// \param expectedBoxIndicesInContacts The vertex indices of the box which are in collision
+/// with the double-sided plane.
+/// \param box The box shape.
+/// \param boxTrans The box translation in global co-ordinate system.
+/// \param boxQuat The box orientation in global co-ordinate system.
+/// \param plane The double-sided plane shape.
+/// \param planeTrans The double-sided plane translation in global co-ordinate system.
+/// \param planeQuat The double-sided plane orientation in global co-ordinate system.
+/// \param collisionNormalIsPlaneNormal Flag to represent if the box is in collision
+void generateBoxDoubleSidedPlaneContact(std::list<std::shared_ptr<Contact>>* expectedContacts,
+ const int expectedNumberOfContacts,
+ const int* expectedBoxIndicesInContacts,
+ const std::shared_ptr<BoxShape> box,
+ const Vector3d& boxTrans, const Quaterniond& boxQuat,
+ const std::shared_ptr<DoubleSidedPlaneShape> plane,
+ const Vector3d& planeTrans, const Quaterniond& planeQuat,
+ const bool collisionNormalIsPlaneNormal);
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/UnitTests/DefaultContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/DefaultContactCalculationTests.cpp
new file mode 100644
index 0000000..97397c2
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/DefaultContactCalculationTests.cpp
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/Collision/DefaultContactCalculation.h"
+#include "SurgSim/Math/SphereShape.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+TEST(DefaultContactCalculationTests, UnitTests)
+{
+ std::shared_ptr<SurgSim::Math::Shape> sphereShape = std::make_shared<SurgSim::Math::SphereShape>(1.0);
+
+ std::shared_ptr<ShapeCollisionRepresentation> rep0 = std::make_shared<ShapeCollisionRepresentation>("TestSphere 1");
+ rep0->setShape(sphereShape);
+ rep0->setLocalPose(SurgSim::Math::makeRigidTransform(Quaterniond::Identity(), Vector3d(3.0,0.0,0.0)));
+
+ std::shared_ptr<ShapeCollisionRepresentation> rep1 = std::make_shared<ShapeCollisionRepresentation>("TestSphere 2");
+ rep1->setShape(sphereShape);
+ rep1->setLocalPose(SurgSim::Math::makeRigidTransform(Quaterniond::Identity(), Vector3d(0.5,0.0,0.0)));
+
+ std::shared_ptr<CollisionPair> pair01 = std::make_shared<CollisionPair>(rep0, rep1);
+
+ DefaultContactCalculation calcShouldLog(false);
+ EXPECT_NO_THROW(calcShouldLog.calculateContact(pair01));
+ EXPECT_FALSE(pair01->hasContacts());
+
+ DefaultContactCalculation calcShouldThrow(true);
+ EXPECT_ANY_THROW(calcShouldThrow.calculateContact(pair01));
+ EXPECT_FALSE(pair01->hasContacts());
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/OctreeContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/OctreeContactCalculationTests.cpp
new file mode 100644
index 0000000..1cebbfa
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/OctreeContactCalculationTests.cpp
@@ -0,0 +1,427 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "SurgSim/DataStructures/OctreeNode.h"
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Collision/DcdCollision.h"
+#include "SurgSim/Collision/OctreeDcdContact.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Math/Shapes.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::DataStructures::OctreeNode;
+using SurgSim::DataStructures::OctreePath;
+using SurgSim::Collision::ShapeCollisionRepresentation;
+using SurgSim::Math::CapsuleShape;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::Geometry::DistanceEpsilon;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+using SurgSim::Math::OctreeShape;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Shape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+struct OctreeData
+{
+ std::string name;
+};
+
+std::list<std::shared_ptr<Contact>> doCollision(std::shared_ptr<Shape> octree,
+ const Quaterniond& octreeQuat,
+ const Vector3d& octreeTrans,
+ std::shared_ptr<Shape> shape,
+ const Quaterniond& shapeQuat,
+ const Vector3d& shapeTrans,
+ ContactCalculation& calculator)
+{
+ std::shared_ptr<ShapeCollisionRepresentation> octreeRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Octree 0");
+ octreeRep->setShape(octree);
+ octreeRep->setLocalPose(makeRigidTransform(octreeQuat, octreeTrans));
+
+ std::shared_ptr<ShapeCollisionRepresentation> shapeRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Capsule 0");
+ shapeRep->setShape(shape);
+ shapeRep->setLocalPose(makeRigidTransform(shapeQuat, shapeTrans));
+
+ // Perform collision detection.
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(octreeRep, shapeRep);
+ calculator.calculateContact(pair);
+ return pair->getContacts();
+}
+
+void checkContacts(const std::list<std::shared_ptr<Contact>>& contacts, std::shared_ptr<OctreeNode<OctreeData>> octree)
+{
+ for(auto contact = contacts.cbegin(); contact != contacts.cend(); ++contact)
+ {
+ Location& location = (*contact)->penetrationPoints.first;
+ ASSERT_TRUE(location.octreeNodePath.hasValue());
+ ASSERT_TRUE(location.rigidLocalPosition.hasValue());
+
+ auto nodeBoundingBox = octree->getNode(location.octreeNodePath.getValue())->getBoundingBox();
+ double distanceFromBox = nodeBoundingBox.squaredExteriorDistance(location.rigidLocalPosition.getValue());
+ EXPECT_GT(DistanceEpsilon, distanceFromBox) << "Location of contact is not inside the node";
+ }
+}
+
+bool nodeInContacts(const std::string& name, const std::list<std::shared_ptr<Contact>>& contacts,
+ std::shared_ptr<OctreeNode<OctreeData>> octree)
+{
+ for(auto contact = contacts.cbegin(); contact != contacts.cend(); ++contact)
+ {
+ OctreePath path = (*contact)->penetrationPoints.first.octreeNodePath.getValue();
+ std::shared_ptr<OctreeNode<OctreeData>> node = octree->getNode(path);
+ if (node->data.name == name)
+ {
+ return true;
+ }
+ }
+ return false;
+}
+
+std::shared_ptr<OctreeNode<OctreeData>> buildTestOctree()
+{
+ // To keep things simple, create an 4 level octree with leaf nodes of size 1x1x1.
+ // As a result, the root bounding box is 2^4 x 2^4 x 2^4, or 16x16x16
+ Eigen::AlignedBox<double, 3> boundingBox;
+ const int numLevels = 4;
+ boundingBox.min() = Vector3d::Zero();
+ boundingBox.max() = Vector3d::Ones() * pow(2.0, numLevels);
+ std::shared_ptr<OctreeNode<OctreeData> > rootNode = std::make_shared<OctreeNode<OctreeData> >(boundingBox);
+
+ OctreeData data;
+ data.name = "center";
+ rootNode->addData(Vector3d( 8.5, 8.5, 8.5), data, numLevels);
+
+ data.name = "corner0";
+ rootNode->addData(Vector3d( 0.5, 0.5, 0.5), data, numLevels);
+
+ data.name = "corner1";
+ rootNode->addData(Vector3d(15.5, 0.5, 0.5), data, numLevels);
+
+ data.name = "corner2";
+ rootNode->addData(Vector3d( 0.5, 15.5, 0.5), data, numLevels);
+
+ data.name = "corner3";
+ rootNode->addData(Vector3d(15.5, 15.5, 0.5), data, numLevels);
+
+ data.name = "corner4";
+ rootNode->addData(Vector3d( 0.5, 0.5, 15.5), data, numLevels);
+
+ data.name = "corner5";
+ rootNode->addData(Vector3d(15.5, 0.5, 15.5), data, numLevels);
+
+ data.name = "corner6";
+ rootNode->addData(Vector3d( 0.5, 15.5, 15.5), data, numLevels);
+
+ data.name = "corner7";
+ rootNode->addData(Vector3d(15.5, 15.5, 15.5), data, numLevels);
+
+ return rootNode;
+}
+
+TEST(OctreeContactCalculationTests, Capsule)
+{
+ std::shared_ptr<OctreeNode<OctreeData>> octree = buildTestOctree();
+ std::shared_ptr<OctreeShape> octreeShape = std::make_shared<OctreeShape>(*octree);
+ std::shared_ptr<Shape> capsuleShape = std::make_shared<CapsuleShape>(16.0, 1.0);
+ OctreeDcdContact calculator(std::make_shared<BoxCapsuleDcdContact>());
+
+ std::list<std::shared_ptr<Contact>> contacts;
+ {
+ SCOPED_TRACE("No intersection, capsule outside octree");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(5.0, 0.0, 0.0),
+ capsuleShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ calculator);
+ EXPECT_EQ(0, contacts.size());
+ }
+
+ {
+ SCOPED_TRACE("No intersection, capsule inside octree, but not contacting active nodes");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ capsuleShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(5.0, 3.0, 0.0),
+ calculator);
+ EXPECT_EQ(0, contacts.size());
+ }
+
+ {
+ SCOPED_TRACE("Intersection, capsule is in the center of the octree");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ capsuleShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(8.0, 8.0, 8.0),
+ calculator);
+ checkContacts(contacts, octree);
+ EXPECT_TRUE(nodeInContacts("center", contacts, octree));
+ }
+
+ {
+ SCOPED_TRACE("Intersection, capsule intersection 2 nodes");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ capsuleShape,
+ makeRotationQuaternion(M_PI_2, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(8.0, 0.0, 0.0),
+ calculator);
+ checkContacts(contacts, octree);
+ EXPECT_TRUE(nodeInContacts("corner0", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner1", contacts, octree));
+ }
+
+ {
+ SCOPED_TRACE("Intersection, octree rotated");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(M_PI_4, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ capsuleShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 12.0, 0.0),
+ calculator);
+ checkContacts(contacts, octree);
+ EXPECT_TRUE(nodeInContacts("corner3", contacts, octree));
+ }
+}
+
+TEST(OctreeContactCalculationTests, Plane)
+{
+ std::shared_ptr<OctreeNode<OctreeData>> octree = buildTestOctree();
+ std::shared_ptr<OctreeShape> octreeShape = std::make_shared<OctreeShape>(*octree);
+ std::shared_ptr<Shape> planeShape = std::make_shared<PlaneShape>();
+ OctreeDcdContact calculator(std::make_shared<BoxPlaneDcdContact>());
+
+ std::list<std::shared_ptr<Contact>> contacts;
+ {
+ SCOPED_TRACE("No intersection, plane outside octree");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 5.0, 0.0),
+ planeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ calculator);
+ EXPECT_EQ(0, contacts.size());
+ }
+
+ {
+ SCOPED_TRACE("Intersection, plane inside octree");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ planeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 2.0, 0.0),
+ calculator);
+ checkContacts(contacts, octree);
+ EXPECT_TRUE(nodeInContacts("corner0", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner1", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner4", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner5", contacts, octree));
+ }
+
+ {
+ SCOPED_TRACE("Intersection, rotated octree");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(M_PI_4, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ planeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 2.0, 0.0),
+ calculator);
+ checkContacts(contacts, octree);
+ EXPECT_TRUE(nodeInContacts("corner0", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner4", contacts, octree));
+ }
+
+ {
+ SCOPED_TRACE("Intersection, rotated plane");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ planeShape,
+ makeRotationQuaternion(M_PI_4, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 8.0, 0.0),
+ calculator);
+ checkContacts(contacts, octree);
+ EXPECT_TRUE(nodeInContacts("corner0", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner1", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner3", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner4", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner5", contacts, octree));
+ }
+}
+
+TEST(OctreeContactCalculationTests, DoubleSidedPlane)
+{
+ std::shared_ptr<OctreeNode<OctreeData>> octree = buildTestOctree();
+ std::shared_ptr<OctreeShape> octreeShape = std::make_shared<OctreeShape>(*octree);
+ std::shared_ptr<Shape> planeShape = std::make_shared<DoubleSidedPlaneShape>();
+ OctreeDcdContact calculator(std::make_shared<BoxDoubleSidedPlaneDcdContact>());
+
+ std::list<std::shared_ptr<Contact>> contacts;
+ {
+ SCOPED_TRACE("No intersection, plane outside octree");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 5.0, 0.0),
+ planeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ calculator);
+ EXPECT_EQ(0, contacts.size());
+ }
+
+ {
+ SCOPED_TRACE("Intersection, plane along bottom face");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ planeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ calculator);
+ checkContacts(contacts, octree);
+ EXPECT_TRUE(nodeInContacts("corner0", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner1", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner4", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner5", contacts, octree));
+ }
+
+ {
+ SCOPED_TRACE("Intersection, plane along diagnol");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ planeShape,
+ makeRotationQuaternion(M_PI_4, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ calculator);
+ checkContacts(contacts, octree);
+ EXPECT_TRUE(nodeInContacts("center", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner0", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner3", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner4", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner7", contacts, octree));
+ }
+}
+
+TEST(OctreeContactCalculationTests, Sphere)
+{
+ std::shared_ptr<OctreeNode<OctreeData>> octree = buildTestOctree();
+ std::shared_ptr<OctreeShape> octreeShape = std::make_shared<OctreeShape>(*octree);
+ std::shared_ptr<Shape> sphereShape = std::make_shared<SphereShape>(9);
+ OctreeDcdContact calculator(std::make_shared<BoxSphereDcdContact>());
+
+ std::list<std::shared_ptr<Contact>> contacts;
+ {
+ SCOPED_TRACE("No intersection, sphere outside octree");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 10.0, 0.0),
+ sphereShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ calculator);
+ EXPECT_EQ(0, contacts.size());
+ }
+
+ {
+ SCOPED_TRACE("Intersection, sphere at center of octree");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ sphereShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(8.0, 8.0, 8.0),
+ calculator);
+ checkContacts(contacts, octree);
+ EXPECT_TRUE(nodeInContacts("center", contacts, octree));
+ }
+
+ {
+ SCOPED_TRACE("Intersection, sphere center on box face");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 0.0),
+ sphereShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(8.0, 8.0, 0.0),
+ calculator);
+ checkContacts(contacts, octree);
+ EXPECT_TRUE(nodeInContacts("corner0", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner1", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner2", contacts, octree));
+ EXPECT_TRUE(nodeInContacts("corner3", contacts, octree));
+ }
+
+
+ {
+ SCOPED_TRACE("No intersection, sphere inside octree, but not contacting active nodes");
+ contacts = doCollision(
+ octreeShape,
+ makeRotationQuaternion(0.0, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(0.0, 0.0, 4.0),
+ sphereShape,
+ makeRotationQuaternion(M_PI_4, Vector3d(0.0, 0.0, 1.0)),
+ Vector3d(8.0, 8.0, 0.0),
+ calculator);
+ EXPECT_EQ(0, contacts.size());
+ }
+}
+
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/RepresentationTest.cpp b/SurgSim/Collision/UnitTests/RepresentationTest.cpp
new file mode 100644
index 0000000..2ff5aa1
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/RepresentationTest.cpp
@@ -0,0 +1,151 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/DataStructures/BufferedValue.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Collision::Contact;
+using SurgSim::Collision::ContactMapType;
+using SurgSim::Collision::ShapeCollisionRepresentation;
+using SurgSim::DataStructures::Location;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+struct RepresentationTest : public ::testing::Test
+{
+ virtual void SetUp()
+ {
+ element = std::make_shared<BasicSceneElement>("Element");
+ plane = std::make_shared<PlaneShape>();
+ sphere = std::make_shared<SphereShape>(1.0);
+ planeRep = std::make_shared<ShapeCollisionRepresentation>("PlaneShape");
+ sphereRep = std::make_shared<ShapeCollisionRepresentation>("SphereShape");
+
+ planeRep->setShape(plane);
+ planeRep->setLocalPose(RigidTransform3d::Identity());
+
+ sphereRep->setShape(sphere);
+ sphereRep->setLocalPose(RigidTransform3d::Identity());
+
+ element->addComponent(planeRep);
+ element->addComponent(sphereRep);
+ element->initialize();
+ planeRep->wakeUp();
+ sphereRep->wakeUp();
+ }
+
+ virtual void TearDown()
+ {
+ }
+
+ std::shared_ptr<BasicSceneElement> element;
+ std::shared_ptr<PlaneShape> plane;
+ std::shared_ptr<SphereShape> sphere;
+ std::shared_ptr<ShapeCollisionRepresentation> planeRep;
+ std::shared_ptr<ShapeCollisionRepresentation> sphereRep;
+};
+
+TEST_F(RepresentationTest, InitTest)
+{
+ EXPECT_NO_THROW(ShapeCollisionRepresentation("Plane"));
+}
+
+TEST_F(RepresentationTest, PoseTest)
+{
+ RigidTransform3d initialPose = makeRigidTransform(Quaterniond::Identity(), Vector3d(1.0, 2.0, 3.0));
+ planeRep->setLocalPose(initialPose);
+ EXPECT_TRUE(initialPose.isApprox(planeRep->getPose(), epsilon));
+
+ RigidTransform3d pose = makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 2.0, 0.0));
+ element->setPose(pose);
+ EXPECT_TRUE(pose.isApprox(sphereRep->getPose(), epsilon));
+
+ sphereRep->setLocalPose(initialPose);
+ EXPECT_TRUE((pose * initialPose).isApprox(sphereRep->getPose(), epsilon));
+}
+
+TEST_F(RepresentationTest, ShapeTest)
+{
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_PLANE, planeRep->getShapeType());
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_SPHERE, sphereRep->getShapeType());
+
+ EXPECT_EQ(plane, planeRep->getShape());
+ EXPECT_EQ(sphere, sphereRep->getShape());
+}
+
+TEST_F(RepresentationTest, EmptyCollisionTest)
+{
+ EXPECT_TRUE(planeRep->getCollisions().unsafeGet().empty());
+ EXPECT_TRUE(sphereRep->getCollisions().unsafeGet().empty());
+}
+
+TEST_F(RepresentationTest, CollisionTest)
+{
+ ContactMapType& unsafePlaneCollisions = planeRep->getCollisions().unsafeGet();
+ ContactMapType& unsafeSphereCollisions = sphereRep->getCollisions().unsafeGet();
+ std::shared_ptr<const ContactMapType> safePlaneCollisions = planeRep->getCollisions().safeGet();
+
+ EXPECT_TRUE(unsafePlaneCollisions.empty());
+ EXPECT_TRUE(unsafeSphereCollisions.empty());
+ EXPECT_TRUE(safePlaneCollisions->empty());
+
+ std::shared_ptr<Contact> dummyContact =
+ std::make_shared<Contact>(0.0, Vector3d::Zero(), Vector3d::Zero(), std::make_pair(Location(), Location()));
+ unsafeSphereCollisions[planeRep].push_back(dummyContact);
+
+ auto spherePlanePair = unsafeSphereCollisions.find(planeRep);
+ EXPECT_NE(unsafeSphereCollisions.end(), spherePlanePair);
+ std::list<std::shared_ptr<SurgSim::Collision::Contact>> spherePlaneContacts = spherePlanePair->second;
+ EXPECT_EQ(dummyContact, spherePlaneContacts.front());
+
+ // Collision is only added to 'sphereRep', thus the plane should have no collisions.
+ EXPECT_TRUE(unsafePlaneCollisions.empty());
+
+ // The thread-safe collision map is buffered, so it should still be empty before the publish.
+ EXPECT_TRUE(planeRep->getCollisions().safeGet()->empty());
+
+ // After the publish the thread-safe collision map should be up-to-date.
+ planeRep->getCollisions().publish();
+ EXPECT_EQ(unsafePlaneCollisions, *planeRep->getCollisions().safeGet());
+}
+
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/RepresentationUtilities.cpp b/SurgSim/Collision/UnitTests/RepresentationUtilities.cpp
new file mode 100644
index 0000000..d291f29
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/RepresentationUtilities.cpp
@@ -0,0 +1,87 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/UnitTests/RepresentationUtilities.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+#include "SurgSim/Math/Shapes.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+std::shared_ptr<SurgSim::Collision::Representation> makeSphereRepresentation(
+ const double& radius,
+ const Quaterniond& rotation,
+ const Vector3d& position)
+{
+
+ std::shared_ptr<SurgSim::Math::Shape> sphere = std::make_shared<SurgSim::Math::SphereShape>(radius);
+ auto result = std::make_shared<ShapeCollisionRepresentation>("TestSphereShapeCollisionRep");
+ result->setShape(sphere);
+ result->setLocalPose(SurgSim::Math::makeRigidTransform(rotation, position));
+
+ return result;
+}
+
+std::shared_ptr<SurgSim::Collision::Representation> makeDoubleSidedPlaneRepresentation(
+ const Quaterniond& rotation,
+ const Vector3d& position)
+{
+ std::shared_ptr<SurgSim::Math::Shape> plane = std::make_shared<SurgSim::Math::DoubleSidedPlaneShape>();
+ auto result = std::make_shared<ShapeCollisionRepresentation>("TestDoubleSidedPlaneCollisionRep");
+ result->setShape(plane);
+ result->setLocalPose(SurgSim::Math::makeRigidTransform(rotation, position));
+
+ return result;
+}
+
+std::shared_ptr<SurgSim::Collision::Representation> makePlaneRepresentation(
+ const Quaterniond& rotation,
+ const Vector3d& position)
+{
+ std::shared_ptr<SurgSim::Math::Shape> plane = std::make_shared<SurgSim::Math::PlaneShape>();
+ auto result = std::make_shared<ShapeCollisionRepresentation>("TestPlaneRepresentation");
+ result->setShape(plane);
+ result->setLocalPose(SurgSim::Math::makeRigidTransform(rotation, position));
+
+ return result;
+}
+
+std::shared_ptr<SurgSim::Collision::Representation> makeCapsuleRepresentation(
+ const double& length,
+ const double& radius,
+ const Quaterniond& rotation,
+ const Vector3d& position)
+{
+
+ std::shared_ptr<SurgSim::Math::Shape> capsule = std::make_shared<SurgSim::Math::CapsuleShape>(length, radius);
+ auto result = std::make_shared<ShapeCollisionRepresentation>("TestCapsuleShapeCollisionRep");
+ result->setShape(capsule);
+ result->setLocalPose(SurgSim::Math::makeRigidTransform(rotation, position));
+
+ return result;
+}
+
+}; // Collision
+}; // SurgSim
diff --git a/SurgSim/Collision/UnitTests/RepresentationUtilities.h b/SurgSim/Collision/UnitTests/RepresentationUtilities.h
new file mode 100644
index 0000000..0e7b04e
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/RepresentationUtilities.h
@@ -0,0 +1,55 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_COLLISION_UNITTESTS_REPRESENTATIONUTILITIES_H
+#define SURGSIM_COLLISION_UNITTESTS_REPRESENTATIONUTILITIES_H
+
+#include <memory>
+
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+
+#include "SurgSim/Collision/Representation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class Representation;
+
+std::shared_ptr<SurgSim::Collision::Representation> makeSphereRepresentation(
+ const double& radius = 1.0,
+ const SurgSim::Math::Quaterniond& rotation = SurgSim::Math::Quaterniond::Identity(),
+ const SurgSim::Math::Vector3d& position = SurgSim::Math::Vector3d::Zero());
+
+std::shared_ptr<SurgSim::Collision::Representation> makeDoubleSidedPlaneRepresentation(
+ const SurgSim::Math::Quaterniond& rotation = SurgSim::Math::Quaterniond::Identity(),
+ const SurgSim::Math::Vector3d& position = SurgSim::Math::Vector3d::Zero());
+
+std::shared_ptr<SurgSim::Collision::Representation> makePlaneRepresentation(
+ const SurgSim::Math::Quaterniond& rotation = SurgSim::Math::Quaterniond::Identity(),
+ const SurgSim::Math::Vector3d& position = SurgSim::Math::Vector3d::Zero());
+
+std::shared_ptr<SurgSim::Collision::Representation> makeCapsuleRepresentation(
+ const double& length = 1.0,
+ const double& radius = 1.0,
+ const SurgSim::Math::Quaterniond& rotation = SurgSim::Math::Quaterniond::Identity(),
+ const SurgSim::Math::Vector3d& position = SurgSim::Math::Vector3d::Zero());
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/UnitTests/ShapeCollisionRepresentationTest.cpp b/SurgSim/Collision/UnitTests/ShapeCollisionRepresentationTest.cpp
new file mode 100644
index 0000000..320b811
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/ShapeCollisionRepresentationTest.cpp
@@ -0,0 +1,109 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace {
+static const double dt = 0.001;
+}
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+TEST(ShapeCollisionRepresentationTest, MeshUpdateTest)
+{
+ SurgSim::Framework::Runtime runtime("config.txt");
+
+ const std::string fileName = "MeshShapeData/staple_collision.ply";
+ auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+ EXPECT_NO_THROW(meshShape->load(fileName));
+
+ auto collisionRepresentation = std::make_shared<ShapeCollisionRepresentation>("Collision");
+ collisionRepresentation->setShape(meshShape);
+ collisionRepresentation->setLocalPose(SurgSim::Math::RigidTransform3d::Identity());
+ collisionRepresentation->update(dt);
+
+ auto originalMesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(*meshShape->getMesh());
+ auto expectedMesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(*meshShape->getMesh());
+ auto actualMesh
+ = std::static_pointer_cast<SurgSim::Math::MeshShape>(collisionRepresentation->getShape())->getMesh();
+
+ EXPECT_EQ(expectedMesh->getVertices(), actualMesh->getVertices());
+ EXPECT_EQ(expectedMesh->getTriangles(), actualMesh->getTriangles());
+
+ RigidTransform3d transform = SurgSim::Math::makeRigidTransform(
+ Vector3d(4.3, 2.1, 6.5), Vector3d(-1.5, 7.5, -2.5), Vector3d(8.7, -4.7, -3.1));
+
+ collisionRepresentation->setLocalPose(transform);
+ collisionRepresentation->update(dt);
+
+ expectedMesh->copyWithTransform(transform, *originalMesh);
+
+ EXPECT_EQ(expectedMesh->getVertices(), actualMesh->getVertices());
+ EXPECT_EQ(expectedMesh->getTriangles(), actualMesh->getTriangles());
+}
+
+TEST(ShapeCollisionRepresentationTest, SerializationTest)
+{
+ SurgSim::Framework::Runtime runtime("config.txt");
+
+ const std::string fileName = "MeshShapeData/staple_collision.ply";
+ std::shared_ptr<SurgSim::Math::Shape> shape = std::make_shared<SurgSim::Math::MeshShape>();
+ auto meshShape = std::dynamic_pointer_cast<SurgSim::Math::MeshShape>(shape);
+ EXPECT_NO_THROW(meshShape->load(fileName));
+
+ auto collisionRepresentation = std::make_shared<ShapeCollisionRepresentation>("Collision");
+ collisionRepresentation->setValue("Shape", shape);
+ RigidTransform3d pose = SurgSim::Math::makeRigidTransform(Vector3d(4.3, 2.1, 6.5),
+ Vector3d(-1.5, 7.5, -2.5),
+ Vector3d(8.7, -4.7, -3.1));
+ collisionRepresentation->setLocalPose(pose);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*collisionRepresentation));
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Collision::ShapeCollisionRepresentation"];
+ EXPECT_EQ(5u, data.size());
+
+ std::shared_ptr<ShapeCollisionRepresentation> newShapeCollisionRepresentation;
+ ASSERT_NO_THROW(newShapeCollisionRepresentation = std::dynamic_pointer_cast<ShapeCollisionRepresentation>
+ (node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ EXPECT_TRUE(pose.isApprox(newShapeCollisionRepresentation->getPose()));
+
+ auto mesh = std::dynamic_pointer_cast<SurgSim::Math::MeshShape>(
+ newShapeCollisionRepresentation->getValue<std::shared_ptr<SurgSim::Math::Shape>>("Shape"));
+ ASSERT_TRUE(nullptr != mesh);
+ EXPECT_EQ(meshShape->getMesh()->getNumEdges(), mesh->getMesh()->getNumEdges());
+ EXPECT_EQ(meshShape->getMesh()->getNumTriangles(), mesh->getMesh()->getNumTriangles());
+ EXPECT_EQ(meshShape->getMesh()->getNumVertices(), mesh->getMesh()->getNumVertices());
+}
+
+} // namespace Collision
+} // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/SphereDoubleSidedPlaneContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/SphereDoubleSidedPlaneContactCalculationTests.cpp
new file mode 100644
index 0000000..fa6cb18
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/SphereDoubleSidedPlaneContactCalculationTests.cpp
@@ -0,0 +1,168 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/SphereShape.h"
+
+using SurgSim::Math::Shape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::DoubleSidedPlaneShape;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+void doSphereDoubleSidedPlaneTest(std::shared_ptr<SphereShape> sphere,
+ const Quaterniond& sphereQuat,
+ const Vector3d& sphereTrans,
+ std::shared_ptr<DoubleSidedPlaneShape> plane,
+ const Quaterniond& planeQuat,
+ const Vector3d& planeTrans,
+ bool expectedIntersect,
+ const double& expectedDepth = 0 ,
+ const Vector3d& expectedNorm = Vector3d::Zero())
+{
+ using SurgSim::Math::Geometry::ScalarEpsilon;
+ using SurgSim::Math::Geometry::DistanceEpsilon;
+
+ std::shared_ptr<ShapeCollisionRepresentation> planeRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Plane");
+ planeRep->setShape(plane);
+ planeRep->setLocalPose(SurgSim::Math::makeRigidTransform(planeQuat, planeTrans));
+
+ std::shared_ptr<ShapeCollisionRepresentation> sphereRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Sphere");
+ sphereRep->setShape(sphere);
+ sphereRep->setLocalPose(SurgSim::Math::makeRigidTransform(sphereQuat, sphereTrans));
+
+ SphereDoubleSidedPlaneDcdContact calcNormal;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(sphereRep, planeRep);
+
+ // Again this replicates the way this is calculated in the contact calculation just with different
+ // starting values
+ Vector3d sphereLocalNormal = sphereQuat.inverse() * expectedNorm;
+ Vector3d spherePenetration = -sphereLocalNormal * sphere->getRadius();
+ Vector3d planePenetration = -sphereLocalNormal * (sphere->getRadius() - expectedDepth);
+ planePenetration = (sphereQuat * planePenetration) + sphereTrans;
+ planePenetration = planeQuat.inverse() * (planePenetration - planeTrans);
+
+ calcNormal.calculateContact(pair);
+ if (expectedIntersect)
+ {
+ ASSERT_TRUE(pair->hasContacts());
+ std::shared_ptr<Contact> contact = pair->getContacts().front();
+ EXPECT_NEAR(expectedDepth, contact->depth, 1e-10);
+ EXPECT_TRUE(eigenEqual(expectedNorm, contact->normal));
+ EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(eigenEqual(spherePenetration,
+ contact->penetrationPoints.first.rigidLocalPosition.getValue()));
+ EXPECT_TRUE(eigenEqual(planePenetration,
+ contact->penetrationPoints.second.rigidLocalPosition.getValue()));
+ }
+ else
+ {
+ EXPECT_FALSE(pair->hasContacts());
+ }
+}
+
+TEST(SphereDoubleSidedPlaneContactCalculationTests, UnitTests)
+{
+ std::shared_ptr<DoubleSidedPlaneShape> plane = std::make_shared<DoubleSidedPlaneShape>();
+ std::shared_ptr<SphereShape> sphere = std::make_shared<SphereShape>(1.0);
+
+ {
+ SCOPED_TRACE("No Intersection, no transformation");
+ doSphereDoubleSidedPlaneTest(sphere, Quaterniond::Identity(), Vector3d(0.0,2.0,0.0),
+ plane, Quaterniond::Identity(), Vector3d(0.0,0.5,0.0), false);
+ }
+
+ {
+ SCOPED_TRACE("Intersection front, no transformation");
+ doSphereDoubleSidedPlaneTest(sphere, Quaterniond::Identity(), Vector3d(0.0,1.0,0.0),
+ plane,Quaterniond::Identity(), Vector3d(0.0,0.5,0.0),
+ true, 0.5, Vector3d(0.0,1.0,0.0));
+ }
+
+ {
+ SCOPED_TRACE("Intersection back, no transformation");
+ doSphereDoubleSidedPlaneTest(sphere, Quaterniond::Identity(), Vector3d(0.0,0.0,0.0),
+ plane, Quaterniond::Identity(), Vector3d(0.0,0.5,0.0),
+ true, 0.5, Vector3d(0.0,-1.0,0.0));
+ }
+
+ {
+ SCOPED_TRACE("Intersection front, sphere center on the plane, rotated plane");
+ doSphereDoubleSidedPlaneTest(sphere, Quaterniond::Identity(), Vector3d(0.0,0,0.0),
+ plane, SurgSim::Math::makeRotationQuaternion(M_PI_2, Vector3d(1.0,0.0,0.0)),
+ Vector3d(0.0,0.0,0.0), true, 1.0, Vector3d(0.0,0.0,1.0));
+ }
+
+ {
+ SCOPED_TRACE("Intersection front, rotated Plane");
+ doSphereDoubleSidedPlaneTest(sphere, Quaterniond::Identity(), Vector3d(0.0,0.0,0.5),
+ plane, SurgSim::Math::makeRotationQuaternion(M_PI_2, Vector3d(1.0,0.0,0.0)),
+ Vector3d(0.0,0.0,0.0), true, 0.5, Vector3d(0.0,0.0,1.0));
+ }
+
+ {
+ Vector3d planeTrans(365.321,-342.324,325.324);
+ Quaterniond planeQuat = SurgSim::Math::makeRotationQuaternion(1.23456,
+ Vector3d(0.234,-0.986,0.646).normalized());
+ SCOPED_TRACE("Intersection front, rotated plane 2");
+ doSphereDoubleSidedPlaneTest(sphere, Quaterniond::Identity(), planeQuat * (Vector3d(0.0,0.5,0.0)) + planeTrans,
+ plane, planeQuat, planeTrans, true, 0.5, planeQuat * Vector3d(0.0, 1.0, 0.0));
+ }
+}
+
+TEST(SphereDoubleSidedPlaneContactCalculationTests, ShouldFail)
+{
+ std::shared_ptr<Shape> sphereShape = std::make_shared<SphereShape>(1.0);
+ std::shared_ptr<Shape> doubleSidedPlaneShape = std::make_shared<DoubleSidedPlaneShape>();
+
+ std::shared_ptr<ShapeCollisionRepresentation> reps0 =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Sphere 0");
+ reps0->setShape(sphereShape);
+ reps0->setLocalPose(SurgSim::Math::makeRigidTransform(Quaterniond::Identity(), Vector3d(1.0,0.0,0.0)));
+
+ std::shared_ptr<ShapeCollisionRepresentation> repp0 =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Plane 0");
+ repp0->setShape(doubleSidedPlaneShape);
+ repp0->setLocalPose(SurgSim::Math::makeRigidTransform(Quaterniond::Identity(), Vector3d(0.5,0.0,0.0)));
+
+ std::shared_ptr<ShapeCollisionRepresentation> reps1 =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Sphere 1");
+ reps1->setShape(sphereShape);
+ reps1->setLocalPose(SurgSim::Math::makeRigidTransform(Quaterniond::Identity(), Vector3d(1.0,0.0,0.0)));
+
+ std::shared_ptr<ShapeCollisionRepresentation> repp1 =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Plane 1");
+ repp1->setShape(doubleSidedPlaneShape);
+ repp1->setLocalPose(SurgSim::Math::makeRigidTransform(Quaterniond::Identity(), Vector3d(0.5,0.0,0.0)));
+
+ std::shared_ptr<CollisionPair> pairpp = std::make_shared<CollisionPair>(repp0, repp1);
+ std::shared_ptr<CollisionPair> pairss = std::make_shared<CollisionPair>(reps0, reps1);
+
+ SphereDoubleSidedPlaneDcdContact contact;
+
+ EXPECT_ANY_THROW(contact.calculateContact(pairpp));
+ EXPECT_ANY_THROW(contact.calculateContact(pairss));
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/SpherePlaneContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/SpherePlaneContactCalculationTests.cpp
new file mode 100644
index 0000000..2c3a7c7
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/SpherePlaneContactCalculationTests.cpp
@@ -0,0 +1,121 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/Collision/SpherePlaneDcdContact.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/SphereShape.h"
+
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::PlaneShape;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+void doSpherePlaneTest(std::shared_ptr<SphereShape> sphere,
+ const Quaterniond& sphereQuat,
+ const Vector3d& sphereTrans,
+ std::shared_ptr<PlaneShape> plane,
+ const Quaterniond& planeQuat,
+ const Vector3d& planeTrans,
+ bool expectedIntersect,
+ const double& expectedDepth = 0,
+ const Vector3d& expectedNorm = Vector3d::Zero())
+{
+ std::shared_ptr<ShapeCollisionRepresentation> planeRep =
+ std::make_shared<ShapeCollisionRepresentation>("Plane Shape");
+ planeRep->setShape(plane);
+ planeRep->setLocalPose(SurgSim::Math::makeRigidTransform(planeQuat,planeTrans));
+
+ std::shared_ptr<ShapeCollisionRepresentation> sphereRep =
+ std::make_shared<ShapeCollisionRepresentation>("Sphere Shape");
+ sphereRep->setShape(sphere);
+ sphereRep->setLocalPose(SurgSim::Math::makeRigidTransform(sphereQuat, sphereTrans));
+
+ SpherePlaneDcdContact calcNormal;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(sphereRep, planeRep);
+
+ // Again this replicates the way this is calculated in the contact calculation just with different
+ // starting values
+ Vector3d sphereLocalNormal = sphereQuat.inverse() * expectedNorm;
+ Vector3d spherePenetration = -sphereLocalNormal * sphere->getRadius();
+ Vector3d planePenetration = -sphereLocalNormal * (sphere->getRadius() - expectedDepth);
+ planePenetration = (sphereQuat * planePenetration) + sphereTrans;
+ planePenetration = planeQuat.inverse() * (planePenetration - planeTrans);
+
+ calcNormal.calculateContact(pair);
+ if (expectedIntersect)
+ {
+ ASSERT_TRUE(pair->hasContacts());
+ std::shared_ptr<Contact> contact = pair->getContacts().front();
+ EXPECT_NEAR(expectedDepth, contact->depth, SurgSim::Math::Geometry::DistanceEpsilon);
+ EXPECT_TRUE(eigenEqual(expectedNorm, contact->normal));
+ EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(eigenEqual(spherePenetration,
+ contact->penetrationPoints.first.rigidLocalPosition.getValue()));
+ EXPECT_TRUE(eigenEqual(planePenetration,
+ contact->penetrationPoints.second.rigidLocalPosition.getValue()));
+ }
+ else
+ {
+ EXPECT_FALSE(pair->hasContacts());
+ }
+}
+
+TEST(SpherePlaneContactCalculationTests, UnitTests)
+{
+ std::shared_ptr<PlaneShape> plane = std::make_shared<PlaneShape>();
+ std::shared_ptr<SphereShape> sphere = std::make_shared<SphereShape>(1.0);
+
+ {
+ SCOPED_TRACE("No Intersection, no transformation");
+ doSpherePlaneTest(sphere, Quaterniond::Identity(), Vector3d(0.0,2.0,0.0),
+ plane, Quaterniond::Identity(), Vector3d(0.0,0.5,0.0), false);
+ }
+
+ {
+ SCOPED_TRACE("Intersection front, no transformation");
+ doSpherePlaneTest(sphere, Quaterniond::Identity(), Vector3d(0.0,1.0,0.0),
+ plane,Quaterniond::Identity(), Vector3d(0.0,0.5,0.0),
+ true, 0.5, Vector3d(0.0,1.0,0.0));
+ }
+
+ {
+ SCOPED_TRACE("Intersection back, no transformation");
+ doSpherePlaneTest(sphere, Quaterniond::Identity(), Vector3d(0.0,0.0,0.0),
+ plane, Quaterniond::Identity(), Vector3d(0.0,0.5,0.0),
+ true, 1.5, Vector3d(0.0,1.0,0.0));
+ }
+
+ {
+ SCOPED_TRACE("Intersection front, sphere center on the plane, rotated plane");
+ doSpherePlaneTest(sphere, Quaterniond::Identity(), Vector3d(0.0,0,0.0),
+ plane, SurgSim::Math::makeRotationQuaternion(M_PI_2, Vector3d(1.0,0.0,0.0)),
+ Vector3d(0.0,0.0,0.0), true, 1.0, Vector3d(0.0,0.0,1.0));
+ }
+
+ {
+ SCOPED_TRACE("Intersection front, rotated Plane");
+ doSpherePlaneTest(sphere, Quaterniond::Identity(), Vector3d(0.0,0.0,0.5),
+ plane, SurgSim::Math::makeRotationQuaternion(M_PI_2, Vector3d(1.0,0.0,0.0)),
+ Vector3d(0.0,0.0,0.0), true, 0.5, Vector3d(0.0,0.0,1.0));
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/SphereSphereContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/SphereSphereContactCalculationTests.cpp
new file mode 100644
index 0000000..c0f323e
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/SphereSphereContactCalculationTests.cpp
@@ -0,0 +1,100 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/Collision/SphereSphereDcdContact.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/SphereShape.h"
+
+using SurgSim::Math::Geometry::DistanceEpsilon;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+void doSphereSphereTest(double r0, Vector3d p0, double r1, Vector3d p1, bool hasContacts,
+ double expectedDepth = 0.0, Vector3d expectedNormal = Vector3d::UnitX(),
+ Vector3d expectedPenetrationPoint0 = Vector3d::Zero(),
+ Vector3d expectedPenetrationPoint1 = Vector3d::Zero())
+{
+ SphereSphereDcdContact calc;
+
+ auto sphere1 = std::make_shared<SurgSim::Math::SphereShape>(r0);
+ auto sphereRep1 = std::make_shared<ShapeCollisionRepresentation>("TestSphereShapeCollisionRep1");
+ sphereRep1->setShape(sphere1);
+ sphereRep1->setLocalPose(SurgSim::Math::makeRigidTransform(Quaterniond::Identity(), p0));
+
+ auto sphere2 = std::make_shared<SurgSim::Math::SphereShape>(r1);
+ auto sphereRep2 = std::make_shared<ShapeCollisionRepresentation>("TestSphereShapeCollisionRep2");
+ sphereRep2->setShape(sphere2);
+ sphereRep2->setLocalPose(SurgSim::Math::makeRigidTransform(Quaterniond::Identity(), p1));
+
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(sphereRep1, sphereRep2);
+
+ calc.calculateContact(pair);
+ EXPECT_EQ(hasContacts, pair->hasContacts());
+ if (pair->hasContacts())
+ {
+ std::shared_ptr<Contact> contact = pair->getContacts().front();
+
+ EXPECT_LT(-DistanceEpsilon, contact->depth);
+ const double maxDepth = std::max(sphere1->getRadius(), sphere2->getRadius());
+ EXPECT_GT(maxDepth + DistanceEpsilon, contact->depth);
+
+ const Vector3d sphere2ToSphere1 = sphereRep1->getPose().translation() - sphereRep2->getPose().translation();
+ if (!sphere2ToSphere1.isZero())
+ {
+ EXPECT_TRUE(eigenEqual(sphere2ToSphere1.normalized(), contact->normal));
+ }
+
+ EXPECT_TRUE(eigenEqual(expectedNormal, contact->normal));
+ EXPECT_NEAR(expectedDepth, contact->depth, SurgSim::Math::Geometry::DistanceEpsilon);
+ EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());
+
+ EXPECT_TRUE(eigenEqual(expectedPenetrationPoint0,
+ contact->penetrationPoints.first.rigidLocalPosition.getValue()));
+ EXPECT_TRUE(eigenEqual(expectedPenetrationPoint1,
+ contact->penetrationPoints.second.rigidLocalPosition.getValue()));
+ }
+}
+
+TEST(SphereSphereContactCalculationTests, UnitTests)
+{
+ using SurgSim::Math::Geometry::DistanceEpsilon;
+
+ {
+ SCOPED_TRACE("No Intersection");
+ doSphereSphereTest(0.1, Vector3d(0.0,0.0,0.0), 0.1, Vector3d(1.0,1.0,1.0), false);
+ }
+
+ {
+ SCOPED_TRACE("Sphere-Sphere intersection at origin");
+ doSphereSphereTest(0.5,
+ Vector3d(-0.5+DistanceEpsilon/2.0,0.0,0.0), 0.5, Vector3d(0.5-DistanceEpsilon/2.0,0.0,0.0),
+ true, DistanceEpsilon, Vector3d(-1.0,0.0,0.0), Vector3d(0.5,0.0,0.0),
+ Vector3d(-0.5,0.0,0.0));
+ }
+
+ {
+ SCOPED_TRACE("Sphere-Sphere intersection");
+ doSphereSphereTest(0.5, Vector3d(0.0,0.0,0.0), 0.5, Vector3d(0.5,0.0,0.0), true, 0.5,
+ Vector3d(-1.0,0.0,0.0), Vector3d(0.5,0.0,0.0), Vector3d(-0.5,0.0,0.0));
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/TriangleMeshPlaneContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/TriangleMeshPlaneContactCalculationTests.cpp
new file mode 100644
index 0000000..79a77aa
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/TriangleMeshPlaneContactCalculationTests.cpp
@@ -0,0 +1,416 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/TriangleMeshPlaneDcdContact.h"
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::TriangleMeshPlain;
+using SurgSim::Math::Geometry::DistanceEpsilon;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+namespace
+{
+// CUBE
+// 3*----------*2
+// / /|
+// 7*----------* |
+// | 6| |
+// | *0 | *1
+// | |/
+// 4*----------*5
+
+static const int cubeNumPoints = 8;
+static const SurgSim::Math::Vector3d cubePoints[8] =
+{
+ Vector3d(-1.0 / 2.0, -1.0 / 2.0, -1.0 / 2.0),
+ Vector3d(1.0 / 2.0, -1.0 / 2.0, -1.0 / 2.0),
+ Vector3d(1.0 / 2.0, 1.0 / 2.0, -1.0 / 2.0),
+ Vector3d(-1.0 / 2.0, 1.0 / 2.0, -1.0 / 2.0),
+
+ Vector3d(-1.0 / 2.0, -1.0 / 2.0, 1.0 / 2.0),
+ Vector3d(1.0 / 2.0, -1.0 / 2.0, 1.0 / 2.0),
+ Vector3d(1.0 / 2.0, 1.0 / 2.0, 1.0 / 2.0),
+ Vector3d(-1.0 / 2.0, 1.0 / 2.0, 1.0 / 2.0)
+};
+
+static const int cubeNumEdges = 12;
+static const int cubeEdges[12][2] =
+{
+ {0, 1}, {3, 2}, {4, 5}, {7, 6}, // +X
+ {0, 3}, {1, 2}, {4, 7}, {5 , 6}, // +Y
+ {0, 4}, {1, 5}, {2, 6}, {3, 7} // +Z
+};
+
+static const int cubeNumTriangles = 12;
+static const int cubeTrianglesCCW[12][3] =
+{
+ {6, 2, 3}, {6, 3, 7}, // Top ( 0 1 0) [6237]
+ {0, 1, 5}, {0, 5, 4}, // Bottom ( 0 -1 0) [0154]
+ {4, 5, 6}, {4, 6, 7}, // Front ( 0 0 1) [4567]
+ {0, 3, 2}, {0, 2, 1}, // Back ( 0 0 -1) [0321]
+ {1, 2, 6}, {1, 6, 5}, // Right ( 1 0 0) [1265]
+ {0, 4, 7}, {0, 7, 3} // Left (-1 0 0) [0473]
+};
+
+Vector3d calculateTriangleMeshVertex(const int i,
+ const Quaterniond& quat,
+ const Vector3d& trans)
+{
+
+ return (quat * Vector3d(cubePoints[i][0], cubePoints[i][1], cubePoints[i][2])) + trans;
+}
+
+void generateTriangleMeshPlaneContact(std::list<std::shared_ptr<Contact>>* expectedContacts,
+ const int expectedNumberOfContacts, const int* expectedMeshIndicesInContacts,
+ const Vector3d& meshTrans, const Quaterniond& meshQuat,
+ const Vector3d& planeNormal, const double planeD,
+ const Vector3d& planeTrans, const Quaterniond& planeQuat)
+{
+ Vector3d vertex;
+ Vector3d boxLocalVertex, planeLocalVertex;
+ Vector3d planeNormalGlobal = planeQuat * planeNormal;
+ Vector3d pointOnPlane = planeTrans + (planeNormalGlobal * planeD);
+ double depth = 0.0;
+ Vector3d collisionNormal = planeNormalGlobal;
+
+ for (int i = 0; i < expectedNumberOfContacts; ++i)
+ {
+ vertex = calculateTriangleMeshVertex(expectedMeshIndicesInContacts[i], meshQuat, meshTrans);
+ depth = -planeNormalGlobal.dot(vertex - pointOnPlane);
+
+ boxLocalVertex = calculateTriangleMeshVertex(expectedMeshIndicesInContacts[i],
+ Quaterniond::Identity(), Vector3d::Zero());
+ planeLocalVertex = vertex + planeNormalGlobal * depth;
+ planeLocalVertex = planeQuat.inverse() * (planeLocalVertex - planeTrans);
+
+ std::pair<Location, Location> penetrationPoint;
+ penetrationPoint.first.rigidLocalPosition.setValue(boxLocalVertex);
+ penetrationPoint.second.rigidLocalPosition.setValue(planeLocalVertex);
+ expectedContacts->push_back(std::make_shared<Contact>(depth, Vector3d::Zero(),
+ collisionNormal, penetrationPoint));
+ }
+}
+
+void doTriangleMeshPlaneTest(std::shared_ptr<SurgSim::Math::MeshShape> mesh,
+ const Quaterniond& meshQuat,
+ const Vector3d& meshTrans,
+ std::shared_ptr<PlaneShape> plane,
+ const Quaterniond& planeQuat,
+ const Vector3d& planeTrans,
+ const int expectedNumberOfContacts,
+ const int* expectedMeshIndicesInContacts)
+{
+ std::shared_ptr<ShapeCollisionRepresentation> meshRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Mesh 0");
+ meshRep->setShape(mesh);
+ meshRep->setLocalPose(SurgSim::Math::makeRigidTransform(meshQuat, meshTrans));
+
+ std::shared_ptr<ShapeCollisionRepresentation> planeRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Plane 0");
+ planeRep->setShape(plane);
+ planeRep->setLocalPose(SurgSim::Math::makeRigidTransform(planeQuat, planeTrans));
+
+ // First calculate the expected contact info.
+ std::list<std::shared_ptr<Contact>> expectedContacts;
+ if (expectedNumberOfContacts > 0)
+ {
+ generateTriangleMeshPlaneContact(&expectedContacts, expectedNumberOfContacts, expectedMeshIndicesInContacts,
+ meshTrans, meshQuat, plane->getNormal(), plane->getD(), planeTrans, planeQuat);
+ }
+
+ // Perform collision detection.
+ TriangleMeshPlaneDcdContact calcContact;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(meshRep, planeRep);
+ calcContact.calculateContact(pair);
+
+ const Vector3d globalPlaneNormal = planeRep->getPose().linear() * plane->getNormal();
+ mesh->updateAabbTree();
+ const double maxRadius = mesh->getAabbTree()->getAabb().diagonal().norm() / 2.0;
+ const Vector3d planeToMesh = mesh->getCenter() - planeTrans;
+ Vector3d nearestPointOnPlane;
+ const double distanceMeshPlane = SurgSim::Math::distancePointPlane(planeToMesh, globalPlaneNormal,
+ plane->getD(), &nearestPointOnPlane);
+
+ const double minDepth = -distanceMeshPlane - maxRadius;
+ const double maxDepth = -distanceMeshPlane + maxRadius;
+
+ for (auto contact : pair->getContacts())
+ {
+ EXPECT_LT(-DistanceEpsilon, contact->depth);
+ EXPECT_LT(minDepth - DistanceEpsilon, contact->depth);
+ EXPECT_GT(maxDepth + DistanceEpsilon, contact->depth);
+ EXPECT_TRUE(eigenEqual(globalPlaneNormal, contact->normal));
+ }
+
+ // Compare the contact info.
+ contactsInfoEqualityTest(expectedContacts, pair->getContacts());
+}
+
+
+TEST(TriangleMeshPlaneContactCalculationTests, UnitTests)
+{
+ // Create a Mesh Cube
+ std::shared_ptr<TriangleMeshPlain> mesh = std::make_shared<TriangleMeshPlain>();
+ for (int i = 0; i < cubeNumPoints; ++i)
+ {
+ Vector3d p;
+ p[0] = cubePoints[i][0];
+ p[1] = cubePoints[i][1];
+ p[2] = cubePoints[i][2];
+ TriangleMeshPlain::VertexType v(p);
+ mesh->addVertex(v);
+ }
+ for (int i = 0; i < cubeNumEdges; ++i)
+ {
+ std::array<size_t, 2> edgePoints;
+ for (int j = 0; j < 2; ++j)
+ {
+ edgePoints[j] = cubeEdges[i][j];
+ }
+ TriangleMeshPlain::EdgeType e(edgePoints);
+ mesh->addEdge(e);
+ }
+ for (int i = 0; i < cubeNumTriangles; ++i)
+ {
+ std::array<size_t, 3> trianglePoints;
+ for (int j = 0; j < 3; ++j)
+ {
+ trianglePoints[j] = cubeTrianglesCCW[i][j];
+ }
+ TriangleMeshPlain::TriangleType t(trianglePoints);
+ mesh->addTriangle(t);
+ }
+
+ std::shared_ptr<SurgSim::Math::MeshShape> cubeMesh = std::make_shared<SurgSim::Math::MeshShape>(*mesh);
+
+ std::shared_ptr<PlaneShape> plane = std::make_shared<PlaneShape>();
+ Quaterniond meshQuat;
+ Vector3d meshTrans;
+ Quaterniond planeQuat;
+ Vector3d planeTrans;
+ Quaterniond globalQuat;
+ Vector3d planNormal;
+ Matrix33d mRotation;
+
+ const double epsilonTrans = 0.1;
+ const double cubeSize = 1.0;
+
+ {
+ SCOPED_TRACE("No intersection, box in front of plane, no rotation");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ planeQuat = Quaterniond::Identity();
+ planeTrans = Vector3d(0.0, -(cubeSize / 2.0 + epsilonTrans), 0.0);
+ int expectedNumberOfContacts = 0;
+ int expectedBoxIndicesInContacts[] = {0};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("Intersection, 04 contacts, no rotation");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ planeQuat = Quaterniond::Identity();
+ planeTrans = Vector3d::Zero();
+ int expectedNumberOfContacts = 4;
+ int expectedBoxIndicesInContacts[] = {0, 1, 4, 5};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 08 contacts, no rotation");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ planeQuat = Quaterniond::Identity();
+ planeTrans = Vector3d(0.0, (cubeSize / 2.0 + epsilonTrans), 0.0);
+ int expectedNumberOfContacts = 8;
+ int expectedBoxIndicesInContacts[] = {0, 1, 2, 3, 4, 5, 6, 7};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 02 contacts, plane rotate(Z, -45)");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ mRotation = Eigen::AngleAxisd(-M_PI_4, Vector3d::UnitZ());
+ planeQuat = Quaterniond(mRotation);
+ planeTrans = Vector3d(-1.0, -1.0, 0.0) * (cubeSize / 2.0 - epsilonTrans);
+ int expectedNumberOfContacts = 2;
+ int expectedBoxIndicesInContacts[] = {0, 4};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 06 contacts, plane rotate(Z, 45)");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ mRotation = Eigen::AngleAxisd(-M_PI_4, Vector3d::UnitZ());
+ planeQuat = Quaterniond(mRotation);
+ planNormal = mRotation * Vector3d::UnitY();
+ planeTrans = planNormal * (cubeSize / 2 - epsilonTrans);
+ int expectedNumberOfContacts = 6;
+ int expectedBoxIndicesInContacts[] = {0, 4, 1, 3, 7, 5};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ // Test cases to cover all collisions between each corner of the cube with the plane
+ {
+ SCOPED_TRACE("intersection, 01 contact, plane rotate(XZ, -45)");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ mRotation = Eigen::AngleAxisd(-M_PI_4, Vector3d(1.0, 0.0, 1.0).normalized());
+ planeQuat = Quaterniond(mRotation);
+ planNormal = Vector3d::UnitY();
+ planeTrans = -1.0 * planNormal * (sqrt(3.0) * cubeSize / 2.0 - epsilonTrans);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {4};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 01 contacts, plane rotate(+XZ,-M_PI/2-45)");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ mRotation = Eigen::AngleAxisd(-M_PI_2 - M_PI_4, Vector3d(1.0, 0.0, 1.0).normalized());
+ planeQuat = Quaterniond(mRotation);
+ planNormal = mRotation * Vector3d::UnitY();
+ planeTrans = -1.0 * planNormal * (sqrt(3.0) * cubeSize / 2.0 - epsilonTrans);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {7};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 01 contacts, plane rotate(+X-Z, +45)");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ mRotation = Eigen::AngleAxisd(M_PI_4, Vector3d(1.0, 0.0, -1.0).normalized());
+ planeQuat = Quaterniond(mRotation);
+ planNormal = mRotation * Vector3d::UnitY();
+ planeTrans = -1.0 * planNormal * (sqrt(3.0) * cubeSize / 2.0 - epsilonTrans);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {0};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 01 contacts, plane rotate(+X-Z, M_PI/2+45)");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ mRotation = Eigen::AngleAxisd(0.75 * M_PI, Vector3d(1.0, 0.0, -1.0).normalized());
+ planeQuat = Quaterniond(mRotation);
+ planNormal = mRotation * Vector3d::UnitY();
+ planeTrans = -1.0 * planNormal * (sqrt(3.0) * cubeSize / 2.0 - epsilonTrans);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {3};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 01 contacts, plane rotate(+X-Z, -45)");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ mRotation = Eigen::AngleAxisd(-M_PI_4, Vector3d(1.0, 0.0, -1.0).normalized());
+ planeQuat = Quaterniond(mRotation);
+ planNormal = mRotation * Vector3d::UnitY();
+ planeTrans = -1.0 * planNormal * (sqrt(3.0) * cubeSize / 2.0 - epsilonTrans);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {5};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 01 contacts, plane rotate(+X-Z,-M_PI/2-45)");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ mRotation = Eigen::AngleAxisd(-M_PI_2 - M_PI_4, Vector3d(1.0, 0.0, -1.0).normalized());
+ planeQuat = Quaterniond(mRotation);
+ planNormal = mRotation * Vector3d::UnitY();
+ planeTrans = -1.0 * planNormal * (sqrt(3.0) * cubeSize / 2.0 - epsilonTrans);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {6};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 01 contacts, plane rotate(+X+Z, +45)");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ mRotation = Eigen::AngleAxisd(M_PI_4, Vector3d(1.0, 0.0, 1.0).normalized());
+ planeQuat = Quaterniond(mRotation);
+ planNormal = mRotation * Vector3d::UnitY();
+ planeTrans = -1.0 * planNormal * (sqrt(3.0) * cubeSize / 2.0 - epsilonTrans);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {1};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 01 contacts, plane rotate(+X+Z, M_PI/2+45)");
+ meshQuat = Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ mRotation = Eigen::AngleAxisd(M_PI_2 + M_PI_4, Vector3d(1.0, 0.0, 1.0).normalized());
+ planeQuat = Quaterniond(mRotation);
+ planNormal = mRotation * Vector3d::UnitY();
+ planeTrans = -1.0 * planNormal * (sqrt(3.0) * cubeSize / 2.0 - epsilonTrans);
+ int expectedNumberOfContacts = 1;
+ int expectedBoxIndicesInContacts[] = {2};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+
+ {
+ SCOPED_TRACE("intersection, 01 contacts, plane & cube -0.8*M_PI*rotate(Z, -M_PI/4)");
+ globalQuat = SurgSim::Math::makeRotationQuaternion<double, Vector3d::Options>(-1.318, Vector3d::UnitZ());
+ meshQuat = globalQuat * Quaterniond::Identity();
+ meshTrans = Vector3d::Zero();
+ planeQuat = globalQuat * Quaterniond(Eigen::AngleAxisd(-M_PI_4, Vector3d::UnitZ()));
+ planeTrans = Vector3d(-1.0, -1.0, 0.0) * (cubeSize / 2.0 - epsilonTrans);
+ int expectedNumberOfContacts = 2;
+ int expectedBoxIndicesInContacts[] = {0, 4};
+ doTriangleMeshPlaneTest(cubeMesh, meshQuat, meshTrans,
+ plane, planeQuat, planeTrans, expectedNumberOfContacts, expectedBoxIndicesInContacts);
+ }
+}
+
+
+}
+
+}; // Physics
+}; // Surgsim
diff --git a/SurgSim/Collision/UnitTests/TriangleMeshTriangleMeshContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/TriangleMeshTriangleMeshContactCalculationTests.cpp
new file mode 100644
index 0000000..45ba02c
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/TriangleMeshTriangleMeshContactCalculationTests.cpp
@@ -0,0 +1,489 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.h"
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::TriangleMeshPlain;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+/* CUBE
+ 3*----------*2
+ / /|
+ 7*----------* |
+ | 6| |
+ | *0 | *1
+ | |/
+ 4*----------*5
+*/
+static const int cubeNumPoints = 8;
+static const Vector3d cubePoints[8] =
+{
+ Vector3d(-1.0 / 2.0, -1.0 / 2.0, -1.0 / 2.0),
+ Vector3d(1.0 / 2.0, -1.0 / 2.0, -1.0 / 2.0),
+ Vector3d(1.0 / 2.0, 1.0 / 2.0, -1.0 / 2.0),
+ Vector3d(-1.0 / 2.0, 1.0 / 2.0, -1.0 / 2.0),
+ Vector3d(-1.0 / 2.0, -1.0 / 2.0, 1.0 / 2.0),
+ Vector3d(1.0 / 2.0, -1.0 / 2.0, 1.0 / 2.0),
+ Vector3d(1.0 / 2.0, 1.0 / 2.0, 1.0 / 2.0),
+ Vector3d(-1.0 / 2.0, 1.0 / 2.0, 1.0 / 2.0)
+};
+
+static const int cubeNumEdges = 12;
+static const int cubeEdges[12][2] =
+{
+ {0, 1}, {3, 2}, {4, 5}, {7, 6}, // +X
+ {0, 3}, {1, 2}, {4, 7}, {5 , 6}, // +Y
+ {0, 4}, {1, 5}, {2, 6}, {3, 7} // +Z
+};
+
+static const int cubeNumTriangles = 12;
+static const int cubeTrianglesCCW[12][3] =
+{
+ {6, 2, 3}, {6, 3, 7}, // Top ( 0 1 0) [6237]
+ {0, 1, 5}, {0, 5, 4}, // Bottom ( 0 -1 0) [0154]
+ {4, 5, 6}, {4, 6, 7}, // Front ( 0 0 1) [4567]
+ {0, 3, 2}, {0, 2, 1}, // Back ( 0 0 -1) [0321]
+ {1, 2, 6}, {1, 6, 5}, // Right ( 1 0 0) [1265]
+ {0, 4, 7}, {0, 7, 3} // Left (-1 0 0) [0473]
+};
+
+void doTriangleMeshTriangleMeshTest(std::shared_ptr<MeshShape> meshA,
+ const RigidTransform3d& meshATransform,
+ std::shared_ptr<MeshShape> meshB,
+ const RigidTransform3d& meshBTransform,
+ const std::list<std::shared_ptr<Contact>> expectedContacts)
+{
+ std::shared_ptr<ShapeCollisionRepresentation> meshARep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Mesh 0");
+ meshARep->setShape(meshA);
+ meshARep->setLocalPose(meshATransform);
+
+ std::shared_ptr<ShapeCollisionRepresentation> meshBRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Mesh 1");
+ meshBRep->setShape(meshB);
+ meshBRep->setLocalPose(meshBTransform);
+
+ // Perform collision detection.
+ TriangleMeshTriangleMeshDcdContact calcContact;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(meshARep, meshBRep);
+ calcContact.calculateContact(pair);
+
+ contactsInfoEqualityTest(expectedContacts, pair->getContacts(), true);
+}
+
+TEST(TriangleMeshTriangleMeshContactCalculationTests, NonintersectionTest)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRotationQuaternion;
+
+ // Create a Mesh Cube
+ std::shared_ptr<TriangleMeshPlain> mesh = std::make_shared<TriangleMeshPlain>();
+ for (int i = 0; i < cubeNumPoints; ++i)
+ {
+ Vector3d p;
+ p[0] = cubePoints[i][0];
+ p[1] = cubePoints[i][1];
+ p[2] = cubePoints[i][2];
+ TriangleMeshPlain::VertexType v(p);
+ mesh->addVertex(v);
+ }
+ for (int i = 0; i < cubeNumEdges; ++i)
+ {
+ std::array<size_t, 2> edgePoints;
+ for (int j = 0; j < 2; j++)
+ {
+ edgePoints[j] = cubeEdges[i][j];
+ }
+ TriangleMeshPlain::EdgeType e(edgePoints);
+ mesh->addEdge(e);
+ }
+ for (int i = 0; i < cubeNumTriangles; ++i)
+ {
+ std::array<size_t, 3> trianglePoints;
+ for (int j = 0; j < 3; j++)
+ {
+ trianglePoints[j] = cubeTrianglesCCW[i][j];
+ }
+ TriangleMeshPlain::TriangleType t(trianglePoints);
+ mesh->addTriangle(t);
+ }
+
+ std::shared_ptr<SurgSim::Math::MeshShape> cubeMeshA = std::make_shared<SurgSim::Math::MeshShape>(*mesh);
+ std::shared_ptr<SurgSim::Math::MeshShape> cubeMeshB = std::make_shared<SurgSim::Math::MeshShape>(*mesh);
+
+ SurgSim::Math::RigidTransform3d cubeMeshATransform;
+ SurgSim::Math::RigidTransform3d cubeMeshBTransform;
+ SurgSim::Math::RigidTransform3d globalTransform;
+ const std::list<std::shared_ptr<Contact>> emptyContacts;
+
+ double cubeSize = 1.0;
+ double epsilonTrans = 0.001;
+
+ {
+ SCOPED_TRACE("No intersection, boxB above boxA");
+ cubeMeshATransform.setIdentity();
+ cubeMeshBTransform = Eigen::Translation<double, 3>(0.0, cubeSize + epsilonTrans, 0.0);
+ globalTransform = makeRigidTransform(makeRotationQuaternion(1.234, Vector3d(1.2, 3.4, 5.6).normalized()),
+ Vector3d(34.4, 567.6, 234.5));
+ cubeMeshATransform = globalTransform * cubeMeshATransform;
+ cubeMeshBTransform = globalTransform * cubeMeshBTransform;
+
+ doTriangleMeshTriangleMeshTest(cubeMeshA, cubeMeshATransform, cubeMeshB, cubeMeshBTransform, emptyContacts);
+ }
+
+ {
+ SCOPED_TRACE("No intersection, boxB below boxA");
+ cubeMeshATransform.setIdentity();
+ cubeMeshBTransform = Eigen::Translation<double, 3>(0.0, -(cubeSize + epsilonTrans), 0.0);
+ globalTransform = makeRigidTransform(makeRotationQuaternion(1.4, Vector3d(10.2, 34.4, 15.6).normalized()),
+ Vector3d(3.4, 6.6, 2.5));
+ cubeMeshATransform = globalTransform * cubeMeshATransform;
+ cubeMeshBTransform = globalTransform * cubeMeshBTransform;
+
+ doTriangleMeshTriangleMeshTest(cubeMeshA, cubeMeshATransform, cubeMeshB, cubeMeshBTransform, emptyContacts);
+ }
+
+ {
+ SCOPED_TRACE("No intersection, boxB to the left of boxA");
+ cubeMeshATransform.setIdentity();
+ cubeMeshBTransform = Eigen::Translation<double, 3>(-(cubeSize + epsilonTrans), 0.0, 0.0);
+ globalTransform = makeRigidTransform(makeRotationQuaternion(1.4, Vector3d(1.2, 3.4, 5.6).normalized()),
+ Vector3d(340.4, 567.6, 234.5));
+ cubeMeshATransform = globalTransform * cubeMeshATransform;
+ cubeMeshBTransform = globalTransform * cubeMeshBTransform;
+
+ doTriangleMeshTriangleMeshTest(cubeMeshA, cubeMeshATransform, cubeMeshB, cubeMeshBTransform, emptyContacts);
+ }
+
+ {
+ SCOPED_TRACE("No intersection, boxB to the right of boxA");
+ cubeMeshATransform.setIdentity();
+ cubeMeshBTransform = Eigen::Translation<double, 3>((cubeSize + epsilonTrans), 0.0, 0.0);
+ globalTransform = makeRigidTransform(makeRotationQuaternion(1.2, Vector3d(11.2, 13.4, 15.6).normalized()),
+ Vector3d(3.4, 5.6, 2.5));
+ cubeMeshATransform = globalTransform * cubeMeshATransform;
+ cubeMeshBTransform = globalTransform * cubeMeshBTransform;
+
+ doTriangleMeshTriangleMeshTest(cubeMeshA, cubeMeshATransform, cubeMeshB, cubeMeshBTransform, emptyContacts);
+ }
+
+ {
+ SCOPED_TRACE("No intersection, boxB in front of boxA");
+ cubeMeshATransform.setIdentity();
+ cubeMeshBTransform = Eigen::Translation<double, 3>(0.0, 0.0, (cubeSize + epsilonTrans));
+ globalTransform = makeRigidTransform(makeRotationQuaternion(2.234, Vector3d(10.2, 30.4, 50.6).normalized()),
+ Vector3d(84.4, 56.6, 24.5));
+ cubeMeshATransform = globalTransform * cubeMeshATransform;
+ cubeMeshBTransform = globalTransform * cubeMeshBTransform;
+
+ doTriangleMeshTriangleMeshTest(cubeMeshA, cubeMeshATransform, cubeMeshB, cubeMeshBTransform, emptyContacts);
+ }
+
+ {
+ SCOPED_TRACE("No intersection, boxB behind boxA");
+ cubeMeshATransform.setIdentity();
+ cubeMeshBTransform = Eigen::Translation<double, 3>(0.0, 0.0, -(cubeSize + epsilonTrans));
+ globalTransform = makeRigidTransform(makeRotationQuaternion(1.24, Vector3d(9.2, 7.4, 5.6).normalized()),
+ Vector3d(39.4, 67.6, 34.5));
+ cubeMeshATransform = globalTransform * cubeMeshATransform;
+ cubeMeshBTransform = globalTransform * cubeMeshBTransform;
+
+ doTriangleMeshTriangleMeshTest(cubeMeshA, cubeMeshATransform, cubeMeshB, cubeMeshBTransform, emptyContacts);
+ }
+}
+
+
+void addNewTriangle(std::shared_ptr<TriangleMeshPlain> mesh,
+ SurgSim::Math::Vector3d point0, SurgSim::Math::Vector3d point1, SurgSim::Math::Vector3d point2)
+{
+
+ // Add vertices
+ TriangleMeshPlain::VertexType vertexMesh(Vector3d::Zero());
+
+ vertexMesh = TriangleMeshPlain::VertexType(point0);
+ size_t index0 = mesh->addVertex(vertexMesh);
+
+ vertexMesh = TriangleMeshPlain::VertexType(point1);
+ size_t index1 = mesh->addVertex(vertexMesh);
+
+ vertexMesh = TriangleMeshPlain::VertexType(point2);
+ size_t index2 = mesh->addVertex(vertexMesh);
+
+ // Add edges
+ std::array<size_t, 2> edge;
+ TriangleMeshPlain::EdgeType meshEdge(edge);
+
+ edge[0] = index0;
+ edge[1] = index1;
+ meshEdge = TriangleMeshPlain::EdgeType(edge);
+ mesh->addEdge(meshEdge);
+
+ edge[0] = index1;
+ edge[1] = index2;
+ meshEdge = TriangleMeshPlain::EdgeType(edge);
+ mesh->addEdge(meshEdge);
+
+ edge[0] = index2;
+ edge[1] = index0;
+ meshEdge = TriangleMeshPlain::EdgeType(edge);
+ mesh->addEdge(meshEdge);
+
+ // Add triangle
+ std::array<size_t, 3> triangle = {index0, index1, index2};
+ TriangleMeshPlain::TriangleType meshTriangle(triangle);
+ mesh->addTriangle(meshTriangle);
+}
+
+TEST(TriangleMeshTriangleMeshContactCalculationTests, IntersectionTest)
+{
+ using SurgSim::Math::MeshShape;
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::RigidTransform3d;
+
+ RigidTransform3d pose;
+ pose = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(0.3468, Vector3d(0.2577, 0.8245, 1.0532).normalized()),
+ Vector3d(120.34, 567.23, -832.84));
+ {
+ // The second mesh.
+ auto intersectingTriangle = std::make_shared<TriangleMeshPlain>();
+ addNewTriangle(intersectingTriangle,
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 0.0, 1.0),
+ Vector3d(1.0, 0.0, 0.5));
+ auto triangleMesh = std::make_shared<MeshShape>(*intersectingTriangle);
+
+ // The first mesh.
+ auto baseTriangles = std::make_shared<TriangleMeshPlain>();
+
+ static const int numTriangles = 100;
+
+ std::list<std::shared_ptr<Contact>> expectedContacts;
+ double expectedDepth;
+ double zOffset = 0.5 / numTriangles;
+ double zValue;
+ std::pair<Location, Location> expectedPenetrationPoints;
+ Vector3d expectedPoint0, expectedPoint1;
+ Vector3d expectedNormal, expectedContact(0, 0, 0);
+ for (int i = 0; i < numTriangles - 1; i++)
+ {
+ zValue = static_cast<double>(i) / numTriangles + zOffset;
+ addNewTriangle(baseTriangles,
+ Vector3d(0.5, 0.5, zValue),
+ Vector3d(-0.5, 0.5, zValue),
+ Vector3d(0.0, -0.5, zValue));
+ expectedDepth = zValue;
+ if (expectedDepth >= 0.5)
+ {
+ expectedDepth = 0.5;
+ expectedNormal = pose.linear() * Vector3d(0, 1, 0);
+ expectedPoint0 = Vector3d(0, -0.5, zValue);
+ expectedPoint1 = Vector3d(0, 0, zValue);
+ }
+ else
+ {
+ expectedNormal = pose.linear() * Vector3d(0, 0, -1);
+ expectedPoint0 = Vector3d(0, 0, zValue);
+ expectedPoint1 = Vector3d(0, 0, 0);
+ }
+ if (expectedDepth > 0.0)
+ {
+ expectedPenetrationPoints.first.rigidLocalPosition.setValue(expectedPoint0);
+ expectedPenetrationPoints.second.rigidLocalPosition.setValue(expectedPoint1);
+ SurgSim::DataStructures::IndexedLocalCoordinate triangleLocalPosition;
+ triangleLocalPosition.index = i;
+ expectedPenetrationPoints.first.meshLocalCoordinate.setValue(triangleLocalPosition);
+ triangleLocalPosition.index = 0;
+ expectedPenetrationPoints.second.meshLocalCoordinate.setValue(triangleLocalPosition);
+ auto contact = std::make_shared<TriangleContact>(expectedDepth, expectedContact, expectedNormal,
+ expectedPenetrationPoints);
+ contact->firstVertices = baseTriangles->getTrianglePositions(baseTriangles->getNumTriangles() - 1);
+ contact->secondVertices = intersectingTriangle->getTrianglePositions(0);
+ for (size_t i = 0; i < 3; ++i)
+ {
+ contact->firstVertices[i] = contact->firstVertices[i];
+ contact->secondVertices[i] = contact->secondVertices[i];
+ }
+ expectedContacts.push_back(contact);
+ }
+ }
+ auto baseMesh = std::make_shared<MeshShape>(*baseTriangles);
+
+ // Looking in -y, triangle A points in +y, +z is left, +x is down
+ // |-------| => k
+ // * * * * * * * * * * * * * -----
+ // * * * |
+ // * * * | => 2*k on right, 2*(1-k) on left
+ // * * * |
+ // * * -----
+ // * *
+ // *
+ //
+ // Looking in -z, triangle B points in +z, +y is up, +x is right
+ // (1) When triangle A sticks sufficiently out of triangle B
+ // i.e. k >= 1/8 && k <= 7/8
+ //
+ // * -----
+ // * * | => 1/2
+ // * * |
+ // * * * * * -----
+ // * * | => 1/2
+ // * * |
+ // * * * * * * * * * * * * * -----
+ // |-----| => 1/4
+ //
+ // (2) When triangle A only partially sticks out of triangle B
+ // i.e. k > 7/8 || k < 1/8
+ // * -----
+ // * * | => 1/2
+ // * * |
+ // * * * -----
+ // * * | => 1/2
+ // * * |
+ // * * * * * * * * * * * * * -----
+ // |--| => 2k
+ //
+
+ doTriangleMeshTriangleMeshTest(baseMesh, pose, triangleMesh,
+ pose, expectedContacts);
+ }
+}
+
+TEST(TriangleMeshTriangleMeshContactCalculationTests, IntersectionTestAtIdenticalDepth)
+{
+ using SurgSim::Math::MeshShape;
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::RigidTransform3d;
+
+ RigidTransform3d pose;
+ pose = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(0.4638, Vector3d(0.5727, 0.2485, 1.532).normalized()),
+ Vector3d(210.34, 675.23, -283.84));
+ pose = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(0.0, Vector3d(0.5727, 0.2485, 1.532).normalized()),
+ Vector3d(0, 0, 0));
+
+ {
+ // The second mesh.
+ const double e = 8e-11;
+ auto intersectingTriangle = std::make_shared<TriangleMeshPlain>();
+ addNewTriangle(intersectingTriangle,
+ Vector3d(e, 0.0, 0.0),
+ Vector3d(-0.5, 0.0, 1.0),
+ Vector3d(e, 0.0, 1.0));
+ auto triangleMesh = std::make_shared<MeshShape>(*intersectingTriangle);
+
+ // The first mesh.
+ auto baseTriangles = std::make_shared<TriangleMeshPlain>();
+
+ static const size_t numTriangles = 100;
+ SurgSim::DataStructures::IndexedLocalCoordinate triangleLocalPosition;
+
+ std::list<std::shared_ptr<Contact>> expectedContacts;
+ double expectedDepth;
+ double interval = 1.0 / static_cast<double>(numTriangles + 1);
+ double coordinate;
+ std::pair<Location, Location> expectedPenetrationPoints;
+ Vector3d expectedPoint0, expectedPoint1;
+ Vector3d expectedNormal, expectedContact(0, 0, 0);
+ for (size_t i = 0; i < numTriangles; i++)
+ {
+ coordinate = interval * static_cast<double>(i + 1);
+
+ addNewTriangle(baseTriangles,
+ Vector3d(-e, -coordinate, coordinate),
+ Vector3d(0.5, 1.0 - coordinate, coordinate),
+ Vector3d(-e, 1.0 - coordinate, coordinate));
+ expectedDepth = coordinate;
+ {
+ expectedPenetrationPoints.first.rigidLocalPosition.setValue(Vector3d(0, 0, coordinate));
+ expectedPenetrationPoints.second.rigidLocalPosition.setValue(Vector3d(0, 0, 0));
+ triangleLocalPosition.index = i;
+ expectedPenetrationPoints.first.meshLocalCoordinate.setValue(triangleLocalPosition);
+ triangleLocalPosition.index = 0;
+ expectedPenetrationPoints.second.meshLocalCoordinate.setValue(triangleLocalPosition);
+ auto contact = std::make_shared<TriangleContact>(expectedDepth, expectedContact,
+ pose.linear() * Vector3d(0, 0, -1),
+ expectedPenetrationPoints);
+ contact->firstVertices = baseTriangles->getTrianglePositions(baseTriangles->getNumTriangles() - 1);
+ contact->secondVertices = intersectingTriangle->getTrianglePositions(0);
+ for (size_t i = 0; i < 3; ++i)
+ {
+ contact->firstVertices[i] = contact->firstVertices[i];
+ contact->secondVertices[i] = contact->secondVertices[i];
+ }
+ expectedContacts.push_back(contact);
+ }
+ {
+ expectedPenetrationPoints.first.rigidLocalPosition.setValue(Vector3d(0, -coordinate, coordinate));
+ expectedPenetrationPoints.second.rigidLocalPosition.setValue(Vector3d(0, 0, coordinate));
+ triangleLocalPosition.index = i;
+ expectedPenetrationPoints.first.meshLocalCoordinate.setValue(triangleLocalPosition);
+ triangleLocalPosition.index = 0;
+ expectedPenetrationPoints.second.meshLocalCoordinate.setValue(triangleLocalPosition);
+ auto contact = std::make_shared<TriangleContact>(expectedDepth, expectedContact,
+ pose.linear() * Vector3d(0, 1, 0),
+ expectedPenetrationPoints);
+ contact->firstVertices = baseTriangles->getTrianglePositions(baseTriangles->getNumTriangles() - 1);
+ contact->secondVertices = intersectingTriangle->getTrianglePositions(0);
+ for (size_t i = 0; i < 3; ++i)
+ {
+ contact->firstVertices[i] = contact->firstVertices[i];
+ contact->secondVertices[i] = contact->secondVertices[i];
+ }
+ expectedContacts.push_back(contact);
+ }
+ }
+ auto baseMesh = std::make_shared<MeshShape>(*baseTriangles);
+
+ std::shared_ptr<ShapeCollisionRepresentation> meshARep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Mesh 0");
+ meshARep->setShape(baseMesh);
+ meshARep->setLocalPose(pose);
+
+ std::shared_ptr<ShapeCollisionRepresentation> meshBRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Mesh 1");
+ meshBRep->setShape(triangleMesh);
+ meshBRep->setLocalPose(pose);
+
+ // Perform collision detection.
+ TriangleMeshTriangleMeshDcdContact calcContact;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(meshARep, meshBRep);
+ calcContact.calculateContact(pair);
+
+ auto& calculatedContacts = pair->getContacts();
+ EXPECT_EQ(numTriangles, calculatedContacts.size());
+
+ for (auto it = calculatedContacts.begin(); it != calculatedContacts.end(); ++it)
+ {
+ EXPECT_TRUE(isContactPresentInList(*it, expectedContacts, false));
+ }
+ }
+}
+
+
+}; // namespace Collision
+}; // namespace Surgsim
diff --git a/SurgSim/Collision/UnitTests/config.txt.in b/SurgSim/Collision/UnitTests/config.txt.in
new file mode 100644
index 0000000..9c1cf05
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/config.txt.in
@@ -0,0 +1 @@
+${CMAKE_CURRENT_BINARY_DIR}/Data/
\ No newline at end of file
diff --git a/SurgSim/DataStructures/AabbTree.cpp b/SurgSim/DataStructures/AabbTree.cpp
new file mode 100644
index 0000000..52e4477
--- /dev/null
+++ b/SurgSim/DataStructures/AabbTree.cpp
@@ -0,0 +1,117 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/AabbTreeNode.h"
+
+#include <memory>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+AabbTree::AabbTree() :
+ m_maxObjectsPerNode(3)
+{
+ m_typedRoot = std::make_shared<AabbTreeNode>();
+ setRoot(m_typedRoot);
+}
+
+AabbTree::AabbTree(size_t maxObjectsPerNode) :
+ m_maxObjectsPerNode(maxObjectsPerNode)
+{
+ m_typedRoot = std::make_shared<AabbTreeNode>();
+ setRoot(m_typedRoot);
+}
+
+AabbTree::~AabbTree()
+{
+
+}
+
+void AabbTree::add(const SurgSim::Math::Aabbd& aabb, size_t objectId)
+{
+ m_typedRoot->addData(aabb, objectId, m_maxObjectsPerNode);
+}
+
+size_t AabbTree::getMaxObjectsPerNode() const
+{
+ return m_maxObjectsPerNode;
+}
+
+const SurgSim::Math::Aabbd& AabbTree::getAabb() const
+{
+ return m_typedRoot->getAabb();
+}
+
+std::list<AabbTree::TreeNodePairType> AabbTree::spatialJoin(const AabbTree& otherTree) const
+{
+ std::list<TreeNodePairType> result;
+
+ spatialJoin(std::static_pointer_cast<AabbTreeNode>(getRoot()),
+ std::static_pointer_cast<AabbTreeNode>(otherTree.getRoot()),
+ &result);
+
+ return result;
+}
+
+void AabbTree::spatialJoin(std::shared_ptr<AabbTreeNode> lhsParent,
+ std::shared_ptr<AabbTreeNode> rhsParent,
+ std::list<TreeNodePairType>* result) const
+{
+ if (!SurgSim::Math::doAabbIntersect(lhsParent->getAabb(), rhsParent->getAabb()))
+ {
+ return;
+ }
+
+ if ((lhsParent->getNumChildren() == 0) && (rhsParent->getNumChildren() == 0))
+ {
+ result->emplace_back(lhsParent, rhsParent);
+ }
+ else if (lhsParent->getNumChildren() == 0)
+ {
+ for (size_t j = 0; j < rhsParent->getNumChildren(); j++)
+ {
+ auto rhs = std::static_pointer_cast<AabbTreeNode>(rhsParent->getChild(j));
+ spatialJoin(lhsParent, rhs, result);
+ }
+ }
+ else if (rhsParent->getNumChildren() == 0)
+ {
+ for (size_t i = 0; i < lhsParent->getNumChildren(); i++)
+ {
+ auto lhs = std::static_pointer_cast<AabbTreeNode>(lhsParent->getChild(i));
+ spatialJoin(lhs, rhsParent, result);
+ }
+ }
+ else
+ {
+ for (size_t i = 0; i < lhsParent->getNumChildren(); i++)
+ {
+ auto lhs = std::static_pointer_cast<AabbTreeNode>(lhsParent->getChild(i));
+
+ for (size_t j = 0; j < rhsParent->getNumChildren(); j++)
+ {
+ auto rhs = std::static_pointer_cast<AabbTreeNode>(rhsParent->getChild(j));
+ spatialJoin(lhs, rhs, result);
+ }
+ }
+ }
+}
+
+}
+}
+
diff --git a/SurgSim/DataStructures/AabbTree.h b/SurgSim/DataStructures/AabbTree.h
new file mode 100644
index 0000000..4eb85a8
--- /dev/null
+++ b/SurgSim/DataStructures/AabbTree.h
@@ -0,0 +1,90 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_AABBTREE_H
+#define SURGSIM_DATASTRUCTURES_AABBTREE_H
+
+#include <list>
+
+#include "SurgSim/DataStructures/Tree.h"
+
+#include "SurgSim/Math/Aabb.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+class AabbTreeNode;
+
+/// AabbTree is a tree that is organized by the bounding boxes of the referenced objects, the bounding box used is
+/// the Axis Aligned Bounding Box (AABB), with the extents of an AABB describing the min and max of each coordinate
+/// for the given object.
+class AabbTree : public Tree
+{
+public:
+
+ /// Constructor
+ AabbTree();
+
+ /// Constructor
+ /// \param maxObjectsPerNode if the number of objects exceeds this a split of the node will be triggered
+ explicit AabbTree(size_t maxObjectsPerNode);
+
+ /// Destructor
+ virtual ~AabbTree();
+
+ /// \return the number of objects per node that will trigger a split for this tree
+ size_t getMaxObjectsPerNode() const;
+
+
+ /// Add a give object identified by objectId to the tree, this id should be unqiue on the users side, but no
+ /// checks are made in the inside of the tree
+ /// \param aabb AABB of this object.
+ /// \param objectId Id for the object to be identified with this bounding box
+ void add(const SurgSim::Math::Aabbd& aabb, size_t objectId);
+
+ const SurgSim::Math::Aabbd& getAabb() const;
+
+ /// Type indicating a relationship between two AabbTreeNodes
+ typedef std::pair<std::shared_ptr<AabbTreeNode>, std::shared_ptr<AabbTreeNode>> TreeNodePairType;
+
+ /// Query to find all pairs of intersecting nodes between two aabb r-trees.
+ /// \param otherTree The other tree to compare against
+ /// return The list of all pairs of intersecting nodes
+ std::list<TreeNodePairType> spatialJoin(const AabbTree& otherTree) const;
+
+ /// Query to find all pairs of intersecting nodes between two aabb r-trees.
+ /// \param lhsParent root node of the first tree
+ /// \param rhsParent root node of the second tree
+ /// \param result the list of all pairs of intersecting nodes
+ void spatialJoin(std::shared_ptr<AabbTreeNode> lhsParent,
+ std::shared_ptr<AabbTreeNode> rhsParent,
+ std::list<TreeNodePairType>* result) const;
+
+private:
+
+ /// Number of objects in a node that will trigger a split
+ size_t m_maxObjectsPerNode;
+
+ /// A typed version of the root for access without typecasting
+ std::shared_ptr<AabbTreeNode> m_typedRoot;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/DataStructures/AabbTreeData.cpp b/SurgSim/DataStructures/AabbTreeData.cpp
new file mode 100644
index 0000000..5a0fe99
--- /dev/null
+++ b/SurgSim/DataStructures/AabbTreeData.cpp
@@ -0,0 +1,133 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/AabbTreeData.h"
+
+#include <algorithm>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+AabbTreeData::AabbTreeData()
+{
+
+}
+
+AabbTreeData::~AabbTreeData()
+{
+
+}
+
+bool AabbTreeData::isEqual(const TreeData* data) const
+{
+ // The type safety of this is guaranteed by the == operator in TreeData
+ const AabbTreeData* treeData = static_cast<const AabbTreeData*>(data);
+ bool result = false;
+ if (getSize() == treeData->getSize() && getAabb().isApprox(treeData->getAabb()))
+ {
+ result = true;
+ auto it = m_data.cbegin();
+ auto endIt = m_data.cend();
+ auto otherEnd = treeData->m_data.cend();
+ auto functor = [it](const Item & other)
+ {
+ return ((*it).second == other.second && (*it).first.isApprox(other.first));
+ };
+ for (; it != endIt; ++it)
+ {
+ if (std::find_if(treeData->m_data.cbegin(), treeData->m_data.cend(), functor) == otherEnd)
+ {
+ result = false;
+ break;
+ }
+ }
+
+ }
+ return result;
+}
+
+void AabbTreeData::add(const SurgSim::Math::Aabbd aabb, size_t id)
+{
+ m_aabb.extend(aabb);
+ m_data.emplace_back(aabb, id);
+}
+
+const SurgSim::Math::Aabbd& AabbTreeData::getAabb() const
+{
+ return m_aabb;
+}
+
+
+bool AabbTreeData::isEmpty() const
+{
+ return m_data.empty();
+}
+
+size_t AabbTreeData::getSize() const
+{
+ return m_data.size();
+}
+
+std::shared_ptr<AabbTreeData> AabbTreeData::takeLargerElements()
+{
+ std::shared_ptr<AabbTreeData> result(std::make_shared<AabbTreeData>());
+
+ int axis;
+ m_aabb.sizes().maxCoeff(&axis);
+ double centerValue = m_aabb.center()(axis);
+ auto functor = [centerValue, axis](const Item & item)
+ {
+ return item.first.center()(axis) < centerValue;
+ };
+ auto split = std::partition(m_data.begin(), m_data.end(), functor);
+ result->m_data.splice(result->m_data.end(), m_data, split, m_data.end());
+ recalculateAabb();
+ result->recalculateAabb();
+
+ return std::move(result);
+}
+
+void AabbTreeData::recalculateAabb()
+{
+ m_aabb.setNull();
+ std::for_each(m_data.begin(), m_data.end(), [&](const Item & item)
+ {
+ m_aabb.extend(item.first);
+ });
+}
+
+void AabbTreeData::getIntersections(const SurgSim::Math::Aabbd& aabb, std::list<size_t>* result) const
+{
+ std::for_each(m_data.begin(), m_data.end(), [&](const Item & item)
+ {
+ if (SurgSim::Math::doAabbIntersect(item.first, aabb))
+ {
+ result->push_back(item.second);
+ }
+ });
+}
+
+bool AabbTreeData::hasIntersections(const SurgSim::Math::Aabbd& aabb) const
+{
+ return SurgSim::Math::doAabbIntersect(m_aabb, aabb);
+}
+
+
+}
+}
+
+
diff --git a/SurgSim/DataStructures/AabbTreeData.h b/SurgSim/DataStructures/AabbTreeData.h
new file mode 100644
index 0000000..8baf14e
--- /dev/null
+++ b/SurgSim/DataStructures/AabbTreeData.h
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_AABBTREEDATA_H
+#define SURGSIM_DATASTRUCTURES_AABBTREEDATA_H
+
+#include "SurgSim/DataStructures/TreeData.h"
+
+#include "SurgSim/Math/Aabb.h"
+
+#include <utility>
+#include <list>
+#include <memory>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// Internal class to hold a list of AABBs and their respective object ids, it can calculate the elements
+/// that intersect with a given aabb each node in the AABB tree holds one of these.
+class AabbTreeData : public TreeData
+{
+public:
+
+ /// Constructor
+ AabbTreeData();
+
+ /// Copy Constructor
+ AabbTreeData(const AabbTreeData& data);
+
+ /// Destructor
+ ~AabbTreeData();
+
+ typedef std::pair<SurgSim::Math::Aabbd, size_t> Item;
+
+ /// Add an item to the data
+ /// \param aabb the AABB of the item
+ /// \param id an object identifier assigned by the user of this class
+ void add(const SurgSim::Math::Aabbd aabb, size_t id);
+
+ /// \return the combined AABB of all the contained items
+ const SurgSim::Math::Aabbd& getAabb() const;
+
+ /// \return true when there are no items, false otherwise
+ bool isEmpty() const;
+
+ /// \return the number of items
+ size_t getSize() const;
+
+ /// Split the current items into two geometric halves, keep the first half and return a pointer to the second half.
+ /// The split is done along the longest axis of the enclosing aabb, the center of this axis is the point where
+ /// the split occurs. This object will keep items that have a smaller coordinate than the center, the result will
+ /// receive all items that have a larger coordinate on the determined axis.
+ /// \return AabbTreeData with the items to the right of the center of the longest axis.
+ std::shared_ptr<AabbTreeData> takeLargerElements();
+
+ /// Check whether there could be any intersections with a given bounding box.
+ /// \param aabb bounding box to use for the intersection check.
+ /// \return true if the given AABB intersects with the AABB of all contained items.
+ bool hasIntersections(const SurgSim::Math::Aabbd& aabb) const;
+
+ /// Check all items bounding boxes against the one passed as a parameter and append items that overlap
+ /// to the list given as a parameter
+ /// \param aabb the bounding box being queried
+ /// \param [out] result list to be used for intersecting items
+ void getIntersections(const SurgSim::Math::Aabbd& aabb, std::list<size_t>* result) const;
+
+private:
+ /// Recalculate the aabb of this class, in case items where updated
+ void recalculateAabb();
+
+ virtual bool isEqual(const TreeData* data) const override;
+
+ /// AABB containg all items
+ SurgSim::Math::Aabbd m_aabb;
+
+ /// The items that were added to this list
+ std::list<Item> m_data;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/DataStructures/AabbTreeIntersectionVisitor.cpp b/SurgSim/DataStructures/AabbTreeIntersectionVisitor.cpp
new file mode 100644
index 0000000..8bc03b7
--- /dev/null
+++ b/SurgSim/DataStructures/AabbTreeIntersectionVisitor.cpp
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/AabbTreeIntersectionVisitor.h"
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/DataStructures/AabbTreeNode.h"
+#include "SurgSim/Math/Aabb.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+AabbTreeIntersectionVisitor::AabbTreeIntersectionVisitor()
+{
+
+}
+
+AabbTreeIntersectionVisitor::AabbTreeIntersectionVisitor(const SurgSim::Math::Aabbd& aabb) :
+ m_aabb(aabb)
+{
+
+}
+
+AabbTreeIntersectionVisitor::~AabbTreeIntersectionVisitor()
+{
+
+}
+
+bool AabbTreeIntersectionVisitor::handle(TreeNode* node)
+{
+ SURGSIM_FAILURE() << "Can only handle AabbTreeNodes, this is of a different type.";
+ return false;
+}
+
+bool AabbTreeIntersectionVisitor::handle(AabbTreeNode* node)
+{
+ bool result = false;
+
+ if (node->getNumChildren() == 0)
+ {
+ // On a leaf node, perform the intersections
+ node->getIntersections(m_aabb, &m_intersections);
+ }
+ else
+ {
+ // On non leaf nodes check if we have an intersection with the common aabb
+ result = SurgSim::Math::doAabbIntersect(m_aabb, node->getAabb());
+ }
+
+ return result;
+}
+
+
+void AabbTreeIntersectionVisitor::reset()
+{
+ m_intersections.clear();
+}
+
+SurgSim::Math::Aabbd AabbTreeIntersectionVisitor::getAabb() const
+{
+ return m_aabb;
+}
+
+
+void AabbTreeIntersectionVisitor::setAabb(const SurgSim::Math::Aabbd& aabb)
+{
+ m_aabb = aabb;
+ reset();
+}
+
+
+const std::list<size_t>& AabbTreeIntersectionVisitor::getIntersections() const
+{
+ return m_intersections;
+}
+
+bool AabbTreeIntersectionVisitor::hasIntersections() const
+{
+ return !m_intersections.empty();
+}
+
+}
+}
+
diff --git a/SurgSim/DataStructures/AabbTreeIntersectionVisitor.h b/SurgSim/DataStructures/AabbTreeIntersectionVisitor.h
new file mode 100644
index 0000000..eae01c5
--- /dev/null
+++ b/SurgSim/DataStructures/AabbTreeIntersectionVisitor.h
@@ -0,0 +1,76 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_AABBTREEINTERSECTIONVISITOR_H
+#define SURGSIM_DATASTRUCTURES_AABBTREEINTERSECTIONVISITOR_H
+
+#include "SurgSim/DataStructures/TreeVisitor.h"
+#include "SurgSim/Math/Aabb.h"
+
+#include <list>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// Visitor class to collect the items that intersect with a given bounding box
+class AabbTreeIntersectionVisitor : public TreeVisitor
+{
+public:
+
+ /// Constructor
+ AabbTreeIntersectionVisitor();
+
+ /// Constructor
+ /// \param aabb the bounding box to be used.
+ explicit AabbTreeIntersectionVisitor(const SurgSim::Math::Aabbd& aabb);
+
+ /// Destructor
+ virtual ~AabbTreeIntersectionVisitor();
+
+ virtual bool handle(TreeNode* node) override;
+
+ virtual bool handle(AabbTreeNode* node) override;
+
+ /// \return true if the visitor has found intersections
+ bool hasIntersections() const;
+
+ /// Resets the data in the tree
+ void reset();
+
+ /// \return the bounding box to be used for the test.
+ SurgSim::Math::Aabbd getAabb() const;
+
+ /// Sets a new bounding box, will also call reset()
+ /// \param aabb The new bounding box.
+ void setAabb(const SurgSim::Math::Aabbd& aabb);
+
+ /// \return a reference to the found intersections.
+ const std::list<size_t>& getIntersections() const;
+
+private:
+
+ /// List of ids found for intersections
+ std::list<size_t> m_intersections;
+
+ /// Bounding box used for intersection test
+ SurgSim::Math::Aabbd m_aabb;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/DataStructures/AabbTreeNode.cpp b/SurgSim/DataStructures/AabbTreeNode.cpp
new file mode 100644
index 0000000..c9ff71f
--- /dev/null
+++ b/SurgSim/DataStructures/AabbTreeNode.cpp
@@ -0,0 +1,115 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/AabbTreeNode.h"
+#include "SurgSim/DataStructures/AabbTreeData.h"
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+AabbTreeNode::AabbTreeNode()
+{
+}
+
+AabbTreeNode::~AabbTreeNode()
+{
+
+}
+
+void AabbTreeNode::splitNode()
+{
+ auto leftData = std::static_pointer_cast<AabbTreeData>(getData());
+ if (leftData->getSize() > 0)
+ {
+ std::shared_ptr<AabbTreeData> rightData = leftData->takeLargerElements();
+ if (getNumChildren() != 2)
+ {
+ std::shared_ptr<TreeNode> child = std::make_shared<AabbTreeNode>();
+ addChild(std::move(child));
+
+ child = std::make_shared<AabbTreeNode>();
+ addChild(std::move(child));
+ }
+
+ // Update the local aabb
+ // The axis won't change after it has been split
+ m_aabb = leftData->getAabb();
+ m_aabb.extend(rightData->getAabb());
+ m_aabb.sizes().maxCoeff(&m_axis);
+
+ getChild(0)->setData(std::move(leftData));
+ getChild(1)->setData(std::move(rightData));
+ setData(nullptr);
+ }
+}
+
+const SurgSim::Math::Aabbd& AabbTreeNode::getAabb() const
+{
+ auto data = std::static_pointer_cast<AabbTreeData>(getData());
+ if (data == nullptr)
+ {
+ return m_aabb;
+ }
+ else
+ {
+ return data->getAabb();
+ }
+}
+
+void AabbTreeNode::addData(const SurgSim::Math::Aabbd& aabb, size_t id, size_t maxNodeData)
+{
+
+ if (getNumChildren() > 0)
+ {
+ size_t childIndex = (aabb.center()(m_axis) < m_aabb.center()(m_axis)) ? 0 : 1;
+ auto childNode = std::static_pointer_cast<AabbTreeNode>(getChild(childIndex));
+ childNode->addData(aabb, id, maxNodeData);
+ m_aabb.extend(aabb);
+ }
+ else
+ {
+ auto data = std::static_pointer_cast<AabbTreeData>(getData());
+ if (data == nullptr)
+ {
+ data = std::make_shared<AabbTreeData>();
+ setData(data);
+ }
+ data->add(aabb, id);
+ if (maxNodeData > 0 && data->getSize() > maxNodeData)
+ {
+ splitNode();
+ }
+ }
+}
+
+bool AabbTreeNode::doAccept(TreeVisitor* visitor)
+{
+ return visitor->handle(this);
+}
+
+void AabbTreeNode::getIntersections(const SurgSim::Math::Aabbd& aabb, std::list<size_t>* result)
+{
+ auto data = std::static_pointer_cast<AabbTreeData>(getData());
+ SURGSIM_ASSERT(data != nullptr) << "AabbTreeNode data is nullptr.";
+ data->getIntersections(aabb, result);
+}
+
+}
+}
+
diff --git a/SurgSim/DataStructures/AabbTreeNode.h b/SurgSim/DataStructures/AabbTreeNode.h
new file mode 100644
index 0000000..eb92679
--- /dev/null
+++ b/SurgSim/DataStructures/AabbTreeNode.h
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_AABBTREENODE_H
+#define SURGSIM_DATASTRUCTURES_AABBTREENODE_H
+
+#include "SurgSim/DataStructures/TreeNode.h"
+#include "SurgSim/DataStructures/AabbTreeData.h"
+
+#include "SurgSim/Math/Aabb.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// Node class for the AabbTree, this handles groups of items and subdivision if the number of items gets too big
+class AabbTreeNode : public TreeNode
+{
+public:
+
+ /// Constructor
+ AabbTreeNode();
+
+ /// Destructor
+ virtual ~AabbTreeNode();
+
+ /// Splits the data into two parts, creates two children and puts the split data into the children
+ /// the aabb of this node does not change, the data of this node will be empty after this.
+ void splitNode();
+
+ /// Get the aabb of this node, it is the union of the aabb of all the items in the data when the node
+ /// has data, or all the union of the aabb trees of all the sub-nodes.
+ /// \return The aabb box of this node.
+ const SurgSim::Math::Aabbd& getAabb() const;
+
+ /// Add data to this node, if maxNodeData is >0 the node will split if the number of data items exceeds maxNodeData
+ /// \param aabb The aabb for the item to be added.
+ /// \param id The id for the item that is being added, handled by the user of this class.
+ /// \param maxNodeData number of maximum items of data in this node, if more, the node will split,
+ /// if -1 the node will not be split.
+ void addData(const SurgSim::Math::Aabbd& aabb, size_t id, size_t maxNodeData = -1);
+
+ /// Fetch a list of items that have AABBs intersecting with the given AABB.
+ /// \param aabb The bounding box for the query.
+ /// \param [out] result location to receive the results of the call.
+ void getIntersections(const SurgSim::Math::Aabbd& aabb, std::list<size_t>* result);
+
+protected:
+
+ virtual bool doAccept(TreeVisitor* visitor) override;
+
+private:
+
+ /// The internal bounding box for this node, it is used when the node does not have any data
+ SurgSim::Math::Aabbd m_aabb;
+
+ /// Cache for the index of the longest axis on this node
+ size_t m_axis;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/DataStructures/BufferedValue-inl.h b/SurgSim/DataStructures/BufferedValue-inl.h
new file mode 100644
index 0000000..bde013d
--- /dev/null
+++ b/SurgSim/DataStructures/BufferedValue-inl.h
@@ -0,0 +1,65 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_BUFFEREDVALUE_INL_H
+#define SURGSIM_DATASTRUCTURES_BUFFEREDVALUE_INL_H
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <class T>
+BufferedValue<T>::BufferedValue()
+{
+ m_safeValue = std::make_shared<const T>();
+}
+
+template <class T>
+BufferedValue<T>::BufferedValue(const T& value) :
+ m_value(value)
+{
+ m_safeValue = std::make_shared<const T>(m_value);
+}
+
+template <class T>
+BufferedValue<T>::~BufferedValue()
+{
+}
+
+template <class T>
+void BufferedValue<T>::publish()
+{
+ UniqueLock lock(m_mutex);
+ m_safeValue = std::make_shared<const T>(m_value);
+}
+
+template <class T>
+T& BufferedValue<T>::unsafeGet()
+{
+ return m_value;
+}
+
+template <class T>
+std::shared_ptr<const T> BufferedValue<T>::safeGet() const
+{
+ SharedLock lock(m_mutex);
+ return m_safeValue;
+}
+
+} // DataStructures
+} // SurgSim
+
+#endif
diff --git a/SurgSim/DataStructures/BufferedValue.h b/SurgSim/DataStructures/BufferedValue.h
new file mode 100644
index 0000000..7ba8e68
--- /dev/null
+++ b/SurgSim/DataStructures/BufferedValue.h
@@ -0,0 +1,78 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_BUFFEREDVALUE_H
+#define SURGSIM_DATASTRUCTURES_BUFFEREDVALUE_H
+
+#include <memory>
+#include <utility>
+#include <boost/thread.hpp>
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// BufferedValue is a class to enable a representation of two values for one variable, where both values need to be
+/// accessible at the same time, one in a thread safe, single threaded context, the other in a thread unsafe context.
+/// \tparam T Type that is used for the value.
+template <class T>
+class BufferedValue
+{
+public:
+
+ // Default Constructor
+ BufferedValue();
+
+ // Constructor
+ /// \param value Default value.
+ explicit BufferedValue(const T& value);
+
+ /// Destructor
+ ~BufferedValue();
+
+ /// Make the current value the one returned by calls to safeGet.
+ void publish();
+
+ /// Get the value
+ /// \return A reference to the value.
+ T& unsafeGet();
+
+ /// Get the buffered value
+ /// \return The value at the last call to publish.
+ std::shared_ptr<const T> safeGet() const;
+
+private:
+ typedef boost::shared_lock<boost::shared_mutex> SharedLock;
+ typedef boost::unique_lock<boost::shared_mutex> UniqueLock;
+
+ /// The raw value
+ T m_value;
+
+ /// The buffered value
+ std::shared_ptr<const T> m_safeValue;
+
+ /// The mutex used to lock for reading and writing
+ mutable boost::shared_mutex m_mutex;
+
+};
+
+} // DataStructures
+} // SurgSim
+
+#include "SurgSim/DataStructures/BufferedValue-inl.h"
+
+#endif
diff --git a/SurgSim/DataStructures/CMakeLists.txt b/SurgSim/DataStructures/CMakeLists.txt
new file mode 100644
index 0000000..7165ff2
--- /dev/null
+++ b/SurgSim/DataStructures/CMakeLists.txt
@@ -0,0 +1,108 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+set(SURGSIM_DATA_STRUCTURES_SOURCES
+ AabbTree.cpp
+ AabbTreeData.cpp
+ AabbTreeIntersectionVisitor.cpp
+ AabbTreeNode.cpp
+ DataGroup.cpp
+ DataGroupBuilder.cpp
+ DataGroupCopier.cpp
+ IndexDirectory.cpp
+ IndexedLocalCoordinate.cpp
+ OctreeNode.cpp
+ ply.c
+ PlyReader.cpp
+ Tree.cpp
+ TreeData.cpp
+ TreeNode.cpp
+ TriangleMesh.cpp
+ TriangleMeshUtilities.cpp
+)
+
+set(SURGSIM_DATA_STRUCTURES_HEADERS
+ AabbTree.h
+ AabbTreeData.h
+ AabbTreeIntersectionVisitor.h
+ AabbTreeNode.h
+ BufferedValue.h
+ BufferedValue-inl.h
+ DataGroup.h
+ DataGroupBuilder.h
+ DataGroupCopier.h
+ DataStructuresConvert.h
+ DataStructuresConvert-inl.h
+ EmptyData.h
+ Image.h
+ Image-inl.h
+ IndexDirectory.h
+ IndexedLocalCoordinate.h
+ Location.h
+ MeshElement.h
+ NamedData.h
+ NamedData-inl.h
+ NamedDataBuilder.h
+ NamedVariantData.h
+ NamedVariantData-inl.h
+ OctreeNode.h
+ OctreeNode-inl.h
+ OptionalValue.h
+ ply.h
+ PlyReader.h
+ PlyReaderDelegate.h
+ TetrahedronMesh.h
+ TetrahedronMesh-inl.h
+ Tree.h
+ TreeData.h
+ TreeNode.h
+ TreeVisitor.h
+ TriangleMesh.h
+ TriangleMesh-inl.h
+ TriangleMeshBase.h
+ TriangleMeshBase-inl.h
+ TriangleMeshPlyReaderDelegate.h
+ TriangleMeshPlyReaderDelegate-inl.h
+ TriangleMeshUtilities.h
+ TriangleMeshUtilities-inl.h
+ Vertex.h
+ Vertices.h
+)
+
+surgsim_add_library(
+ SurgSimDataStructures
+ "${SURGSIM_DATA_STRUCTURES_SOURCES}"
+ "${SURGSIM_DATA_STRUCTURES_HEADERS}"
+ "SurgSim/DataStructures"
+)
+
+set(LIBS
+ SurgSimFramework
+ ${Boost_LIBRARIES}
+)
+
+target_link_libraries(SurgSimDataStructures ${LIBS}
+)
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+
+ if(BUILD_PERFORMANCE_TESTING)
+ add_subdirectory(PerformanceTests)
+ endif()
+endif()
+
+set_target_properties(SurgSimDataStructures PROPERTIES FOLDER "DataStructures")
diff --git a/SurgSim/DataStructures/DataGroup.cpp b/SurgSim/DataStructures/DataGroup.cpp
new file mode 100644
index 0000000..15f6ca8
--- /dev/null
+++ b/SurgSim/DataStructures/DataGroup.cpp
@@ -0,0 +1,173 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/DataGroup.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+DataGroup::DataGroup()
+{
+}
+
+DataGroup::DataGroup(const DataGroup& dataGroup) :
+ m_poses(dataGroup.m_poses),
+ m_vectors(dataGroup.m_vectors),
+ m_matrices(dataGroup.m_matrices),
+ m_scalars(dataGroup.m_scalars),
+ m_integers(dataGroup.m_integers),
+ m_booleans(dataGroup.m_booleans),
+ m_strings(dataGroup.m_strings),
+ m_customData(dataGroup.m_customData)
+{
+}
+
+DataGroup& DataGroup::operator=(const DataGroup& dataGroup)
+{
+ m_poses = dataGroup.m_poses;
+ m_vectors = dataGroup.m_vectors;
+ m_matrices = dataGroup.m_matrices;
+ m_scalars = dataGroup.m_scalars;
+ m_integers = dataGroup.m_integers;
+ m_booleans = dataGroup.m_booleans;
+ m_strings = dataGroup.m_strings;
+ m_customData = dataGroup.m_customData;
+
+ return *this;
+}
+
+DataGroup& DataGroup::operator=(DataGroup&& dataGroup)
+{
+ m_poses = std::move(dataGroup.m_poses);
+ m_vectors = std::move(dataGroup.m_vectors);
+ m_matrices = std::move(dataGroup.m_matrices);
+ m_scalars = std::move(dataGroup.m_scalars);
+ m_integers = std::move(dataGroup.m_integers);
+ m_booleans = std::move(dataGroup.m_booleans);
+ m_strings = std::move(dataGroup.m_strings);
+ m_customData = std::move(dataGroup.m_customData);
+
+ return *this;
+}
+
+NamedData<DataGroup::PoseType>& DataGroup::poses()
+{
+ return m_poses;
+}
+
+const NamedData<DataGroup::PoseType>& DataGroup::poses() const
+{
+ return m_poses;
+}
+
+NamedData<DataGroup::VectorType>& DataGroup::vectors()
+{
+ return m_vectors;
+}
+
+const NamedData<DataGroup::VectorType>& DataGroup::vectors() const
+{
+ return m_vectors;
+}
+
+NamedData<DataGroup::DynamicMatrixType>& DataGroup::matrices()
+{
+ return m_matrices;
+}
+
+const NamedData<DataGroup::DynamicMatrixType>& DataGroup::matrices() const
+{
+ return m_matrices;
+}
+
+
+NamedData<DataGroup::ScalarType>& DataGroup::scalars()
+{
+ return m_scalars;
+}
+
+const NamedData<DataGroup::ScalarType>& DataGroup::scalars() const
+{
+ return m_scalars;
+}
+
+NamedData<DataGroup::IntegerType>& DataGroup::integers()
+{
+ return m_integers;
+}
+
+const NamedData<DataGroup::IntegerType>& DataGroup::integers() const
+{
+ return m_integers;
+}
+
+NamedData<DataGroup::BooleanType>& DataGroup::booleans()
+{
+ return m_booleans;
+}
+
+const NamedData<DataGroup::BooleanType>& DataGroup::booleans() const
+{
+ return m_booleans;
+}
+
+NamedData<DataGroup::StringType>& DataGroup::strings()
+{
+ return m_strings;
+}
+
+const NamedData<DataGroup::StringType>& DataGroup::strings() const
+{
+ return m_strings;
+}
+
+NamedVariantData& DataGroup::customData()
+{
+ return m_customData;
+}
+
+const NamedVariantData& DataGroup::customData() const
+{
+ return m_customData;
+}
+
+void DataGroup::resetAll()
+{
+ m_poses.resetAll();
+ m_vectors.resetAll();
+ m_matrices.resetAll();
+ m_scalars.resetAll();
+ m_integers.resetAll();
+ m_booleans.resetAll();
+ m_strings.resetAll();
+ m_customData.resetAll();
+}
+
+bool DataGroup::isEmpty() const
+{
+ return !(m_poses.isValid() ||
+ m_vectors.isValid() ||
+ m_matrices.isValid() ||
+ m_scalars.isValid() ||
+ m_integers.isValid() ||
+ m_booleans.isValid() ||
+ m_strings.isValid() ||
+ m_customData.isValid());
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/DataGroup.h b/SurgSim/DataStructures/DataGroup.h
new file mode 100644
index 0000000..a8be01c
--- /dev/null
+++ b/SurgSim/DataStructures/DataGroup.h
@@ -0,0 +1,229 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_DATAGROUP_H
+#define SURGSIM_DATASTRUCTURES_DATAGROUP_H
+
+#include <Eigen/Core>
+
+#include "SurgSim/DataStructures/NamedData.h"
+#include "SurgSim/DataStructures/NamedVariantData.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// A collection of \ref NamedData objects.
+///
+/// A DataGroup object contains a NamedData for each of several predefined types:
+/// \li \em Poses contain the position and orientation of an object in space, represented as a 3D rigid-body
+/// (isometric) transformation.
+/// \li \em Vectors contain a vector quantity that does not change when the coordinate system is translated,
+/// such as a force or an oriented distance.
+/// \li \em Matrices contain a matrix.
+/// \li \em Scalars contain a scalar value (i.e. anything that can be represented as a double).
+/// \li \em Integers contain an integer value.
+/// \li \em Booleans contain a Boolean logic value (true or false).
+/// \li \em Strings contain a text value.
+/// \li \em CustomData contain a custom data structure, \ref NamedVariantData.
+///
+/// The entries (names and indices) are unique within each NamedData member, but not necessarily across different types
+/// (i.e. there could be a scalar and a vector both named "friction", or a pose and a boolean both at index 1).
+/// It is recommended that you keep names separate between different types to avoid confusion.
+///
+/// A DataGroup object constructed by the default constructor starts out empty, meaning all its NamedData member
+/// objects are "invalid". An empty DataGroup object can be made non-empty by:
+/// \li using the \ref DataGroupBuilder class,
+/// \li copy construction,
+/// \li assigning from a non-empty DataGroup object, or
+/// \li assigning a "valid" NamedData object (of the correct template type) to one or more NamedData members.
+///
+/// Assignment to a non-empty DataGroup object is only possible if either of the two objects in the assignment was made
+/// non-empty based on the other object (see the above list items about copy construction and assignment from a
+/// non-empty DataGroup object).
+///
+/// Once a DataGroup is non-empty, the "entries" (i.e., the strings and indices that are used to access the data)
+/// cannot be changed, added to, removed from, or made empty. These properties ensure that a stable data layout is
+/// available to the code using this class. For example, the calling code can cache the entries' indices and from then
+/// on use the faster index-based lookup instead of the slower string-based lookup.
+///
+/// \sa SurgSim::DataStructures::NamedData, SurgSim::DataStructers::DataGroupBuilder
+class DataGroup
+{
+public:
+ /// The type used for poses.
+ typedef SurgSim::Math::RigidTransform3d PoseType;
+ /// The type used for vectors.
+ typedef SurgSim::Math::Vector3d VectorType;
+ /// The type used for matrices.
+ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> DynamicMatrixType;
+ /// The type used for scalars.
+ typedef double ScalarType;
+ /// The type used for integers.
+ typedef int IntegerType;
+ /// The type used for booleans.
+ typedef bool BooleanType;
+ /// The type used for strings.
+ typedef std::string StringType;
+
+ /// Construct an empty object, with no associated names and indices yet.
+ DataGroup();
+
+ /// Construct an object as a copy of the data from another object.
+ /// \param dataGroup The object to copy from.
+ DataGroup(const DataGroup& dataGroup);
+
+ /// Copy the data from another object.
+ ///
+ /// The object being assigned into must either be empty (not yet associated with a set of names and indices), or
+ /// the two objects must share the same data layout, resulting from earlier copy construction or assignment.
+ /// ~~~~~
+ /// DataGroup initial;
+ /// // ...initialize "initial" to some non-empty value...
+ /// DataGroup copyConstructed(initial); // Layout is shared with initial
+ /// copyConstructed = initial // OK, using the same layout
+ /// DataGroup another; // Object is empty (no layout)
+ /// another = initial; // OK, layout is now shared with initial
+ /// another = initial // OK, using the same layout
+ /// ~~~~~
+ ///
+ /// Note that the data layout must be the same, i.e. related to one another by object assignment or copy
+ /// construction. Objects that merely contain entries with the same names and indices are not acceptable!
+ /// (Otherwise, we'd need to inefficiently compare layout contents each time we assign.)
+ /// ~~~~~
+ /// DataGroupBuilder builder;
+ /// // ...initialize the entries in the builder...
+ /// NamedData first = builder.createData(); // Layout of entries created from builder
+ /// NamedData second = builder.createData(); // Another layout of entries created; names and indices match
+ /// second = first; // ERROR at run-time, layouts were created separately!
+ /// ~~~~~
+ ///
+ /// \param dataGroup The object to copy from.
+ /// \return The object that was assigned into.
+ DataGroup& operator=(const DataGroup& dataGroup);
+
+ /// Move the data from another object.
+ ///
+ /// The same restrictions on object compatibility apply as in the case of the copy assignment
+ /// operator=(const DataGroup&).
+ ///
+ /// \param [in,out] dataGroup The object to copy from, which will be left in an unusable state.
+ /// \return The object that was assigned into.
+ DataGroup& operator=(DataGroup&& dataGroup);
+
+ /// Return the pose data structure.
+ /// \return the mutable pose data.
+ NamedData<PoseType>& poses();
+
+ /// Return the pose data structure.
+ /// \return the read-only pose data.
+ const NamedData<PoseType>& poses() const;
+
+ /// Return the vector data structure.
+ /// \return the mutable vector data.
+ NamedData<VectorType>& vectors();
+
+ /// Return the vector data structure.
+ /// \return the read-only vector data.
+ const NamedData<VectorType>& vectors() const;
+
+ /// Return the matrix data structure.
+ /// \return the mutable matrix data.
+ NamedData<DynamicMatrixType>& matrices();
+
+ /// Return the matrix data structure.
+ /// \return the read-only matrix data.
+ const NamedData<DynamicMatrixType>& matrices() const;
+
+ /// Return the scalar data structure.
+ /// \return the mutable scalar data.
+ NamedData<ScalarType>& scalars();
+
+ /// Return the scalar data structure.
+ /// \return the read-only scalar data.
+ const NamedData<ScalarType>& scalars() const;
+
+ /// Return the integer data structure.
+ /// \return the mutable integer data.
+ NamedData<IntegerType>& integers();
+
+ /// Return the integer data structure.
+ /// \return the read-only integer data.
+ const NamedData<IntegerType>& integers() const;
+
+ /// Return the boolean data structure.
+ /// \return the mutable Boolean data.
+ NamedData<BooleanType>& booleans();
+
+ /// Return the boolean data structure.
+ /// \return the read-only Boolean data.
+ const NamedData<BooleanType>& booleans() const;
+
+ /// Return the string data structure.
+ /// \return the mutable string data.
+ NamedData<StringType>& strings();
+
+ /// Return the string data structure.
+ /// \return the read-only string data.
+ const NamedData<StringType>& strings() const;
+
+ /// Return the custom data structure.
+ /// \return the mutable data.
+ NamedVariantData& customData();
+
+ /// Return the custom data structure.
+ /// \return the read-only data.
+ const NamedVariantData& customData() const;
+
+ /// Mark all data as not current.
+ void resetAll();
+
+ /// An empty DataGroup can be assigned to by any DataGroup with only valid NamedData.
+ /// return true if all the NamedData are invalid.
+ bool isEmpty() const;
+
+private:
+ /// The pose values.
+ NamedData<PoseType> m_poses;
+
+ /// The vector values.
+ NamedData<VectorType> m_vectors;
+
+ /// The matrix values.
+ NamedData<DynamicMatrixType> m_matrices;
+
+ /// The scalar values.
+ NamedData<ScalarType> m_scalars;
+
+ /// The integer values.
+ NamedData<IntegerType> m_integers;
+
+ /// The boolean values.
+ NamedData<BooleanType> m_booleans;
+
+ /// The string values.
+ NamedData<StringType> m_strings;
+
+ /// The custom data values.
+ NamedVariantData m_customData;
+};
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_DATAGROUP_H
diff --git a/SurgSim/DataStructures/DataGroupBuilder.cpp b/SurgSim/DataStructures/DataGroupBuilder.cpp
new file mode 100644
index 0000000..2a86267
--- /dev/null
+++ b/SurgSim/DataStructures/DataGroupBuilder.cpp
@@ -0,0 +1,194 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+
+DataGroupBuilder::DataGroupBuilder()
+{
+}
+
+DataGroup DataGroupBuilder::createData() const
+{
+ DataGroup data;
+ data.poses() = poses().createData();
+ data.vectors() = vectors().createData();
+ data.matrices() = matrices().createData();
+ data.scalars() = scalars().createData();
+ data.integers() = integers().createData();
+ data.booleans() = booleans().createData();
+ data.strings() = strings().createData();
+ data.customData() = customData().createData();
+ return data;
+}
+
+std::shared_ptr<DataGroup> DataGroupBuilder::createSharedData() const
+{
+ return std::make_shared<DataGroup>(createData());
+}
+
+NamedDataBuilder<DataGroupBuilder::PoseType>& DataGroupBuilder::poses()
+{
+ return m_poses;
+}
+
+const NamedDataBuilder<DataGroupBuilder::PoseType>& DataGroupBuilder::poses() const
+{
+ return m_poses;
+}
+
+NamedDataBuilder<DataGroupBuilder::VectorType>& DataGroupBuilder::vectors()
+{
+ return m_vectors;
+}
+
+const NamedDataBuilder<DataGroupBuilder::VectorType>& DataGroupBuilder::vectors() const
+{
+ return m_vectors;
+}
+
+NamedDataBuilder<DataGroupBuilder::DynamicMatrixType>& DataGroupBuilder::matrices()
+{
+ return m_matrices;
+}
+
+const NamedDataBuilder<DataGroupBuilder::DynamicMatrixType>& DataGroupBuilder::matrices() const
+{
+ return m_matrices;
+}
+
+NamedDataBuilder<DataGroupBuilder::ScalarType>& DataGroupBuilder::scalars()
+{
+ return m_scalars;
+}
+
+const NamedDataBuilder<DataGroupBuilder::ScalarType>& DataGroupBuilder::scalars() const
+{
+ return m_scalars;
+}
+
+NamedDataBuilder<DataGroupBuilder::IntegerType>& DataGroupBuilder::integers()
+{
+ return m_integers;
+}
+
+const NamedDataBuilder<DataGroupBuilder::IntegerType>& DataGroupBuilder::integers() const
+{
+ return m_integers;
+}
+
+NamedDataBuilder<DataGroupBuilder::BooleanType>& DataGroupBuilder::booleans()
+{
+ return m_booleans;
+}
+
+const NamedDataBuilder<DataGroupBuilder::BooleanType>& DataGroupBuilder::booleans() const
+{
+ return m_booleans;
+}
+
+NamedDataBuilder<DataGroupBuilder::StringType>& DataGroupBuilder::strings()
+{
+ return m_strings;
+}
+
+const NamedDataBuilder<DataGroupBuilder::StringType>& DataGroupBuilder::strings() const
+{
+ return m_strings;
+}
+
+NamedVariantDataBuilder& DataGroupBuilder::customData()
+{
+ return m_customData;
+}
+
+const NamedVariantDataBuilder& DataGroupBuilder::customData() const
+{
+ return m_customData;
+}
+
+void DataGroupBuilder::addPose(const std::string& name)
+{
+ poses().addEntry(name);
+}
+
+void DataGroupBuilder::addVector(const std::string& name)
+{
+ vectors().addEntry(name);
+}
+
+void DataGroupBuilder::addMatrix(const std::string& name)
+{
+ matrices().addEntry(name);
+}
+
+void DataGroupBuilder::addScalar(const std::string& name)
+{
+ scalars().addEntry(name);
+}
+
+void DataGroupBuilder::addInteger(const std::string& name)
+{
+ integers().addEntry(name);
+}
+
+void DataGroupBuilder::addBoolean(const std::string& name)
+{
+ booleans().addEntry(name);
+}
+
+void DataGroupBuilder::addString(const std::string& name)
+{
+ strings().addEntry(name);
+}
+
+void DataGroupBuilder::addCustom(const std::string& name)
+{
+ customData().addEntry(name);
+}
+
+
+void DataGroupBuilder::addEntriesFrom(const DataGroupBuilder& builder)
+{
+ poses().addEntriesFrom(builder.poses());
+ vectors().addEntriesFrom(builder.vectors());
+ matrices().addEntriesFrom(builder.matrices());
+ scalars().addEntriesFrom(builder.scalars());
+ integers().addEntriesFrom(builder.integers());
+ booleans().addEntriesFrom(builder.booleans());
+ strings().addEntriesFrom(builder.strings());
+ customData().addEntriesFrom(builder.customData());
+}
+
+void DataGroupBuilder::addEntriesFrom(const DataGroup& data)
+{
+ poses().addEntriesFrom(data.poses());
+ vectors().addEntriesFrom(data.vectors());
+ matrices().addEntriesFrom(data.matrices());
+ scalars().addEntriesFrom(data.scalars());
+ integers().addEntriesFrom(data.integers());
+ booleans().addEntriesFrom(data.booleans());
+ strings().addEntriesFrom(data.strings());
+ customData().addEntriesFrom(data.customData());
+}
+
+
+}; // namespace Input
+}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/DataGroupBuilder.h b/SurgSim/DataStructures/DataGroupBuilder.h
new file mode 100644
index 0000000..26bf489
--- /dev/null
+++ b/SurgSim/DataStructures/DataGroupBuilder.h
@@ -0,0 +1,206 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_DATAGROUPBUILDER_H
+#define SURGSIM_DATASTRUCTURES_DATAGROUPBUILDER_H
+
+#include "SurgSim/DataStructures/NamedDataBuilder.h"
+#include "SurgSim/DataStructures/NamedVariantData.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include <Eigen/Core>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// A class that allows you to build a \ref DataGroup structure.
+///
+/// Since the data layout of a \ref DataGroup object cannot be modified, this class is helpful in initially setting
+/// up the names and their corresponding indices. You can add entries to the builder using the \ref addPose,
+/// \ref addVector, \ref addScalar, \ref addInteger, \ref addBoolean, \ref addString, and \ref addEntriesFrom
+/// calls, or using similar calls on one of the type-specific element groups; and then create the DataGroup
+/// instance with createData() or createSharedData().
+///
+/// \sa DataGroup
+class DataGroupBuilder
+{
+public:
+ /// The type used for poses.
+ typedef DataGroup::PoseType PoseType;
+ /// The type used for vectors.
+ typedef DataGroup::VectorType VectorType;
+ /// The type used for matrices.
+ typedef DataGroup::DynamicMatrixType DynamicMatrixType;
+ /// The type used for scalars.
+ typedef DataGroup::ScalarType ScalarType;
+ /// The type used for integers.
+ typedef DataGroup::IntegerType IntegerType;
+ /// The type used for booleans.
+ typedef DataGroup::BooleanType BooleanType;
+ /// The type used for strings.
+ typedef DataGroup::StringType StringType;
+
+ /// Constructs an empty builder object.
+ DataGroupBuilder();
+
+ /// Produces a \ref DataGroup object with an immutable set of names and indices.
+ /// None of the values will contain any current data.
+ /// \return the DataGroup object *by value*.
+ DataGroup createData() const;
+
+ /// Produce a shared pointer to an empty \ref DataGroup object with an immutable set of names and indices.
+ /// None of the values will contain any current data.
+ /// \return a shared pointer to the DataGroup object.
+ std::shared_ptr<DataGroup> createSharedData() const;
+
+ /// Provides access to the pose value entries.
+ /// \return a writable reference to the sub-object that contains pose value entries.
+ NamedDataBuilder<PoseType>& poses();
+
+ /// Provides access to the pose value entries.
+ /// \return a read-only reference to the sub-object that contains pose value entries.
+ const NamedDataBuilder<PoseType>& poses() const;
+
+ /// Provides access to the vector value entries.
+ /// \return a writable reference to the sub-object that contains vector value entries.
+ NamedDataBuilder<VectorType>& vectors();
+
+ /// Provides access to the vector value entries.
+ /// \return a read-only reference to the sub-object that contains vector value entries.
+ const NamedDataBuilder<VectorType>& vectors() const;
+
+ /// Provides access to the matrix value entries.
+ /// \return a writable reference to the sub-object that contains matrix value entries.
+ NamedDataBuilder<DynamicMatrixType>& matrices();
+
+ /// Provides access to the matrix value entries.
+ /// \return a read-only reference to the sub-object that contains matrix value entries.
+ const NamedDataBuilder<DynamicMatrixType>& matrices() const;
+
+ /// Provides access to the scalar value entries.
+ /// \return a writable reference to the sub-object that contains scalar value entries.
+ NamedDataBuilder<ScalarType>& scalars();
+
+ /// Provides access to the scalar value entries.
+ /// \return a read-only reference to the sub-object that contains scalar value entries.
+ const NamedDataBuilder<ScalarType>& scalars() const;
+
+ /// Provides access to the integer value entries.
+ /// \return a writable reference to the sub-object that contains integer value entries.
+ NamedDataBuilder<IntegerType>& integers();
+
+ /// Provides access to the integer value entries.
+ /// \return a read-only reference to the sub-object that contains integer value entries.
+ const NamedDataBuilder<IntegerType>& integers() const;
+
+ /// Provides access to the Boolean value entries.
+ /// \return a writable reference to the sub-object that contains Boolean value entries.
+ NamedDataBuilder<BooleanType>& booleans();
+
+ /// Provides access to the Boolean value entries.
+ /// \return a read-only reference to the sub-object that contains Boolean value entries.
+ const NamedDataBuilder<BooleanType>& booleans() const;
+
+ /// Provides access to the string value entries.
+ /// \return a writable reference to the sub-object that contains string value entries.
+ NamedDataBuilder<StringType>& strings();
+
+ /// Provides access to the string value entries.
+ /// \return a read-only reference to the sub-object that contains string value entries.
+ const NamedDataBuilder<StringType>& strings() const;
+
+ /// Provides access to the custom data entries.
+ /// \return a writable reference to the sub-object that contains custom data entries.
+ NamedVariantDataBuilder& customData();
+
+ /// Provides access to the custom data entries.
+ /// \return a read-only reference to the sub-object that contains custom data entries.
+ const NamedVariantDataBuilder& customData() const;
+
+ /// A shortcut for adding a named pose entry.
+ /// Identical to <code>%poses().addEntry(name)</code>.
+ void addPose(const std::string& name);
+
+ /// A shortcut for adding a named vector entry.
+ /// Identical to <code>%vectors().addEntry(name)</code>.
+ void addVector(const std::string& name);
+
+ /// A shortcut for adding a named matrix entry.
+ /// Identical to <code>%matrices().addEntry(name)</code>.
+ void addMatrix(const std::string& name);
+
+ /// A shortcut for adding a named scalar entry.
+ /// Identical to <code>%scalars().addEntry(name)</code>.
+ void addScalar(const std::string& name);
+
+ /// A shortcut for adding a named integer entry.
+ /// Identical to <code>%integers().addEntry(name)</code>.
+ void addInteger(const std::string& name);
+
+ /// A shortcut for adding a named boolean entry.
+ /// Identical to <code>%booleans().addEntry(name)</code>.
+ void addBoolean(const std::string& name);
+
+ /// A shortcut for adding a named string entry.
+ /// Identical to <code>%strings().addEntry(name)</code>.
+ void addString(const std::string& name);
+
+ /// A shortcut for adding a named custom data entry.
+ /// Identical to <code>%customData().addEntry(name)</code>.
+ void addCustom(const std::string& name);
+
+ /// Create new entries from another DataGroupBuilder.
+ /// \param builder The other builder.
+ void addEntriesFrom(const DataGroupBuilder& builder);
+
+ /// Create new entries from an already initialized DataGroup.
+ /// \param data The data object.
+ void addEntriesFrom(const DataGroup& data);
+
+private:
+ // Prevent copy construction and copy assignment.
+ DataGroupBuilder(const DataGroupBuilder&);
+ DataGroupBuilder& operator=(const DataGroupBuilder&);
+
+ /// The subsidiary builder used for pose values.
+ NamedDataBuilder<PoseType> m_poses;
+
+ /// The subsidiary builder used for vector values.
+ NamedDataBuilder<VectorType> m_vectors;
+
+ /// The subsidiary builder used for matrix values.
+ NamedDataBuilder<DynamicMatrixType> m_matrices;
+
+ /// The subsidiary builder used for scalar values.
+ NamedDataBuilder<ScalarType> m_scalars;
+
+ /// The subsidiary builder used for integer values.
+ NamedDataBuilder<IntegerType> m_integers;
+
+ /// The subsidiary builder used for boolean values.
+ NamedDataBuilder<BooleanType> m_booleans;
+
+ /// The subsidiary builder used for string values.
+ NamedDataBuilder<StringType> m_strings;
+
+ /// The subsidiary builder used for custom data.
+ NamedVariantDataBuilder m_customData;
+};
+
+}; // namespace Input
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_DATAGROUPBUILDER_H
diff --git a/SurgSim/DataStructures/DataGroupCopier.cpp b/SurgSim/DataStructures/DataGroupCopier.cpp
new file mode 100644
index 0000000..d9a4ca5
--- /dev/null
+++ b/SurgSim/DataStructures/DataGroupCopier.cpp
@@ -0,0 +1,76 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/DataGroupCopier.h"
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/IndexDirectory.h"
+
+using SurgSim::DataStructures::IndexDirectory;
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+DataGroupCopier::DataGroupCopier(const DataGroup& source, DataGroup& target) :
+ m_source(source),
+ m_target(target)
+{
+ findMap();
+}
+
+void DataGroupCopier::copy()
+{
+ m_target.poses().copy(m_source.poses(), m_map[0]);
+ m_target.vectors().copy(m_source.vectors(), m_map[1]);
+ m_target.matrices().copy(m_source.matrices(), m_map[2]);
+ m_target.scalars().copy(m_source.scalars(), m_map[3]);
+ m_target.integers().copy(m_source.integers(), m_map[4]);
+ m_target.booleans().copy(m_source.booleans(), m_map[5]);
+ m_target.strings().copy(m_source.strings(), m_map[6]);
+ m_target.customData().copy(m_source.customData(), m_map[7]);
+}
+
+void DataGroupCopier::findMap()
+{
+ m_map[0] = findMap(m_source.poses().getDirectory(), m_target.poses().getDirectory());
+ m_map[1] = findMap(m_source.vectors().getDirectory(), m_target.vectors().getDirectory());
+ m_map[2] = findMap(m_source.matrices().getDirectory(), m_target.matrices().getDirectory());
+ m_map[3] = findMap(m_source.scalars().getDirectory(), m_target.scalars().getDirectory());
+ m_map[4] = findMap(m_source.integers().getDirectory(), m_target.integers().getDirectory());
+ m_map[5] = findMap(m_source.booleans().getDirectory(), m_target.booleans().getDirectory());
+ m_map[6] = findMap(m_source.strings().getDirectory(), m_target.strings().getDirectory());
+ m_map[7] = findMap(m_source.customData().getDirectory(), m_target.customData().getDirectory());
+}
+
+NamedDataCopyMap DataGroupCopier::findMap(std::shared_ptr<const IndexDirectory> source,
+ std::shared_ptr<const IndexDirectory> target) const
+{
+ NamedDataCopyMap map;
+ const std::vector<std::string>& sourceNames = source->getAllNames();
+ for (auto it = sourceNames.cbegin(); it != sourceNames.cend(); ++it)
+ {
+ const int targetIndex = target->getIndex(*it);
+ if (targetIndex > -1)
+ {
+ map[source->getIndex(*it)] = targetIndex;
+ }
+ }
+ return map;
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/DataGroupCopier.h b/SurgSim/DataStructures/DataGroupCopier.h
new file mode 100644
index 0000000..f1199b4
--- /dev/null
+++ b/SurgSim/DataStructures/DataGroupCopier.h
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_DATAGROUPCOPIER_H
+#define SURGSIM_DATASTRUCTURES_DATAGROUPCOPIER_H
+
+#include <array>
+#include <memory>
+#include <unordered_map>
+
+#include "SurgSim/DataStructures/NamedData.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class DataGroup;
+class IndexDirectory;
+
+/// The type used for copying values between two DataGroups that cannot assign to each other.
+typedef std::array<NamedDataCopyMap, 8> DataGroupCopyMap;
+
+/// A class that assists in copying from one DataGroup to another, when assignment is not possible.
+/// \sa SurgSim::DataStructures::DataGroup
+class DataGroupCopier
+{
+public:
+ /// Construct a copier.
+ /// \param source The source DataGroup.
+ /// \param target The target DataGroup.
+ DataGroupCopier(const DataGroup& source, DataGroup& target);
+
+ /// Copies the NamedData entries with the same names. Resets entries in the target that are reset in the source.
+ void copy();
+
+private:
+ /// Find the entries (by name) from the source to target DataGroups.
+ void findMap();
+
+ /// Find the entries (by name) from the source to target IndexDirectories, and return the matching entries.
+ /// \param source The source IndexDirectory.
+ /// \param target The target IndexDirectory.
+ /// \return The map from source to target indices.
+ NamedDataCopyMap findMap(std::shared_ptr<const IndexDirectory> source,
+ std::shared_ptr<const IndexDirectory> target) const;
+
+ /// The source DataGroup.
+ const DataGroup& m_source;
+
+ /// The target DataGroup.
+ DataGroup& m_target;
+
+ /// The map from source to target.
+ DataGroupCopyMap m_map;
+};
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+
+#endif // SURGSIM_DATASTRUCTURES_DATAGROUPCOPIER_H
diff --git a/SurgSim/DataStructures/DataStructuresConvert-inl.h b/SurgSim/DataStructures/DataStructuresConvert-inl.h
new file mode 100644
index 0000000..1d59228
--- /dev/null
+++ b/SurgSim/DataStructures/DataStructuresConvert-inl.h
@@ -0,0 +1,187 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_DATASTRUCTURESCONVERT_INL_H
+#define SURGSIM_DATASTRUCTURES_DATASTRUCTURESCONVERT_INL_H
+
+#include <string>
+
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+namespace Convert
+{
+const std::string serializeLogger = "Serialization";
+const std::string hasValueName = "HasValue";
+const std::string valueName = "Value";
+};
+};
+};
+
+template <class T>
+YAML::Node YAML::convert<SurgSim::DataStructures::OptionalValue<T>>::encode(
+ const SurgSim::DataStructures::OptionalValue<T>& rhs)
+{
+ Node node;
+ node[SurgSim::DataStructures::Convert::hasValueName] = rhs.hasValue();
+ if (rhs.hasValue())
+ {
+ node[SurgSim::DataStructures::Convert::valueName] = rhs.getValue();
+ }
+ else
+ {
+ node[SurgSim::DataStructures::Convert::valueName] = "Not set";
+ }
+ return node;
+}
+
+template <class T>
+bool YAML::convert<SurgSim::DataStructures::OptionalValue<T>>::decode(
+ const Node& node, SurgSim::DataStructures::OptionalValue<T>& rhs)
+{
+ bool result = true;
+ if (node[SurgSim::DataStructures::Convert::hasValueName].as<bool>())
+ {
+ try
+ {
+ rhs.setValue(node[SurgSim::DataStructures::Convert::valueName].as<T>());
+ }
+ catch (YAML::RepresentationException)
+ {
+ result = false;
+ auto logger = SurgSim::Framework::Logger::getLogger(SurgSim::DataStructures::Convert::serializeLogger);
+ SURGSIM_LOG(logger, WARNING) << "Bad conversion";
+ }
+ }
+ else
+ {
+ rhs.invalidate();
+ }
+ return result;
+}
+
+template <class T, size_t N>
+YAML::Node YAML::convert<std::array<T, N>>::encode(const std::array<T, N>& rhs)
+{
+ Node node(NodeType::Sequence);
+ for (auto it = rhs.cbegin(); it != rhs.cend(); ++it)
+ {
+ node.push_back(*it);
+ }
+ return node;
+}
+
+template <class T, size_t N>
+bool YAML::convert<std::array<T, N>>::decode(const Node& node, std::array<T, N>& rhs)
+{
+ if (!node.IsSequence() || node.size() != N)
+ {
+ return false;
+ }
+
+ bool result = true;
+ auto rhsit = rhs.begin();
+ for (YAML::const_iterator it = node.begin(); it != node.end(); ++it, ++rhsit)
+ {
+ try
+ {
+ (*rhsit) = it->as<T>();
+ }
+ catch (YAML::RepresentationException)
+ {
+ result = false;
+ auto logger = SurgSim::Framework::Logger::getLogger(SurgSim::DataStructures::Convert::serializeLogger);
+ SURGSIM_LOG(logger, WARNING) << __FUNCTION__ << ": Bad conversion";
+ }
+ }
+ return result;
+}
+
+template <class Key, class T>
+YAML::Node YAML::convert<std::unordered_map<Key, T>>::encode(const std::unordered_map<Key, T>& rhs)
+{
+ Node node(NodeType::Map);
+ for (auto it = std::begin(rhs); it != std::end(rhs); ++it)
+ {
+ node[it->first] = it->second;
+ }
+ return node;
+}
+
+template <class Key, class T>
+bool YAML::convert<std::unordered_map<Key, T>>::decode(const Node& node, std::unordered_map<Key, T>& rhs)
+{
+ if (!node.IsMap())
+ {
+ return false;
+ }
+
+ bool result = true;
+ for (auto it = node.begin(); it != node.end(); ++it)
+ {
+ try
+ {
+ rhs[it->first.as<Key>()] = it->second.as<T>();
+ }
+ catch (YAML::RepresentationException)
+ {
+ result = false;
+ auto logger = SurgSim::Framework::Logger::getLogger(SurgSim::DataStructures::Convert::serializeLogger);
+ SURGSIM_LOG(logger, WARNING) << __FUNCTION__ << ": Bad conversion";
+ }
+ }
+ return result;
+}
+
+template <class Value>
+YAML::Node YAML::convert<std::unordered_set<Value>>::encode(const std::unordered_set<Value>& rhs)
+{
+ Node node(NodeType::Sequence);
+ for (auto it = std::begin(rhs); it != std::end(rhs); ++it)
+ {
+ node.push_back(*it);
+ }
+ return node;
+}
+
+template <class Value>
+bool YAML::convert<std::unordered_set<Value>>::decode(const Node& node, std::unordered_set<Value>& rhs)
+{
+ if (!node.IsSequence())
+ {
+ return false;
+ }
+
+ bool result = true;
+ for (auto it = node.begin(); it != node.end(); ++it)
+ {
+ try
+ {
+ rhs.insert(it->as<Value>());
+ }
+ catch (YAML::RepresentationException)
+ {
+ result = false;
+ auto logger = SurgSim::Framework::Logger::getLogger(SurgSim::DataStructures::Convert::serializeLogger);
+ SURGSIM_LOG(logger, WARNING) << __FUNCTION__ << ": Bad conversion";
+ }
+ }
+ return result;
+}
+
+#endif // SURGSIM_DATASTRUCTURES_DATASTRUCTURESCONVERT_INL_H
\ No newline at end of file
diff --git a/SurgSim/DataStructures/DataStructuresConvert.h b/SurgSim/DataStructures/DataStructuresConvert.h
new file mode 100644
index 0000000..335f66b
--- /dev/null
+++ b/SurgSim/DataStructures/DataStructuresConvert.h
@@ -0,0 +1,71 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_DATASTRUCTURESCONVERT_H
+#define SURGSIM_DATASTRUCTURES_DATASTRUCTURESCONVERT_H
+
+#include <array>
+#include <memory>
+#include <unordered_map>
+#include <unordered_set>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Framework/Macros.h"
+
+namespace YAML
+{
+
+/// YAML::convert specialization for OptionalValue.
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class T>
+struct convert<SurgSim::DataStructures::OptionalValue<T>>
+{
+ static Node encode(const SurgSim::DataStructures::OptionalValue<T>& rhs);
+ static bool decode(const Node& node, SurgSim::DataStructures::OptionalValue<T>& rhs);
+};
+
+/// YAML::convert specialization for std::array.
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class T, size_t N>
+struct convert<std::array<T, N>>
+{
+ static Node encode(const std::array<T, N>& rhs);
+ static bool decode(const Node& node, std::array<T, N>& rhs);
+};
+
+/// YAML::convert specialization for std::unordered_map.
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Key, class T>
+struct convert<std::unordered_map<Key, T>>
+{
+ static Node encode(const std::unordered_map<Key, T>& rhs);
+ static bool decode(const Node& node, std::unordered_map<Key, T>& rhs);
+};
+
+/// YAML::convert specialization for std::unordered_set.
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Value>
+struct convert<std::unordered_set<Value>>
+{
+ static Node encode(const std::unordered_set<Value>& rhs);
+ static bool decode(const Node& node, std::unordered_set<Value>& rhs);
+};
+
+} // namespace YAML
+
+#include "SurgSim/DataStructures/DataStructuresConvert-inl.h"
+
+#endif // SURGSIM_DATASTRUCTURES_DATASTRUCTURESCONVERT_H
\ No newline at end of file
diff --git a/SurgSim/DataStructures/EmptyData.h b/SurgSim/DataStructures/EmptyData.h
new file mode 100644
index 0000000..0633b07
--- /dev/null
+++ b/SurgSim/DataStructures/EmptyData.h
@@ -0,0 +1,39 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_EMPTYDATA_H
+#define SURGSIM_DATASTRUCTURES_EMPTYDATA_H
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+/// EmptyData class
+class EmptyData
+{
+public:
+
+ /// Comparison operator
+ /// \param data The data to compare it to.
+ /// \return true for all cases.
+ bool operator==(const EmptyData& data) const
+ {
+ return true;
+ }
+};
+
+}
+}
+#endif //SURGSIM_DATASTRUCTURES_EMPTYDATA_H
diff --git a/SurgSim/DataStructures/Image-inl.h b/SurgSim/DataStructures/Image-inl.h
new file mode 100644
index 0000000..19da1f8
--- /dev/null
+++ b/SurgSim/DataStructures/Image-inl.h
@@ -0,0 +1,150 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef SURGSIM_DATASTRUCTURES_IMAGE_INL_H
+#define SURGSIM_DATASTRUCTURES_IMAGE_INL_H
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template<class T>
+Image<T>::Image() :
+ m_width(0), m_height(0), m_channels(0)
+{
+}
+
+
+template<class T>
+Image<T>::Image(size_t width, size_t height, size_t channels) :
+ m_width(width), m_height(height), m_channels(channels), m_data(new T[m_width * m_height * m_channels])
+{
+}
+
+template<class T>
+Image<T>::Image(size_t width, size_t height, size_t channels, const T* const data) :
+ m_width(width), m_height(height), m_channels(channels), m_data(new T[m_width * m_height * m_channels])
+{
+ std::copy(data, data + width * height * channels, m_data.get());
+}
+
+template<class T>
+Image<T>::Image(const Image<T>& other) :
+ m_width(other.getWidth()), m_height(other.getHeight()), m_channels(other.getNumChannels()),
+ m_data(new T[m_width * m_height * m_channels])
+{
+ std::copy(other.m_data.get(), other.m_data.get() + m_width * m_height * m_channels, m_data.get());
+}
+
+template<class T>
+Image<T>::Image(Image<T>&& other)
+{
+ // Can use the move assignment operator to construct
+ *this = std::move(other);
+}
+
+template<class T>
+Image<T>& Image<T>::operator=(const Image<T>& other)
+{
+ if (this != &other)
+ {
+ size_t newDataSize = other.getWidth() * other.getHeight() * other.getNumChannels();
+ size_t oldDataSize = getWidth() * getHeight() * getNumChannels();
+ if (newDataSize != oldDataSize)
+ {
+ m_data.reset(new T[newDataSize]);
+ }
+ m_width = other.getWidth();
+ m_height = other.getHeight();
+ m_channels = other.getNumChannels();
+ std::copy(other.m_data.get(), other.m_data.get() + newDataSize, m_data.get());
+ }
+ return *this;
+}
+
+template<class T>
+Image<T>& Image<T>::operator=(Image<T>&& other)
+{
+ if (this != &other)
+ {
+ m_data = std::move(other.m_data);
+ m_width = other.getWidth();
+ m_height = other.getHeight();
+ m_channels = other.getNumChannels();
+
+ other.m_width = 0;
+ other.m_height = 0;
+ other.m_channels = 0;
+ }
+ return *this;
+}
+
+template<class T>
+Image<T>::~Image()
+{
+}
+
+template<class T>
+typename Image<T>::ChannelType Image<T>::getChannel(size_t channel)
+{
+ SURGSIM_ASSERT(channel < m_channels) << "channel number is larger than the number of channels";
+ return ChannelType(m_data.get() + channel, m_width, m_height, Eigen::InnerStride<>(m_channels));
+}
+
+template<class T>
+size_t Image<T>::getWidth() const
+{
+ return m_width;
+}
+
+template<class T>
+size_t Image<T>::getHeight() const
+{
+ return m_height;
+}
+
+template<class T>
+std::array<size_t, 3> Image<T>::getSize() const
+{
+ std::array<size_t, 3> size = {m_width, m_height, m_channels};
+ return std::move(size);
+}
+
+template<class T>
+size_t Image<T>::getNumChannels() const
+{
+ return m_channels;
+}
+
+template<class T>
+T* const Image<T>::getData()
+{
+ return m_data.get();
+}
+
+template<class T>
+const T* const Image<T>::getData() const
+{
+ return m_data.get();
+}
+
+}
+}
+
+#endif //SURGSIM_DATASTRUCTURES_IMAGE_INL_H
diff --git a/SurgSim/DataStructures/Image.h b/SurgSim/DataStructures/Image.h
new file mode 100644
index 0000000..09aba6b
--- /dev/null
+++ b/SurgSim/DataStructures/Image.h
@@ -0,0 +1,120 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_IMAGE_H
+#define SURGSIM_DATASTRUCTURES_IMAGE_H
+
+#include <array>
+#include <memory>
+
+#include <Eigen/Core>
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+/// A templated Image class
+///
+/// \tparam T the data type stored in the Image
+template<class T>
+class Image
+{
+public:
+ /// Default Constructor
+ Image();
+
+ /// Constructor
+ /// \param width the image width
+ /// \param height the image height
+ /// \param channels the number of channels in the image
+ Image(size_t width, size_t height, size_t channels);
+
+ /// Copy constructor from a data pointer
+ /// \param width the image width
+ /// \param height the image height
+ /// \param channels the number of channels in the image
+ /// \param data pointer to the data to copy from
+ Image(size_t width, size_t height, size_t channels, const T* const data);
+
+ /// Copy constructor
+ /// \param other Image to copy from
+ Image(const Image<T>& other);
+
+ /// Move constructor
+ /// \param other Image to move data from
+ Image(Image<T>&& other);
+
+ /// Destructor
+ virtual ~Image();
+
+ /// Assignment Operator
+ /// \param other The Image to copy from
+ /// \return The Image that was assigned into
+ Image<T>& operator=(const Image<T>& other);
+
+ /// Move Assignment Operator
+ /// \param other The Image to move data from
+ /// \return The Image that was assigned into
+ Image<T>& operator=(Image<T>&& other);
+
+ /// Get the Image width
+ /// \return the width
+ size_t getWidth() const;
+
+ /// Get the Image height
+ /// \return the height
+ size_t getHeight() const;
+
+ /// Get the Image size
+ /// \return the image size as (width, height, channels)
+ std::array<size_t, 3> getSize() const;
+
+ /// Get the number of channels in this Image
+ /// \return the number of channels
+ size_t getNumChannels() const;
+
+ /// Type of the channel returned by getChannel
+ typedef Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>, 0, Eigen::InnerStride<>> ChannelType;
+
+ /// Get the data in the channel as an eigen matrix
+ /// \param channel the channel number
+ /// \return an eigen matrix
+ ChannelType getChannel(size_t channel);
+
+ /// Get the pointer to the data
+ /// \return the data
+ T* const getData();
+
+ /// Get the pointer to the data, constant version
+ /// \return the data
+ const T* const getData() const;
+
+private:
+ size_t m_width;
+ size_t m_height;
+ size_t m_channels;
+ std::unique_ptr<T[]> m_data;
+};
+
+typedef Image<float> Imagef;
+
+}
+}
+
+#include "SurgSim/DataStructures/Image-inl.h"
+
+#endif //SURGSIM_DATASTRUCTURES_IMAGE_H
diff --git a/SurgSim/DataStructures/IndexDirectory.cpp b/SurgSim/DataStructures/IndexDirectory.cpp
new file mode 100644
index 0000000..546805e
--- /dev/null
+++ b/SurgSim/DataStructures/IndexDirectory.cpp
@@ -0,0 +1,67 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/IndexDirectory.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+
+IndexDirectory::IndexDirectory()
+{
+}
+
+IndexDirectory::IndexDirectory(const std::vector<std::string>& names)
+{
+ for (auto it = names.cbegin(); it != names.cend(); ++it)
+ {
+ addEntry(*it);
+ }
+}
+
+const std::vector<std::string>& IndexDirectory::getAllNames() const
+{
+ return m_names;
+}
+
+IndexDirectory::IndexDirectory(const IndexDirectory& directory) :
+ m_names(directory.m_names), m_indices(directory.m_indices)
+{
+}
+
+IndexDirectory& IndexDirectory::operator =(const IndexDirectory& directory)
+{
+ m_names = directory.m_names;
+ m_indices = directory.m_indices;
+ return *this;
+}
+
+int IndexDirectory::addEntry(const std::string& name)
+{
+ if ((name.length() == 0) || hasEntry(name))
+ {
+ return -1;
+ }
+ int index = static_cast<int>(m_names.size());
+ m_names.push_back(name);
+ m_indices[name] = index;
+ return index;
+}
+
+
+}; // namespace Input
+}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/IndexDirectory.h b/SurgSim/DataStructures/IndexDirectory.h
new file mode 100644
index 0000000..d3f709a
--- /dev/null
+++ b/SurgSim/DataStructures/IndexDirectory.h
@@ -0,0 +1,140 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_INDEXDIRECTORY_H
+#define SURGSIM_DATASTRUCTURES_INDEXDIRECTORY_H
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// A simple bidirectional mapping between names (strings) and distinct consecutive non-negative indices.
+///
+/// Access to this class is thread-safe if all of the threads are only performing const operations, i.e. reading
+/// the names and indices.
+class IndexDirectory
+{
+public:
+ /// Create an empty directory object.
+ IndexDirectory();
+
+ /// Create a directory object initialized to a list of names.
+ /// \param names The names.
+ explicit IndexDirectory(const std::vector<std::string>& names);
+
+ /// Given a name, return the corresponding index (or -1).
+ /// \param name The name.
+ /// \return the index for that name if one exists; -1 otherwise.
+ int getIndex(const std::string& name) const
+ {
+ if (name.length() == 0)
+ {
+ return -1;
+ }
+ auto entry = m_indices.find(name);
+ if (entry == m_indices.cend())
+ {
+ return -1;
+ }
+ else
+ {
+ return entry->second;
+ }
+ }
+
+ /// Given an index, return the corresponding name (or "").
+ /// \param index The index.
+ /// \return the name for that index if one exists; an empty string otherwise.
+ std::string getName(int index) const
+ {
+ if ((index < 0) || (index >= static_cast<int>(m_names.size())))
+ {
+ return "";
+ }
+ else
+ {
+ return m_names[index];
+ }
+ }
+
+ /// Get a list of all the names available from the index directory.
+ /// \return all the names, in index order.
+ const std::vector<std::string>& getAllNames() const;
+
+ /// Check whether the specified name exists in the directory.
+ ///
+ /// \param name The name.
+ /// \return true if the entry exists.
+ bool hasEntry(const std::string& name) const
+ {
+ return ((name.length() > 0) && (m_indices.count(name) > 0));
+ }
+
+ /// Check the number of existing entries in the directory.
+ /// \return the size of the directory.
+ /// \sa getNumEntries()
+ size_t size() const
+ {
+ return m_names.size();
+ }
+
+ /// Check the number of existing entries in the directory.
+ /// \return the size of the directory.
+ /// \sa size()
+ int getNumEntries() const
+ {
+ return static_cast<int>(m_names.size());
+ }
+
+protected:
+ template <typename T>
+ friend class NamedDataBuilder;
+ friend class DataGroupBuilder;
+
+ /// Copy constructor.
+ /// Not generally accessible by external code, but is used by friend classes.
+ /// \sa NamedDataBuilder, DataGroupBuilder
+ IndexDirectory(const IndexDirectory& directory);
+
+ /// Assignment operator.
+ /// Not generally accessible by external code, but is used by friend classes.
+ /// \sa NamedDataBuilder, DataGroupBuilder
+ IndexDirectory& operator =(const IndexDirectory& directory);
+
+ /// Create a new entry for the specified name.
+ /// Not generally accessible by external code, but is used by friend classes.
+ /// \sa NamedDataBuilder, DataGroupBuilder
+ ///
+ /// \param name The name, which should be non-empty and should not already exist in the directory.
+ /// \return the index of the created entry, or -1 if the entry could not be added.
+ int addEntry(const std::string& name);
+
+private:
+ /// The array of entry names, in index order.
+ std::vector<std::string> m_names;
+
+ /// A mapping of entry names to indices.
+ std::unordered_map<std::string, int> m_indices;
+};
+
+}; // namespace Input
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_INDEXDIRECTORY_H
diff --git a/SurgSim/DataStructures/IndexedLocalCoordinate.cpp b/SurgSim/DataStructures/IndexedLocalCoordinate.cpp
new file mode 100644
index 0000000..d2539d7
--- /dev/null
+++ b/SurgSim/DataStructures/IndexedLocalCoordinate.cpp
@@ -0,0 +1,35 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+IndexedLocalCoordinate::IndexedLocalCoordinate() : index(0)
+{
+}
+
+IndexedLocalCoordinate::IndexedLocalCoordinate(size_t index, const SurgSim::Math::Vector& coordinate)
+ : index(index), coordinate(coordinate)
+{
+}
+
+} // namespace DataStructures
+
+} // namespace SurgSim
diff --git a/SurgSim/DataStructures/IndexedLocalCoordinate.h b/SurgSim/DataStructures/IndexedLocalCoordinate.h
new file mode 100644
index 0000000..80194ac
--- /dev/null
+++ b/SurgSim/DataStructures/IndexedLocalCoordinate.h
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_INDEXEDLOCALCOORDINATE_H
+#define SURGSIM_DATASTRUCTURES_INDEXEDLOCALCOORDINATE_H
+
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+/// A generic (size_t index, Vector coordinate) pair. The coordinate is a dynamic size vector.
+/// E.g. This can be used to represent a barycentric coordinate within a simplex (identified by the index).
+struct IndexedLocalCoordinate
+{
+ /// Default constructor with no initialization.
+ IndexedLocalCoordinate();
+
+ /// Constructor with initialization.
+ /// \param index Numeric index.
+ /// \param coordinate Coordinates with respect to the entity identified by the index.
+ /// \note Constructor does not throw when given malformed parameters.
+ IndexedLocalCoordinate(size_t index, const SurgSim::Math::Vector& coordinate);
+
+ /// Numeric index to indicate the entity w.r.t which the barycentricCoordinate is defined.
+ size_t index;
+
+ /// Coordinates with respect to the entity identified by the index.
+ SurgSim::Math::Vector coordinate;
+};
+
+} // namespace DataStructures
+
+} // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_INDEXEDLOCALCOORDINATE_H
diff --git a/SurgSim/DataStructures/Location.h b/SurgSim/DataStructures/Location.h
new file mode 100644
index 0000000..5e03bdb
--- /dev/null
+++ b/SurgSim/DataStructures/Location.h
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_LOCATION_H
+#define SURGSIM_DATASTRUCTURES_LOCATION_H
+
+#include <vector>
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/DataStructures/OctreeNode.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+struct Location
+{
+public:
+ /// Default constructor
+ Location()
+ {
+ }
+
+ /// Constructor for rigid local position
+ /// \param localPosition The 3D local position to set this location to
+ explicit Location(const SurgSim::Math::Vector3d& localPosition)
+ {
+ rigidLocalPosition.setValue(localPosition);
+ }
+
+ /// Constructor for octree node path
+ /// \param nodePath The octree node path to set this location to
+ explicit Location(const SurgSim::DataStructures::OctreePath& nodePath)
+ {
+ octreeNodePath.setValue(nodePath);
+ }
+
+ /// Constructor for mesh local coordinate
+ /// \param localCoordinate The mesh local coordinate
+ explicit Location(const SurgSim::DataStructures::IndexedLocalCoordinate& localCoordinate)
+ {
+ meshLocalCoordinate.setValue(localCoordinate);
+ }
+
+ SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d> rigidLocalPosition;
+ SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::OctreePath> octreeNodePath;
+ SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate> meshLocalCoordinate;
+};
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_LOCATION_H
diff --git a/SurgSim/DataStructures/MeshElement.h b/SurgSim/DataStructures/MeshElement.h
new file mode 100644
index 0000000..bc15026
--- /dev/null
+++ b/SurgSim/DataStructures/MeshElement.h
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_MESHELEMENT_H
+#define SURGSIM_DATASTRUCTURES_MESHELEMENT_H
+
+#include <array>
+#include <memory>
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+/// Element structure for meshes. MeshElement combines Vertices to form the structure of a mesh and can store extra
+/// per-element data.
+///
+/// MeshElement is to be used purely as a data structure and not provide implementation of algorithms.
+/// For example, a physics FEM's elements are not subclasses of MeshElement if they provide code that is part of the FEM
+/// algorithm, but they may used with a Mesh to store the structure of the FEM.
+///
+/// The extra Data is left up to the particular use of Mesh to specify. For example, for use collision detection,
+/// a vertex may need a normal and adjacent triangle information, which could be stored in a struct.
+///
+/// If no extra Data is needed, a specialization exists for void, in which case the constructor takes no data.
+///
+/// \tparam N Number of vertices in the element
+/// \tparam Data Type of extra data stored in the element (void for no data)
+/// \sa Vertices
+template <size_t N, class Data>
+struct MeshElement
+{
+ /// Constructor
+ /// \param verticesId IDs of the N element vertices
+ /// \param data Extra data to be stored with the element
+ MeshElement(const std::array<size_t, N>& verticesId, const Data& data) :
+ verticesId(verticesId),
+ data(data),
+ isValid(true)
+ {
+ }
+
+ /// Constructor where the Data is constructed by its default constructor.
+ /// \param verticesId IDs of the N element vertices
+ explicit MeshElement(const std::array<size_t, N>& verticesId) :
+ verticesId(verticesId),
+ isValid(true)
+ {
+ }
+
+ typedef std::array<size_t, N> IdType;
+
+ /// Element vertices.
+ IdType verticesId;
+
+ /// Extra element data.
+ Data data;
+
+ /// Is this a valid element
+ bool isValid;
+
+ /// Compare the element with another one (equality)
+ /// \param element The MeshElement to compare it to
+ /// \return True if the two MeshElements are equals, False otherwise
+ bool operator==(const MeshElement<N, Data>& element) const
+ {
+ return isValid == element.isValid && verticesId == element.verticesId && data == element.data;
+ }
+
+ /// Compare the element with another one (inequality)
+ /// \param element The MeshElement to compare it to
+ /// \return False if the two MeshElements are equals, True otherwise
+ bool operator!=(const MeshElement<N, Data>& element) const
+ {
+ return !((*this) == element);
+ }
+};
+
+
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_MESHELEMENT_H
diff --git a/SurgSim/DataStructures/NamedData-inl.h b/SurgSim/DataStructures/NamedData-inl.h
new file mode 100644
index 0000000..953dc27
--- /dev/null
+++ b/SurgSim/DataStructures/NamedData-inl.h
@@ -0,0 +1,346 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_NAMEDDATA_INL_H
+#define SURGSIM_DATASTRUCTURES_NAMEDDATA_INL_H
+
+#include <type_traits>
+
+#include "SurgSim/DataStructures/NamedData.h"
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <typename T>
+inline NamedData<T>::NamedData()
+{
+}
+
+template <typename T>
+inline NamedData<T>::NamedData(std::shared_ptr<const IndexDirectory> directory) :
+ m_directory(directory)
+{
+ SURGSIM_ASSERT(isValid());
+ m_data.resize(m_directory->getNumEntries());
+ m_isDataValid.resize(m_directory->getNumEntries(), false);
+}
+
+template <typename T>
+inline NamedData<T>::NamedData(const std::vector<std::string>& names) :
+ m_directory(std::make_shared<const IndexDirectory>(names))
+{
+ SURGSIM_ASSERT(isValid());
+ m_data.resize(m_directory->getNumEntries());
+ m_isDataValid.resize(m_directory->getNumEntries(), false);
+}
+
+template <typename T>
+inline NamedData<T>::NamedData(const NamedData& namedData) :
+ m_directory(namedData.m_directory),
+ m_data(namedData.m_data),
+ m_isDataValid(namedData.m_isDataValid)
+{
+ SURGSIM_ASSERT(isValid());
+}
+
+template <typename T>
+inline NamedData<T>& NamedData<T>::operator=(const NamedData& namedData)
+{
+ SURGSIM_ASSERT(namedData.isValid()) <<
+ "Cannot use an invalid (empty) NamedData on the right-hand side of an assignment!";
+
+ if (!isValid())
+ {
+ m_directory = namedData.m_directory;
+ }
+ else
+ {
+ SURGSIM_ASSERT(m_directory == namedData.m_directory) << "Incompatible NamedData contents in assignment!";
+ }
+
+ m_data = namedData.m_data;
+ m_isDataValid = namedData.m_isDataValid;
+
+ SURGSIM_ASSERT(isValid()) << "NamedData is not valid after assignment!";
+ SURGSIM_ASSERT(m_data.size() == m_directory->size() && m_isDataValid.size() == m_directory->size()) <<
+ "NamedData is not correctly sized after assignment!";
+
+ return *this;
+}
+
+template <typename T>
+inline NamedData<T>::NamedData(NamedData&& namedData) :
+ m_directory(std::move(namedData.m_directory)),
+ m_data(std::move(namedData.m_data)),
+ m_isDataValid(std::move(namedData.m_isDataValid))
+{
+ SURGSIM_ASSERT(isValid());
+}
+
+template <typename T>
+inline NamedData<T>& NamedData<T>::operator=(NamedData&& namedData)
+{
+ SURGSIM_ASSERT(namedData.isValid()) <<
+ "Cannot use an invalid (empty) NamedData on the right-hand side of an assignment!";
+
+ if (!isValid())
+ {
+ m_directory = std::move(namedData.m_directory);
+ }
+ else
+ {
+ SURGSIM_ASSERT(m_directory == namedData.m_directory) << "Incompatible NamedData contents in assignment!";
+ }
+
+ m_data = std::move(namedData.m_data);
+ m_isDataValid = std::move(namedData.m_isDataValid);
+
+ SURGSIM_ASSERT(isValid()) << "NamedData is not valid after assignment!";
+ SURGSIM_ASSERT(m_data.size() == m_directory->size() && m_isDataValid.size() == m_directory->size()) <<
+ "NamedData is not correctly sized after assignment!";
+
+ return *this;
+}
+
+template <typename T>
+inline bool NamedData<T>::isValid() const
+{
+ return static_cast<bool>(m_directory);
+}
+
+template <typename T>
+inline std::shared_ptr<const IndexDirectory> NamedData<T>::getDirectory() const
+{
+ return m_directory;
+}
+
+template <typename T>
+inline int NamedData<T>::getIndex(const std::string& name) const
+{
+ if (! isValid())
+ {
+ return -1;
+ }
+ return m_directory->getIndex(name);
+}
+
+template <typename T>
+inline std::string NamedData<T>::getName(int index) const
+{
+ if (! isValid())
+ {
+ return "";
+ }
+ return m_directory->getName(index);
+}
+
+template <typename T>
+inline bool NamedData<T>::hasEntry(int index) const
+{
+ return ((index >= 0) && (index < static_cast<int>(m_data.size())));
+}
+
+template <typename T>
+inline bool NamedData<T>::hasEntry(const std::string& name) const
+{
+ if (! isValid())
+ {
+ return false;
+ }
+ return m_directory->hasEntry(name);
+}
+
+template <typename T>
+inline bool NamedData<T>::hasData(int index) const
+{
+ return hasEntry(index) && m_isDataValid[index];
+}
+
+template <typename T>
+inline bool NamedData<T>::hasData(const std::string& name) const
+{
+ if (! isValid())
+ {
+ return false;
+ }
+ int index = m_directory->getIndex(name);
+ if (index < 0)
+ {
+ return false;
+ }
+ else
+ {
+ SURGSIM_ASSERT(hasEntry(index));
+ return m_isDataValid[index];
+ }
+}
+
+template <typename T>
+inline bool NamedData<T>::get(int index, T* value) const
+{
+ if (! hasData(index))
+ {
+ return false;
+ }
+ else
+ {
+ *value = m_data[index];
+ return true;
+ }
+}
+
+template <typename T>
+inline bool NamedData<T>::get(const std::string& name, T* value) const
+{
+ if (! isValid())
+ {
+ return false;
+ }
+ int index = m_directory->getIndex(name);
+ if ((index < 0) || ! m_isDataValid[index])
+ {
+ return false;
+ }
+ else
+ {
+ SURGSIM_ASSERT(hasEntry(index));
+ *value = m_data[index];
+ return true;
+ }
+}
+
+template <typename T>
+inline bool NamedData<T>::set(int index, const T& value)
+{
+ if (! hasEntry(index))
+ {
+ return false;
+ }
+ else
+ {
+ m_data[index] = value;
+ m_isDataValid[index] = true;
+ return true;
+ }
+}
+
+template <typename T>
+inline bool NamedData<T>::set(const std::string& name, const T& value)
+{
+ if (! isValid())
+ {
+ return false;
+ }
+ int index = m_directory->getIndex(name);
+ if (index < 0)
+ {
+ return false;
+ }
+ else
+ {
+ SURGSIM_ASSERT(hasEntry(index));
+ m_data[index] = value;
+ m_isDataValid[index] = true;
+ return true;
+ }
+}
+
+template <typename T>
+inline bool NamedData<T>::reset(int index)
+{
+ if (! hasEntry(index))
+ {
+ return false;
+ }
+ else
+ {
+ m_isDataValid[index] = false;
+ return true;
+ }
+}
+
+template <typename T>
+inline bool NamedData<T>::reset(const std::string& name)
+{
+ if (! isValid())
+ {
+ return false;
+ }
+ int index = m_directory->getIndex(name);
+ if (index < 0)
+ {
+ return false;
+ }
+ else
+ {
+ SURGSIM_ASSERT(hasEntry(index));
+ m_isDataValid[index] = false;
+ return true;
+ }
+}
+
+template <typename T>
+inline void NamedData<T>::resetAll()
+{
+ m_isDataValid.assign(m_data.size(), false);
+}
+
+template <typename T>
+inline size_t NamedData<T>::size() const
+{
+ return m_data.size();
+}
+
+template <typename T>
+inline int NamedData<T>::getNumEntries() const
+{
+ return static_cast<int>(m_data.size());
+}
+
+template <typename T>
+template <typename N>
+inline void NamedData<T>::copy(const NamedData<N>& source, const NamedDataCopyMap& map)
+{
+ static_assert(std::is_same<T, N>::value, "NamedData<T>::copy can only copy from another NamedData<T>.");
+ for (auto it = map.cbegin(); it != map.cend(); ++it)
+ {
+ T value;
+ if (source.get(it->first, &value))
+ {
+ set(it->second, value);
+ }
+ else
+ {
+ reset(it->second);
+ }
+ }
+}
+
+template <typename T>
+void SurgSim::DataStructures::NamedData<T>::cacheIndex(const std::string& name, int* index) const
+{
+ if (*index < 0)
+ {
+ *index = getIndex(name);
+ }
+}
+
+}; // namespace Input
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_NAMEDDATA_INL_H
diff --git a/SurgSim/DataStructures/NamedData.h b/SurgSim/DataStructures/NamedData.h
new file mode 100644
index 0000000..0fd86fc
--- /dev/null
+++ b/SurgSim/DataStructures/NamedData.h
@@ -0,0 +1,305 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_NAMEDDATA_H
+#define SURGSIM_DATASTRUCTURES_NAMEDDATA_H
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/DataStructures/IndexDirectory.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+/// The type used for copying values between two NamedData objects that cannot assign to each other. The keys are
+/// the source indices. The values are the target indices.
+typedef std::unordered_map<int, int> NamedDataCopyMap;
+
+/// Common strings for NamedData.
+
+namespace Names
+{
+static const char* const BUTTON_0 = "button0";
+static const char* const BUTTON_1 = "button1";
+static const char* const BUTTON_2 = "button2";
+static const char* const BUTTON_3 = "button3";
+static const char* const BUTTON_4 = "button4";
+
+static const char* const POSE = "pose";
+static const char* const INPUT_POSE = "inputPose";
+
+static const char* const ANGULAR_VELOCITY = "angularVelocity";
+static const char* const LINEAR_VELOCITY = "linearVelocity";
+static const char* const INPUT_ANGULAR_VELOCITY = "inputAngularVelocity";
+static const char* const INPUT_LINEAR_VELOCITY = "inputLinearVelocity";
+
+static const char* const FORCE = "force";
+static const char* const TORQUE = "torque";
+
+static const char* const DAMPER_JACOBIAN = "damperJacobian";
+static const char* const SPRING_JACOBIAN = "springJacobian";
+
+static const char* const IS_HOMED = "isHomed";
+static const char* const IS_ORIENTATION_HOMED = "isOrientationHomed";
+static const char* const IS_POSITION_HOMED = "isPositionHomed";
+
+static const char* const DIGITAL_INPUT_PREFIX = "digitalInput";
+static const char* const DIGITAL_OUTPUT_PREFIX = "digitalOutput";
+static const char* const TIMER_INPUT_PREFIX = "timerInput";
+static const char* const TIMER_OUTPUT_PREFIX = "timerOutput";
+static const char* const ANALOG_INPUT_PREFIX = "analogInput";
+static const char* const ANALOG_OUTPUT_PREFIX = "analogOutput";
+};
+
+/// A templated dictionary in which data can be accessed by name or index, with immutable names & indices.
+///
+/// A NamedData object consists of a collection of entries of type \a T. The data value for each entry can be accessed
+/// by either the entry's unique name (a std::string) or the entry's unique index (a non-negative integer). Access by
+/// name is more convenient, but less efficient.
+///
+/// A NamedData object constructed by the default constructor has no entries, meaning it has not been associated with a
+/// set of names and indices, and is called <i>invalid</i> or <i>empty</i>.
+///
+/// An <i>non</i>-empty object contains an immutable collection of entries. For a non-empty object: entries cannot be
+/// added or removed, and the entries' names and indices <b>cannot be changed</b>. Further, a non-empty object cannot
+/// become empty. These properties ensure that a stable data layout is available to the code using this class so
+/// that it can, for example, record entry indices and use them to retrieve the same entries later on.
+///
+/// The data associated with an entry (e.g., the true or false associated with a particular name and index in a
+/// NamedData<bool>) can be changed, and each entry can be reset to a "missing" state. A reset entry remains in the
+/// collection, but has no associated data.
+///
+/// The entries (i.e., names & indices) in a NamedData object can be set by passing a vector of names to the
+/// constructor, or by using the \ref NamedDataBuilder class. Given one non-empty object, other objects with the same
+/// entries can be created via copy construction or assignment of the non-empty object to an empty
+/// (default-constructed) object.
+///
+/// \tparam T the data type used for values contained in this collection.
+template <typename T>
+class NamedData
+{
+public:
+ /// Create an empty object, with no associated names and indices yet.
+ inline NamedData();
+
+ /// Create an object containing items from an index directory.
+ /// You should probably use \ref NamedDataBuilder or copy construction/assignment instead.
+ ///
+ /// \param directory The IndexDirectory object describing the names and indices to be used.
+ inline explicit NamedData(std::shared_ptr<const IndexDirectory> directory);
+
+ /// Construct an object, using the names from a vector of strings.
+ /// The indices corresponding to each name's entry will be the same as that name's index in the vector.
+ ///
+ /// \param names The names, which should be unique.
+ inline explicit NamedData(const std::vector<std::string>& names);
+
+ /// Construct an object as a copy of the data from another object.
+ /// This is used in the NamedVariantData copy constructor.
+ /// \param namedData The object to copy from.
+ inline NamedData(const NamedData& namedData);
+
+ /// Copy the data from another object.
+ ///
+ /// The object being assigned into must either be empty (not yet associated with a set of names and indices), or
+ /// the two objects must share the same data layout, resulting from earlier copy construction or assignment.
+ /// ~~~~~
+ /// DataGroup initial;
+ /// // ...initialize "initial" to some non-empty value...
+ /// NamedData copyConstructed(initial); // Layout is shared with initial
+ /// copyConstructed = initial // OK, using the same layout
+ /// NamedData another; // Object is empty (no layout)
+ /// another = initial; // OK, layout is now shared with initial
+ /// another = initial // OK, using the same layout
+ /// ~~~~~
+ ///
+ /// Note that the data layout must be the same, i.e. related to one another by object assignment or copy
+ /// construction. Objects that merely contain entries with the same names and indices are not acceptable!
+ /// (Otherwise, we'd need to inefficiently compare layout contents each time we assign.)
+ /// ~~~~~
+ /// std::vector<std::string> names // = ...initialized to some value...;
+ /// NamedData first(names); // Layout of entries created from names
+ /// NamedData second(names); // Another layout of entries created from names; names and indices match
+ /// second = first; // ERROR at run-time, layouts were created separately!
+ /// ~~~~~
+ ///
+ /// \param namedData The object to copy from.
+ /// \return The object that was assigned into.
+ inline NamedData& operator=(const NamedData& namedData);
+
+ /// Create an object and move the data from another object.
+ ///
+ /// \param [in,out] namedData The object to copy from, which will be left in an unusable state.
+ inline NamedData(NamedData&& namedData);
+
+ /// Move the data from another object.
+ ///
+ /// The same restrictions on object compatibility apply as in the case of the copy assignment
+ /// operator=(const NamedData&).
+ ///
+ /// \param [in,out] namedData The object to copy from, which will be left in an unusable state.
+ /// \return The object that was assigned into.
+ inline NamedData& operator=(NamedData&& namedData);
+
+ /// Check if the object has been initialized, which means it has a set of entries (i.e., names, indices, and the
+ /// map between them). If the object has not been initialized, it can become initialized on assignment from an
+ /// initialized object.
+ ///
+ /// \return true if initialized.
+ inline bool isValid() const;
+
+ /// Return the object's layout directory, which is its collection of names and indices.
+ /// In most cases, you should use direct assignment instead of doing things via the directory.
+ /// \return The IndexDirectory object containing the names and indices of entries.
+ inline std::shared_ptr<const IndexDirectory> getDirectory() const;
+
+ /// Given a name, return the corresponding index (or -1).
+ /// \param name The name.
+ /// \return the index for that name if one exists; -1 otherwise.
+ inline int getIndex(const std::string& name) const;
+
+ /// Given an index, return the corresponding name (or "").
+ /// \param index The index.
+ /// \return the name for that index if one exists; an empty string otherwise.
+ inline std::string getName(int index) const;
+
+ /// Check whether the object contains an entry with the specified index.
+ /// Logically equivalent to <code>getName(index) != ""</code>.
+ ///
+ /// \param index The index corresponding to the entry.
+ /// \return true if that entry exists, false if not.
+ inline bool hasEntry(int index) const;
+
+ /// Check whether the object contains an entry with the specified name.
+ /// Logically equivalent to <code>getIndex(name) != -1</code>.
+ ///
+ /// \param name The name corresponding to the entry.
+ /// \return true if that entry exists, false if not.
+ inline bool hasEntry(const std::string& name) const;
+
+ /// Check whether the entry with the specified index contains valid data.
+ /// The check verifies that the entry's data was %set using set(int, const T&) or
+ /// set(const std::string&, const T&), without being subsequently invalidated by reset(int)
+ /// or reset(const std::string&).
+ ///
+ /// \param index The index of the entry.
+ /// \return true if that entry exists and contains valid data.
+ inline bool hasData(int index) const;
+
+ /// Check whether the entry with the specified name contains valid data.
+ /// The check verifies that the entry's data was %set using set(int, const T&) or
+ /// set(const std::string&, const T&), without being subsequently invalidated by reset(int)
+ /// or reset(const std::string&).
+ ///
+ /// \param name The name of the entry.
+ /// \return true if that entry exists and contains valid data.
+ inline bool hasData(const std::string& name) const;
+
+ /// Given an index, get the corresponding value.
+ /// It's only possible to get the value if the data was %set using set(int, const T&) or
+ /// set(const std::string&, const T&), without being subsequently invalidated by reset(int)
+ /// or reset(const std::string&). In other words, get returns the same value as hasData would return.
+ ///
+ /// \param index The index of the entry.
+ /// \param [out] value The location for the retrieved value. Must not be null.
+ /// \return true if a valid value is available and was written to \a value.
+ inline bool get(int index, T* value) const;
+
+ /// Given a name, get the corresponding value.
+ /// It's only possible to get the value if the data was %set using set(int, const T&) or
+ /// set(const std::string&, const T&), without being subsequently invalidated by reset(int)
+ /// or reset(const std::string&). In other words, get returns the same value as hasData would return.
+ ///
+ /// \param name The name of the entry.
+ /// \param [out] value The location for the retrieved value. Must not be null.
+ /// \return true if a valid value is available and was written to \a value.
+ inline bool get(const std::string& name, T* value) const;
+
+ /// Record the data for an entry specified by an index.
+ /// The entry will also be marked as containing valid data.
+ ///
+ /// \param index The index of the entry.
+ /// \param value The value to be set.
+ /// \return true if successful.
+ inline bool set(int index, const T& value);
+
+ /// Record the data for an entry specified by a name.
+ /// The entry will also be marked as containing valid data.
+ ///
+ /// \param name The name of the entry.
+ /// \param value The value to be set.
+ /// \return true if successful.
+ inline bool set(const std::string& name, const T& value);
+
+ /// Invalidate an entry— mark it as not containing any valid data.
+ ///
+ /// \param index The index of the entry.
+ /// \return true if successful.
+ inline bool reset(int index);
+
+ /// Invalidate an entry— mark it as not containing any valid data.
+ ///
+ /// \param name The name of the entry.
+ /// \return true if successful.
+ inline bool reset(const std::string& name);
+
+ /// Invalidate all entries— mark everything as not containing any valid data.
+ inline void resetAll();
+
+ /// Check the number of existing entries.
+ /// \return the size of the data collection.
+ /// \sa getNumEntries()
+ inline size_t size() const;
+
+ /// Check the number of existing entries.
+ /// \return the size of the data collection.
+ /// \sa size()
+ inline int getNumEntries() const;
+
+ /// Copy the data from another NamedData, based on a map of indices. Resets entries that are reset in the other.
+ /// \param source The source NamedData.
+ /// \param map The map of indices.
+ /// \exception Asserts if the objects do not have the same template type.
+ template <typename N>
+ void copy(const NamedData<N>& source, const NamedDataCopyMap& map);
+
+ /// Caches an entry's index if it is not already cached. An index is considered already cached if it is >= 0.
+ /// \param name The name of the entry.
+ /// \param [in,out] index The cached index.
+ void cacheIndex(const std::string& name, int* index) const;
+
+private:
+ /// The mapping between names and indices.
+ std::shared_ptr<const IndexDirectory> m_directory;
+
+ /// The array of values.
+ std::vector<T> m_data;
+
+ /// The array storing whether the data is currently valid.
+ std::vector<bool> m_isDataValid;
+};
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+
+#include "SurgSim/DataStructures/NamedData-inl.h"
+
+
+#endif // SURGSIM_DATASTRUCTURES_NAMEDDATA_H
diff --git a/SurgSim/DataStructures/NamedDataBuilder.h b/SurgSim/DataStructures/NamedDataBuilder.h
new file mode 100644
index 0000000..f759943
--- /dev/null
+++ b/SurgSim/DataStructures/NamedDataBuilder.h
@@ -0,0 +1,163 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_NAMEDDATABUILDER_H
+#define SURGSIM_DATASTRUCTURES_NAMEDDATABUILDER_H
+
+#include <memory>
+
+#include "SurgSim/DataStructures/NamedData.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// A class that allows you to build a \ref NamedData structure.
+///
+/// Since the data layout of a \ref NamedData object cannot be modified, this class can be helpful in initially
+/// setting up the names and their corresponding indices. You can add entries to the builder using \ref addEntry
+/// and \ref addEntriesFrom calls, then create the NamedData instance with createData() or createSharedData().
+///
+/// \sa NamedData
+template <typename T>
+class NamedDataBuilder
+{
+public:
+ /// Constructs an empty builder object.
+ NamedDataBuilder()
+ {
+ }
+
+ /// Produces a \ref NamedData object with an immutable set of names and indices.
+ /// None of the values will contain any current data.
+ /// \return the NamedData object *by value*.
+ NamedData<T> createData() const
+ {
+ // NB: can't use copy construction in the std::make_shared call, because access is protected.
+ std::shared_ptr<IndexDirectory> dir = std::make_shared<IndexDirectory>();
+ *dir = m_directory;
+ return NamedData<T>(dir);
+ }
+
+ /// Produces a shared pointer to an empty \ref NamedData object with an immutable set of names and indices.
+ /// None of the values will contain any current data.
+ /// \return a shared pointer to the NamedData object.
+ std::shared_ptr<NamedData<T>> createSharedData() const
+ {
+ return std::make_shared<NamedData<T>>(createData());
+ }
+
+ /// Creates a new entry for the specified name.
+ ///
+ /// \param name The name, which should be non-empty and should not already exist in the data.
+ /// \return the index of the created entry, or -1 if the entry could not be added.
+ int addEntry(const std::string& name)
+ {
+ return m_directory.addEntry(name);
+ }
+
+ /// Create new entries from a vector of names.
+ /// \param names The names.
+ void addEntriesFrom(const std::vector<std::string>& names)
+ {
+ for (auto it = names.cbegin(); it != names.cend(); ++it)
+ {
+ addEntry(*it);
+ }
+ }
+
+ /// Create new entries from another NamedDataBuilder.
+ /// \tparam typename U The data type of the other NamedDataBuilder.
+ /// \param builder The other builder.
+ template <typename U>
+ void addEntriesFrom(const NamedDataBuilder<U>& builder)
+ {
+ addEntriesFrom(builder.getAllNames());
+ }
+
+ /// Create new entries from an already initialized NamedData.
+ /// \tparam typename U The data type of the NamedData.
+ /// \param data The data object.
+ template <typename U>
+ void addEntriesFrom(const NamedData<U>& data)
+ {
+ addEntriesFrom(data.getDirectory()->getAllNames());
+ }
+
+ /// Create new entries from an IndexDirectory.
+ /// \param directory The index directory object.
+ void addEntriesFrom(const IndexDirectory& directory)
+ {
+ addEntriesFrom(directory.getAllNames());
+ }
+
+ /// Given a name, return the corresponding index (or -1).
+ /// \param name The name.
+ /// \return the index for that name if one exists; -1 otherwise.
+ int getIndex(const std::string& name) const
+ {
+ return m_directory.getIndex(name);
+ }
+
+ /// Given an index, return the corresponding name (or "").
+ /// \param index The index.
+ /// \return the name for that index if one exists; an empty string otherwise.
+ std::string getName(int index) const
+ {
+ return m_directory.getName(index);
+ }
+
+ /// Get a list of all the names available in the builder.
+ /// \return all the names.
+ const std::vector<std::string>& getAllNames() const
+ {
+ return m_directory.getAllNames();
+ }
+
+ /// Check whether the specified name exists in the builder.
+ ///
+ /// \param name The name.
+ /// \return true if the entry exists.
+ bool hasEntry(const std::string& name) const
+ {
+ return m_directory.hasEntry(name);
+ }
+
+ /// Check the number of existing entries in the builder.
+ /// \return the number of entries.
+ /// \sa getNumEntries()
+ size_t size() const
+ {
+ return m_directory.size();
+ }
+
+ /// Check the number of existing entries in the builder.
+ /// \return the number of entries.
+ /// \sa size()
+ int getNumEntries() const
+ {
+ return m_directory.getNumEntries();
+ }
+
+private:
+ /// The mapping between names and indices that will be used to create the NamedData instance.
+ IndexDirectory m_directory;
+};
+
+}; // namespace Input
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_NAMEDDATABUILDER_H
diff --git a/SurgSim/DataStructures/NamedVariantData-inl.h b/SurgSim/DataStructures/NamedVariantData-inl.h
new file mode 100644
index 0000000..90e4d38
--- /dev/null
+++ b/SurgSim/DataStructures/NamedVariantData-inl.h
@@ -0,0 +1,96 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_NAMEDVARIANTDATA_INL_H
+#define SURGSIM_DATASTRUCTURES_NAMEDVARIANTDATA_INL_H
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+inline NamedVariantData::NamedVariantData()
+{
+}
+
+inline NamedVariantData::NamedVariantData(const NamedData<boost::any>& namedData) :
+ NamedData<boost::any>(namedData)
+{
+}
+
+template <typename T>
+inline bool NamedVariantData::hasTypedData(int index) const
+{
+ if (! hasData(index))
+ {
+ return false;
+ }
+
+ boost::any a;
+ if (! NamedData::get(index, &a))
+ {
+ return false;
+ }
+
+ if (a.empty())
+ {
+ return false;
+ }
+
+ return (a.type() == typeid(T));
+}
+
+template <typename T>
+inline bool NamedVariantData::hasTypedData(const std::string& name) const
+{
+ if (! hasData(name))
+ {
+ return false;
+ }
+ int index = getIndex(name);
+ return hasTypedData<T>(index);
+}
+
+template <typename T>
+inline bool NamedVariantData::get(int index, T* value) const
+{
+ boost::any a;
+ if (!NamedData::get(index, &a))
+ return false;
+ try
+ {
+ *value = boost::any_cast<T>(a);
+ }
+ catch(const boost::bad_any_cast &)
+ {
+ SURGSIM_FAILURE() << "Cannot cast the named value to the specified type.";
+ }
+ return true;
+}
+
+template <typename T>
+inline bool NamedVariantData::get(const std::string& name, T* value) const
+{
+ int index = getIndex(name);
+ return get(index, value);
+}
+
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_NAMEDVARIANTDATA_INL_H
diff --git a/SurgSim/DataStructures/NamedVariantData.h b/SurgSim/DataStructures/NamedVariantData.h
new file mode 100644
index 0000000..7d36a59
--- /dev/null
+++ b/SurgSim/DataStructures/NamedVariantData.h
@@ -0,0 +1,101 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_NAMEDVARIANTDATA_H
+#define SURGSIM_DATASTRUCTURES_NAMEDVARIANTDATA_H
+
+#include <boost/any.hpp>
+#include "SurgSim/DataStructures/NamedData.h"
+#include "SurgSim/DataStructures/NamedDataBuilder.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+typedef NamedDataBuilder<boost::any> NamedVariantDataBuilder;
+
+/// A NamedData collection of variant data type.
+///
+/// A NamedVariantData collection is a collection variant datatypes. Each entry in the collection can be
+/// accessed by using either its unique name (a std::string) or its unique index (a non-negative integer).
+/// Access by name is more convenient, but also less efficient.
+///
+/// This sub-class of NameData encapsulates the boost::any variant type, and adds two get functions that
+/// provide typed access to the contained data.
+
+class NamedVariantData : public NamedData<boost::any>
+{
+public:
+ NamedVariantData();
+
+ /// Implicit conversion for NamedVariantData constructor is used on purpose.
+ NamedVariantData(const NamedData<boost::any>& namedData); //NOLINT
+
+ /// Check whether the entry with the specified index contains valid data.
+ /// The check verifies that the entry's data is of type T, was %set using
+ /// set(int, const T&) or set(const std::string&, const T&), without being
+ /// subsequently invalidated by reset(int) or reset(const std::string&).
+ ///
+ /// \tparam T the data type to check for at the given index.
+ /// \param index The index of the entry.
+ /// \return true if that entry exists, is of type T, and contains valid data.
+ template <typename T>
+ inline bool hasTypedData(int index) const;
+
+ /// Check whether the entry with the specified name contains valid data.
+ /// The check verifies that the entry's data is of type T, was %set using
+ /// set(int, const T&) or set(const std::string&, const T&), without being
+ /// subsequently invalidated by reset(int) or reset(const std::string&).
+ ///
+ /// \tparam T the data type to check for at the given index.
+ /// \param name The name of the entry.
+ /// \return true if that entry exists, is of type T, and contains valid data.
+ template <typename T>
+ inline bool hasTypedData(const std::string& name) const;
+
+ /// Given an index, get the corresponding value.
+ /// It's only possible to get the value if the data was %set using set(int, const T&) or
+ /// set(const std::string&, const T&), without being subsequently invalidated by reset(int)
+ /// or reset(const std::string&). In other words, get returns the same value as hasData would return.
+ ///
+ /// \tparam T the data type used for the value at the given index.
+ /// \param index The index of the entry.
+ /// \param [out] value The location for the retrieved value. Must not be null.
+ /// \return true if a valid value is available, was written to \a value, and value is the correct type.
+ template <typename T>
+ inline bool get(int index, T* value) const;
+
+ /// Given a name, get the corresponding value.
+ /// It's only possible to get the value if the data was %set using set(int, const T&) or
+ /// set(const std::string&, const T&), without being subsequently invalidated by reset(int)
+ /// or reset(const std::string&). In other words, get returns the same value as hasData would return.
+ ///
+ /// \tparam T the data type used for the value with the given name.
+ /// \param name The name of the entry.
+ /// \param [out] value The location for the retrieved value. Must not be null.
+ /// \return true if a valid value is available, was written to \a value, and value is the correct type.
+ template <typename T>
+ inline bool get(const std::string& name, T* value) const;
+};
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+
+#include "SurgSim/DataStructures/NamedVariantData-inl.h"
+
+
+#endif // SURGSIM_DATASTRUCTURES_NAMEDVARIANTDATA_H
diff --git a/SurgSim/DataStructures/OctreeNode-inl.h b/SurgSim/DataStructures/OctreeNode-inl.h
new file mode 100644
index 0000000..d711511
--- /dev/null
+++ b/SurgSim/DataStructures/OctreeNode-inl.h
@@ -0,0 +1,266 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H
+#define SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H
+
+#include <array>
+#include <cmath>
+#include <fstream>
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+template<class Data>
+OctreeNode<Data>::OctreeNode() :
+ m_isActive(false),
+ m_hasChildren(false)
+{
+}
+
+
+template<class Data>
+OctreeNode<Data>::OctreeNode(const SurgSim::Math::Aabbd& boundingBox) :
+ m_boundingBox(boundingBox),
+ m_isActive(false),
+ m_hasChildren(false)
+{
+}
+
+template<class Data>
+SurgSim::DataStructures::OctreeNode<Data>::OctreeNode(const OctreeNode& other)
+{
+ m_boundingBox = other.m_boundingBox;
+ m_hasChildren = other.m_hasChildren;
+ m_isActive = other.m_isActive;
+
+ // Also copy the data since they are the same type
+ data = other.data;
+
+ for (size_t i = 0; i < other.m_children.size(); i++)
+ {
+ if (other.getChild(i) == nullptr)
+ {
+ m_children[i] = nullptr;
+ }
+ else
+ {
+ m_children[i] = std::make_shared<OctreeNode<Data>>(*other.m_children[i]);
+ }
+ }
+}
+
+template <class Data>
+template <class T>
+SurgSim::DataStructures::OctreeNode<Data>::OctreeNode(const OctreeNode<T>& other)
+{
+ m_boundingBox = other.getBoundingBox();
+ m_hasChildren = other.hasChildren();
+ m_isActive = other.isActive();
+
+ for (size_t i = 0; i < m_children.size(); i++)
+ {
+ auto child = other.getChild(i);
+ if (child == nullptr)
+ {
+ m_children[i] = nullptr;
+ }
+ else
+ {
+ m_children[i] = std::make_shared<OctreeNode<Data>>(*child);
+ }
+ }
+}
+
+template<class Data>
+OctreeNode<Data>::~OctreeNode()
+{
+}
+
+template<class Data>
+const SurgSim::Math::Aabbd& OctreeNode<Data>::getBoundingBox() const
+{
+ return m_boundingBox;
+}
+
+template<class Data>
+bool OctreeNode<Data>::isActive() const
+{
+ return m_isActive;
+}
+
+template<class Data>
+void OctreeNode<Data>::setIsActive(bool isActive)
+{
+ m_isActive = isActive;
+}
+
+template<class Data>
+bool OctreeNode<Data>::hasChildren() const
+{
+ return m_hasChildren;
+}
+
+template<class Data>
+void OctreeNode<Data>::subdivide()
+{
+ using SurgSim::Math::Vector3d;
+
+ if (! m_hasChildren)
+ {
+ Vector3d childsSize = (m_boundingBox.max() - m_boundingBox.min()) / 2.0;
+ AxisAlignedBoundingBox childsBoundingBox;
+ for (int i = 0; i < 8; i++)
+ {
+ // Use the index to pick one of the eight regions
+ Vector3d regionIndex = Vector3d(((i & 1) == 0) ? 0 : 1,
+ ((i & 2) == 0) ? 0 : 1,
+ ((i & 4) == 0) ? 0 : 1);
+ childsBoundingBox.min() = m_boundingBox.min().array() + regionIndex.array() * childsSize.array();
+ childsBoundingBox.max() = childsBoundingBox.min() + childsSize;
+ m_children[i] = std::make_shared<OctreeNode<Data>>(childsBoundingBox);
+ }
+ m_hasChildren = true;
+ }
+}
+
+template<class Data>
+bool OctreeNode<Data>::addData(const SurgSim::Math::Vector3d& position, const Data& nodeData, const int level)
+{
+ return doAddData(position, nodeData, level, 1);
+}
+
+template<class Data>
+bool OctreeNode<Data>::doAddData(const SurgSim::Math::Vector3d& position, const Data& nodeData, const int level,
+ const int currentLevel)
+{
+ if (! m_boundingBox.contains(position))
+ {
+ return false;
+ }
+
+ if (currentLevel == level)
+ {
+ data = nodeData;
+ m_isActive = true;
+ return true;
+ }
+
+ if (! m_hasChildren)
+ {
+ subdivide();
+ }
+ for (auto child = m_children.begin(); child != m_children.end(); ++child)
+ {
+ if ((*child)->doAddData(position, nodeData, level, currentLevel + 1))
+ {
+ m_isActive = true;
+ return true;
+ }
+ }
+ return false;
+}
+
+template<class Data>
+std::array<std::shared_ptr<OctreeNode<Data> >, 8>& OctreeNode<Data>::getChildren()
+{
+ return m_children;
+}
+
+template<class Data>
+const std::array<std::shared_ptr<OctreeNode<Data> >, 8>& OctreeNode<Data>::getChildren() const
+{
+ return m_children;
+}
+
+template<class Data>
+std::shared_ptr<OctreeNode<Data> > OctreeNode<Data>::getChild(size_t index)
+{
+ return m_children[index];
+}
+
+template<class Data>
+const std::shared_ptr<OctreeNode<Data> > OctreeNode<Data>::getChild(size_t index) const
+{
+ return m_children[index];
+}
+
+template<class Data>
+std::shared_ptr<OctreeNode<Data>> OctreeNode<Data>::getNode(const OctreePath& path, bool returnLastValid)
+{
+ std::shared_ptr<OctreeNode<Data>> node = this->shared_from_this();
+ std::shared_ptr<OctreeNode<Data>> previous;
+ for (auto index = path.cbegin(); index != path.cend(); ++index)
+ {
+ previous = std::move(node);
+ node = previous->getChild(*index);
+ if (node == nullptr)
+ {
+ if (returnLastValid)
+ {
+ node = std::move(previous);
+ break;
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Octree path is invalid. Path is longer than octree is deep in this given branch.";
+ }
+ }
+ }
+ return node;
+}
+
+template<class Data>
+bool SurgSim::DataStructures::OctreeNode<Data>::doLoad(const std::string& filePath)
+{
+ std::ifstream octreeData(filePath, std::ios::in);
+ SURGSIM_ASSERT(octreeData) << "Could not open file (" << filePath << ")" << std::endl;
+
+ SurgSim::Math::Vector3d spacing, boundsMin, boundsMax;
+ std::array<int, 3> dimensions;
+ octreeData >> dimensions[0] >> dimensions[1] >> dimensions[2];
+ octreeData >> spacing[0] >> spacing[1] >> spacing[2];
+ octreeData >> boundsMin[0] >> boundsMax[0] >> boundsMin[1] >> boundsMax[1] >> boundsMin[2] >> boundsMax[2];
+
+ int maxDimension = dimensions[0];
+ maxDimension = maxDimension >= dimensions[1] ?
+ (maxDimension >= dimensions[2] ? maxDimension : dimensions[2]) :
+ (dimensions[1] >= dimensions[2] ? dimensions[1] : dimensions[2]);
+
+ int numLevels = static_cast<int>(std::ceil(std::log(maxDimension) / std::log(2.0)));
+ SurgSim::Math::Vector3d octreeDimensions = SurgSim::Math::Vector3d::Ones() * std::pow(2.0, numLevels);
+
+ m_boundingBox.min() = boundsMin;
+ m_boundingBox.max() = boundsMin.array() + octreeDimensions.array() * spacing.array();
+
+ SurgSim::Math::Vector3d position;
+ while (octreeData >> position[0] >> position[1] >> position[2])
+ {
+ addData(position, Data(), numLevels);
+ }
+
+ return true;
+}
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_OCTREENODE_INL_H
diff --git a/SurgSim/DataStructures/OctreeNode.cpp b/SurgSim/DataStructures/OctreeNode.cpp
new file mode 100644
index 0000000..40c4ce9
--- /dev/null
+++ b/SurgSim/DataStructures/OctreeNode.cpp
@@ -0,0 +1,275 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/OctreeNode.h"
+
+#include "SurgSim/Framework/Assert.h"
+
+#include <array>
+#include <cmath>
+#include <fstream>
+
+#include <boost/container/static_vector.hpp>
+
+
+namespace
+{
+
+using SurgSim::DataStructures::SYMBOL_HALT;
+using SurgSim::DataStructures::SYMBOL_LEFT;
+using SurgSim::DataStructures::SYMBOL_RIGHT;
+using SurgSim::DataStructures::SYMBOL_FRONT;
+using SurgSim::DataStructures::SYMBOL_BACK;
+using SurgSim::DataStructures::SYMBOL_UP;
+using SurgSim::DataStructures::SYMBOL_DOWN;
+using SurgSim::DataStructures::Symbol;
+
+struct TransitionCode
+{
+ size_t nodeId;
+ Symbol symbol;
+};
+
+/// State machine codes, paired by new Value, new Directions value
+static TransitionCode transitionTable[6][8] =
+{
+ // Transition codes for 'Down'
+ {
+ {2u, SYMBOL_DOWN}, {3u, SYMBOL_DOWN},
+ {0u, SYMBOL_HALT}, {1u, SYMBOL_HALT},
+ {6u, SYMBOL_DOWN}, {7u, SYMBOL_DOWN},
+ {4u, SYMBOL_HALT}, {5u, SYMBOL_HALT}
+ },
+
+ // Transition Codes for 'Up'
+ {
+ {2u, SYMBOL_HALT}, {3u, SYMBOL_HALT},
+ {0u, SYMBOL_UP}, {1u, SYMBOL_UP},
+ {6u, SYMBOL_HALT}, {7u, SYMBOL_HALT},
+ {4u, SYMBOL_UP}, {5u, SYMBOL_UP}
+ },
+
+ // Transition Codes for 'Right'
+ {
+ {1u, SYMBOL_HALT}, {0u, SYMBOL_RIGHT},
+ {3u, SYMBOL_HALT}, {2u, SYMBOL_RIGHT},
+ {5u, SYMBOL_HALT}, {4u, SYMBOL_RIGHT},
+ {7u, SYMBOL_HALT}, {6u, SYMBOL_RIGHT}
+ },
+
+ // Transition Codes for 'Left'
+ {
+ {1u, SYMBOL_LEFT}, {0u, SYMBOL_HALT},
+ {3u, SYMBOL_LEFT}, {2u, SYMBOL_HALT},
+ {5u, SYMBOL_LEFT}, {4u, SYMBOL_HALT},
+ {7u, SYMBOL_LEFT}, {6u, SYMBOL_HALT}
+ },
+
+ // Transition Codes for 'Back'
+ {
+ {4u, SYMBOL_BACK}, {5u, SYMBOL_BACK},
+ {6u, SYMBOL_BACK}, {7u, SYMBOL_BACK},
+ {0u, SYMBOL_HALT}, {1u, SYMBOL_HALT},
+ {2u, SYMBOL_HALT}, {3u, SYMBOL_HALT}
+ },
+
+ // Transition Codes for 'Front'
+ {
+ {4u, SYMBOL_HALT}, {5u, SYMBOL_HALT},
+ {6u, SYMBOL_HALT}, {7u, SYMBOL_HALT},
+ {0u, SYMBOL_FRONT}, {1u, SYMBOL_FRONT},
+ {2u, SYMBOL_FRONT}, {3u, SYMBOL_FRONT}
+ }
+};
+
+static const std::array<std::array<Symbol, 3>, 6> FaceNeighbors =
+{
+ SYMBOL_DOWN, SYMBOL_HALT, SYMBOL_HALT,
+ SYMBOL_UP, SYMBOL_HALT, SYMBOL_HALT,
+ SYMBOL_RIGHT, SYMBOL_HALT, SYMBOL_HALT,
+ SYMBOL_LEFT, SYMBOL_HALT, SYMBOL_HALT,
+ SYMBOL_BACK, SYMBOL_HALT, SYMBOL_HALT,
+ SYMBOL_FRONT, SYMBOL_HALT, SYMBOL_HALT,
+};
+
+static const std::array<std::array<Symbol, 3>, 12> EdgeNeighbors =
+{
+ SYMBOL_DOWN, SYMBOL_RIGHT, SYMBOL_HALT,
+ SYMBOL_DOWN, SYMBOL_LEFT, SYMBOL_HALT,
+ SYMBOL_DOWN, SYMBOL_FRONT, SYMBOL_HALT,
+ SYMBOL_DOWN, SYMBOL_BACK, SYMBOL_HALT,
+ SYMBOL_UP, SYMBOL_RIGHT, SYMBOL_HALT,
+ SYMBOL_UP, SYMBOL_LEFT, SYMBOL_HALT,
+ SYMBOL_UP, SYMBOL_FRONT, SYMBOL_HALT,
+ SYMBOL_UP, SYMBOL_BACK, SYMBOL_HALT,
+ SYMBOL_BACK, SYMBOL_RIGHT, SYMBOL_HALT,
+ SYMBOL_BACK, SYMBOL_LEFT, SYMBOL_HALT,
+ SYMBOL_FRONT, SYMBOL_RIGHT, SYMBOL_HALT,
+ SYMBOL_FRONT, SYMBOL_LEFT, SYMBOL_HALT,
+};
+
+static const std::array<std::array<Symbol, 3>, 8> VertexNeighbors =
+{
+ SYMBOL_DOWN, SYMBOL_RIGHT, SYMBOL_FRONT,
+ SYMBOL_DOWN, SYMBOL_RIGHT, SYMBOL_BACK,
+ SYMBOL_DOWN, SYMBOL_LEFT, SYMBOL_FRONT,
+ SYMBOL_DOWN, SYMBOL_LEFT, SYMBOL_BACK,
+ SYMBOL_UP, SYMBOL_RIGHT, SYMBOL_FRONT,
+ SYMBOL_UP, SYMBOL_RIGHT, SYMBOL_BACK,
+ SYMBOL_UP, SYMBOL_LEFT, SYMBOL_FRONT,
+ SYMBOL_UP, SYMBOL_LEFT, SYMBOL_BACK,
+};
+
+}
+
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+
+
+std::shared_ptr<OctreeNode<EmptyData>> loadOctree(const std::string& fileName)
+{
+ std::ifstream octreeData(fileName, std::ios::in);
+ SURGSIM_ASSERT(octreeData) << "Could not open file (" << fileName << ")" << std::endl;
+
+ SurgSim::Math::Vector3d spacing, boundsMin, boundsMax;
+ std::array<int, 3> dimensions;
+ octreeData >> dimensions[0] >> dimensions[1] >> dimensions[2];
+ octreeData >> spacing[0] >> spacing[1] >> spacing[2];
+ octreeData >> boundsMin[0] >> boundsMax[0] >> boundsMin[1] >> boundsMax[1] >> boundsMin[2] >> boundsMax[2];
+
+ int maxDimension = dimensions[0];
+ maxDimension = maxDimension >= dimensions[1] ?
+ (maxDimension >= dimensions[2] ? maxDimension : dimensions[2]) :
+ (dimensions[1] >= dimensions[2] ? dimensions[1] : dimensions[2]);
+
+ int numLevels = static_cast<int>(std::ceil(std::log(maxDimension) / std::log(2.0)));
+ SurgSim::Math::Vector3d octreeDimensions = SurgSim::Math::Vector3d::Ones() * std::pow(2.0, numLevels);
+
+ typedef OctreeNode<SurgSim::DataStructures::EmptyData> OctreeNodeType;
+ OctreeNodeType::AxisAlignedBoundingBox boundingBox;
+ boundingBox.min() = boundsMin;
+ boundingBox.max() = boundsMin.array() + octreeDimensions.array() * spacing.array();
+ std::shared_ptr<OctreeNodeType> octree = std::make_shared<OctreeNodeType>(boundingBox);
+
+ SurgSim::Math::Vector3d position;
+ while (octreeData >> position[0] >> position[1] >> position[2])
+ {
+ octree->addData(position, SurgSim::DataStructures::EmptyData(), numLevels);
+ }
+ return octree;
+}
+
+SurgSim::DataStructures::OctreePath getNeighbor(const OctreePath& origin, const std::array<Symbol, 3>& direction)
+{
+ // Early Exit
+ if (origin.size() == 0 || direction[0] == SYMBOL_HALT)
+ {
+ return OctreePath();
+ }
+
+ boost::container::static_vector<int, 3> currentDirection;
+ boost::container::static_vector<int, 3> newDirection;
+
+ for (size_t i = 0; i < direction.size() && direction[i] != SYMBOL_HALT; ++i)
+ {
+ currentDirection.push_back(direction[i]);
+ }
+
+ OctreePath result(origin);
+
+ bool didHalt = false;
+
+ // For each level iterate over all directions and remember the new node value, and any directions
+ // if there were any new directions (transitionCode.second != SYMBOL_HALT) then do the same for the
+ // preceding level. If there are no new directions then we are done and the superior level are unchanged
+ // If after the topmost level was dealt with there are still directions to work with, the neighbor is outside
+ // of the octree, an empty array is returned in that case.
+
+ for (ptrdiff_t pathIndex = origin.size() - 1; pathIndex >= 0; --pathIndex)
+ {
+ newDirection.clear();
+ size_t currentNodeId = origin[pathIndex];
+
+ for (size_t directionIndex = 0; directionIndex < currentDirection.size(); ++directionIndex)
+ {
+ TransitionCode code = transitionTable[currentDirection[directionIndex]][currentNodeId];
+ currentNodeId = code.nodeId;
+ if (code.symbol != SYMBOL_HALT)
+ {
+ newDirection.push_back(code.symbol);
+ }
+ }
+
+ currentDirection = newDirection;
+ result[pathIndex] = currentNodeId;
+
+ // If now other new direction was found then we are done and can break of the algorithm
+ if (currentDirection.empty())
+ {
+ didHalt = true;
+ break;
+ }
+ }
+
+ // If the last symbol was not a Halt, then the node is outside of the tree
+ if (!didHalt)
+ {
+ result.clear();
+ }
+
+ return std::move(result);
+}
+
+std::vector<OctreePath> getNeighbors(const OctreePath& origin, int type)
+{
+ std::vector<OctreePath> result;
+ auto f = [&origin, &result](const std::array<Symbol, 3>& direction)
+ {
+ auto n = getNeighbor(origin, direction);
+ if (! n.empty())
+ {
+ result.push_back(std::move(n));
+ }
+ };
+
+ size_t size = (((NEIGHBORHOOD_FACE & type) == 0) ? 0 : 6) +
+ (((NEIGHBORHOOD_EDGE & type) == 0) ? 0 : 12) +
+ (((NEIGHBORHOOD_VERTEX & type) == 0) ? 0 : 8);
+
+ result.reserve(size);
+
+ if ((NEIGHBORHOOD_FACE & type) != 0)
+ {
+ std::for_each(FaceNeighbors.begin(), FaceNeighbors.end(), f);
+ }
+ if ((NEIGHBORHOOD_EDGE & type) != 0)
+ {
+ std::for_each(EdgeNeighbors.begin(), EdgeNeighbors.end(), f);
+ }
+ if ((NEIGHBORHOOD_VERTEX & type) != 0)
+ {
+ std::for_each(VertexNeighbors.begin(), VertexNeighbors.end(), f);
+ }
+ return std::move(result);
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
diff --git a/SurgSim/DataStructures/OctreeNode.h b/SurgSim/DataStructures/OctreeNode.h
new file mode 100644
index 0000000..4dd76f9
--- /dev/null
+++ b/SurgSim/DataStructures/OctreeNode.h
@@ -0,0 +1,256 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_OCTREENODE_H
+#define SURGSIM_DATASTRUCTURES_OCTREENODE_H
+
+#include <array>
+#include <memory>
+#include <functional>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Aabb.h"
+
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class OctreeShape;
+}
+
+namespace DataStructures
+{
+
+/// Typedef of octree path
+/// The path is a vector of children indexes (each within 0 to 7) that lead to
+/// the specific node the front of the vector holds the index of the root's children.
+typedef std::vector<size_t> OctreePath;
+
+/// Enable the OctreePath to be used as a key in an unordered map, if the int range is exceeded this will just
+/// push the least significant numbers (root addresses) out of scope, it loses a little bit of address space as
+/// octree ids only go from 0-7
+class OctreePathHash
+{
+public:
+ size_t operator()(const OctreePath& path) const
+ {
+ size_t result = 0;
+ for (auto i : path)
+ {
+ result = (result << 3) | i;
+ }
+
+ return m_hasher(result);
+ }
+private:
+ std::hash<size_t> m_hasher;
+};
+
+/// Indicates what neighbors to grab
+enum Neighborhood
+{
+ NEIGHBORHOOD_NONE = 0x0,
+ NEIGHBORHOOD_FACE = 0x1,
+ NEIGHBORHOOD_EDGE = 0x2,
+ NEIGHBORHOOD_VERTEX = 0x4,
+ NEIGHBORHOOD_ALL = 0x1 | 0x2 | 0x4
+};
+
+/// Direction code for the neighborhood search
+enum Symbol
+{
+ SYMBOL_HALT = -1,
+ SYMBOL_DOWN = 0,
+ SYMBOL_UP = 1,
+ SYMBOL_RIGHT = 2,
+ SYMBOL_LEFT = 3,
+ SYMBOL_BACK = 4,
+ SYMBOL_FRONT = 5
+};
+
+/// Calculate the neighbor of an node in the octree by traversing a state machine, see
+/// http://ww1.ucmss.com/books/LFS/CSREA2006/MSV4517.pdf for detailed description.
+/// The information about the location of a nodes neighbor is encoded in a state machine, this machine is traversed
+/// until a 'Halt' instruction is found. If the neighbor is across a boundary on the octree, a new search direction
+/// is determined by the algorithm.
+/// \note The numbering in the paper and our numbering is slightly different, this means the following transformations
+/// took place.
+/// a) The colums where reordered to match our numbering
+/// b) All the numbers where replaced to match out numbering
+/// The number changes where as following: 0 => 6, 1 => 7, 2 => 4, 3 => 5, 4 => 2, 5 => 3, 6 => 0, 7 => 1
+/// \param origin the node whose neighbor is needed
+/// \param direction a set of directions, for face neighbors use 1, for edge neighbors use 2 and for vertex neighbors
+/// use 3 direction, fill the other spots with SYMBOL_HALT. E.g. to find the left neighbor use
+/// {SYMBOL_LEFT, SYMBOL_HALT, SYMBOL_HALT}, for the vertex neighber on the upper left front corner use
+/// {SYMBOL_LEFT, SYMBOLD_FRONT, SYMBOL_UP}
+/// \return a OctreePath to the correct neighbor, empty if the neighbor is outside of the tree
+OctreePath getNeighbor(const OctreePath& origin, const std::array<Symbol, 3>& direction);
+
+/// Fetch a list of neighbors, indicated by the type, Face, Edge and Vertex are possible types and can be combined
+/// via OR
+/// \param origin the node whose neighbor is needed
+/// \param type the kind of neighborhood that is needed, \sa Neighborhood
+/// \return list of paths with neighbors of the node at origin
+std::vector<OctreePath> getNeighbors(const OctreePath& origin, int type);
+
+/// Octree data structure
+///
+/// The octree node consists of an axis aligned bounding box, that can be
+/// subdivided into 8 equally sized subregions. Each subregion is an octree
+/// node and can be further subdivided.
+/// with x-right and y-up on a right handed coordinate system this is the ordering of the nodes of the tree, looking
+/// down the z-axis
+/// Back Face
+/// 2 3
+/// 0 1
+/// Front Face
+/// 6 7
+/// 4 5
+///
+/// \tparam Data Type of extra data stored in each node
+template<class Data>
+class OctreeNode : public SurgSim::Framework::Asset,
+ public std::enable_shared_from_this<OctreeNode<Data>>
+{
+ friend class SurgSim::Math::OctreeShape;
+
+public:
+
+ /// Bounding box type for convenience
+ typedef Eigen::AlignedBox<double, 3> AxisAlignedBoundingBox;
+
+ /// Constructor
+ OctreeNode();
+
+ /// Copy constructor when the template data is the same type
+ /// \param other the octree to copy from
+ OctreeNode(const OctreeNode& other);
+
+ /// Copy constructor when the template data is a different type
+ /// In this case, no data will be copied
+ /// \tparam T type of data stored in the other node
+ /// \param other the octree to copy from
+ template <class T>
+ OctreeNode(const OctreeNode<T>& other);
+
+ /// Constructor
+ /// \param boundingBox The region contained by this octree node
+ explicit OctreeNode(const SurgSim::Math::Aabbd& boundingBox);
+
+ /// Destructor
+ virtual ~OctreeNode();
+
+ /// Get the bounding box for this octree node
+ /// \return the bounding box
+ const SurgSim::Math::Aabbd& getBoundingBox() const;
+
+ /// Is this node active
+ /// \return true if the octree node is active
+ bool isActive() const;
+
+ /// Set active flag for this octree node
+ /// \param isActive True if the octree node is being activated, False otherwise
+ void setIsActive(bool isActive);
+
+ /// Does this node have children
+ /// \return true if this node has children
+ bool hasChildren() const;
+
+ /// Subdivide the node into 8 equal regions. Each subregion will be stored
+ /// as this nodes children.
+ /// \note The data stored in the current node will not be automatically subdivided.
+ void subdivide();
+
+ /// Add data to a node in this octree
+ /// The octree will build the octree as necessary to add the
+ /// node at the specified level
+ /// \param position The position to add the data at
+ /// \param nodeData The data to store in the node
+ /// \param level The number of levels down the octree to store the data
+ /// \return true if data is added
+ bool addData(const SurgSim::Math::Vector3d& position, const Data& nodeData, const int level);
+
+ /// Get the children of this node (non const version)
+ /// \return vector of all eight children
+ std::array<std::shared_ptr<OctreeNode<Data> >, 8>& getChildren();
+
+ /// Get the children of this node
+ /// \return vector of all eight children
+ const std::array<std::shared_ptr<OctreeNode<Data> >, 8>& getChildren() const;
+
+ /// Get a child of this node (non const version)
+ /// \throws SurgSim::Framework::AssertionFailure if the index >= 8
+ /// \param index the child index
+ /// \return the requested octree node
+ std::shared_ptr<OctreeNode<Data> > getChild(size_t index);
+
+ /// Get a child of this node
+ /// \throws SurgSim::Framework::AssertionFailure if the index >= 8
+ /// \param index the child index
+ /// \return the requested octree node
+ const std::shared_ptr<OctreeNode<Data> > getChild(size_t index) const;
+
+ /// Get the node at the supplied path
+ /// \throws SurgSim::Framework::AssertionFailure if returnLastValid is false and the node does not exist.
+ /// \param path the path to the specific node
+ /// \param returnLastValid if true and the path is longer than the tree deep, the function will return
+ // the last node on a given path, otherwise it will throw.
+ /// \return the requested octree node
+ virtual std::shared_ptr<OctreeNode<Data> > getNode(const OctreePath& path, bool returnLastValid = false);
+
+ /// Extra node data
+ Data data;
+
+protected:
+ /// Recursive function that does the adding of the data to the octree
+ /// \param position The position to add the data at
+ /// \param nodeData The data to store in the node
+ /// \param level The number of levels down the octree to store the data
+ /// \param currentLevel Used to keep track of the current level during recursive calls
+ /// \return true if data is added
+ bool doAddData(const SurgSim::Math::Vector3d& position, const Data& nodeData, const int level,
+ const int currentLevel);
+
+ virtual bool doLoad(const std::string& filePath) override;
+
+ /// The bounding box of the current OctreeNode
+ SurgSim::Math::Aabbd m_boundingBox;
+
+ /// True if there is any data inside this node, including data held by children, children's children, etc.
+ bool m_isActive;
+
+ /// True if the node has children
+ bool m_hasChildren;
+
+ /// The children of this node
+ std::array<std::shared_ptr<OctreeNode<Data> >, 8> m_children;
+};
+
+
+/// A free function to load an octree from file.
+/// \param fileName Name of the external file which contains an octree.
+/// \return A std::shared_ptr<> pointing to an OctreeNode.
+std::shared_ptr<OctreeNode<SurgSim::DataStructures::EmptyData>> loadOctree(const std::string& fileName);
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#include "SurgSim/DataStructures/OctreeNode-inl.h"
+
+#endif // SURGSIM_DATASTRUCTURES_OCTREENODE_H
diff --git a/SurgSim/DataStructures/OptionalValue.h b/SurgSim/DataStructures/OptionalValue.h
new file mode 100644
index 0000000..2f35483
--- /dev/null
+++ b/SurgSim/DataStructures/OptionalValue.h
@@ -0,0 +1,173 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_OPTIONALVALUE_H
+#define SURGSIM_DATASTRUCTURES_OPTIONALVALUE_H
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <class T>
+/// Container class that can indicate whether the object has been assigned a value.
+/// \tparam Class of the value that this object contains
+class OptionalValue
+{
+public:
+
+ /// Default Constructor, no value.
+ OptionalValue() : m_hasValue(false)
+ {
+ }
+
+ /// Constructor that assigns a value.
+ /// \param value The value that should be used.
+ explicit OptionalValue(const T& value) : m_hasValue(true), m_value(value)
+ {
+ }
+
+ /// Copy constructor
+ /// \param other The value used for copying.
+ OptionalValue(const OptionalValue& other) : m_hasValue(other.m_hasValue)
+ {
+ if (m_hasValue)
+ {
+ m_value = other.m_value;
+ }
+ }
+
+ /// Query if this object has been assigned a value.
+ /// \return true if yes, false if not.
+ bool hasValue() const
+ {
+ return m_hasValue;
+ }
+
+ /// Mark this object as invalid
+ void invalidate()
+ {
+ m_hasValue = false;
+ }
+
+ /// Set the value of this object, and mark it as valid
+ /// \param val The value of the object
+ void setValue(const T& val)
+ {
+ m_hasValue = true;
+ m_value = val;
+ }
+
+ /// Gets the value.
+ /// \throws SurgSim::Framework::AssertionFailure if the value was not set
+ /// \return The assigned value if set.
+ const T& getValue() const
+ {
+ SURGSIM_ASSERT(m_hasValue) << "Tried to fetch a value from an invalid OptionalValue";
+ return m_value;
+ }
+
+ /// Gets the value
+ /// \note do not implement T& operator*(), because *optionalValue = X; would not be able to set
+ /// the hasValue() property properly.
+ /// \throws SurgSim::Framework::AssertionFailure if the value was not set
+ /// \return the contained value.
+ const T& operator*() const
+ {
+ SURGSIM_ASSERT(m_hasValue) << "Tried to fetch a value from an invalid OptionalValue";
+ return m_value;
+ }
+
+ /// Equality operator.
+ /// \param rhs The right hand side.
+ /// \return true if the parameters are considered equivalent.
+ bool operator==(const OptionalValue<T>& rhs) const
+ {
+ if (m_hasValue == true && rhs.m_hasValue == true)
+ {
+ return m_value == rhs.m_value;
+ }
+ else
+ {
+ return m_hasValue == rhs.m_hasValue;
+ }
+ }
+
+
+ /// Equality operator.
+ /// \param rhs The right hand side with the specific template type.
+ /// \return true if the parameters are considered equivalent.
+ bool operator==(const T& rhs) const
+ {
+ if (m_hasValue)
+ {
+ return m_value == rhs;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+ /// Inequality operator
+ /// \param rhs the right hand side.
+ /// \return true if the parameters are not considered equivalent
+ bool operator!=(const OptionalValue<T>& rhs) const
+ {
+ return !(*this == rhs);
+ }
+
+ /// Inequality operator
+ /// \param rhs the right hand side.
+ /// \return true if the parameters are not considered equivalent
+ bool operator!=(const T& rhs) const
+ {
+ return !(*this == rhs);
+ }
+
+ /// Assignment operator.
+ /// \param rhs The right hand side of the operator.
+ /// \return reference to this.
+ OptionalValue& operator=(const OptionalValue& rhs)
+ {
+ m_hasValue = rhs.m_hasValue;
+ if (m_hasValue)
+ {
+ m_value = rhs.m_value;
+ }
+ return *this;
+ }
+
+ /// Assignment operator from template type, after this hasValue() is true even if the
+ /// right hand side was not initialized
+ /// \param rhs the value to be assigned to this optional value
+ /// \return reference to this.
+ OptionalValue& operator=(const T& rhs)
+ {
+ setValue(rhs);
+ return *this;
+ }
+
+private:
+ bool m_hasValue;
+ T m_value;
+};
+
+}; // Datastructures
+}; // SurgSim
+
+#endif
diff --git a/SurgSim/DataStructures/PerformanceTests/CMakeLists.txt b/SurgSim/DataStructures/PerformanceTests/CMakeLists.txt
new file mode 100644
index 0000000..9f85eef
--- /dev/null
+++ b/SurgSim/DataStructures/PerformanceTests/CMakeLists.txt
@@ -0,0 +1,34 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ NamedDataPerformanceTest.cpp
+)
+
+set(UNIT_TEST_HEADERS
+)
+
+set(LIBS
+ SurgSimDataStructures
+)
+
+surgsim_add_unit_tests(SurgSimDataStructuresPerformanceTest)
+
+set_target_properties(SurgSimDataStructuresPerformanceTest PROPERTIES FOLDER "DataStructures")
diff --git a/SurgSim/DataStructures/PerformanceTests/NamedDataPerformanceTest.cpp b/SurgSim/DataStructures/PerformanceTests/NamedDataPerformanceTest.cpp
new file mode 100644
index 0000000..9c066ac
--- /dev/null
+++ b/SurgSim/DataStructures/PerformanceTests/NamedDataPerformanceTest.cpp
@@ -0,0 +1,124 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <boost/exception/to_string.hpp>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include "SurgSim/DataStructures/NamedData.h"
+#include "SurgSim/DataStructures/NamedDataBuilder.h"
+#include "SurgSim/Framework/Timer.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+class NamedDataTest : public ::testing::Test
+{
+public:
+
+ virtual void SetUp()
+ {
+ numberOfLoops = 100000;
+
+ const std::string array[] =
+ {"uzgfk1f41V",
+ "PbpZficBR2",
+ "M3OYjZgHXX",
+ "WVIEjG3QaX",
+ "2o6nN2nuaW",
+ "3rWXCXR2gi",
+ "XO2dATxWFq",
+ "M2xpjE6PAL",
+ "cT1Bmj3Z1U",
+ "65UtrrfXn7",
+ "tNltVIcurd",
+ "jFVkKzn17i",
+ "0QlA0FAPc3",
+ "jCXGBopngK",
+ "GTwY4YyVnC",
+ "0uVPK4S9Le",
+ "iNdq3p6ZQb",
+ "LfREPeFczN",
+ "9RKdlFnm1I",
+ "N2acPVhGY2"};
+
+ names = std::vector<std::string>(std::begin(array), std::end(array));
+
+ NamedDataBuilder<int> builder;
+ for (std::string name : names)
+ {
+ builder.addEntry(name);
+ }
+
+ data = builder.createData();
+
+ for (int i = 0; i < static_cast<int>(names.size()); ++i)
+ {
+ ASSERT_TRUE(data.set(i, i));
+ indices.push_back(i);
+ }
+ }
+
+ virtual void TearDown()
+ {
+ }
+
+ NamedData<int> data;
+ std::vector<int> indices;
+ std::vector<std::string> names;
+ int numberOfLoops;
+};
+
+TEST_F(NamedDataTest, GetByName)
+{
+ int value;
+ const size_t numberOfEntries = names.size();
+
+ SurgSim::Framework::Timer timer;
+ for (int i = 0; i < numberOfLoops; ++i)
+ {
+ for (size_t j = 0; j < numberOfEntries; ++j)
+ {
+ ASSERT_TRUE(data.get(names[j], &value));
+ }
+ }
+ timer.endFrame();
+ RecordProperty("Duration", boost::to_string(timer.getCumulativeTime()));
+}
+
+TEST_F(NamedDataTest, GetByIndex)
+{
+ int value;
+ const size_t numberOfEntries = indices.size();
+
+ SurgSim::Framework::Timer timer;
+ for (int i = 0; i < numberOfLoops; ++i)
+ {
+ for (size_t j = 0; j < numberOfEntries; ++j)
+ {
+ ASSERT_TRUE(data.get(indices[j], &value));
+ }
+ }
+ timer.endFrame();
+ RecordProperty("Duration", boost::to_string(timer.getCumulativeTime()));
+}
+
+} // namespace DataStructures
+} // namespace SurgSim
diff --git a/SurgSim/DataStructures/PlyReader.cpp b/SurgSim/DataStructures/PlyReader.cpp
new file mode 100644
index 0000000..e19c901
--- /dev/null
+++ b/SurgSim/DataStructures/PlyReader.cpp
@@ -0,0 +1,365 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <algorithm>
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/PlyReaderDelegate.h"
+#include "SurgSim/DataStructures/ply.h"
+
+#include "SurgSim/Framework/Log.h"
+
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+// Pimpl data, keep ply.h out of the SurgSim include chain
+struct PlyReader::Data
+{
+ Data() :
+ plyFile(nullptr),
+ elementNames(nullptr)
+ {
+ types[TYPE_INVALID] = PLY_START_TYPE;
+ types[TYPE_CHAR] = PLY_CHAR;
+ types[TYPE_SHORT] = PLY_SHORT;
+ types[TYPE_INT] = PLY_INT;
+ types[TYPE_UNSIGNED_CHAR] = PLY_UCHAR;
+ types[TYPE_UNSIGNED_SHORT] = PLY_USHORT;
+ types[TYPE_UNSIGNED_INT] = PLY_UINT;
+ types[TYPE_FLOAT] = PLY_FLOAT;
+ types[TYPE_DOUBLE] = PLY_DOUBLE;
+ }
+
+ PlyFile* plyFile;
+ int file_type;
+ float version;
+ int elementCount;
+ char** elementNames;
+ std::unordered_map<int, int> types;
+};
+
+
+PlyReader::PlyReader(std::string filename) :
+ m_filename(filename), m_data(new Data())
+{
+ m_data->plyFile = ply_open_for_reading(
+ filename.data(),
+ &m_data->elementCount,
+ &m_data->elementNames,
+ &m_data->file_type,
+ &m_data->version);
+
+ SURGSIM_LOG_IF(!isValid(), SurgSim::Framework::Logger::getLogger("InputOutput"), WARNING) <<
+ "'" << m_filename << "' is an invalid .ply file";
+}
+
+PlyReader::~PlyReader()
+{
+ if (isValid())
+ {
+ for (int i=0; i < m_data->elementCount; ++i)
+ {
+ free(m_data->elementNames[i]);
+ }
+ free(m_data->elementNames);
+
+ ply_close(m_data->plyFile);
+ m_data->plyFile = nullptr;
+ }
+}
+
+bool PlyReader::isValid() const
+{
+ return m_data->plyFile != nullptr;
+}
+
+bool PlyReader::requestElement(std::string elementName,
+ std::function<void* (const std::string&, size_t)> startElementCallback,
+ std::function<void (const std::string&)> processElementCallback,
+ std::function<void (const std::string&)> endElementCallback)
+{
+ SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
+
+ bool result = false;
+
+ if (hasElement(elementName) && m_requestedElements.find(elementName) == m_requestedElements.end())
+ {
+ ElementInfo info;
+ info.name = elementName;
+ info.startElementCallback = startElementCallback;
+ info.endElementCallback = endElementCallback;
+ info.processElementCallback = processElementCallback;
+
+ m_requestedElements[elementName] = info;
+ result = true;
+ }
+
+ return result;
+}
+
+bool PlyReader::requestScalarProperty(std::string elementName, std::string propertyName, int dataType, int dataOffset)
+{
+ return requestProperty(elementName, propertyName, dataType, dataOffset, 0, 0);
+}
+
+bool PlyReader::requestListProperty(std::string elementName,
+ std::string propertyName,
+ int dataType, int dataOffset,
+ int countType, int countOffset)
+{
+ return requestProperty(elementName, propertyName, dataType, dataOffset, countType, countOffset);
+}
+
+void PlyReader::setStartParseFileCallback(std::function<void (void)> startParseFileCallback)
+{
+ m_startParseFileCallback = startParseFileCallback;
+}
+
+void PlyReader::setEndParseFileCallback(std::function<void (void)> endParseFileCallback)
+{
+ m_endParseFileCallback = endParseFileCallback;
+}
+
+bool PlyReader::requestProperty(std::string elementName,
+ std::string propertyName,
+ int dataType, int dataOffset,
+ int countType, int countOffset)
+{
+ SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
+
+ SURGSIM_ASSERT(hasElement(elementName)) <<
+ "The element <" << elementName << "> has not been requested yet, you cannot access properties for it";
+ SURGSIM_ASSERT(hasProperty(elementName, propertyName)) <<
+ "The requested property <" << propertyName << "> cannot be found in element <" << elementName << ">.";
+
+
+
+ bool result = false;
+
+ bool scalar = isScalar(elementName, propertyName);
+
+ bool wantScalar = (countType == 0);
+ if (wantScalar && !scalar)
+ {
+ SURGSIM_FAILURE() << "Trying to access a list property as a scalar." <<
+ "for element <" << elementName << "> and property <" << propertyName << ">.";
+ }
+ else if (!wantScalar && scalar)
+ {
+ SURGSIM_FAILURE() << "Trying to access a scalar property as a list." <<
+ "for element <" << elementName << "> and property <" << propertyName << ">.";
+ }
+
+ if (hasProperty(elementName, propertyName) && (scalar == wantScalar))
+ {
+ auto itBegin = std::begin(m_requestedElements[elementName].requestedProperties);
+ auto itEnd = std::end(m_requestedElements[elementName].requestedProperties);
+
+ bool doAdd = std::find_if(itBegin, itEnd,
+ [propertyName](PropertyInfo p){return p.propertyName == propertyName;}) == itEnd;
+
+ if (doAdd)
+ {
+ PropertyInfo info;
+ info.propertyName = propertyName;
+ info.dataType = m_data->types[dataType];
+ info.dataOffset = dataOffset;
+ info.countType = m_data->types[countType];
+ info.countOffset = countOffset;
+ m_requestedElements[elementName].requestedProperties.push_back(info);
+ result = true;
+ }
+ }
+
+ return result;
+}
+
+bool PlyReader::setDelegate(std::shared_ptr<PlyReaderDelegate> delegate)
+{
+ SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
+
+ bool result = false;
+ if (delegate != nullptr)
+ {
+ if (delegate->fileIsAcceptable(*this))
+ {
+ result = delegate->registerDelegate(this);
+ }
+ }
+ return result;
+}
+
+void PlyReader::parseFile()
+{
+ SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
+
+ if (m_startParseFileCallback != nullptr)
+ {
+ m_startParseFileCallback();
+ }
+
+ char* currentElementName;
+ for (int elementIndex = 0; elementIndex < m_data->elementCount; ++elementIndex)
+ {
+ currentElementName = m_data->elementNames[elementIndex];
+
+ int numberOfElements;
+ int propertyCount;
+
+ // Free this after we are done with it
+ PlyProperty** properties =
+ ply_get_element_description(m_data->plyFile, currentElementName, &numberOfElements, &propertyCount);
+
+ std::vector<int> listOffsets;
+ // Check if the user wanted this element, if yes process
+ if (m_requestedElements.find(currentElementName) != m_requestedElements.end())
+ {
+ ElementInfo& elementInfo = m_requestedElements[currentElementName];
+
+ // Build the propertyinfo structure
+ for (size_t propertyIndex = 0; propertyIndex < elementInfo.requestedProperties.size(); ++propertyIndex)
+ {
+ PropertyInfo& propertyInfo = elementInfo.requestedProperties[propertyIndex];
+ PlyProperty requestedProperty = {nullptr, 0, 0, 0, 0, 0, 0, 0};
+
+ // Create temp char*
+ std::vector<char> writable(propertyInfo.propertyName.size() + 1);
+ std::copy(propertyInfo.propertyName.begin(), propertyInfo.propertyName.end(), writable.begin());
+ requestedProperty.name = &writable[0];
+ requestedProperty.internal_type = propertyInfo.dataType;
+ requestedProperty.offset = propertyInfo.dataOffset;
+ requestedProperty.count_internal = propertyInfo.countType;
+ requestedProperty.count_offset = propertyInfo.countOffset;
+ requestedProperty.is_list = (propertyInfo.countType != 0) ? PLY_LIST : PLY_SCALAR;
+
+ if (requestedProperty.is_list == PLY_LIST)
+ {
+ listOffsets.push_back(propertyInfo.dataOffset);
+ }
+
+ // Tell ply that we want this property to be read and put into the readbuffer
+ ply_get_property(m_data->plyFile, currentElementName, &requestedProperty);
+ }
+
+ void* readBuffer = elementInfo.startElementCallback(currentElementName, numberOfElements);
+
+ for (int element = 0; element < numberOfElements; ++element)
+ {
+ ply_get_element(m_data->plyFile, readBuffer);
+ if (elementInfo.processElementCallback != nullptr)
+ {
+ elementInfo.processElementCallback(currentElementName);
+ }
+
+ // Free the lists that where allocated by plyreader
+ // This gains access to the buffer, where ply.c put the address of
+ // the memory that was allocated to carry the information for the list property
+ // it does that for all properties that where marked as lists
+ for (size_t i = 0; i<listOffsets.size(); ++i)
+ {
+ void** item = (void **)((char *)readBuffer + listOffsets[i]); // NOLINT
+ free(item[0]);
+ }
+
+ }
+
+ if (elementInfo.endElementCallback != nullptr)
+ {
+ elementInfo.endElementCallback(currentElementName);
+ }
+ }
+ else
+ {
+ // Inefficient way to skip an element, but there does not seem to be an
+ // easy way to ignore an element
+ // The data for other is stored internally in the plyFile data structure
+ // and should not be freed
+ ply_get_other_element(m_data->plyFile, currentElementName, numberOfElements);
+
+ }
+
+ // Free the data allocated in the ply_get_element_description call
+ for (int i = 0; i < propertyCount; ++i)
+ {
+ free(properties[i]->name);
+ free(properties[i]);
+ }
+ free(properties);
+ }
+
+ if (m_endParseFileCallback != nullptr)
+ {
+ m_endParseFileCallback();
+ }
+}
+
+bool PlyReader::parseWithDelegate(std::shared_ptr<PlyReaderDelegate> delegate)
+{
+ bool result = setDelegate(delegate);
+ if (result)
+ {
+ parseFile();
+ }
+ return result;
+}
+
+bool PlyReader::hasElement(std::string elementName) const
+{
+ SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
+
+ return find_element(m_data->plyFile, elementName.c_str()) != nullptr;
+}
+
+bool PlyReader::hasProperty(std::string elementName, std::string propertyName) const
+{
+ SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
+
+ bool result = false;
+ PlyElement* element = find_element(m_data->plyFile, elementName.c_str());
+ if (element != nullptr)
+ {
+ int index;
+ result = find_property(element, propertyName.c_str(), &index) != nullptr;
+ }
+ return result;
+}
+
+bool PlyReader::isScalar(std::string elementName, std::string propertyName) const
+{
+ SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
+
+ bool result = false;
+ PlyElement* element = find_element(m_data->plyFile, elementName.c_str());
+ if (element != nullptr)
+ {
+ int index;
+ PlyProperty* property = find_property(element, propertyName.c_str(), &index);
+ if (property != nullptr)
+ {
+ result = (property->is_list == PLY_SCALAR);
+ }
+ }
+ return result;
+}
+
+} // namespace DataStructures
+} // namespace SurgSim
+
diff --git a/SurgSim/DataStructures/PlyReader.h b/SurgSim/DataStructures/PlyReader.h
new file mode 100644
index 0000000..c159d2f
--- /dev/null
+++ b/SurgSim/DataStructures/PlyReader.h
@@ -0,0 +1,269 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_PLYREADER_H
+#define SURGSIM_DATASTRUCTURES_PLYREADER_H
+
+#include <string>
+#include <functional>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+class PlyReaderDelegate;
+
+/// Wrapper for the C .ply file parser
+/// This class wraps the main functionality for the original C .ply file parser at
+/// http://paulbourke.net/dataformats/ply/
+/// it enables customization of the parsing process either through a delegate class or through executing
+/// the requestElement and requestProperty functions.
+/// The file needs to be a valid .ply file, either ascii or binary, for the reader to be a valid reader.
+/// ## General Information
+/// A ply file consists of a header description followed by the data, a header might look like this
+/// ply
+/// format ascii 1.0 { ascii/binary, format version number }
+/// comment made by Greg Turk { comments keyword specified, like all lines }
+/// comment this file is a cube
+/// element vertex 8 { define "vertex" element, 8 of them in file }
+/// property float x { vertex contains float "x" coordinate }
+/// property float y { y coordinate is also a vertex property }
+/// property float z { z coordinate, too }
+/// element face 6 { there are 6 "face" elements in the file }
+/// property list uchar int vertex_indices { "vertex_indices" is a list of ints }
+/// end_header { delimits the end of the header }
+///
+/// As you can see there are elements with properties, there can be multiple elements and multiple
+/// properties per element. An element can have scalar and/or list properties.
+/// To work correctly users will have to preallocate the memory that will be used by the parser
+/// to deposit the information from the file, and set offsets to the correct value to match
+/// the expected locations in the preallocated receiving memory.
+///
+/// ## Initialisation
+/// The constructor for the PlyReader accepts the file name of the file, it will make sure
+/// the the file exists and is actually a .ply file, after the constructor has executed
+/// isValid() should be true. At this time all the information in the header is available and
+/// users of the reader can tell the reader which elements and which properties are of interest
+/// using the requestElement() and requestProperty() functions.
+///
+/// ## The three types of callback functions
+/// Parsing is accomplished via a set of callback functions. There are three kinds of callback
+/// function each with a different responsibility.
+/// - The BeginElement callback is called whenever the section for an element is encountered in
+/// the file, it will pass the name of the element and the number of elements that will be read
+/// it should return a pointer to the preallocated memory that will be used for reading.
+///
+/// - The ProcessElement callback is called whenever a new element has been read from the file,
+/// the read values can be copied from the preallocated memory that was sent to the parser in the
+/// previous callback. If list data was requested, the memory for the list data needs to be freed
+/// at this time.
+///
+/// - The EndElement callback is called whenever processing of the element is concluded, this
+/// gives users a chance to finalize processing on their side.
+///
+/// ## The delegate
+/// The PlyReaderDelegate interface should be used to create classes that encapsulate the needed
+/// callbacks and their processing.
+
+
+class PlyReader
+{
+public:
+ /// Values that represent the data type/size of requested data.
+ enum Type
+ {
+ TYPE_INVALID = 0,
+ TYPE_CHAR,
+ TYPE_SHORT,
+ TYPE_INT,
+ TYPE_UNSIGNED_CHAR,
+ TYPE_UNSIGNED_SHORT,
+ TYPE_UNSIGNED_INT,
+ TYPE_FLOAT,
+ TYPE_DOUBLE,
+ TYPE_COUNT
+ };
+
+ /// The callback that is being used to indicate the start of an element, the parameters that are passed
+ /// into the callback are the name of the element that is being processed and the number of elements
+ /// that will be processed. The callback needs to return allocated memory that is used by the reader
+ /// to deposit the read information
+ typedef std::function<void* (const std::string&, size_t)> StartElementCallbackType;
+
+ /// The callback that is used for the processing and the end of processing, the parameter passed is
+ /// the name of the element that is being processed.
+ typedef std::function<void (const std::string&)> StandardCallbackType;
+
+ /// Constructor.
+ /// \param filename Filename of the .ply file.
+ explicit PlyReader(std::string filename);
+
+ /// Destructor.
+ virtual ~PlyReader();
+
+ /// Query if this object is valid.
+ /// \return true if the file was successfully opened, false if not.
+ bool isValid() const;
+
+ /// Request element to be processed during parsing.
+ ///
+ /// \param elementName Name of the element that is needed.
+ /// \param startElementCallback The callback to be used when the element is first encountered.
+ /// \param processElementCallback The callback to be used when one element of this type is read.
+ /// \param endElementCallback The callback to be used when all of the elements of this type have been read.
+ ///
+ /// \return true if there is a element elementName in the .ply file and it has not been requested yet.
+ bool requestElement(std::string elementName,
+ std::function<void* (const std::string&, size_t)> startElementCallback,
+ std::function<void (const std::string&)> processElementCallback,
+ std::function<void (const std::string&)> endElementCallback);
+
+ /// Request a scalar property for parsing.
+ /// Use this for when you want the information from a scalar property from the .ply file. With this call
+ /// you register the type that you want for storing the data and the offset in the data structure where the
+ /// information should be stored. The data actually comes from the startElementCallback that was supplied
+ /// in the previous call
+ /// \warning If the offset is wrong or the data type provided and the actual data type in your structure
+ /// does not match there could be a buffer overrun, use this with caution
+ /// \param elementName Name of the element that contains this property.
+ /// \param propertyName Name of the property that you want stored.
+ /// \param dataType The type of the data that should be stored.
+ /// \param dataOffset The offset of the data in your data structure where the data should be stored.
+ /// \return true if the property exists and has not been registered yet and is a scalar property,
+ /// otherwise false.
+ bool requestScalarProperty(std::string elementName, std::string propertyName, int dataType, int dataOffset);
+
+ /// Request a list property for parsing.
+ /// Use this for when you want the information from a list property from the .ply file. The item in your
+ /// data structure should be a pointer of the type of data that you want, the reader will allocate the needed
+ /// space and deposit all the items in the list in this space.
+ /// \warning If the offset is wrong or the data type provided and the actual data type in your structure
+ /// does not match there could be a buffer overrun, use this with caution.
+ /// \param elementName Name of the element that contains this property.
+ /// \param propertyName Name of the property that you want stored.
+ /// \param dataType The type of the data that should be stored.
+ /// \param dataOffset The offset of the data in your data structure where the data should be stored.
+ /// \param countType The type of the number of element that should be stored.
+ /// \param countOffset The offset for storing the count.
+ ///
+ /// \return true if it succeeds, false if it fails.
+ bool requestListProperty(std::string elementName,
+ std::string propertyName,
+ int dataType, int dataOffset,
+ int countType, int countOffset);
+
+ /// Query if this elementName is in the .ply file
+ /// \param elementName Name of the element.
+ /// \return true if yes, false otherwise.
+ bool hasElement(std::string elementName) const;
+
+ /// Query if 'elementName' has the given property.
+ /// \param elementName Name of the element.
+ /// \param propertyName Name of the property.
+ /// \return true if the element exists and has the property, false otherwise.
+ bool hasProperty(std::string elementName, std::string propertyName) const;
+
+ /// Query if the property of the give element is scalar.
+ /// \param elementName Name of the element.
+ /// \param propertyName Name of the property.
+ /// \return true if the element exists and has the property and it is a scalar value.
+ bool isScalar(std::string elementName, std::string propertyName) const;
+
+ /// Sets a delegate for parsing and then parse the file.
+ /// \param delegate The delegate.
+ /// \return true if set and parse are successful; otherwise false.
+ bool parseWithDelegate(std::shared_ptr<PlyReaderDelegate> delegate);
+
+ /// Register callback to be called at the begining of parseFile.
+ void setStartParseFileCallback(std::function<void (void)> startParseFileCallback);
+
+ /// Register callback to be called at the end of parseFile.
+ void setEndParseFileCallback(std::function<void (void)> endParseFileCallback);
+
+ /// Sets a delegate for parsing.
+ /// \param delegate The delegate.
+ /// \return true if it succeeds and the properties in the ply file satisfy the delegates fileIsAcceptable().
+ bool setDelegate(std::shared_ptr<PlyReaderDelegate> delegate);
+
+ /// Parse the file.
+ void parseFile();
+
+private:
+ friend class PlyReaderTests;
+
+ /// Generic Internal function to handle list and scalar properties, see requestScalarProperty() and
+ /// requestListProperty() for full documentation.
+ /// \param elementName Name of the element that contains this property.
+ /// \param propertyName Name of the property that you want stored.
+ /// \param dataType The type of the data that should be stored.
+ /// \param dataOffset The offset of the data in your data structure where the data should be stored.
+ /// \param countType The type of the number of element that should be stored.
+ /// \param countOffset The offset for storing the count.
+ ///
+ /// \return true if it succeeds, false if it fails.
+ bool requestProperty(std::string elementName,
+ std::string propertyName,
+ int dataType, int dataOffset,
+ int countType, int countOffset);
+
+ /// The name of the .ply file
+ std::string m_filename;
+
+ /// Information about the property on the .ply file.
+ struct PropertyInfo
+ {
+ std::string propertyName; ///< Name of the property.
+ int dataType; ///< Type of the receiving data.
+ int dataOffset; ///< Location for the receiving data.
+ int countType; ///< For lists, type of the receiving data for the count of listelements.
+ int countOffset; ///< For lists, location of the receiving data for the count.
+ };
+
+ /// Information about the element in the .ply file.
+ struct ElementInfo
+ {
+ std::string name; ///< Name of the element
+ StartElementCallbackType startElementCallback; ///< Callback to be used when the element is first encountered.
+ StandardCallbackType processElementCallback; ///< Callback to be used for each processed element.
+ StandardCallbackType endElementCallback; ///< Callback to be used after all the elements have been processed.
+ std::vector<PropertyInfo> requestedProperties; ///< All the properties that are wanted
+ };
+
+ std::unordered_map<std::string, ElementInfo> m_requestedElements;
+
+ ///@{
+ /// Pimpl Data to wrap ply reader local information
+ struct Data;
+ std::unique_ptr<Data> m_data;
+ ///@}
+
+ /// The delegate.
+ std::shared_ptr<PlyReaderDelegate> m_delegate;
+
+ /// Callback to be executed at the start of 'parseFile'.
+ std::function<void(void)> m_startParseFileCallback;
+
+ /// Callback to be executed at the end of 'parseFile'.
+ std::function<void(void)> m_endParseFileCallback;
+};
+
+} // DataStructures
+} // SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_PLYREADER_H
\ No newline at end of file
diff --git a/SurgSim/DataStructures/PlyReaderDelegate.h b/SurgSim/DataStructures/PlyReaderDelegate.h
new file mode 100644
index 0000000..abddcfb
--- /dev/null
+++ b/SurgSim/DataStructures/PlyReaderDelegate.h
@@ -0,0 +1,55 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_PLYREADERDELEGATE_H
+#define SURGSIM_DATASTRUCTURES_PLYREADERDELEGATE_H
+
+
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+class PlyReader;
+
+/// PlyReaderDelegate abstract class.
+/// The purpose of this class is to customize the parsing and contain the callback functions that
+/// are being used in the parsing process.
+class PlyReaderDelegate
+{
+public:
+
+ /// Virtual destructor.
+ virtual ~PlyReaderDelegate()
+ {
+ }
+
+ /// Registers the delegate with the reader.
+ /// \param [out] reader The reader that should be used by the delegate.
+ /// \return true usually if the reader is valid and fileIsAcceptable() is true.
+ virtual bool registerDelegate(PlyReader* reader) = 0;
+
+ /// Check whether the file in the reader can be used with this delegate,
+ /// this gives the delegate a chance to make sure that all the elements and
+ /// properties that are required are available in the file encapsulated by
+ /// the reader
+ virtual bool fileIsAcceptable(const PlyReader& reader) = 0;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/DataStructures/TetrahedronMesh-inl.h b/SurgSim/DataStructures/TetrahedronMesh-inl.h
new file mode 100644
index 0000000..cad13e1
--- /dev/null
+++ b/SurgSim/DataStructures/TetrahedronMesh-inl.h
@@ -0,0 +1,292 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TETRAHEDRONMESH_INL_H
+#define SURGSIM_DATASTRUCTURES_TETRAHEDRONMESH_INL_H
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ TetrahedronMesh()
+{
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ ~TetrahedronMesh()
+{
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+size_t
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ addEdge(const EdgeType& edge)
+{
+ m_edges.push_back(edge);
+ return m_edges.size() - 1;
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+size_t
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ addTriangle(const TriangleType& triangle)
+{
+ m_triangles.push_back(triangle);
+ return m_triangles.size() - 1;
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+size_t
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ addTetrahedron(const TetrahedronType& tetrahedron)
+{
+ m_tetrahedrons.push_back(tetrahedron);
+ return m_tetrahedrons.size() - 1;
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+size_t
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getNumEdges() const
+{
+ return m_edges.size();
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+size_t
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getNumTriangles() const
+{
+ return m_triangles.size();
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+size_t
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getNumTetrahedrons() const
+{
+ return m_tetrahedrons.size();
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+const std::vector<typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::EdgeType>&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getEdges() const
+{
+ return m_edges;
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+std::vector<typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::EdgeType>&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getEdges()
+{
+ return m_edges;
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+const std::vector<typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::TriangleType>&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getTriangles() const
+{
+ return m_triangles;
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+std::vector<typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::TriangleType>&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getTriangles()
+{
+ return m_triangles;
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+const std::vector<typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::TetrahedronType>&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getTetrahedrons() const
+{
+ return m_tetrahedrons;
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+std::vector<typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::TetrahedronType>&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getTetrahedrons()
+{
+ return m_tetrahedrons;
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+const typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::EdgeType&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getEdge(size_t id) const
+{
+ return m_edges[id];
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::EdgeType&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getEdge(size_t id)
+{
+ return m_edges[id];
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+const typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::TriangleType&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getTriangle(size_t id) const
+{
+ return m_triangles[id];
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::TriangleType&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getTriangle(size_t id)
+{
+ return m_triangles[id];
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+const typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::TetrahedronType&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getTetrahedron(size_t id) const
+{
+ return m_tetrahedrons[id];
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::TetrahedronType&
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ getTetrahedron(size_t id)
+{
+ return m_tetrahedrons[id];
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+bool
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ isValid() const
+{
+ typedef typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::EdgeType
+ EdgeType;
+ typedef typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::TriangleType
+ TriangleType;
+ typedef typename TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::TetrahedronType
+ TetrahedronType;
+
+ size_t numVertices = Vertices<VertexData>::getNumVertices();
+
+ // Test edges validity
+ for (typename std::vector<EdgeType>::const_iterator it = m_edges.begin();
+ it != m_edges.end();
+ it++)
+ {
+ for (size_t vertexId = 0; vertexId < 2; vertexId++)
+ {
+ if (it->verticesId[vertexId] >= numVertices)
+ {
+ return false;
+ }
+ }
+ }
+
+ // Test triangles validity
+ for (typename std::vector<TriangleType>::const_iterator it = m_triangles.begin();
+ it != m_triangles.end();
+ it++)
+ {
+ for (size_t vertexId = 0; vertexId < 3; vertexId++)
+ {
+ if (it->verticesId[vertexId] >= numVertices)
+ {
+ return false;
+ }
+ }
+ }
+
+ // Test tetrahedrons validity
+ for (typename std::vector<TetrahedronType>::const_iterator it = m_tetrahedrons.begin();
+ it != m_tetrahedrons.end();
+ it++)
+ {
+ for (size_t vertexId = 0; vertexId < 4; vertexId++)
+ {
+ if (it->verticesId[vertexId] >= numVertices)
+ {
+ return false;
+ }
+ }
+ }
+
+ return true;
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+void
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ doClearEdges()
+{
+ m_edges.clear();
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+void
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ doClearTriangles()
+{
+ m_triangles.clear();
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+void
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ doClearTetrahedrons()
+{
+ m_tetrahedrons.clear();
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+bool
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ isEqual(const Vertices<VertexData>& mesh) const
+{
+ const TetrahedronMesh& tetrahedronMesh = static_cast<const TetrahedronMesh&>(mesh);
+ return Vertices<VertexData>::isEqual(mesh) && m_edges == tetrahedronMesh.getEdges() &&
+ m_triangles == tetrahedronMesh.getTriangles() && m_tetrahedrons == tetrahedronMesh.getTetrahedrons();
+}
+
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+void
+ TetrahedronMesh<VertexData, EdgeData, TriangleData, TetrahedronData>::
+ doClear()
+{
+ doClearTetrahedrons();
+ doClearTriangles();
+ doClearEdges();
+ Vertices<VertexData>::doClearVertices();
+}
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_TETRAHEDRONMESH_INL_H
diff --git a/SurgSim/DataStructures/TetrahedronMesh.h b/SurgSim/DataStructures/TetrahedronMesh.h
new file mode 100644
index 0000000..d417f3e
--- /dev/null
+++ b/SurgSim/DataStructures/TetrahedronMesh.h
@@ -0,0 +1,207 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TETRAHEDRONMESH_H
+#define SURGSIM_DATASTRUCTURES_TETRAHEDRONMESH_H
+
+#include <vector>
+
+#include "SurgSim/DataStructures/MeshElement.h"
+#include "SurgSim/DataStructures/Vertices.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+/// Basic class for storing Tetrahedron Meshes, handling basic vertex, edge, triangle and tetrahedron functionality.
+///
+/// TetrahedronMesh is to be used purely as a data structure and not provide implementation of algorithms.
+/// For example, a physics 3D FEM is not a subclass of TetrahedronMesh, but may use a TetrahedronMesh for storing the
+/// structure of the FEM.
+///
+/// It is recommended that subclasses with a specific purpose (such as for use in collision detection) provide
+/// convenience methods for creation of vertices, edges, triangles, tetrahedrons and the data each contains.
+/// Methods such as createVertex(position, other data...), createEdge(vertices, other data...),
+/// createTriangle(vertices, other data...) and createTetrahedron(vertices, other data...) simplify the creation of
+/// vertices and elements and the data required.
+/// These methods would use the addVertex(), addEdge(), addTriangle() and addTetrahedron() methods to add the created
+/// vertices and elements to the TetrahedronMesh.
+///
+/// Overriding isEqual(const Mesh&) is necessary to do more than just basic list comparison of the
+/// vertices, edges, triangles and tetrahedrons which is dependent on order in the list.
+///
+/// Override doUpdate() to provide update functionality when vertices are changed, such as recalculating surface
+/// normals.
+///
+/// A subclass that is designed for a specific use (such as collision detection) may also specify the VertexData,
+/// EdgeData, TriangleData and TetrahedronData to what is required.
+///
+/// \tparam VertexData Type of extra data stored in each vertex
+/// \tparam EdgeData Type of extra data stored in each edge
+/// \tparam TriangleData Type of extra data stored in each triangle
+/// \tparam TetrahedronData Type of extra data stored in each tetrahedron
+/// \sa Vertices
+/// \sa MeshElement
+template <class VertexData, class EdgeData, class TriangleData, class TetrahedronData>
+class TetrahedronMesh : public Vertices<VertexData>
+{
+public:
+ /// Edge type for convenience (Ids of the 2 vertices)
+ typedef MeshElement<2, EdgeData> EdgeType;
+ /// Triangle type for convenience (Ids of the 3 vertices)
+ typedef MeshElement<3, TriangleData> TriangleType;
+ /// Tetrahedron type for convenience (Ids of the 4 vertices)
+ typedef MeshElement<4, TetrahedronData> TetrahedronType;
+
+ /// Constructor. The mesh is initially empty (no vertices, no edges, no triangles, no tetrahedrons).
+ TetrahedronMesh();
+
+ /// Destructor
+ virtual ~TetrahedronMesh();
+
+ /// Adds an edge to the mesh.
+ /// No checking on the edge's vertices is performed.
+ /// Recommend that subclasses with a specific purpose (such as for use in collision detection) have a
+ /// createEdge(vertices, other data...) method which performs any checking desired and sets up the edge data based
+ /// on the vertices and other parameters.
+ /// \param edge Edge to add to the mesh
+ /// \return Unique ID of the new edge.
+ size_t addEdge(const EdgeType& edge);
+
+ /// Adds a triangle to the mesh.
+ /// \param triangle Triangle to add to the mesh
+ /// Recommend that subclasses with a specific purpose (such as for use in collision detection) have a
+ /// createTriangle(vertices, other data...) method which performs any checking desired and sets up the triangle data
+ /// based on the vertices and other parameters.
+ /// \return Unique ID of the new triangle.
+ size_t addTriangle(const TriangleType& triangle);
+
+ /// Adds a tetrahedron to the mesh.
+ /// \param tetrahedron Tetrahedron to add to the mesh
+ /// Recommend that subclasses with a specific purpose (such as for use in collision detection) have a
+ /// createTetrahedron(vertices, other data...) method which performs any checking desired and sets up the
+ /// tetrahedron data based on the vertices and other parameters.
+ /// \return Unique ID of the new tetrahedron.
+ size_t addTetrahedron(const TetrahedronType& tetrahedron);
+
+ /// Returns the number of edges in this mesh.
+ /// \return The number of edges
+ size_t getNumEdges() const;
+
+ /// Returns the number of triangles in this mesh.
+ /// \return The number of triangles
+ size_t getNumTriangles() const;
+
+ /// Returns the number of tetrahedrons in this mesh.
+ /// \return The number of tetrahedrons
+ size_t getNumTetrahedrons() const;
+
+ /// Returns a vector containing the position of each edge.
+ /// \return The vector containing all edges
+ const std::vector<EdgeType>& getEdges() const;
+ /// Returns a vector containing the position of each edge (non const version).
+ /// \return The vector containing all edges
+ std::vector<EdgeType>& getEdges();
+
+ /// Returns a vector containing the position of each triangle.
+ /// \return The vector containing all triangles
+ const std::vector<TriangleType>& getTriangles() const;
+ /// Returns a vector containing the position of each triangle (non const version).
+ /// \return The vector containing all triangles
+ std::vector<TriangleType>& getTriangles();
+
+ /// Returns a vector containing the position of each tetrahedron.
+ /// \return The vector containing all tetrahedrons
+ const std::vector<TetrahedronType>& getTetrahedrons() const;
+ /// Returns a vector containing the position of each tetrahedron (non const version).
+ /// \return The vector containing all tetrahedrons
+ std::vector<TetrahedronType>& getTetrahedrons();
+
+ /// Returns the specified edge.
+ /// \param id The edge's id
+ /// \return The edge id
+ /// \note No check is performed on the id
+ const EdgeType& getEdge(size_t id) const;
+ /// Returns the specified edge (non const version).
+ /// \param id The edge's id
+ /// \return The edge id
+ /// \note No check is performed on the id
+ EdgeType& getEdge(size_t id);
+
+ /// Returns the specified triangle.
+ /// \param id The triangle's id
+ /// \return The triangle id
+ /// \note No check is performed on the id
+ const TriangleType& getTriangle(size_t id) const;
+ /// Returns the specified triangle (non const version).
+ /// \param id The triangle's id
+ /// \return The triangle id
+ /// \note No check is performed on the id
+ TriangleType& getTriangle(size_t id);
+
+ /// Returns the specified tetrahedron.
+ /// \param id The tetrahedron's id
+ /// \return The tetrahedron id
+ /// \note No check is performed on the id
+ const TetrahedronType& getTetrahedron(size_t id) const;
+ /// Returns the specified tetrahedron (non const version).
+ /// \param id The tetrahedron's id
+ /// \return The tetrahedron id
+ /// \note No check is performed on the id
+ TetrahedronType& getTetrahedron(size_t id);
+
+ /// Test if the TetrahedronMesh is valid (valid vertex Ids used in all MeshElements)
+ /// \return True if the TetrahedronMesh is valid, False otherwise (the topology is then broken)
+ bool isValid() const;
+
+protected:
+ /// Remove all edges from the mesh.
+ virtual void doClearEdges();
+
+ /// Remove all triangles from the mesh.
+ virtual void doClearTriangles();
+
+ /// Remove all tetrahedrons from the mesh.
+ virtual void doClearTetrahedrons();
+
+ /// Internal comparison of meshes of the same type: returns true if equal, false if not equal.
+ /// Override this method to provide custom comparison. Basic TriangleMesh implementation compares vertices,
+ /// edges and triangles: the order of vertices, edges, and triangles must also match to be considered equal.
+ /// \param mesh Mesh must be of the same type as that which it is compared against
+ virtual bool isEqual(const Vertices<VertexData>& mesh) const;
+private:
+
+ /// Clear mesh to return to an empty state (no vertices, no edges, no triangles, no m_tetrahedrons).
+ virtual void doClear();
+
+ /// Edges
+ std::vector<EdgeType> m_edges;
+
+ /// Triangles
+ std::vector<TriangleType> m_triangles;
+
+ /// Tetrahedrons
+ std::vector<TetrahedronType> m_tetrahedrons;
+};
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
+
+#include "SurgSim/DataStructures/TetrahedronMesh-inl.h"
+
+#endif // SURGSIM_DATASTRUCTURES_TETRAHEDRONMESH_H
diff --git a/SurgSim/DataStructures/Tree.cpp b/SurgSim/DataStructures/Tree.cpp
new file mode 100644
index 0000000..512eafd
--- /dev/null
+++ b/SurgSim/DataStructures/Tree.cpp
@@ -0,0 +1,55 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/Tree.h"
+#include "SurgSim/DataStructures/TreeNode.h"
+
+#include <typeinfo>
+
+using SurgSim::DataStructures::Tree;
+using SurgSim::DataStructures::TreeNode;
+
+Tree::Tree()
+{
+}
+
+Tree::~Tree()
+{
+}
+
+bool Tree::operator==(const Tree& tree) const
+{
+ return (typeid(*this) == typeid(tree)) && isEqual(tree);
+}
+
+bool Tree::operator!=(const Tree& tree) const
+{
+ return (typeid(*this) != typeid(tree)) || ! isEqual(tree);
+}
+
+bool Tree::isEqual(const Tree& tree) const
+{
+ return *m_root == *tree.m_root;
+}
+
+void SurgSim::DataStructures::Tree::setRoot(std::shared_ptr<TreeNode> root)
+{
+ m_root = root;
+}
+
+std::shared_ptr<TreeNode> SurgSim::DataStructures::Tree::getRoot() const
+{
+ return m_root;
+}
diff --git a/SurgSim/DataStructures/Tree.h b/SurgSim/DataStructures/Tree.h
new file mode 100644
index 0000000..087c2d2
--- /dev/null
+++ b/SurgSim/DataStructures/Tree.h
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TREE_H
+#define SURGSIM_DATASTRUCTURES_TREE_H
+
+#include <memory>
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+class TreeNode;
+
+/// Basic tree structure.
+/// The tree is composed of TreeNodes, which are all accessible from the root.
+/// \sa TreeNode TreeData
+class Tree
+{
+public:
+ /// Constructor. After construction, the root is null.
+ Tree();
+
+ /// Destructor
+ virtual ~Tree();
+
+ /// Sets the root of the tree.
+ /// \param root The new root of the tree.
+ void setRoot(std::shared_ptr<TreeNode> root);
+
+ /// \return The root of the tree.
+ std::shared_ptr<TreeNode> getRoot() const;
+
+ /// If the trees are not of the same type, returns false;
+ /// otherwise, compares with the implementation of isEqual(const Tree&).
+ /// \param tree Other tree for comparison.
+ /// \return true if the trees are equal; otherwise, returns false.
+ bool operator==(const Tree& tree) const;
+
+ /// If the trees are not of the same type, returns false;
+ /// otherwise, compares with the implementation of isEqual(const Tree&).
+ /// \param tree Other tree for comparison.
+ /// \return true if the trees are not equal; otherwise, returns false.
+ bool operator!=(const Tree& tree) const;
+
+protected:
+ /// Recurses through the tree, starting at the root.
+ /// Override this method in derived classes to implement different comparisons.
+ /// \param tree Other tree for comparison.
+ /// \return true if the trees are equal; otherwise, returns false.
+ virtual bool isEqual(const Tree& tree) const;
+
+private:
+ /// Root of the tree.
+ std::shared_ptr<TreeNode> m_root;
+
+};
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_TREE_H
diff --git a/SurgSim/DataStructures/TreeData.cpp b/SurgSim/DataStructures/TreeData.cpp
new file mode 100644
index 0000000..5e5e867
--- /dev/null
+++ b/SurgSim/DataStructures/TreeData.cpp
@@ -0,0 +1,37 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/TreeData.h"
+
+#include <typeinfo>
+
+using SurgSim::DataStructures::TreeData;
+
+TreeData::TreeData()
+{
+}
+TreeData::~TreeData()
+{
+}
+
+bool TreeData::operator==(const TreeData& data) const
+{
+ return (typeid(*this) == typeid(data)) && isEqual(&data);
+}
+
+bool TreeData::operator!=(const TreeData& data) const
+{
+ return (typeid(*this) != typeid(data)) || ! isEqual(&data);
+}
diff --git a/SurgSim/DataStructures/TreeData.h b/SurgSim/DataStructures/TreeData.h
new file mode 100644
index 0000000..47f9b71
--- /dev/null
+++ b/SurgSim/DataStructures/TreeData.h
@@ -0,0 +1,65 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TREEDATA_H
+#define SURGSIM_DATASTRUCTURES_TREEDATA_H
+
+#include <typeinfo>
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+/// Abstract base class for data stored in a Tree. Each TreeNode has a pointer to a TreeData object.
+/// \sa TreeNode Tree
+class TreeData
+{
+public:
+
+ /// Constructor
+ TreeData();
+
+ /// Destructor
+ virtual ~TreeData();
+
+ /// If the data are not of the same type, returns false;
+ /// otherwise, compares with the implementation of isEqual(const TreeData&).
+ /// \param data Other TreeData for comparison.
+ /// \return true if the data are equal; otherwise, returns false.
+ bool operator==(const TreeData& data) const;
+
+ /// Returns true if the data are not equal; otherwise, returns false.
+ /// If the data are not of the same type, returns false;
+ /// otherwise, compares with the implementation of isEqual(const TreeData&).
+ /// \param data Other TreeData for comparison.
+ /// \return true if the data are equal; otherwise, returns false.
+ bool operator!=(const TreeData& data) const;
+
+private:
+
+ /// Returns true if the trees are equal; otherwise, returns false.
+ /// Implement this method in derived classes to do the comparison.
+ /// \param data Other TreeData for comparison.
+ /// \return true if the data are equal; otherwise, returns false.
+ virtual bool isEqual(const TreeData* data) const = 0;
+};
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_TREEDATA_H
diff --git a/SurgSim/DataStructures/TreeNode.cpp b/SurgSim/DataStructures/TreeNode.cpp
new file mode 100644
index 0000000..ab9cee0
--- /dev/null
+++ b/SurgSim/DataStructures/TreeNode.cpp
@@ -0,0 +1,112 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/TreeNode.h"
+
+#include "SurgSim/DataStructures/TreeData.h"
+#include "SurgSim/Framework/Assert.h"
+
+#include <typeinfo>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+TreeNode::TreeNode() : m_data(nullptr)
+{
+}
+TreeNode::~TreeNode()
+{
+}
+
+bool TreeNode::operator==(const TreeNode& node) const
+{
+ return (typeid(*this) == typeid(node)) && isEqual(node);
+}
+
+bool TreeNode::operator!=(const TreeNode& node) const
+{
+ return (typeid(*this) != typeid(node)) || ! isEqual(node);
+}
+
+bool TreeNode::isEqual(const TreeNode& node) const
+{
+ if ((m_data != nullptr && node.m_data == nullptr) || (m_data == nullptr && node.m_data != nullptr))
+ {
+ return false;
+ }
+ if (m_data != nullptr && node.m_data != nullptr)
+ {
+ return *m_data == *node.m_data;
+ }
+ return true;
+}
+
+void TreeNode::setData(std::shared_ptr<TreeData> data)
+{
+ m_data = data;
+}
+
+std::shared_ptr<TreeData> TreeNode::getData() const
+{
+ return m_data;
+}
+
+void TreeNode::setNumChildren(size_t numChildren)
+{
+ m_children.resize(numChildren);
+}
+
+size_t TreeNode::getNumChildren() const
+{
+ return m_children.size();
+}
+
+void TreeNode::addChild(const std::shared_ptr<TreeNode>& node)
+{
+ m_children.emplace_back(node);
+}
+
+void TreeNode::addChild(const std::shared_ptr<TreeNode>&& node)
+{
+ m_children.emplace_back(node);
+}
+
+void TreeNode::setChild(size_t index, const std::shared_ptr<TreeNode>& node)
+{
+ SURGSIM_ASSERT(index < m_children.size()) << "setChild() with invalid index for child.";
+ m_children[index] = node;
+}
+
+std::shared_ptr<TreeNode> TreeNode::getChild(size_t index) const
+{
+ SURGSIM_ASSERT(index < m_children.size()) << "getChild() with invalid index for child.";
+ return m_children[index];
+}
+
+void TreeNode::accept(TreeVisitor* visitor)
+{
+ if (doAccept(visitor))
+ {
+ for (size_t i = 0; i < getNumChildren(); ++i)
+ {
+ getChild(i)->accept(visitor);
+ }
+ }
+}
+
+}
+}
+
diff --git a/SurgSim/DataStructures/TreeNode.h b/SurgSim/DataStructures/TreeNode.h
new file mode 100644
index 0000000..4577104
--- /dev/null
+++ b/SurgSim/DataStructures/TreeNode.h
@@ -0,0 +1,124 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TREENODE_H
+#define SURGSIM_DATASTRUCTURES_TREENODE_H
+
+#include <vector>
+#include <memory>
+
+#include "SurgSim/DataStructures/TreeVisitor.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+class TreeData;
+
+
+/// Basic tree node structure.
+/// The nodes build up the structure of a Tree.
+/// \sa Tree TreeData
+class TreeNode
+{
+public:
+ /// Constructor. After construction, the node has no children, and the data is null.
+ TreeNode();
+
+ /// Destructor
+ virtual ~TreeNode();
+
+ /// Sets the data of this node.
+ /// \param data The data for this node.
+ void setData(std::shared_ptr<TreeData> data);
+
+ /// \return The data of this node.
+ std::shared_ptr<TreeData> getData() const;
+
+ /// \return The number of children of this node.
+ size_t getNumChildren() const;
+
+ /// Returns the specified child of this node.
+ /// \param index Index of the child
+ /// \return Child at the specified index
+ std::shared_ptr<TreeNode> getChild(size_t index) const;
+
+ /// Public entry point for visitor, currently this performs pre-order traversal of the tree
+ /// \param visitor The visitor that wants to traverse the tree
+ void accept(TreeVisitor* visitor);
+
+ /// Returns true if the nodes are equal; otherwise, returns false.
+ /// If the nodes are not of the same type, returns false;
+ /// otherwise, compares with the implementation of isEqual(const TreeNode&).
+ /// \param node The node for comparison.
+ /// \return true is this node and the one from the parameter are equal.
+ bool operator==(const TreeNode& node) const;
+
+ /// Returns true if the nodes are not equal; otherwise, returns false.
+ /// If the nodes are not of the same type, returns false;
+ /// otherwise, compares with the implementation of isEqual(const TreeNode&).
+ /// \param node The node for comparison.
+ /// \return true if this node and the parameter are not equal
+ bool operator!=(const TreeNode& node) const;
+
+protected:
+
+ /// Returns true if the nodes are equal; otherwise, returns false.
+ /// Recurses on children.
+ /// Override this method in derived classes to implement different comparisons.
+ /// \param node The node for comparison.
+ /// \return true if this node is equal to the node in the parameter.
+ virtual bool isEqual(const TreeNode& node) const;
+
+ /// Private function for use with the visitor pattern, this needs to be implemented
+ /// to make the correct double dispatch call to the dynamic type of this class.
+ /// \param visitor The visitor that is trying to traverse the tree.
+ /// \return true to indicate proceeding with the visitor, false indicates to abort the traversal.
+ virtual bool doAccept(TreeVisitor* visitor) = 0;
+
+ /// Sets the number of children of this node.
+ /// Any added children will be null.
+ /// \param numChildren The new number of children.
+ void setNumChildren(size_t numChildren);
+
+ /// Add a child to this node.
+ /// \param node The new child node.
+ void addChild(const std::shared_ptr<TreeNode>& node);
+
+ /// Add a child to this node.
+ /// \param node The new child node.
+ void addChild(const std::shared_ptr<TreeNode>&& node);
+
+ /// Set a specific child of this node.
+ /// \param index Index of the child
+ /// \param node Node to become a child
+ void setChild(size_t index, const std::shared_ptr<TreeNode>& node);
+
+private:
+
+ /// Children of this node.
+ std::vector<std::shared_ptr<TreeNode>> m_children;
+
+ /// Data of this node.
+ std::shared_ptr<TreeData> m_data;
+};
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_TREENODE_H
diff --git a/SurgSim/DataStructures/TreeVisitor.h b/SurgSim/DataStructures/TreeVisitor.h
new file mode 100644
index 0000000..c197998
--- /dev/null
+++ b/SurgSim/DataStructures/TreeVisitor.h
@@ -0,0 +1,59 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TREEVISITOR_H
+#define SURGSIM_DATASTRUCTURES_TREEVISITOR_H
+
+
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+class TreeNode;
+class AabbTreeNode;
+
+/// Abstract Class for visitors, this needs to be extended for other tree nodes when necessary
+/// return false from handle() to abort traversal.
+class TreeVisitor
+{
+public:
+
+ /// Destructor
+ virtual ~TreeVisitor()
+ {
+
+ }
+
+ /// Handle TreeNode basic type.
+ /// \param node Node to process.
+ /// \return true To indicates that the visitor wishes to continue traversal, false if the visitor wants
+ /// to abort traversal.
+ virtual bool handle(TreeNode* node) = 0;
+
+ /// Handle AabbTreeNode basic type, default body, override for specific work.
+ /// \param node Node to process.
+ /// \return true to continue traversal, false to abort.
+ virtual bool handle(AabbTreeNode* node)
+ {
+ return false;
+ };
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/DataStructures/TriangleMesh-inl.h b/SurgSim/DataStructures/TriangleMesh-inl.h
new file mode 100644
index 0000000..4f5216a
--- /dev/null
+++ b/SurgSim/DataStructures/TriangleMesh-inl.h
@@ -0,0 +1,35 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+
+#ifndef SURGSIM_DATASTRUCTURES_TRIANGLEMESH_INL_H
+#define SURGSIM_DATASTRUCTURES_TRIANGLEMESH_INL_H
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <class VertexDataSource, class EdgeDataSource, class TriangleDataSource>
+TriangleMesh::TriangleMesh(const TriangleMeshBase<VertexDataSource, EdgeDataSource, TriangleDataSource>& mesh) :
+ TriangleMeshBase<EmptyData, EmptyData, NormalData>(mesh)
+{
+ calculateNormals();
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_TRIANGLEMESH_INL_H
\ No newline at end of file
diff --git a/SurgSim/DataStructures/TriangleMesh.cpp b/SurgSim/DataStructures/TriangleMesh.cpp
new file mode 100644
index 0000000..7e975b9
--- /dev/null
+++ b/SurgSim/DataStructures/TriangleMesh.cpp
@@ -0,0 +1,102 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+TriangleMesh::TriangleMesh()
+{
+}
+
+const SurgSim::Math::Vector3d& TriangleMesh::getNormal(size_t triangleId)
+{
+ return getTriangle(triangleId).data.normal;
+}
+
+void TriangleMesh::calculateNormals()
+{
+ for (size_t i = 0; i < getNumTriangles(); ++i)
+ {
+ const SurgSim::Math::Vector3d& vertex0 = getVertexPosition(getTriangle(i).verticesId[0]);
+ const SurgSim::Math::Vector3d& vertex1 = getVertexPosition(getTriangle(i).verticesId[1]);
+ const SurgSim::Math::Vector3d& vertex2 = getVertexPosition(getTriangle(i).verticesId[2]);
+
+ // Calculate normal vector
+ SurgSim::Math::Vector3d normal = (vertex1 - vertex0).cross(vertex2 - vertex0);
+ normal.normalize();
+
+ getTriangle(i).data.normal = normal;
+ }
+}
+
+void TriangleMesh::doUpdate()
+{
+ calculateNormals();
+}
+
+bool TriangleMesh::doLoad(const std::string& fileName)
+{
+ auto triangleMeshDelegate = std::make_shared<TriangleMeshPlyReaderDelegate<TriangleMesh>>(shared_from_this());
+
+ PlyReader reader(fileName);
+ SURGSIM_ASSERT(reader.isValid()) << "'" << fileName << "' is an invalid .ply file.";
+ SURGSIM_ASSERT(reader.parseWithDelegate(triangleMeshDelegate)) <<
+ "The input file " << fileName << " does not have the property required by triangle mesh.";
+
+ calculateNormals();
+
+ return true;
+}
+
+void TriangleMesh::copyWithTransform(const SurgSim::Math::RigidTransform3d& pose, const TriangleMesh& source)
+{
+ SURGSIM_ASSERT(getNumVertices() == source.getNumVertices())
+ << "The source mesh must have the same number of vertices.";
+ SURGSIM_ASSERT(getNumEdges() == source.getNumEdges())
+ << "The source mesh must have the same number of edges";
+ SURGSIM_ASSERT(getNumTriangles() == source.getNumTriangles())
+ << "The source mesh must have the same number of triangles";
+
+ auto targetVertex = getVertices().begin();
+ auto const& vertices = source.getVertices();
+ for (auto it = vertices.cbegin(); it != vertices.cend(); ++it)
+ {
+ targetVertex->position = pose * it->position;
+ ++targetVertex;
+ }
+
+ auto targetTriangle = getTriangles().begin();
+ auto const& triangles = source.getTriangles();
+ for (auto it = triangles.cbegin(); it != triangles.cend(); ++it)
+ {
+ targetTriangle->isValid = it->isValid;
+ if (it->isValid)
+ {
+ targetTriangle->data.normal = pose.linear() * it->data.normal;
+ }
+ ++targetTriangle;
+ }
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/TriangleMesh.h b/SurgSim/DataStructures/TriangleMesh.h
new file mode 100644
index 0000000..b72221b
--- /dev/null
+++ b/SurgSim/DataStructures/TriangleMesh.h
@@ -0,0 +1,107 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TRIANGLEMESH_H
+#define SURGSIM_DATASTRUCTURES_TRIANGLEMESH_H
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class MeshShape;
+}
+
+namespace DataStructures
+{
+
+/// Store normal for each triangle in a triangle mesh.
+struct NormalData
+{
+
+ SurgSim::Math::Vector3d normal;
+
+ /// Equality operator.
+ /// \param rhs The right hand side NormalData.
+ /// \return true if the parameters are considered equivalent.
+ bool operator==(const SurgSim::DataStructures::NormalData& rhs) const
+ {
+ return normal == rhs.normal;
+ }
+
+ /// Inequality operator.
+ /// \param rhs The right hand side NormalData.
+ /// \return true if the parameters are not considered equivalent.
+ bool operator!=(const SurgSim::DataStructures::NormalData& rhs) const
+ {
+ return !((*this) == rhs);
+ }
+};
+
+typedef TriangleMeshBase<EmptyData, EmptyData, EmptyData> TriangleMeshPlain;
+
+/// A TriangleMesh stores normal information for the triangles.
+class TriangleMesh: public std::enable_shared_from_this<TriangleMesh>,
+ public SurgSim::Framework::Asset,
+ public SurgSim::DataStructures::TriangleMeshBase<EmptyData, EmptyData, NormalData>
+{
+friend class SurgSim::Math::MeshShape;
+public:
+
+ /// Constructor
+ TriangleMesh();
+
+ /// Templated constructor this lets us convert one mesh class into another mesh class
+ /// \tparam VertexDataSource Type of extra data stored in each vertex
+ /// \tparam EdgeDataSource Type of extra data stored in each edge
+ /// \tparam TriangleDataSource Type of extra data stored in each triangle
+ /// \param mesh The mesh to be copied from. Vertex, edge and triangle data will be emptied.
+ /// \sa TriangleMeshBase
+ template <class VertexDataSource, class EdgeDataSource, class TriangleDataSource>
+ explicit TriangleMesh(const TriangleMeshBase<VertexDataSource, EdgeDataSource, TriangleDataSource>& mesh);
+
+ /// Get normal for triangle.
+ /// \param triangleId The triangle to get normal.
+ /// \return The normal for the triangle with given ID.
+ const SurgSim::Math::Vector3d& getNormal(size_t triangleId);
+
+ /// Calculate normals for all triangles.
+ /// \note Normals will be normalized.
+ void calculateNormals();
+
+ /// Sets the mesh's vertices and normals by transforming a similar mesh. The two meshes must have the same number
+ /// of vertices, edges, and triangles.
+ /// \param pose the transformation to be applied to the vertices and norms
+ /// \param source the mesh suppling the vertices and norms
+ void copyWithTransform(const SurgSim::Math::RigidTransform3d& pose, const TriangleMesh& source);
+
+protected:
+ virtual void doUpdate() override;
+
+ virtual bool doLoad(const std::string& fileName) override;
+};
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#include "SurgSim/DataStructures/TriangleMesh-inl.h"
+
+#endif // SURGSIM_DATASTRUCTURES_TRIANGLEMESH_H
diff --git a/SurgSim/DataStructures/TriangleMeshBase-inl.h b/SurgSim/DataStructures/TriangleMeshBase-inl.h
new file mode 100644
index 0000000..75cd7cb
--- /dev/null
+++ b/SurgSim/DataStructures/TriangleMeshBase-inl.h
@@ -0,0 +1,263 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TRIANGLEMESHBASE_INL_H
+#define SURGSIM_DATASTRUCTURES_TRIANGLEMESHBASE_INL_H
+
+#include "SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <class VertexData, class EdgeData, class TriangleData>
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::TriangleMeshBase()
+{
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+template <class VertexDataSource, class EdgeDataSource, class TriangleDataSource>
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::TriangleMeshBase(
+ const TriangleMeshBase<VertexDataSource, EdgeDataSource, TriangleDataSource>& mesh)
+{
+ for (size_t iVertex = 0; iVertex < mesh.getNumVertices(); ++iVertex)
+ {
+ VertexType vertexData(mesh.getVertexPosition(iVertex));
+ addVertex(vertexData);
+ }
+ for (size_t iEdge = 0; iEdge < mesh.getNumEdges(); ++iEdge)
+ {
+ EdgeType edgeData((mesh.getEdge(iEdge)).verticesId, EdgeData());
+ addEdge(edgeData);
+ }
+
+ auto& sourceTriangles = mesh.getTriangles();
+ size_t index = 0;
+ m_triangles.reserve(sourceTriangles.size());
+ for (auto sourceTriangle : sourceTriangles)
+ {
+ TriangleType triangleData(sourceTriangle.verticesId, TriangleData());
+ addTriangle(triangleData);
+ if (!sourceTriangle.isValid)
+ {
+ m_freeTriangles.push_back(index);
+ }
+ ++index;
+ }
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::~TriangleMeshBase()
+{
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+size_t TriangleMeshBase<VertexData, EdgeData, TriangleData>::addEdge(const EdgeType& edge)
+{
+ m_edges.push_back(edge);
+ return m_edges.size() - 1;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+size_t TriangleMeshBase<VertexData, EdgeData, TriangleData>::addTriangle(const TriangleType& triangle)
+{
+ size_t result;
+
+ SURGSIM_ASSERT(triangle.isValid) << "Cannot insert invalid triangle into mesh.";
+
+ if (m_freeTriangles.empty())
+ {
+ m_triangles.push_back(triangle);
+ result = m_triangles.size() - 1;
+ }
+ else
+ {
+ result = m_freeTriangles.back();
+ m_freeTriangles.pop_back();
+ m_triangles[result] = triangle;
+ }
+
+ return result;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+size_t TriangleMeshBase<VertexData, EdgeData, TriangleData>::getNumEdges() const
+{
+ return m_edges.size();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+size_t TriangleMeshBase<VertexData, EdgeData, TriangleData>::getNumTriangles() const
+{
+ return m_triangles.size() - m_freeTriangles.size();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+const std::vector<typename TriangleMeshBase<VertexData, EdgeData, TriangleData>::EdgeType>&
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::getEdges() const
+{
+ return m_edges;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+std::vector<typename TriangleMeshBase<VertexData, EdgeData, TriangleData>::EdgeType>&
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::getEdges()
+{
+ return m_edges;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+const std::vector<typename TriangleMeshBase<VertexData, EdgeData, TriangleData>::TriangleType>&
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::getTriangles() const
+{
+ return m_triangles;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+std::vector<typename TriangleMeshBase<VertexData, EdgeData, TriangleData>::TriangleType>&
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::getTriangles()
+{
+ return m_triangles;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+const typename TriangleMeshBase<VertexData, EdgeData, TriangleData>::EdgeType&
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::getEdge(size_t id) const
+{
+ return m_edges[id];
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+typename TriangleMeshBase<VertexData, EdgeData, TriangleData>::EdgeType&
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::getEdge(size_t id)
+{
+ return m_edges[id];
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+const typename TriangleMeshBase<VertexData, EdgeData, TriangleData>::TriangleType&
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::getTriangle(size_t id) const
+{
+ auto const& triangle = m_triangles[id];
+ SURGSIM_ASSERT(triangle.isValid == true) << "Attempted to access invalid or deleted triangle.";
+ return triangle;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+typename TriangleMeshBase<VertexData, EdgeData, TriangleData>::TriangleType&
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::getTriangle(size_t id)
+{
+ auto& triangle = m_triangles[id];
+ SURGSIM_ASSERT(triangle.isValid == true) << "Attempted to access invalid or deleted triangle.";
+ return triangle;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+void TriangleMeshBase<VertexData, EdgeData, TriangleData>::removeTriangle(size_t id)
+{
+ auto& triangle = m_triangles[id];
+ SURGSIM_ASSERT(triangle.isValid) << "This triangle has already been removed.";
+ triangle.isValid = false;
+ m_freeTriangles.push_back(id);
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+std::array<SurgSim::Math::Vector3d, 3>
+TriangleMeshBase<VertexData, EdgeData, TriangleData>::getTrianglePositions(size_t id) const
+{
+ auto& ids = getTriangle(id).verticesId;
+ std::array<SurgSim::Math::Vector3d, 3> result
+ = {{
+ Vertices<VertexData>::getVertex(ids[0]).position,
+ Vertices<VertexData>::getVertex(ids[1]).position,
+ Vertices<VertexData>::getVertex(ids[2]).position
+ }
+ };
+
+ return result;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+bool TriangleMeshBase<VertexData, EdgeData, TriangleData>::isValid() const
+{
+ typedef typename TriangleMeshBase<VertexData, EdgeData, TriangleData>::EdgeType EdgeType;
+ typedef typename TriangleMeshBase<VertexData, EdgeData, TriangleData>::TriangleType TriangleType;
+
+ size_t numVertices = Vertices<VertexData>::getNumVertices();
+
+ // Test edges validity
+ for (typename std::vector<EdgeType>::const_iterator it = m_edges.begin(); it != m_edges.end(); ++it)
+ {
+ for (int vertexId = 0; vertexId < 2; vertexId++)
+ {
+ if (it->verticesId[vertexId] >= numVertices)
+ {
+ return false;
+ }
+ }
+ }
+
+ // Test triangles validity
+ for (typename std::vector<TriangleType>::const_iterator it = m_triangles.begin(); it != m_triangles.end(); ++it)
+ {
+ for (int vertexId = 0; vertexId < 3; vertexId++)
+ {
+ if (it->verticesId[vertexId] >= numVertices)
+ {
+ return false;
+ }
+ }
+ }
+
+ return true;
+}
+
+
+template <class VertexData, class EdgeData, class TriangleData>
+void TriangleMeshBase<VertexData, EdgeData, TriangleData>::doClearEdges()
+{
+ m_edges.clear();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+void TriangleMeshBase<VertexData, EdgeData, TriangleData>::doClearTriangles()
+{
+ m_triangles.clear();
+ m_freeTriangles.clear();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+bool TriangleMeshBase<VertexData, EdgeData, TriangleData>::isEqual(const Vertices<VertexData>& mesh) const
+{
+ const TriangleMeshBase& triangleMesh = static_cast<const TriangleMeshBase&>(mesh);
+ return Vertices<VertexData>::isEqual(triangleMesh) && m_edges == triangleMesh.getEdges() &&
+ m_triangles == triangleMesh.getTriangles();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+void TriangleMeshBase<VertexData, EdgeData, TriangleData>::doClear()
+{
+ doClearTriangles();
+ doClearEdges();
+ this->doClearVertices();
+}
+
+
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_TRIANGLEMESHBASE_INL_H
diff --git a/SurgSim/DataStructures/TriangleMeshBase.h b/SurgSim/DataStructures/TriangleMeshBase.h
new file mode 100644
index 0000000..5e8cf3a
--- /dev/null
+++ b/SurgSim/DataStructures/TriangleMeshBase.h
@@ -0,0 +1,211 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TRIANGLEMESHBASE_H
+#define SURGSIM_DATASTRUCTURES_TRIANGLEMESHBASE_H
+
+#include <array>
+
+#include "SurgSim/DataStructures/MeshElement.h"
+#include "SurgSim/DataStructures/Vertices.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class EmptyData;
+
+/// Basic class for storing Triangle Meshes, handling basic vertex, edge, and triangle functionality.
+///
+/// TriangleMeshBase is to be used purely as a data structure and not provide implementation of algorithms.
+/// For example, a physics 2D FEM is not a subclass of TriangleMeshBase, but may use a TriangleMeshBase for storing the
+/// structure of the FEM.
+///
+/// It is recommended that subclasses with a specific purpose (such as for use in collision detection) provide
+/// convenience methods for creation of vertices, edges, and triangles and the data each contains. Methods such as
+/// createVertex(position, other data...), createEdge(vertices, other data...),
+/// and createTriangle(vertices, other data...) simplify the creation of vertices and elements and the data required.
+/// These methods would use the addVertex(), addEdge(), and addTriangle() methods to add the created vertices and
+/// elements to the TriangleMeshBase.
+///
+/// Overriding isEqual(const Mesh&) is necessary to do more than just basic list comparison of the
+/// vertices, edges, and triangles, which is dependent on order in the list.
+///
+/// Override doUpdate() to provide update functionality when vertices are changes, such as recalculating surface
+/// normals.
+///
+/// A subclass that is designed for a specific use (such as collision detection) may also specify the VertexData,
+/// EdgeData, and TriangleData to what is required.
+///
+/// \tparam VertexData Type of extra data stored in each vertex
+/// \tparam EdgeData Type of extra data stored in each edge
+/// \tparam TriangleData Type of extra data stored in each triangle
+/// \sa Vertex
+/// \sa MeshElement
+template <class VertexData, class EdgeData, class TriangleData>
+class TriangleMeshBase : public Vertices<VertexData>
+{
+public:
+ /// Edge type for convenience (Ids of the 2 vertices)
+ typedef MeshElement<2, EdgeData> EdgeType;
+ /// Triangle type for convenience (Ids of the 3 vertices)
+ typedef MeshElement<3, TriangleData> TriangleType;
+
+ /// Constructor. The mesh is initially empty (no vertices, no edges, no triangles).
+ TriangleMeshBase();
+
+ /// Copy constructor.
+ /// \tparam VertexDataSource Type of extra data stored in each vertex
+ /// \tparam EdgeDataSource Type of extra data stored in each edge
+ /// \tparam TriangleDataSource Type of extra data stored in each triangle
+ /// \param mesh The mesh to be copied from. Vertex, edge and triangle data will be emptied.
+ /// \note: Data of the input mesh, i.e. VertexDataSource, EdgeDataSource and TrianleDataSource will not be copied.
+ template <class VertexDataSource, class EdgeDataSource, class TriangleDataSource>
+ explicit TriangleMeshBase(const TriangleMeshBase<VertexDataSource, EdgeDataSource, TriangleDataSource>& mesh);
+
+ /// Destructor
+ virtual ~TriangleMeshBase();
+
+ /// Adds an edge to the mesh.
+ /// No checking on the edge's vertices is performed.
+ /// Recommend that subclasses with a specific purpose (such as for use in collision detection) have a
+ /// createEdge(vertices, other data...) method which performs any checking desired and sets up the edge data based
+ /// on the vertices and other parameters.
+ /// \param edge Edge to add to the mesh
+ /// \return Unique ID of the new edge.
+ size_t addEdge(const EdgeType& edge);
+
+ /// Adds a triangle to the mesh.
+ /// \param triangle Triangle to add to the mesh
+ /// Recommend that subclasses with a specific purpose (such as for use in collision detection) have a
+ /// createTriangle(vertices, other data...) method which performs any checking desired and sets up the triangle data
+ /// based on the vertices and other parameters.
+ /// \note The ids of deleted triangles will be reused in no particular order
+ /// \return id of the new triangle.
+ size_t addTriangle(const TriangleType& triangle);
+
+ /// Get the number of edges
+ /// \return the number of edges in this mesh.
+ size_t getNumEdges() const;
+
+ /// Get the number of triangles
+ /// \note The number of triangles might not match the size of the array returned by getTriangles(), after deletion
+ /// has occurred it cannot be used to access all triangles.
+ /// \return the number of triangles in this mesh.
+ size_t getNumTriangles() const;
+
+ /// Retrieve all edges
+ /// \return a vector containing the position of each edge.
+ const std::vector<EdgeType>& getEdges() const;
+
+ /// Retrieve all edges (non const version)
+ /// \return a vector containing the position of each edge.
+ std::vector<EdgeType>& getEdges();
+
+ /// Retrieve all triangles
+ /// \note The number of triangles might not match the size of the array returned by getTriangles(), after deletion
+ /// has occurred it cannot be used to access all triangles. When processing this array,
+ /// check \sa TriangleType::isValid to see wether to do something with this triangle
+ /// \return a vector containing the position of each triangle. Some of these triangles might be deleted, they need
+ /// to be checked via \sa isValid before further processing
+ const std::vector<TriangleType>& getTriangles() const;
+
+ /// Retrieve all triangles (non const version)
+ /// \note The number of triangles might not match the size of the array returned by getTriangles(), after deletion
+ /// has occurred it cannot be used to access all triangles. When processing this array,
+ /// check \sa TriangleType::isValid to see wether to do something with this triangle
+ /// \return a vector containing the position of each triangle. Some of these triangles might be deleted, they need
+ /// to be checked via \sa isValid before further processing
+ std::vector<TriangleType>& getTriangles();
+
+ /// Retrieve a specific edge
+ /// \param id the edge to be retrieved.
+ /// \return the specified edge.
+ const EdgeType& getEdge(size_t id) const;
+
+ /// Retrieve a specific edge (non const version)
+ /// \param id the edge to be retrieved.
+ /// \return the specified edge.
+ EdgeType& getEdge(size_t id);
+
+ /// Retrieve a specific triangle
+ /// \throws SurgSim::Framework::AssertionFailure if the given triangle was deleted
+ /// \param id The id of the triangle to retrieve
+ /// \return the specified triangle
+ const TriangleType& getTriangle(size_t id) const;
+
+ /// Retrieve a specific triangle (non const version)
+ /// \throws SurgSim::Framework::AssertionFailure if the give triangle was deleted
+ /// \param id The id of the triangle to retrieve
+ /// \return the specified triangle
+ TriangleType& getTriangle(size_t id);
+
+ /// Marks a triangle as invalid, the triangle cannot be accessed via getTriangle anymore
+ /// \note users of getTriangles() will have to check for deleted triangles if this feature is used
+ /// the size of the vector returned by getTriangles does not reflect the number of triangles anymore
+ /// use getNumTriangles() to figure out the correct number.
+ /// \param id triangle to delete
+ void removeTriangle(size_t id);
+
+ /// Returns an array of the triangle's vertices' positions
+ /// \param id the id of the triangle
+ /// \return an array of the triangle's vertices' positions
+ std::array<SurgSim::Math::Vector3d, 3> getTrianglePositions(size_t id) const;
+
+ /// Test if the TriangleMeshBase is valid (valid vertex Ids used in all MeshElements)
+ /// \return True if the TriangleMeshBase is valid, False otherwise (the topology is then broken)
+ bool isValid() const;
+
+protected:
+ /// Remove all edges from the mesh.
+ virtual void doClearEdges();
+
+ /// Remove all triangles from the mesh.
+ virtual void doClearTriangles();
+
+ /// Internal comparison of meshes of the same type: returns true if equal, false if not equal.
+ /// Override this method to provide custom comparison. Basic TriangleMeshBase implementation compares vertices,
+ /// edges and triangles: the order of vertices, edges, and triangles must also match to be considered equal.
+ /// \param mesh Mesh must be of the same type as that which it is compared against
+ /// \return True if the vertices are equals, False otherwise
+ virtual bool isEqual(const Vertices<VertexData>& mesh) const;
+private:
+
+ /// Clear mesh to return to an empty state (no vertices, no edges, no triangles).
+ virtual void doClear();
+
+ /// Edges
+ std::vector<EdgeType> m_edges;
+
+ /// Triangles
+ std::vector<TriangleType> m_triangles;
+
+ /// List of indices of deleted triangles, to be reused when another triangle is added
+ std::vector<size_t> m_freeTriangles;
+
+public:
+ // Dependent name resolution for inherited functions and typenames from templates
+ using typename Vertices<VertexData>::VertexType;
+ using Vertices<VertexData>::addVertex;
+};
+
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
+
+#include "SurgSim/DataStructures/TriangleMeshBase-inl.h"
+
+#endif // SURGSIM_DATASTRUCTURES_TRIANGLEMESHBASE_H
diff --git a/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate-inl.h b/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate-inl.h
new file mode 100644
index 0000000..9977115
--- /dev/null
+++ b/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate-inl.h
@@ -0,0 +1,159 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TRIANGLEMESHPLYREADERDELEGATE_INL_H
+#define SURGSIM_DATASTRUCTURES_TRIANGLEMESHPLYREADERDELEGATE_INL_H
+
+template <class M>
+SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::TriangleMeshPlyReaderDelegate() :
+ m_mesh(std::make_shared<M>())
+{
+
+}
+
+template <class M>
+SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::TriangleMeshPlyReaderDelegate(std::shared_ptr<M> mesh) :
+ m_mesh(mesh)
+{
+ SURGSIM_ASSERT(mesh != nullptr) << "The mesh cannot be null.";
+ mesh->clear();
+}
+
+template <class M>
+std::shared_ptr<M> SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::getMesh()
+{
+ return m_mesh;
+}
+
+template <class M>
+bool SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::registerDelegate(PlyReader* reader)
+{
+ // Vertex processing
+ reader->requestElement("vertex",
+ std::bind(&TriangleMeshPlyReaderDelegate::beginVertices, this,
+ std::placeholders::_1, std::placeholders::_2),
+ std::bind(&TriangleMeshPlyReaderDelegate::processVertex, this, std::placeholders::_1),
+ std::bind(&TriangleMeshPlyReaderDelegate::endVertices, this, std::placeholders::_1));
+ reader->requestScalarProperty("vertex", "x", PlyReader::TYPE_DOUBLE, offsetof(VertexData, x));
+ reader->requestScalarProperty("vertex", "y", PlyReader::TYPE_DOUBLE, offsetof(VertexData, y));
+ reader->requestScalarProperty("vertex", "z", PlyReader::TYPE_DOUBLE, offsetof(VertexData, z));
+
+ // Normal processing
+ m_hasTextureCoordinates = reader->hasProperty("vertex", "s") && reader->hasProperty("vertex", "t");
+
+ if (m_hasTextureCoordinates)
+ {
+ reader->requestScalarProperty("vertex", "s", PlyReader::TYPE_DOUBLE, offsetof(VertexData, s));
+ reader->requestScalarProperty("vertex", "t", PlyReader::TYPE_DOUBLE, offsetof(VertexData, t));
+ }
+
+ // Face Processing
+ reader->requestElement("face",
+ std::bind(&TriangleMeshPlyReaderDelegate::beginFaces, this,
+ std::placeholders::_1, std::placeholders::_2),
+ std::bind(&TriangleMeshPlyReaderDelegate::processFace, this, std::placeholders::_1),
+ std::bind(&TriangleMeshPlyReaderDelegate::endFaces, this, std::placeholders::_1));
+ reader->requestListProperty("face", "vertex_indices",
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(FaceData, indices),
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(FaceData, edgeCount));
+
+ reader->setEndParseFileCallback(std::bind(&TriangleMeshPlyReaderDelegate::endFile, this));
+
+ return true;
+}
+
+template <class M>
+bool SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::fileIsAcceptable(const PlyReader& reader)
+{
+ bool result = true;
+
+ // Shortcut test if one fails ...
+ result = result && reader.hasProperty("vertex", "x");
+ result = result && reader.hasProperty("vertex", "y");
+ result = result && reader.hasProperty("vertex", "z");
+ result = result && reader.hasProperty("face", "vertex_indices");
+ result = result && !reader.isScalar("face", "vertex_indices");
+
+ return result;
+}
+
+template <class M>
+void* SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::beginVertices(
+ const std::string& elementName,
+ size_t vertexCount)
+{
+ m_vertexData.overrun1 = 0l;
+ m_vertexData.overrun2 = 0l;
+ return &m_vertexData;
+}
+
+template <class M>
+void SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::processVertex(const std::string& elementName)
+{
+ typename M::VertexType vertex(SurgSim::Math::Vector3d(m_vertexData.x, m_vertexData.y, m_vertexData.z));
+ m_mesh->addVertex(vertex);
+}
+
+template <class M>
+void SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::endVertices(const std::string& elementName)
+{
+ SURGSIM_ASSERT(m_vertexData.overrun1 == 0l && m_vertexData.overrun2 == 0l) <<
+ "There was an overrun while reading the vertex structures, it is likely that data " <<
+ "has become corrupted.";
+}
+
+template <class M>
+void* SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::beginFaces(
+ const std::string& elementName,
+ size_t faceCount)
+{
+ m_faceData.overrun = 0l;
+ return &m_faceData;
+}
+
+template <class M>
+void SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::processFace(const std::string& elementName)
+{
+ SURGSIM_ASSERT(m_faceData.edgeCount == 3) << "Can only process triangle meshes.";
+ std::copy(m_faceData.indices, m_faceData.indices + 3, m_indices.begin());
+
+ typename M::TriangleType triangle(m_indices);
+ m_mesh->addTriangle(triangle);
+}
+
+template <class M>
+void SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::endFaces(const std::string& elementName)
+{
+ SURGSIM_ASSERT(m_faceData.overrun == 0l)
+ << "There was an overrun while reading the face structures, it is likely that data "
+ << "has become corrupted.";
+}
+
+template <class M>
+void SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::endFile()
+{
+ m_mesh->update();
+}
+
+template <class M>
+bool SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::hasTextureCoordinates()
+{
+ return m_hasTextureCoordinates;
+}
+
+
+#endif
diff --git a/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h b/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h
new file mode 100644
index 0000000..02b1fc5
--- /dev/null
+++ b/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h
@@ -0,0 +1,135 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TRIANGLEMESHPLYREADERDELEGATE_H
+#define SURGSIM_DATASTRUCTURES_TRIANGLEMESHPLYREADERDELEGATE_H
+
+#include <array>
+#include <memory>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/PlyReaderDelegate.h"
+#include "SurgSim/DataStructures/TriangleMeshBase.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// Implementation of PlyReaderDelegate for simple triangle meshes
+template <class M>
+class TriangleMeshPlyReaderDelegate : public PlyReaderDelegate
+{
+public:
+
+ typedef M MeshType;
+
+ /// Default constructor.
+ TriangleMeshPlyReaderDelegate();
+
+ /// Constructor.
+ /// \param mesh The mesh to be used, it will be cleared by the constructor.
+ explicit TriangleMeshPlyReaderDelegate(std::shared_ptr<MeshType> mesh);
+
+ /// Gets the mesh.
+ /// \return The mesh.
+ std::shared_ptr<MeshType> getMesh();
+
+ /// Registers the delegate with the reader, overridden from \sa PlyReaderDelegate.
+ /// \param reader The reader that should be used.
+ /// \return true if it succeeds, false otherwise.
+ virtual bool registerDelegate(PlyReader* reader) override;
+
+ /// Check whether this file is acceptable to the delegate, overridden from \sa PlyReaderDelegate.
+ /// \param reader The reader that should be used.
+ /// \return true if it succeeds, false otherwise.
+ virtual bool fileIsAcceptable(const PlyReader& reader) override;
+
+ /// Callback function, begin the processing of vertices.
+ /// \param elementName Name of the element.
+ /// \param vertexCount Number of vertices.
+ /// \return memory for vertex data to the reader.
+ void* beginVertices(const std::string& elementName, size_t vertexCount);
+
+ /// Callback function to process one vertex.
+ /// \param elementName Name of the element.
+ virtual void processVertex(const std::string& elementName);
+
+ /// Callback function to finalize processing of vertices.
+ /// \param elementName Name of the element.
+ void endVertices(const std::string& elementName);
+
+ /// Callback function, begin the processing of faces.
+ /// \param elementName Name of the element.
+ /// \param faceCount Number of faces.
+ /// \return memory for face data to the reader.
+ void* beginFaces(const std::string& elementName, size_t faceCount);
+
+ /// Callback function to process one face.
+ /// \param elementName Name of the element.
+ void processFace(const std::string& elementName);
+
+ /// Callback function to finalize processing of faces.
+ /// \param elementName Name of the element.
+ void endFaces(const std::string& elementName);
+
+ /// Callback function to finalize processing of the mesh
+ void endFile();
+
+protected:
+
+ /// \return true if s/t coordinates where found in the ply file on registration.
+ bool hasTextureCoordinates();
+
+ /// Internal structure, the receiver for data from the "vertex" element
+ /// Provide space for standard ply vertex data, x/y/z and s/t
+ struct VertexData
+ {
+ double x;
+ double y;
+ double z;
+ int64_t overrun1; ///< Used to check for buffer overruns
+ double s;
+ double t;
+ int64_t overrun2; ///< Used to check for buffer overruns
+ } m_vertexData;
+
+ /// Internal structure, the received for data from the "face" element
+ struct FaceData
+ {
+ unsigned int edgeCount;
+ unsigned int* indices;
+ int64_t overrun; ///< Used to check for buffer overruns
+ } m_faceData;
+
+ /// The mesh that will be created
+ std::shared_ptr<MeshType> m_mesh;
+
+ // Statically allocated index array to receive data for the faces
+ std::array<size_t, 3> m_indices;
+
+private:
+ /// Set to true if s/t coordinates are found in the .ply file
+ bool m_hasTextureCoordinates;
+
+};
+
+}
+}
+
+#include "SurgSim/DataStructures/TriangleMeshPlyReaderDelegate-inl.h"
+
+#endif
diff --git a/SurgSim/DataStructures/TriangleMeshUtilities-inl.h b/SurgSim/DataStructures/TriangleMeshUtilities-inl.h
new file mode 100644
index 0000000..d4a2a1f
--- /dev/null
+++ b/SurgSim/DataStructures/TriangleMeshUtilities-inl.h
@@ -0,0 +1,35 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TRIANGLEMESHUTILITIES_INL_H
+#define SURGSIM_DATASTRUCTURES_TRIANGLEMESHUTILITIES_INL_H
+
+template <class M>
+std::shared_ptr<M> SurgSim::DataStructures::loadTriangleMesh(const std::string& fileName)
+{
+ auto triangleMeshDelegate = std::make_shared<SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>>();
+
+ PlyReader reader(fileName);
+ if (reader.isValid())
+ {
+ SURGSIM_ASSERT(reader.parseWithDelegate(triangleMeshDelegate)) <<
+ "The input file " << fileName << " does not have the property required by triangle mesh.";
+ }
+
+ return triangleMeshDelegate->getMesh();
+}
+
+
+#endif
diff --git a/SurgSim/DataStructures/TriangleMeshUtilities.cpp b/SurgSim/DataStructures/TriangleMeshUtilities.cpp
new file mode 100644
index 0000000..ac1fa3b
--- /dev/null
+++ b/SurgSim/DataStructures/TriangleMeshUtilities.cpp
@@ -0,0 +1,30 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+std::shared_ptr<TriangleMeshPlain> loadTriangleMesh(const std::string& filename)
+{
+ return loadTriangleMesh<TriangleMeshPlain>(filename);
+}
+
+}
+}
+
diff --git a/SurgSim/DataStructures/TriangleMeshUtilities.h b/SurgSim/DataStructures/TriangleMeshUtilities.h
new file mode 100644
index 0000000..830b55f
--- /dev/null
+++ b/SurgSim/DataStructures/TriangleMeshUtilities.h
@@ -0,0 +1,43 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_TRIANGLEMESHUTILITIES_H
+#define SURGSIM_DATASTRUCTURES_TRIANGLEMESHUTILITIES_H
+
+#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// Helper function to load a mesh from a given filename, does NOT do path resolution.
+/// \throws SurgSim::Framework::AssertionFailure if the reader does not contain mesh information.
+/// \param filename Path to the file that is to be read.
+/// \return the filled mesh a filled mesh if the reading succeeds, nullptr otherwise
+template <class M>
+std::shared_ptr<M> loadTriangleMesh(const std::string& filename);
+
+std::shared_ptr<TriangleMeshPlain> loadTriangleMesh(const std::string& filename);
+
+}
+}
+
+#include "SurgSim/DataStructures/TriangleMeshUtilities-inl.h"
+
+#endif
diff --git a/SurgSim/DataStructures/UnitTests/AabbTreeDataTests.cpp b/SurgSim/DataStructures/UnitTests/AabbTreeDataTests.cpp
new file mode 100644
index 0000000..a8ab156
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/AabbTreeDataTests.cpp
@@ -0,0 +1,181 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/AabbTreeData.h"
+
+#include "SurgSim/Math/Aabb.h"
+#include "SurgSim/Math/Vector.h"
+
+#include "SurgSim/Testing/MathUtilities.h"
+
+using SurgSim::Math::Aabbd;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+TEST(AabbTreeDataTests, InitTest)
+{
+ ASSERT_NO_THROW(AabbTreeData data;);
+
+ AabbTreeData data;
+ EXPECT_TRUE(data.isEmpty());
+ EXPECT_EQ(0u, data.getSize());
+}
+
+TEST(AabbTreeDataTests, EqualityTests)
+{
+ Aabbd a(Vector3d(-1.0, -1.0, -1.0), Vector3d(1.0, 1.0, 1.0));
+ Aabbd b(Vector3d(-2.0, -2.0, -2.0), Vector3d(0.0, 0.0, 0.0));
+
+ AabbTreeData empty;
+ AabbTreeData containsA;
+ AabbTreeData containsAnotherA;
+ AabbTreeData containsB;
+ AabbTreeData containsAB;
+ AabbTreeData containsBA;
+
+ containsA.add(a, 0);
+ containsAnotherA.add(a, 0);
+ containsB.add(b, 0);
+
+ containsAB.add(a, 0);
+ containsAB.add(b, 0);
+
+ containsBA.add(b, 0);
+ containsBA.add(a, 0);
+
+ EXPECT_EQ(empty, empty);
+ EXPECT_NE(empty, containsA);
+ EXPECT_NE(empty, containsAB);
+ EXPECT_NE(containsA, empty);
+ EXPECT_NE(containsBA, empty);
+
+ EXPECT_EQ(containsA, containsA);
+ EXPECT_EQ(containsA, containsAnotherA);
+
+ EXPECT_NE(containsA, containsB);
+ EXPECT_NE(containsA, containsAB);
+
+ EXPECT_EQ(containsAB, containsAB);
+ EXPECT_EQ(containsAB, containsBA);
+ EXPECT_EQ(containsBA, containsAB);
+}
+
+TEST(AabbTreeDataTests, AddTest)
+{
+ Aabbd a(Vector3d(-1.0, -1.0, -1.0), Vector3d(1.0, 1.0, 1.0));
+ Aabbd b(Vector3d(-2.0, -2.0, -2.0), Vector3d(0.0, 0.0, 0.0));
+ Aabbd c = a.merged(b);
+
+ AabbTreeData data;
+
+ EXPECT_NO_THROW(data.add(a, 0));
+ EXPECT_TRUE(a.isApprox(data.getAabb()));
+ EXPECT_EQ(1u, data.getSize());
+ data.add(b, 1);
+
+ EXPECT_EQ(2u, data.getSize());
+ EXPECT_TRUE(c.isApprox(data.getAabb()));
+}
+
+TEST(AabbTreeDataTests, SimpleSplitTest)
+{
+ AabbTreeData data;
+
+ // add 11 elements
+ for (int i = 0; i <= 10; ++i)
+ {
+ data.add(Aabbd(Vector3d(i, -1.0, -1.0), Vector3d(i + 2, 1.0, 1.0)), i);
+ }
+
+ // The original box, encompasses all the members
+ Aabbd original(Vector3d(0.0, -1.0, -1.0), Vector3d(12.0, 1.0, 1.0));
+
+ // The left hand side box, this is what is left in the original data item
+ // all the items with center.x < (12.0 - 0.0) / 2.0, 5 items
+ Aabbd expectedLeft(Vector3d(0.0, -1.0, -1.0), Vector3d(6.0, 1.0, 1.0));
+
+ // The left hand side box, this is what moved to the new data item
+ // all the items with center.x >= (12.0 - 0.0) / 2.0, 6 items
+ Aabbd expectedRight(Vector3d(5.0, -1.0, -1.0), Vector3d(12.0, 1.0, 1.0));
+
+ EXPECT_TRUE(original.isApprox(data.getAabb())) << original << ", " << data.getAabb();
+ std::shared_ptr<AabbTreeData> ptr = data.takeLargerElements();
+
+ EXPECT_EQ(5u, data.getSize());
+ EXPECT_TRUE(expectedLeft.isApprox(data.getAabb())) << expectedLeft << ", " << data.getAabb();
+
+ EXPECT_EQ(6u, ptr->getSize());
+ EXPECT_TRUE(expectedRight.isApprox(ptr->getAabb())) << expectedRight << ", " << ptr->getAabb();
+}
+
+TEST(AabbTreeDataTests, IntersectionTest)
+{
+ Aabbd a(Vector3d(-1.0, -1.0, -1.0), Vector3d(1.0, 1.0, 1.0));
+ Aabbd b(Vector3d(-2.0, -2.0, -2.0), Vector3d(0.0, 0.0, 0.0));
+
+ AabbTreeData data;
+ data.add(Aabbd(Vector3d(-1.0, 0.0, 0.0), Vector3d(-1.0, 0.0, 0.0)), 0);
+ data.add(Aabbd(Vector3d(-2.0, 0.0, 0.0), Vector3d(-2.0, 0.0, 0.0)), 1);
+ data.add(Aabbd(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)), 2);
+ data.add(Aabbd(Vector3d(1.0, 0.0, 0.0), Vector3d(1.0, 0.0, 0.0)), 3);
+
+
+ Aabbd noIntersections(Vector3d(-0.5, -2.0, -2.0), Vector3d(0.5, -1.0, -1.0));
+ Aabbd oneIntersection(Vector3d(-0.5, 0.0, 0.0), Vector3d(0.5, 0.0, 0.0));
+ Aabbd twoIntersections(Vector3d(-0.5, 0.0, 0.0), Vector3d(1.5, 0.0, 0.0));
+ Aabbd threeIntersections(Vector3d(-1.5, 0.0, 0.0), Vector3d(1.5, 0.0, 0.0));
+ Aabbd fourIntersections(Vector3d(-2.5, 0.0, 0.0), Vector3d(2.5, 0.0, 0.0));
+
+
+ std::list<size_t> results;
+ EXPECT_FALSE(data.hasIntersections(noIntersections));
+ data.getIntersections(noIntersections, &results);
+ EXPECT_EQ(0u, results.size());
+ results.size();
+
+ results.clear();
+ EXPECT_TRUE(data.hasIntersections(oneIntersection));
+ data.getIntersections(oneIntersection, &results);
+ EXPECT_EQ(1u, results.size());
+ EXPECT_EQ(2u, results.front());
+
+ results.clear();
+ EXPECT_TRUE(data.hasIntersections(twoIntersections));
+ data.getIntersections(twoIntersections, &results);
+ EXPECT_EQ(2u, results.size());
+
+ results.clear();
+ EXPECT_TRUE(data.hasIntersections(threeIntersections));
+ data.getIntersections(threeIntersections, &results);
+ EXPECT_EQ(3u, results.size());
+
+ results.clear();
+ EXPECT_TRUE(data.hasIntersections(fourIntersections));
+ data.getIntersections(fourIntersections, &results);
+ EXPECT_EQ(4u, results.size());
+}
+
+
+
+
+}
+}
+
diff --git a/SurgSim/DataStructures/UnitTests/AabbTreeNodeTests.cpp b/SurgSim/DataStructures/UnitTests/AabbTreeNodeTests.cpp
new file mode 100644
index 0000000..ec891d1
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/AabbTreeNodeTests.cpp
@@ -0,0 +1,102 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/Aabb.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/DataStructures/TreeVisitor.h"
+#include "SurgSim/DataStructures/AabbTreeNode.h"
+
+using SurgSim::Math::Aabbd;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+TEST(AabbTreeNodeTests, InitTest)
+{
+ EXPECT_NO_THROW(AabbTreeNode node);
+
+ auto node = std::make_shared<AabbTreeNode>();
+ EXPECT_EQ(0u, node->getNumChildren());
+ EXPECT_TRUE(node->getAabb().isEmpty());
+}
+
+TEST(AabbTreeNodeTests, AddTest)
+{
+ auto node = std::make_shared<AabbTreeNode>();
+ Aabbd one(Vector3d(-1.0, -1.0, -1.0), Vector3d(0.0, 0.0, 0.0));
+ Aabbd two(Vector3d(0.0, 0.0, 0.0), Vector3d(1.0, 1.0, 1.0));
+ Aabbd combined(Vector3d(-1.0, -1.0, -1.0), Vector3d(1.0, 1.0, 1.0));
+ EXPECT_NO_THROW(node->addData(one, 0));
+
+ EXPECT_EQ(0u, node->getNumChildren());
+ EXPECT_TRUE(one.isApprox(node->getAabb()));
+
+ node->addData(one, 1, 3);
+ node->addData(two, 2, 3);
+ node->addData(two, 3, 3);
+
+ ASSERT_EQ(2u, node->getNumChildren());
+ EXPECT_TRUE(combined.isApprox(node->getAabb()));
+
+ ASSERT_NE(nullptr, node->getChild(0));
+ ASSERT_NE(nullptr, node->getChild(1));
+
+ auto child = std::dynamic_pointer_cast<AabbTreeNode>(node->getChild(0));
+ ASSERT_NE(nullptr, child);
+ ASSERT_EQ(0u, child->getNumChildren());
+ EXPECT_TRUE(one.isApprox(child->getAabb()));
+
+
+ child = std::dynamic_pointer_cast<AabbTreeNode>(node->getChild(1));
+ ASSERT_NE(nullptr, child);
+ ASSERT_EQ(0u, child->getNumChildren());
+ EXPECT_TRUE(two.isApprox(child->getAabb()));
+
+}
+
+class TestVisitor : public TreeVisitor
+{
+public:
+ virtual ~TestVisitor() {}
+
+ virtual bool handle(TreeNode* node) override
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ return false;
+ }
+
+ virtual bool handle(AabbTreeNode* node) override
+ {
+ return true;
+ }
+
+};
+
+TEST(AabbTreeNodeTests, VisitorTest)
+{
+ TestVisitor visitor;
+ auto node = std::make_shared<AabbTreeNode>();
+ node->accept(&visitor);
+}
+
+
+}
+}
+
diff --git a/SurgSim/DataStructures/UnitTests/AabbTreeTests.cpp b/SurgSim/DataStructures/UnitTests/AabbTreeTests.cpp
new file mode 100644
index 0000000..9d910c7
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/AabbTreeTests.cpp
@@ -0,0 +1,283 @@
+#include "../PlyReader.h"
+#include "../TriangleMeshPlyReaderDelegate.h"
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/AabbTreeIntersectionVisitor.h"
+#include "SurgSim/DataStructures/AabbTreeNode.h"
+#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Timer.h"
+#include "SurgSim/Math/Aabb.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/MathUtilities.h"
+
+using SurgSim::Math::Aabbd;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+TEST(AabbTreeTests, InitTest)
+{
+ ASSERT_NO_THROW({AabbTree tree(3);});
+
+ auto tree = std::make_shared<AabbTree>(3);
+
+ EXPECT_EQ(3u, tree->getMaxObjectsPerNode());
+ EXPECT_NE(nullptr, tree->getRoot());
+}
+
+TEST(AabbTreeTests, AddTest)
+{
+ auto tree = std::make_shared<AabbTree>(3);
+
+ Aabbd one(Vector3d(-1.0, -1.0, -1.0), Vector3d(0.0, 0.0, 0.0));
+ Aabbd two(Vector3d(0.0, 0.0, 0.0), Vector3d(1.0, 1.0, 1.0));
+
+ EXPECT_NO_THROW(tree->add(one, 0));
+ EXPECT_NO_THROW(tree->add(one, 1));
+ EXPECT_NO_THROW(tree->add(two, 2));
+ EXPECT_NO_THROW(tree->add(two, 3));
+
+ EXPECT_EQ(2u, tree->getRoot()->getNumChildren());
+}
+
+TEST(AabbTreeTests, BuildTest)
+{
+ SurgSim::Framework::ApplicationData data("config.txt");
+ auto tree = std::make_shared<AabbTree>(3);
+
+ std::string filename = data.findFile("Geometry/arm_collision.ply");
+ ASSERT_FALSE(filename.empty());
+ auto mesh = loadTriangleMesh(filename);
+
+ SurgSim::Framework::Timer timer;
+ for (size_t i = 0; i < mesh->getNumTriangles(); ++i)
+ {
+ auto triangle = mesh->getTriangle(i);
+ Aabbd aabb(SurgSim::Math::makeAabb(
+ mesh->getVertex(triangle.verticesId[0]).position,
+ mesh->getVertex(triangle.verticesId[1]).position,
+ mesh->getVertex(triangle.verticesId[2]).position));
+ tree->add(aabb, i);
+ }
+ timer.endFrame();
+}
+
+TEST(AabbTreeTests, EasyIntersectionTest)
+{
+ auto tree = std::make_shared<AabbTree>(3);
+
+ Aabbd bigBox;
+ for (int i = 0; i <= 6; ++i)
+ {
+ Aabbd aabb(Vector3d(static_cast<double>(i) - 0.01, -0.01, -0.01),
+ Vector3d(static_cast<double>(i) + 0.01, 0.01, 0.01));
+ bigBox.extend(aabb);
+ tree->add(aabb, i);
+ }
+
+ EXPECT_TRUE(bigBox.isApprox(tree->getAabb())) << bigBox << ", " << tree->getAabb();
+ AabbTreeIntersectionVisitor visitor(bigBox);
+ tree->getRoot()->accept(&visitor);
+
+ EXPECT_EQ(7u, visitor.getIntersections().size());
+
+ Aabbd leftBox(Vector3d(0.0, -0.02, -0.02), Vector3d(3.4, 0.02, 0.02));
+ visitor.setAabb(leftBox);
+ tree->getRoot()->accept(&visitor);
+ EXPECT_EQ(4u, visitor.getIntersections().size()) << "Left Box Incorrect";
+
+ Aabbd middleBox(Vector3d(1.8, -0.02, -0.02), Vector3d(4.4, 0.02, 0.02));
+ visitor.setAabb(middleBox);
+ tree->getRoot()->accept(&visitor);
+ EXPECT_EQ(3u, visitor.getIntersections().size()) << "Middle Box Incorrect";
+
+ Aabbd rightBox(Vector3d(2.8, -0.02, -0.02), Vector3d(6.4, 0.02, 0.02));
+ visitor.setAabb(rightBox);
+ tree->getRoot()->accept(&visitor);
+ EXPECT_EQ(4u, visitor.getIntersections().size()) << "Right Box Incorrect";
+
+}
+
+TEST(AabbTreeTests, MeshIntersectionTest)
+{
+ SurgSim::Framework::ApplicationData data("config.txt");
+ auto tree = std::make_shared<AabbTree>(3);
+
+ std::string filename = data.findFile("Geometry/arm_collision.ply");
+ ASSERT_FALSE(filename.empty());
+ auto mesh = loadTriangleMesh(filename);
+
+ Aabbd expectedBigBox;
+
+ for (size_t i = 0; i < mesh->getNumTriangles(); ++i)
+ {
+ auto triangle = mesh->getTriangle(i);
+ std::array<Vector3d, 3> vertices =
+ {
+ mesh->getVertex(triangle.verticesId[0]).position,
+ mesh->getVertex(triangle.verticesId[1]).position,
+ mesh->getVertex(triangle.verticesId[2]).position
+ };
+ Aabbd aabb(SurgSim::Math::makeAabb(vertices[0], vertices[1], vertices[2]));
+ expectedBigBox.extend(aabb);
+ tree->add(std::move(aabb), i);
+ }
+
+ Aabbd bigBox = tree->getAabb();
+
+ EXPECT_TRUE(expectedBigBox.isApprox(bigBox));
+
+ AabbTreeIntersectionVisitor intersector(bigBox);
+
+ SurgSim::Framework::Timer timer;
+ timer.start();
+ tree->getRoot()->accept(&intersector);
+ timer.endFrame();
+
+ EXPECT_EQ(mesh->getNumTriangles(), intersector.getIntersections().size());
+}
+
+template <typename NodeType>
+class TreeLeavesVisitor : public TreeVisitor
+{
+public:
+
+ virtual bool handle(TreeNode* node) override
+ {
+ SURGSIM_FAILURE() << "Function " << __FUNCTION__ << " not implemented";
+ return false;
+ }
+
+ virtual bool handle(NodeType* node) override
+ {
+ if (node->getNumChildren() == 0)
+ {
+ leaves.push_back(node);
+ }
+ return true;
+ }
+
+ std::vector<NodeType*> leaves;
+};
+
+template <typename PairTypeLhs, typename PairTypeRhs>
+static typename std::list<PairTypeLhs>::const_iterator getEquivalentPair(const std::list<PairTypeLhs>& list,
+ const PairTypeRhs& item)
+{
+ return std::find_if(list.cbegin(), list.cend(),
+ [&item](const PairTypeLhs & pair)
+ {
+ return (pair.first->getAabb().isApprox(item.first->getAabb())
+ && pair.second->getAabb().isApprox(item.second->getAabb()))
+ || (pair.first->getAabb().isApprox(item.second->getAabb())
+ && pair.second->getAabb().isApprox(item.first->getAabb()));
+ }
+ );
+}
+
+TEST(AabbTreeTests, SpatialJoinTest)
+{
+ SurgSim::Framework::Runtime runtime("config.txt");
+ const std::string fileName = "MeshShapeData/staple_collision.ply";
+
+ auto meshA = std::make_shared<SurgSim::Math::MeshShape>();
+ EXPECT_NO_THROW(meshA->load(fileName));
+
+ auto meshB = std::make_shared<SurgSim::Math::MeshShape>();
+ EXPECT_NO_THROW(meshB->load(fileName));
+
+ RigidTransform3d rhsPose = SurgSim::Math::makeRigidTranslation(Vector3d(0.005, 0.0, 0.0));
+ meshB->getMesh()->copyWithTransform(rhsPose, *meshA->getMesh());
+
+ meshA->updateAabbTree();
+ meshB->updateAabbTree();
+
+ auto aabbA = meshA->getAabbTree();
+ auto aabbB = meshB->getAabbTree();
+
+ auto actualIntersection = aabbA->spatialJoin(*aabbB);
+
+ TreeLeavesVisitor<SurgSim::DataStructures::AabbTreeNode> leavesVisitorA;
+ std::static_pointer_cast<SurgSim::DataStructures::AabbTreeNode>(aabbA->getRoot())->accept(&leavesVisitorA);
+ auto& leavesA = leavesVisitorA.leaves;
+
+ TreeLeavesVisitor<SurgSim::DataStructures::AabbTreeNode> leavesVisitorB;
+ std::static_pointer_cast<SurgSim::DataStructures::AabbTreeNode>(aabbB->getRoot())->accept(&leavesVisitorB);
+ auto& leavesB = leavesVisitorB.leaves;
+
+ std::list<std::pair<SurgSim::DataStructures::AabbTreeNode*, SurgSim::DataStructures::AabbTreeNode*>>
+ expectedIntersection;
+ for (auto leafA = leavesA.begin(); leafA != leavesA.end(); ++leafA)
+ {
+ for (auto leafB = leavesB.begin(); leafB != leavesB.end(); ++leafB)
+ {
+ if (SurgSim::Math::doAabbIntersect((*leafA)->getAabb(), (*leafB)->getAabb()))
+ {
+ expectedIntersection.emplace_back(*leafA, *leafB);
+ }
+ }
+ }
+
+ {
+ SCOPED_TRACE("Equivalent sets");
+
+ ASSERT_GT(expectedIntersection.size(), 0u);
+ ASSERT_EQ(expectedIntersection.size(), actualIntersection.size());
+
+ // Sets A and B are equal if and only if A is a subset of B and B is a subset of A.
+ for (auto it = actualIntersection.begin(); it != actualIntersection.end(); ++it)
+ {
+ EXPECT_FALSE(getEquivalentPair(expectedIntersection, *it) == expectedIntersection.cend());
+ }
+
+ for (auto it = expectedIntersection.begin(); it != expectedIntersection.end(); ++it)
+ {
+ EXPECT_FALSE(getEquivalentPair(actualIntersection, *it) == actualIntersection.cend());
+ }
+ }
+
+ {
+ SCOPED_TRACE("Inequivalent sets.");
+
+ auto newNode = std::make_shared<SurgSim::DataStructures::AabbTreeNode>();
+ newNode->addData(
+ SurgSim::Math::makeAabb(Vector3d(-0.1, 0.3, 5.3), Vector3d(5.4, -5.8, 1.1), Vector3d(0, 0.5, 11)), 4543);
+
+ expectedIntersection.emplace_back(newNode.get(), expectedIntersection.front().second);
+ actualIntersection.emplace_back(newNode, actualIntersection.back().first);
+
+ ASSERT_GT(expectedIntersection.size(), 0u);
+ ASSERT_EQ(expectedIntersection.size(), actualIntersection.size());
+
+ EXPECT_TRUE(getEquivalentPair(expectedIntersection, actualIntersection.back()) == expectedIntersection.cend());
+ EXPECT_TRUE(getEquivalentPair(actualIntersection, expectedIntersection.back()) == actualIntersection.cend());
+ }
+}
+
+} // namespace DataStructure
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/DataStructures/UnitTests/BufferedValueTests.cpp b/SurgSim/DataStructures/UnitTests/BufferedValueTests.cpp
new file mode 100644
index 0000000..acac1e5
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/BufferedValueTests.cpp
@@ -0,0 +1,62 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+
+#include "SurgSim/DataStructures/BufferedValue.h"
+
+#include <boost/thread.hpp>
+#include <memory>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+TEST(BufferedValueTests, InitTest)
+{
+ EXPECT_NO_THROW({BufferedValue<int> value(3);});
+ EXPECT_NO_THROW({BufferedValue<int> value;});
+}
+
+TEST(BufferedValueTests, SafeAndUnsafeGettersTest)
+{
+ auto buffer = std::make_shared<BufferedValue<int>>(10);
+ int& value = buffer->unsafeGet();
+ std::shared_ptr<const int> initialBufferedValue = buffer->safeGet();
+
+ EXPECT_EQ(10, value);
+ EXPECT_EQ(10, *initialBufferedValue);
+
+ value = 20;
+ EXPECT_EQ(20, value);
+ EXPECT_EQ(10, *initialBufferedValue);
+
+ std::shared_ptr<const int> postAssignBufferedValue = buffer->safeGet();
+ EXPECT_EQ(initialBufferedValue, postAssignBufferedValue);
+
+ buffer->publish();
+
+ EXPECT_EQ(10, *initialBufferedValue);
+
+ std::shared_ptr<const int> postPublishBufferedValue = buffer->safeGet();
+ EXPECT_EQ(20, *postPublishBufferedValue);
+}
+
+
+}
+}
+
diff --git a/SurgSim/DataStructures/UnitTests/CMakeLists.txt b/SurgSim/DataStructures/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..7c30abb
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/CMakeLists.txt
@@ -0,0 +1,67 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories (
+ ${gtest_SOURCE_DIR}/include
+ ${OSG_INCLUDE_DIR}
+)
+
+set(UNIT_TEST_SOURCES
+ AabbTreeDataTests.cpp
+ AabbTreeNodeTests.cpp
+ AabbTreeTests.cpp
+ BufferedValueTests.cpp
+ DataGroupTests.cpp
+ DataStructuresConvertTests.cpp
+ ImageTest.cpp
+ IndexDirectoryTests.cpp
+ IndexedLocalCoordinateTest.cpp
+ LocationTests.cpp
+ MeshElementTest.cpp
+ MeshTest.cpp
+ MeshVertexTest.cpp
+ NamedDataTests.cpp
+ NamedVariantDataTests.cpp
+ OctreeNodeTests.cpp
+ OptionalValueTests.cpp
+ PlyReaderTests.cpp
+ TetrahedronMeshTest.cpp
+ TriangleMeshBaseTest.cpp
+ TriangleMeshTest.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ MockObjects.h
+)
+
+set(LIBS
+ SurgSimDataStructures
+ SurgSimGraphics
+ SurgSimMath
+)
+
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/TriangleMeshBaseTests DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/MeshShapeData DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Data)
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/OctreeShapeData DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Data)
+
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+surgsim_add_unit_tests(SurgSimDataStructuresTest)
+
+set_target_properties(SurgSimDataStructuresTest PROPERTIES FOLDER "DataStructures")
diff --git a/SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Cube.ply b/SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Cube.ply
new file mode 100644
index 0000000..33fe119
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Cube.ply
@@ -0,0 +1,51 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 26
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+element face 12
+property list uchar uint vertex_indices
+end_header
+1.000000 1.000000 -1.000000 0.000000 0.000000 -1.000000
+1.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000
+-1.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000
+1.000000 0.999999 1.000000 -0.000000 -0.000000 1.000000
+-1.000000 1.000000 1.000000 -0.000000 -0.000000 1.000000
+0.999999 -1.000001 1.000000 -0.000000 -0.000000 1.000000
+1.000000 1.000000 -1.000000 1.000000 0.000000 -0.000000
+1.000000 0.999999 1.000000 1.000000 0.000000 -0.000000
+1.000000 -1.000000 -1.000000 1.000000 0.000000 -0.000000
+1.000000 -1.000000 -1.000000 -0.000000 -1.000000 -0.000000
+0.999999 -1.000001 1.000000 -0.000000 -1.000000 -0.000000
+-1.000000 -1.000000 -1.000000 -0.000000 -1.000000 -0.000000
+-1.000000 -1.000000 -1.000000 -1.000000 0.000000 -0.000000
+-1.000000 -1.000000 1.000000 -1.000000 0.000000 -0.000000
+-1.000000 1.000000 1.000000 -1.000000 0.000000 -0.000000
+1.000000 0.999999 1.000000 0.000000 1.000000 0.000000
+1.000000 1.000000 -1.000000 0.000000 1.000000 0.000000
+-1.000000 1.000000 1.000000 0.000000 1.000000 0.000000
+-1.000000 1.000000 -1.000000 0.000000 0.000000 -1.000000
+1.000000 0.999999 1.000000 1.000000 -0.000001 0.000000
+0.999999 -1.000001 1.000000 1.000000 -0.000001 0.000000
+1.000000 -1.000000 -1.000000 1.000000 -0.000001 0.000000
+-1.000000 1.000000 -1.000000 0.000000 1.000000 0.000000
+-1.000000 -1.000000 1.000000 0.000000 -0.000000 1.000000
+-1.000000 1.000000 -1.000000 -1.000000 0.000000 -0.000000
+-1.000000 -1.000000 1.000000 -0.000000 -1.000000 0.000000
+3 0 1 2
+3 3 4 5
+3 6 7 8
+3 9 10 11
+3 12 13 14
+3 15 16 17
+3 18 0 2
+3 19 20 21
+3 16 22 17
+3 4 23 5
+3 24 12 14
+3 10 25 11
diff --git a/SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Testdata.ply b/SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Testdata.ply
new file mode 100644
index 0000000..603693b
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Testdata.ply
@@ -0,0 +1,23 @@
+ply
+format ascii 1.0
+comment TestData not really a mesh
+element vertex 4
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+element face 4
+property list uchar uint vertex_indices
+property int extra
+end_header
+1.000000 2.000000 3.000000 0.000000 0.000000 -1.000000
+-2.000000 -3.000000 -4.000000 0.000000 0.000000 -1.000000
+3.000000 4.000000 5.000000 0.000000 0.000000 -1.000000
+-4.000000 -5.000000 -6.000000 0.000000 0.000000 -1.000000
+1 0 0
+2 1 2 -1
+3 3 4 5 -2
+4 6 7 8 9 -3
+
diff --git a/SurgSim/DataStructures/UnitTests/DataGroupTests.cpp b/SurgSim/DataStructures/UnitTests/DataGroupTests.cpp
new file mode 100644
index 0000000..1c7ea97
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/DataGroupTests.cpp
@@ -0,0 +1,464 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 DataGroup class.
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/DataStructures/DataGroupCopier.h"
+#include "SurgSim/DataStructures/NamedData.h"
+#include "SurgSim/Framework/LockedContainer.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "gtest/gtest.h"
+
+#include "SurgSim/DataStructures/UnitTests/MockObjects.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::DataStructures::DataGroupCopier;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+const double EPSILON = 1e-9;
+};
+
+/// Creating a named data object.
+TEST(DataGroupTests, CanConstruct)
+{
+ DataGroupBuilder builder;
+ builder.addPose("test");
+ builder.addVector("test");
+ builder.addMatrix("test");
+ builder.addScalar("test");
+ builder.addInteger("test");
+ builder.addBoolean("test");
+ builder.addString("test");
+ builder.addCustom("test");
+ DataGroup data = builder.createData();
+ EXPECT_TRUE(data.poses().hasEntry("test"));
+ EXPECT_FALSE(data.poses().hasData("test"));
+ EXPECT_FALSE(data.strings().hasEntry("missing"));
+ EXPECT_FALSE(data.strings().hasData("missing"));
+
+ DataGroupBuilder builder2;
+ builder2.addInteger("test");
+ DataGroup data2 = builder2.createData();
+ EXPECT_TRUE(data2.integers().hasEntry("test"));
+ EXPECT_FALSE(data2.integers().hasData("test"));
+ EXPECT_FALSE(data2.strings().hasEntry("missing"));
+ EXPECT_FALSE(data2.strings().hasData("missing"));
+
+ DataGroupBuilder builder3;
+ DataGroup data3, data4 = builder3.createData();
+ EXPECT_NO_THROW(data3 = data4); // A DataGroup created by an empty DataGroupBuilder is valid (aka non-empty).
+}
+
+/// Creating a shared_ref to a named data object.
+TEST(DataGroupTests, CanCreateShared)
+{
+ DataGroupBuilder builder;
+ builder.addPose("test");
+ builder.addVector("test");
+ builder.addMatrix("test");
+ builder.addScalar("test");
+ builder.addInteger("test");
+ builder.addBoolean("test");
+ builder.addString("test");
+ builder.addCustom("test");
+ std::shared_ptr<DataGroup> data = builder.createSharedData();
+
+ EXPECT_TRUE(data->poses().hasEntry("test"));
+ EXPECT_FALSE(data->poses().hasData("test"));
+ EXPECT_FALSE(data->strings().hasEntry("missing"));
+ EXPECT_FALSE(data->strings().hasData("missing"));
+}
+
+/// Creating an invalid (aka empty) data object.
+TEST(DataGroupTests, Uninitialized)
+{
+ DataGroup data, data2;
+ EXPECT_THROW(data = data2, SurgSim::Framework::AssertionFailure);
+}
+
+/// Putting data into the container.
+TEST(DataGroupTests, PutName)
+{
+ DataGroupBuilder builder;
+ builder.addPose("pose");
+ builder.addVector("vector");
+ builder.addMatrix("matrix");
+ builder.addScalar("scalar");
+ builder.addInteger("integer");
+ builder.addBoolean("boolean");
+ builder.addString("string");
+ builder.addCustom("mock_data");
+ DataGroup data = builder.createData();
+
+ const SurgSim::Math::Vector3d vector(1.23, 4.56, 7.89);
+ const SurgSim::Math::Quaterniond quat =
+ SurgSim::Math::makeRotationQuaternion(M_PI_2, SurgSim::Math::Vector3d(1, 0, 0));
+ const SurgSim::Math::RigidTransform3d pose = SurgSim::Math::makeRigidTransform(quat, vector);
+ DataGroup::DynamicMatrixType matrix(2,3);
+ matrix.fill(3.0);
+
+ Mock3DData<double> mockData(10,10,10);
+ mockData.set(5, 5, 5, 1.2345);
+
+ data.poses().set("pose", pose);
+ data.vectors().set("vector", vector);
+ data.matrices().set("matrix", matrix);
+ data.scalars().set("scalar", 1.23f);
+ data.integers().set("integer", 123);
+ data.booleans().set("boolean", true);
+ data.strings().set("string", "string");
+ data.customData().set("mock_data", mockData);
+
+ EXPECT_TRUE(data.poses().hasEntry("pose"));
+ EXPECT_TRUE(data.poses().hasData("pose"));
+
+ EXPECT_TRUE(data.vectors().hasEntry("vector"));
+ EXPECT_TRUE(data.vectors().hasData("vector"));
+
+ EXPECT_TRUE(data.matrices().hasEntry("matrix"));
+ EXPECT_TRUE(data.matrices().hasData("matrix"));
+
+ EXPECT_TRUE(data.scalars().hasEntry("scalar"));
+ EXPECT_TRUE(data.scalars().hasData("scalar"));
+
+ EXPECT_TRUE(data.integers().hasEntry("integer"));
+ EXPECT_TRUE(data.integers().hasData("integer"));
+
+ EXPECT_TRUE(data.booleans().hasEntry("boolean"));
+ EXPECT_TRUE(data.booleans().hasData("boolean"));
+
+ EXPECT_TRUE(data.strings().hasEntry("string"));
+ EXPECT_TRUE(data.strings().hasData("string"));
+
+ EXPECT_TRUE(data.customData().hasEntry("mock_data"));
+ EXPECT_TRUE(data.customData().hasData("mock_data"));
+}
+
+/// Getting data from the container.
+TEST(DataGroupTests, GetName)
+{
+ DataGroupBuilder builder;
+ builder.addPose("pose");
+ builder.addVector("vector");
+ builder.addMatrix("matrix");
+ builder.addScalar("scalar");
+ builder.addInteger("integer");
+ builder.addBoolean("boolean");
+ builder.addString("string");
+ builder.addCustom("mock_data");
+ DataGroup data = builder.createData();
+
+ const SurgSim::Math::Vector3d vector(1.23, 4.56, 7.89);
+ const SurgSim::Math::Quaterniond quat =
+ SurgSim::Math::makeRotationQuaternion(M_PI_2, SurgSim::Math::Vector3d(1, 0, 0));
+ const SurgSim::Math::RigidTransform3d pose = SurgSim::Math::makeRigidTransform(quat, vector);
+ DataGroup::DynamicMatrixType matrix(2,3);
+ matrix.fill(3.0);
+
+ Mock3DData<double> mockData(10,10,10);
+ mockData.set(5, 5, 5, 1.23);
+ mockData.set(1, 2, 3, 4.56);
+
+ data.poses().set("pose", pose);
+ data.vectors().set("vector", vector);
+ data.matrices().set("matrix", matrix);
+ data.scalars().set("scalar", 1.23);
+ data.integers().set("integer", 123);
+ data.booleans().set("boolean", true);
+ data.strings().set("string", "string");
+ data.customData().set("mock_data", mockData);
+
+ {
+ SurgSim::Math::RigidTransform3d value = SurgSim::Math::RigidTransform3d::Identity();
+ EXPECT_TRUE(data.poses().get("pose", &value));
+ EXPECT_NEAR(0, (value.linear() - quat.matrix()).norm(), EPSILON);
+ EXPECT_NEAR(0, (value.translation() - vector).norm(), EPSILON);
+ }
+ {
+ SurgSim::Math::Vector3d value(0, 0, 0);
+ EXPECT_TRUE(data.vectors().get("vector", &value));
+ EXPECT_NEAR(0, (value - vector).norm(), EPSILON);
+ }
+ {
+ DataGroup::DynamicMatrixType value;
+ EXPECT_TRUE(data.matrices().get("matrix", &value));
+ EXPECT_EQ(matrix.cols(), value.cols());
+ EXPECT_EQ(matrix.rows(), value.rows());
+ EXPECT_NEAR(0, (value - matrix).norm(), EPSILON);
+ }
+ {
+ double value = 0;
+ EXPECT_TRUE(data.scalars().get("scalar", &value));
+ EXPECT_NEAR(1.23, value, EPSILON);
+ }
+ {
+ int value = 0;
+ EXPECT_TRUE(data.integers().get("integer", &value));
+ EXPECT_EQ(123, value);
+ }
+ {
+ bool value = 0;
+ EXPECT_TRUE(data.booleans().get("boolean", &value));
+ EXPECT_EQ(true, value);
+ }
+ {
+ std::string value = "";
+ EXPECT_TRUE(data.strings().get("string", &value));
+ EXPECT_EQ("string", value);
+ }
+ {
+ Mock3DData<double> value;
+ EXPECT_TRUE(data.customData().get("mock_data", &value));
+ EXPECT_NEAR(1.23, value.get(5, 5, 5), EPSILON);
+ EXPECT_NEAR(4.56, value.get(1, 2, 3), EPSILON);
+ }
+}
+
+/// Resetting the data in the container.
+TEST(DataGroupTests, ResetAll)
+{
+ DataGroupBuilder builder;
+ builder.addPose("first");
+ builder.addScalar("second");
+ builder.addString("third");
+ builder.addCustom("fourth");
+ DataGroup data = builder.createData();
+
+ data.scalars().set("second", 1.23);
+ data.strings().set("third", "hello");
+ data.customData().set("fourth", Mock3DData<double>(10, 10, 10));
+
+ data.resetAll();
+
+ EXPECT_TRUE(data.poses().hasEntry("first"));
+ EXPECT_FALSE(data.poses().hasData("first"));
+
+ EXPECT_TRUE(data.scalars().hasEntry("second"));
+ EXPECT_FALSE(data.scalars().hasData("second"));
+
+ EXPECT_TRUE(data.strings().hasEntry("third"));
+ EXPECT_FALSE(data.strings().hasData("third"));
+
+ EXPECT_TRUE(data.customData().hasEntry("fourth"));
+ EXPECT_FALSE(data.customData().hasData("fourth"));
+}
+
+/// Resetting one data entry at a time.
+TEST(DataGroupTests, ResetOne)
+{
+ DataGroupBuilder builder;
+ builder.addPose("first");
+ builder.addScalar("second");
+ builder.addString("third");
+ builder.addCustom("fourth");
+ DataGroup data = builder.createData();
+
+ data.scalars().set("second", 1.23);
+ data.strings().set("third", "hello");
+ data.customData().set("fourth", Mock3DData<double>(10, 10, 10));
+
+ data.strings().reset("third");
+
+ EXPECT_TRUE(data.poses().hasEntry("first"));
+ EXPECT_FALSE(data.poses().hasData("first"));
+
+ EXPECT_TRUE(data.scalars().hasEntry("second"));
+ EXPECT_TRUE(data.scalars().hasData("second"));
+
+ EXPECT_TRUE(data.strings().hasEntry("third"));
+ EXPECT_FALSE(data.strings().hasData("third"));
+
+ EXPECT_TRUE(data.customData().hasEntry("fourth"));
+ EXPECT_TRUE(data.customData().hasData("fourth"));
+
+ data.scalars().reset("second");
+
+ EXPECT_TRUE(data.scalars().hasEntry("second"));
+ EXPECT_FALSE(data.scalars().hasData("second"));
+}
+
+/// Copy Constructing DataGroups
+TEST(DataGroupTests, CopyConstruction)
+{
+ DataGroupBuilder builder;
+ builder.addPose("test");
+ builder.addBoolean("test");
+ builder.addBoolean("test2");
+ DataGroup data = builder.createData();
+ const bool trueBool = true;
+ data.booleans().set("test2", trueBool);
+ DataGroup copied_data = data;
+ EXPECT_TRUE(copied_data.poses().hasEntry("test"));
+ EXPECT_FALSE(copied_data.poses().hasData("test"));
+ EXPECT_TRUE(copied_data.booleans().hasEntry("test"));
+ EXPECT_FALSE(copied_data.booleans().hasData("test"));
+ EXPECT_TRUE(copied_data.booleans().hasEntry("test2"));
+ EXPECT_TRUE(copied_data.booleans().hasData("test2"));
+ bool outBool, outCopiedBool;
+ data.booleans().get("test2", &outBool);
+ copied_data.booleans().get("test2", &outCopiedBool);
+ EXPECT_EQ(outBool, outCopiedBool);
+ EXPECT_EQ(trueBool, outCopiedBool);
+ EXPECT_FALSE(copied_data.strings().hasEntry("missing"));
+ EXPECT_FALSE(copied_data.strings().hasData("missing"));
+}
+
+/// Assigning DataGroups, testing DataGroup::operator=
+TEST(DataGroupTests, Assignment)
+{
+ DataGroupBuilder builder;
+ builder.addPose("test");
+ builder.addBoolean("test");
+ builder.addBoolean("test2");
+ DataGroup data = builder.createData();
+ const bool trueBool = true;
+ data.booleans().set("test2", trueBool);
+ DataGroup copied_data;
+ copied_data = data;
+ EXPECT_TRUE(copied_data.poses().hasEntry("test"));
+ EXPECT_FALSE(copied_data.poses().hasData("test"));
+ EXPECT_TRUE(copied_data.booleans().hasEntry("test"));
+ EXPECT_FALSE(copied_data.booleans().hasData("test"));
+ EXPECT_TRUE(copied_data.booleans().hasEntry("test2"));
+ EXPECT_TRUE(copied_data.booleans().hasData("test2"));
+ bool outBool, outCopiedBool;
+ data.booleans().get("test2", &outBool);
+ copied_data.booleans().get("test2", &outCopiedBool);
+ EXPECT_EQ(outBool, outCopiedBool);
+ EXPECT_EQ(trueBool, outCopiedBool);
+ EXPECT_FALSE(copied_data.strings().hasEntry("missing"));
+ EXPECT_FALSE(copied_data.strings().hasData("missing"));
+
+ DataGroup data2;
+ EXPECT_THROW(data = data2, SurgSim::Framework::AssertionFailure); // the right-hand DataGroup is not valid
+
+ DataGroup data3 = builder.createData();
+ // Having the same entries is not sufficient for DataGroup assignment.
+ // There are three situations in which assignment will not assert:
+ // 1) the DataGroup being assigned to is "empty" (i.e., was default-constructed and has not yet been assigned to or
+ // otherwise altered),
+ // 2) one of the DataGroups was default-constructed and then the other DataGroup was assigned to it, or
+ // 3) one of the DataGroups was copy-constructed from the other.
+ EXPECT_THROW(data = data3, SurgSim::Framework::AssertionFailure);
+
+ DataGroup data4(data);
+ data4.booleans().set("test2", !trueBool);
+ EXPECT_NO_THROW(data = data4); // data4 can assign to data because data4 was copy-constructed from data
+ EXPECT_NO_THROW(data4 = data);
+}
+
+/// Non-assignment copying DataGroups with DataGroupCopier.
+TEST(DataGroupTests, DataGroupCopier)
+{
+ DataGroupBuilder sourceBuilder;
+ sourceBuilder.addPose("test");
+ sourceBuilder.addBoolean("test");
+ sourceBuilder.addBoolean("test2");
+ DataGroup sourceData = sourceBuilder.createData();
+
+ DataGroupBuilder targetBuilder;
+ targetBuilder.addPose("test2"); //different pose name
+ targetBuilder.addBoolean("test2"); // same boolean name
+ targetBuilder.addEntriesFrom(sourceData);
+ DataGroup targetData = targetBuilder.createData();
+
+ ASSERT_TRUE(targetData.poses().hasEntry("test"));
+ ASSERT_TRUE(targetData.poses().hasEntry("test2"));
+ ASSERT_TRUE(targetData.booleans().hasEntry("test"));
+ ASSERT_TRUE(targetData.booleans().hasEntry("test2"));
+
+ ASSERT_THROW(targetData = sourceData, SurgSim::Framework::AssertionFailure);
+
+ DataGroupCopier copier(sourceData, targetData);
+
+ const RigidTransform3d testPose = SurgSim::Math::makeRigidTransform(Vector3d(1.0, 2.0, 3.0),
+ Vector3d(1.0, 0.0, 0.0), Vector3d(0.0, 1.0, 0.0));
+ ASSERT_TRUE(sourceData.poses().set("test", testPose));
+
+ const bool testBoolean = true;
+ ASSERT_TRUE(sourceData.booleans().set("test", testBoolean));
+
+ const bool testBoolean2 = false;
+ ASSERT_TRUE(sourceData.booleans().set("test2", testBoolean2));
+ // Setting the initial value for the targetData's test2 boolean to the opposite value.
+ ASSERT_TRUE(targetData.booleans().set("test2", !testBoolean2));
+
+ // Copy the data.
+ copier.copy();
+
+ RigidTransform3d outTestPose;
+ ASSERT_TRUE(targetData.poses().get("test", &outTestPose));
+ EXPECT_TRUE(outTestPose.isApprox(testPose, EPSILON));
+
+ EXPECT_FALSE(targetData.poses().hasData("test2"));
+
+ bool outTestBoolean;
+ ASSERT_TRUE(targetData.booleans().get("test", &outTestBoolean));
+ EXPECT_EQ(testBoolean, outTestBoolean);
+
+ bool outTestBoolean2;
+ ASSERT_TRUE(targetData.booleans().get("test2", &outTestBoolean2));
+ EXPECT_EQ(testBoolean2, outTestBoolean2);
+}
+
+TEST(DataGroupTests, DataGroupInLockedContainer)
+{
+ DataGroupBuilder builder;
+ builder.addBoolean("test");
+ DataGroup data = builder.createData();
+ const bool trueBool = true;
+ data.booleans().set("test", trueBool);
+ SurgSim::Framework::LockedContainer<SurgSim::DataStructures::DataGroup> lockedDataGroup;
+ DataGroup copied_data;
+ // the DataGroup in the LockedContainer was default-constructed and so is invalid (aka empty)
+ // you cannot "get" an invalid DataGroup out of the LockedContainer. "set" must be called before "get".
+ EXPECT_THROW(lockedDataGroup.get(&copied_data), SurgSim::Framework::AssertionFailure);
+
+ lockedDataGroup.set(data);
+ lockedDataGroup.get(&copied_data);
+ EXPECT_TRUE(copied_data.booleans().hasEntry("test"));
+ EXPECT_TRUE(copied_data.booleans().hasData("test"));
+ bool outBool, outCopiedBool;
+ data.booleans().get("test", &outBool);
+ copied_data.booleans().get("test", &outCopiedBool);
+ EXPECT_EQ(outBool, outCopiedBool);
+ EXPECT_EQ(trueBool, outCopiedBool);
+}
+
+TEST(DataGroupTests, IsEmpty)
+{
+ DataGroupBuilder builder;
+ builder.addBoolean("test");
+ DataGroup data;
+ EXPECT_TRUE(data.isEmpty());
+
+ data = builder.createData();
+ EXPECT_FALSE(data.isEmpty());
+
+ DataGroup data2;
+ std::vector<std::string> names;
+ names.push_back("test string");
+ data2.scalars() = SurgSim::DataStructures::NamedData<SurgSim::DataStructures::DataGroup::ScalarType>(names);
+ EXPECT_FALSE(data2.isEmpty());
+}
diff --git a/SurgSim/DataStructures/UnitTests/DataStructuresConvertTests.cpp b/SurgSim/DataStructures/UnitTests/DataStructuresConvertTests.cpp
new file mode 100644
index 0000000..63bff0a
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/DataStructuresConvertTests.cpp
@@ -0,0 +1,257 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <gtest/gtest.h>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h" // Used as a mock component
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+using SurgSim::Graphics::OsgBoxRepresentation;
+
+template <typename Type, size_t Size>
+void testStdArraySerialization(const std::array<Type, Size>& value)
+{
+ {
+ SCOPED_TRACE("Normal test");
+
+ // Encode
+ YAML::Node node;
+ EXPECT_NO_THROW(node = value;);
+
+ // Decode
+ std::array<Type, Size> newValue;
+ newValue = node.as<std::array<Type, Size>>();
+
+ // Verify
+ for (size_t i = 0; i < Size; ++i)
+ {
+ EXPECT_EQ(value[i], newValue[i]);
+ }
+ }
+
+ {
+ SCOPED_TRACE("Decode into smaller array");
+
+ typedef std::array<Type, Size - 1> SmallerArray;
+
+ // Encode
+ YAML::Node node;
+ EXPECT_NO_THROW(node = value;);
+
+ // Try decoding into a smaller array.
+ SmallerArray smallerNewValue;
+ EXPECT_ANY_THROW(smallerNewValue = node.as<SmallerArray>());
+ }
+
+ {
+ SCOPED_TRACE("Decode into larger array");
+
+ typedef std::array<Type, Size + 1> LargerArray;
+
+ // Encode
+ YAML::Node node;
+ EXPECT_NO_THROW(node = value;);
+
+ // Try decoding into a larger array.
+ LargerArray largerNewValue;
+ EXPECT_ANY_THROW(largerNewValue = node.as<LargerArray>());
+ }
+}
+
+TEST(DataStructuresConvertTests, StdArray)
+{
+ {
+ SCOPED_TRACE("Serialization of std::array of size 3");
+ std::array<double, 3> doubleArray;
+ doubleArray[0] = 534.34;
+ doubleArray[1] = 0.8435e56;
+ doubleArray[2] = -56754.3;
+ testStdArraySerialization(doubleArray);
+ }
+
+ {
+ SCOPED_TRACE("Serialization of std::array of size 0");
+
+ typedef std::array<double, 0> ZeroSizeArray;
+
+ ZeroSizeArray doubleEmptyArray;
+
+ // Encode
+ YAML::Node node;
+ EXPECT_NO_THROW(node = doubleEmptyArray;);
+
+ // Decode
+ ZeroSizeArray newValue;
+ EXPECT_NO_THROW(newValue = node.as<ZeroSizeArray>(););
+ }
+}
+
+TEST(DataStructuresConvertTests, StdUnorderedMapTests)
+{
+ {
+ SCOPED_TRACE("Serialization of std::unordered_map with double as key and integer as values");
+ typedef std::unordered_map<double, int> TestMapType;
+ TestMapType originalMap;
+
+ originalMap.insert(TestMapType::value_type(1.0, 2));
+ originalMap.insert(TestMapType::value_type(3.0, 4));
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = originalMap);
+ EXPECT_EQ(2u, node.size());
+
+ TestMapType newMap;
+ EXPECT_NO_THROW(newMap = node.as<TestMapType>());
+
+ EXPECT_EQ(originalMap, newMap);
+ }
+
+ {
+ SCOPED_TRACE("Serialization of std::unordered_map with integer as key and std::shared_ptr<> as values");
+ typedef std::unordered_map<int, std::shared_ptr<OsgBoxRepresentation>> TestMapType;
+ TestMapType originalMap;
+
+ auto mockComponent = std::make_shared<OsgBoxRepresentation>("OsgBoxRepresentation");
+ auto mockComponent2 = std::make_shared<OsgBoxRepresentation>("MockComponent2");
+
+ originalMap.insert(TestMapType::value_type(1, mockComponent));
+ originalMap.insert(TestMapType::value_type(2, mockComponent2));
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = originalMap);
+ EXPECT_EQ(2u, node.size());
+
+ TestMapType newMap;
+ EXPECT_NO_THROW(newMap = node.as<TestMapType>());
+
+ EXPECT_EQ(originalMap.size(), newMap.size());
+ for (auto it = std::begin(originalMap); it != std::end(originalMap); ++it)
+ {
+ auto result = std::find_if(std::begin(newMap), std::end(newMap),
+ [&it](const std::pair<int, std::shared_ptr<OsgBoxRepresentation>>& pair)
+ {
+ return it->second->getName() == pair.second->getName();
+ });
+ EXPECT_NE(std::end(newMap), result);
+ }
+ }
+
+ {
+ SCOPED_TRACE("Serialization of std::unordered_map with integer as key and std::unordered_set<> as values");
+ typedef std::unordered_map<int, std::unordered_set<std::shared_ptr<OsgBoxRepresentation>>> TestMapType2;
+ TestMapType2 originalMap;
+
+ std::unordered_set<std::shared_ptr<OsgBoxRepresentation>> set1;
+ std::unordered_set<std::shared_ptr<OsgBoxRepresentation>> set2;
+
+ auto mockComponent = std::make_shared<OsgBoxRepresentation>("OsgBoxRepresentation");
+ auto mockComponent2 = std::make_shared<OsgBoxRepresentation>("MockComponent2");
+ auto mockComponent3 = std::make_shared<OsgBoxRepresentation>("MockComponent3");
+
+ set1.insert(mockComponent);
+ set2.insert(mockComponent2);
+ set2.insert(mockComponent3);
+
+ originalMap.insert(TestMapType2::value_type(1, set1));
+ originalMap.insert(TestMapType2::value_type(2, set2));
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = originalMap);
+ EXPECT_EQ(2u, node.size());
+
+ TestMapType2 newMap;
+ EXPECT_NO_THROW(newMap = node.as<TestMapType2>());
+
+ EXPECT_EQ(originalMap.size(), newMap.size());
+ for (auto it = std::begin(originalMap); it != std::end(originalMap); ++it)
+ {
+ auto representationSet = newMap.find(it->first)->second;
+ EXPECT_EQ(it->second.size(), representationSet.size());
+ for (auto item = std::begin(it->second); item != std::end(it->second); ++item)
+ {
+ auto match = std::find_if(std::begin(representationSet), std::end(representationSet),
+ [&item](const std::shared_ptr<OsgBoxRepresentation> rep)
+ {
+ return rep->getName() == (*item)->getName();
+ });
+ EXPECT_NE(std::end(representationSet), match);
+ }
+ }
+ }
+}
+
+
+TEST(DataStructuresConvertTests, StdUnorderedSetTests)
+{
+ {
+ SCOPED_TRACE("Serialization of std::unordered_set<> of integers");
+ typedef std::unordered_set<int> TestSetType;
+ TestSetType originalSet;
+ originalSet.insert(1);
+ originalSet.insert(2);
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = originalSet);
+ EXPECT_EQ(2u, node.size());
+
+ TestSetType newSet;
+ EXPECT_NO_THROW(newSet = node.as<TestSetType>());
+
+ EXPECT_EQ(originalSet, newSet);
+ }
+
+ {
+ SCOPED_TRACE("Serialization of std::unordered_set<> of std::shared_ptr<>");
+ typedef std::unordered_set<std::shared_ptr<OsgBoxRepresentation>> TestSetType;
+ TestSetType originalSet;
+
+ auto mockComponent = std::make_shared<OsgBoxRepresentation>("OsgBoxRepresentation");
+ auto mockComponent2 = std::make_shared<OsgBoxRepresentation>("MockComponent2");
+
+ originalSet.insert(mockComponent);
+ originalSet.insert(mockComponent2);
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = originalSet);
+ EXPECT_EQ(2u, node.size());
+
+ TestSetType newSet;
+ EXPECT_NO_THROW(newSet = node.as<TestSetType>());
+
+ EXPECT_EQ(originalSet.size(), newSet.size());
+ for (auto it = std::begin(originalSet); it != std::end(originalSet); ++it)
+ {
+ auto result = std::find_if(std::begin(newSet), std::end(newSet),
+ [&it](const std::shared_ptr<OsgBoxRepresentation>& item)
+ {
+ return (*it)->getName() == item->getName();
+ });
+ EXPECT_NE(std::end(newSet), result);
+ }
+ }
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/DataStructures/UnitTests/ImageTest.cpp b/SurgSim/DataStructures/UnitTests/ImageTest.cpp
new file mode 100644
index 0000000..461b25f
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/ImageTest.cpp
@@ -0,0 +1,212 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Image class.
+
+#include "gtest/gtest.h"
+
+#include "SurgSim/DataStructures/Image.h"
+#include "SurgSim/Math/Matrix.h"
+
+namespace
+{
+double epsilon = 1e-10;
+}
+
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <class T>
+class ImageTests : public testing::Test
+{
+public:
+ typedef T Scalar;
+};
+
+typedef ::testing::Types<unsigned char, char, unsigned int, int, float, double> ImageTestTypes;
+TYPED_TEST_CASE(ImageTests, ImageTestTypes);
+
+TYPED_TEST(ImageTests, Construct)
+{
+ typedef typename TestFixture::Scalar T;
+
+ ASSERT_NO_THROW({Image<T> image;});
+ ASSERT_NO_THROW({Image<T> image(10, 10, 1);});
+ ASSERT_NO_THROW({Image<T> image(100, 10, 3);});
+ ASSERT_NO_THROW({Image<T> image(512, 1024, 4);});
+
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ ASSERT_NO_THROW({Image<T> image(3, 3, 1, array);});
+}
+
+TYPED_TEST(ImageTests, Copy)
+{
+ typedef typename TestFixture::Scalar T;
+ Image<T> image(10, 10, 1);
+ Image<T> newImage(image);
+ EXPECT_EQ(image.getSize(), newImage.getSize());
+ EXPECT_NE(image.getData(), newImage.getData());
+}
+
+TYPED_TEST(ImageTests, Assign)
+{
+ typedef typename TestFixture::Scalar T;
+ Image<T> image(10, 10, 1);
+
+ Image<T> newImage;
+ newImage = image;
+
+ EXPECT_EQ(image.getSize(), newImage.getSize());
+ EXPECT_NE(image.getData(), newImage.getData());
+}
+
+TYPED_TEST(ImageTests, Accessors)
+{
+ typedef typename TestFixture::Scalar T;
+ {
+ Image<T> image;
+ EXPECT_EQ(0, image.getWidth());
+ EXPECT_EQ(0, image.getHeight());
+ EXPECT_EQ(0, image.getNumChannels());
+ EXPECT_EQ(nullptr, image.getData());
+
+ std::array<size_t, 3> size = {0, 0, 0};
+ EXPECT_EQ(size, image.getSize());
+ }
+ {
+ Image<T> image(10, 20, 30);
+ EXPECT_EQ(10, image.getWidth());
+ EXPECT_EQ(20, image.getHeight());
+ EXPECT_EQ(30, image.getNumChannels());
+
+ std::array<size_t, 3> size = {10, 20, 30};
+ EXPECT_EQ(size, image.getSize());
+ }
+ {
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ Image<T> image(3, 3, 1, array);
+ EXPECT_EQ(3, image.getWidth());
+ EXPECT_EQ(3, image.getHeight());
+ EXPECT_EQ(1, image.getNumChannels());
+
+ std::array<size_t, 3> size = {3, 3, 1};
+ EXPECT_EQ(size, image.getSize());
+
+ for (int i = 0; i < 9; i++)
+ {
+ EXPECT_NEAR(array[i], image.getData()[i], epsilon);
+ }
+ }
+}
+
+TYPED_TEST(ImageTests, Move)
+{
+ typedef typename TestFixture::Scalar T;
+
+ {
+ Image<T> oldImage(3, 3, 1);
+ T* const dataPtr = oldImage.getData();
+ Image<T> newImage = std::move(oldImage);
+
+ EXPECT_EQ(nullptr, oldImage.getData());
+ EXPECT_NE(dataPtr, oldImage.getData());
+ EXPECT_EQ(0, oldImage.getWidth());
+ EXPECT_EQ(0, oldImage.getHeight());
+ EXPECT_EQ(0, oldImage.getNumChannels());
+
+ EXPECT_EQ(dataPtr, newImage.getData());
+ EXPECT_EQ(3, newImage.getWidth());
+ EXPECT_EQ(3, newImage.getHeight());
+ EXPECT_EQ(1, newImage.getNumChannels());
+ }
+ {
+ Image<T> oldImage(15, 25, 4);
+ T* const dataPtr = oldImage.getData();
+ Image<T> newImage(std::move(oldImage));
+
+ EXPECT_EQ(nullptr, oldImage.getData());
+ EXPECT_EQ(0, oldImage.getWidth());
+ EXPECT_EQ(0, oldImage.getHeight());
+ EXPECT_EQ(0, oldImage.getNumChannels());
+
+ EXPECT_EQ(dataPtr, newImage.getData());
+ EXPECT_EQ(15, newImage.getWidth());
+ EXPECT_EQ(25, newImage.getHeight());
+ EXPECT_EQ(4, newImage.getNumChannels());
+ }
+}
+
+TYPED_TEST(ImageTests, PointerAccess)
+{
+ typedef typename TestFixture::Scalar T;
+
+ Image<T> image(3, 3, 1);
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ std::copy(array, array+9, image.getData());
+
+ for (int i = 0; i < 9; i++)
+ {
+ EXPECT_NEAR(array[i], image.getData()[i], epsilon);
+ }
+}
+
+TYPED_TEST(ImageTests, EigenAccess)
+{
+ typedef typename TestFixture::Scalar T;
+ typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
+
+ {
+ Image<T> image(50, 1000, 2);
+ EXPECT_NO_THROW(image.getChannel(0));
+ EXPECT_NO_THROW(image.getChannel(1));
+ EXPECT_THROW(image.getChannel(2), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(image.getChannel(100), SurgSim::Framework::AssertionFailure);
+ }
+ {
+ Image<T> image(300, 300, 3);
+ image.getChannel(0) = Matrix::Constant(300, 300, T(0));
+ image.getChannel(1) = Matrix::Constant(300, 300, T(1));
+ image.getChannel(2) = Matrix::Constant(300, 300, T(2));
+
+ for (int i = 0; i < 300*300; i++)
+ {
+ EXPECT_NEAR(i % 3, image.getData()[i], epsilon);
+ }
+
+ Matrix total = image.getChannel(0) + image.getChannel(1) + image.getChannel(2);
+ EXPECT_TRUE(total.isApprox(Matrix::Constant(300, 300, T(3))));
+ }
+ {
+ Image<T> image(6, 6, 1);
+ image.getChannel(0) << 0, 1, 2, 3, 4, 5,
+ 10, 11, 12, 13, 14, 15,
+ 20, 21, 22, 23, 24, 25,
+ 30, 31, 32, 33, 34, 35,
+ 30, 41, 42, 43, 44, 45,
+ 50, 51, 52, 53, 54, 55;
+ Matrix matrix = image.getChannel(0);
+ EXPECT_NEAR(24, matrix(2, 4), epsilon);
+ EXPECT_NEAR(15, matrix(1, 5), epsilon);
+ EXPECT_NEAR(30, matrix(3, 0), epsilon);
+ EXPECT_NEAR(54, matrix(5, 4), epsilon);
+ }
+}
+
+}
+}
diff --git a/SurgSim/DataStructures/UnitTests/IndexDirectoryTests.cpp b/SurgSim/DataStructures/UnitTests/IndexDirectoryTests.cpp
new file mode 100644
index 0000000..ba79403
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/IndexDirectoryTests.cpp
@@ -0,0 +1,107 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 IndexDirectory class.
+
+#include "SurgSim/DataStructures/NamedDataBuilder.h"
+#include "SurgSim/DataStructures/IndexDirectory.h"
+#include "gtest/gtest.h"
+
+using SurgSim::DataStructures::NamedDataBuilder;
+using SurgSim::DataStructures::IndexDirectory;
+
+
+/// Run a few tests against an empty index directory.
+TEST(IndexDirectoryTests, EmptyTests)
+{
+ {
+ IndexDirectory dir;
+
+ EXPECT_EQ(0, dir.getNumEntries());
+ EXPECT_EQ("", dir.getName(0));
+
+ EXPECT_EQ(-1, dir.getIndex("missing"));
+ EXPECT_FALSE(dir.hasEntry("missing"));
+ }
+ {
+ NamedDataBuilder<float> builder;
+
+ EXPECT_EQ(0, builder.getNumEntries());
+ EXPECT_EQ("", builder.getName(0));
+
+ EXPECT_EQ(-1, builder.getIndex("missing"));
+ EXPECT_FALSE(builder.hasEntry("missing"));
+
+ std::shared_ptr<const IndexDirectory> dir = builder.createData().getDirectory();
+
+ EXPECT_EQ(0, dir->getNumEntries());
+ EXPECT_EQ("", dir->getName(0));
+
+ EXPECT_EQ(-1, dir->getIndex("missing"));
+ EXPECT_FALSE(dir->hasEntry("missing"));
+ }
+}
+
+/// Populate an index directory and run a few tests against that.
+TEST(IndexDirectoryTests, SmallDirectoryTests)
+{
+ NamedDataBuilder<float> builder;
+ builder.addEntry("first");
+ builder.addEntry("second");
+ std::shared_ptr<const IndexDirectory> dir = builder.createData().getDirectory();
+
+ EXPECT_EQ(2, dir->getNumEntries());
+ EXPECT_EQ(2u, dir->size());
+
+ EXPECT_EQ("first", dir->getName(0));
+ EXPECT_EQ(0, dir->getIndex("first"));
+ EXPECT_TRUE(dir->hasEntry("first"));
+
+ EXPECT_EQ("second", dir->getName(1));
+ EXPECT_EQ(1, dir->getIndex("second"));
+ EXPECT_TRUE(dir->hasEntry("second"));
+
+ EXPECT_EQ(-1, dir->getIndex("missing"));
+ EXPECT_FALSE(dir->hasEntry("missing"));
+}
+
+/// Check return values from populating an index directory.
+TEST(IndexDirectoryTests, ReturnValueFromAdd)
+{
+ NamedDataBuilder<float> builder;
+ EXPECT_EQ(-1, builder.addEntry(""));
+ EXPECT_EQ(0, builder.addEntry("entry"));
+ EXPECT_EQ(-1, builder.addEntry("entry"));
+
+ // Also check that the failed calls did not increase the number of entries:
+ EXPECT_EQ(1, builder.getNumEntries());
+ EXPECT_EQ(1, builder.createData().getDirectory()->getNumEntries());
+}
+
+/// Try passing bad key/index values.
+TEST(IndexDirectoryTests, BadKeyOrIndex)
+{
+ NamedDataBuilder<float> builder;
+ EXPECT_EQ(0, builder.addEntry("first"));
+ EXPECT_EQ(1, builder.addEntry("second"));
+ EXPECT_EQ(-1, builder.addEntry(""));
+
+ std::shared_ptr<const IndexDirectory> dir = builder.createData().getDirectory();
+ EXPECT_EQ("", dir->getName(-1));
+ EXPECT_EQ("", dir->getName(-12345));
+ EXPECT_EQ("", dir->getName(+56789));
+ EXPECT_EQ("", dir->getName(2));
+}
\ No newline at end of file
diff --git a/SurgSim/DataStructures/UnitTests/IndexedLocalCoordinateTest.cpp b/SurgSim/DataStructures/UnitTests/IndexedLocalCoordinateTest.cpp
new file mode 100644
index 0000000..06ad886
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/IndexedLocalCoordinateTest.cpp
@@ -0,0 +1,65 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+TEST(IndexedLocalCoordinateTest, IndexedLocalCoordinate)
+{
+ using SurgSim::Math::Vector4d;
+
+ EXPECT_NO_THROW({
+ IndexedLocalCoordinate coord;
+ });
+
+ EXPECT_NO_THROW({
+ IndexedLocalCoordinate coord(6u, Vector4d(0.25, 0.55, 0.73, 0.11));
+ });
+
+ {
+ IndexedLocalCoordinate coord(6u, Vector4d(0.25, 0.55, 0.73, 0.11));
+ EXPECT_EQ(6u, coord.index);
+ EXPECT_TRUE(Vector4d(0.25, 0.55, 0.73, 0.11).isApprox(coord.coordinate));
+ }
+
+ {
+ IndexedLocalCoordinate coord;
+ coord.index = 12u;
+ coord.coordinate = Vector4d(0.33, 0.1, 0.05, 0.99);
+ EXPECT_EQ(12u, coord.index);
+ EXPECT_TRUE(Vector4d(0.33, 0.1, 0.05, 0.99).isApprox(coord.coordinate));
+ }
+
+ {
+ IndexedLocalCoordinate coord0;
+ coord0.index = 0u;
+ SurgSim::Math::Vector cubeNodes(8);
+ cubeNodes << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+
+ coord0.coordinate = cubeNodes;
+ EXPECT_EQ(0u, coord0.index);
+ EXPECT_TRUE(cubeNodes.isApprox(coord0.coordinate));
+ }
+}
+
+} // namespace Collision
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/DataStructures/UnitTests/LocationTests.cpp b/SurgSim/DataStructures/UnitTests/LocationTests.cpp
new file mode 100644
index 0000000..bb08f2b
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/LocationTests.cpp
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/Location.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+TEST(LocationTests, Constructor)
+{
+ SurgSim::Math::Vector3d rigidLocalPosition = SurgSim::Math::Vector3d::Ones();
+ SurgSim::DataStructures::OctreePath octreeNodePath;
+ octreeNodePath.push_back(1);
+ octreeNodePath.push_back(2);
+ octreeNodePath.push_back(3);
+ SurgSim::DataStructures::IndexedLocalCoordinate meshLocalCoordinate(1, SurgSim::Math::Vector2d(4.0, 5.0));
+
+ EXPECT_NO_THROW({Location location(rigidLocalPosition);});
+ EXPECT_NO_THROW({Location location(octreeNodePath);});
+ EXPECT_NO_THROW({Location location(meshLocalCoordinate);});
+
+ {
+ SCOPED_TRACE("Using rigid local position");
+ Location location(rigidLocalPosition);
+ EXPECT_FALSE(location.meshLocalCoordinate.hasValue());
+ EXPECT_FALSE(location.octreeNodePath.hasValue());
+ EXPECT_TRUE(location.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(location.rigidLocalPosition.getValue().isApprox(rigidLocalPosition));
+ }
+
+ {
+ SCOPED_TRACE("Using octree node path");
+ Location location(octreeNodePath);
+ EXPECT_FALSE(location.meshLocalCoordinate.hasValue());
+ EXPECT_TRUE(location.octreeNodePath.hasValue());
+ EXPECT_FALSE(location.rigidLocalPosition.hasValue());
+ EXPECT_EQ(octreeNodePath, location.octreeNodePath.getValue());
+ }
+
+ {
+ SCOPED_TRACE("Using mesh local coordinate");
+ Location location(meshLocalCoordinate);
+ EXPECT_TRUE(location.meshLocalCoordinate.hasValue());
+ EXPECT_FALSE(location.octreeNodePath.hasValue());
+ EXPECT_FALSE(location.rigidLocalPosition.hasValue());
+ EXPECT_EQ(meshLocalCoordinate.index, location.meshLocalCoordinate.getValue().index);
+ EXPECT_TRUE(location.meshLocalCoordinate.getValue().coordinate.isApprox(meshLocalCoordinate.coordinate));
+ }
+}
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/UnitTests/MeshElementTest.cpp b/SurgSim/DataStructures/UnitTests/MeshElementTest.cpp
new file mode 100644
index 0000000..b5168d6
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/MeshElementTest.cpp
@@ -0,0 +1,145 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MeshElement class.
+/// Edges and Triangles are used as example elements for testing.
+
+#include "gtest/gtest.h"
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/MeshElement.h"
+#include "SurgSim/DataStructures/UnitTests/MockObjects.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::Math::Vector3d;
+
+/// Edge element with ID data.
+typedef SurgSim::DataStructures::MeshElement<2, MockEdgeData> MockEdge;
+/// Triangle element with ID and edge ID data.
+typedef SurgSim::DataStructures::MeshElement<3, MockTriangleData> MockTriangle;
+/// Tetrahedron element with no data
+typedef SurgSim::DataStructures::MeshElement<4, EmptyData> MockTetrahedron;
+
+TEST(MeshElementTest, InitTest)
+{
+ std::array<size_t, 2> edgeVertices = {{0, 1}};
+ MockEdgeData edgeData(0);
+ ASSERT_NO_THROW({MockEdge edge(edgeVertices, edgeData);});
+
+ std::array<size_t, 3> triangleVertices = {{0, 1, 2}};
+ std::array<size_t, 3> triangleEdges = {{0, 1, 2}};
+ MockTriangleData triangleData(0, triangleEdges);
+ ASSERT_NO_THROW({MockTriangle triangle(triangleVertices, triangleData);});
+
+ std::array<size_t, 4> tetrahedronVertices = {{0, 1, 2, 3}};
+ ASSERT_NO_THROW({MockTetrahedron triangle(tetrahedronVertices);});
+}
+
+TEST(MeshElementTest, EdgeTest)
+{
+ std::array<size_t, 2> edgeVertices = {{2, 10}};
+ MockEdgeData edgeData(5);
+ MockEdge edge(edgeVertices, edgeData);
+
+ EXPECT_EQ(edgeVertices, edge.verticesId);
+ EXPECT_EQ(edgeData, edge.data);
+
+ {
+ const MockEdgeData& data = edge.data;
+ EXPECT_EQ(5u, data.getId());
+ }
+
+ /// Check comparisons
+
+ MockEdge sameEdge(edgeVertices, edgeData);
+ EXPECT_TRUE(edge == sameEdge);
+ EXPECT_FALSE(edge != sameEdge);
+
+ std::array<size_t, 2> differentEdgeVertices = {{10, 5}};
+ MockEdgeData differentEdgeData(7);
+
+ MockEdge edgeWithDifferentVertices(differentEdgeVertices, edgeData);
+ EXPECT_FALSE(edge == edgeWithDifferentVertices);
+ EXPECT_TRUE(edge != edgeWithDifferentVertices);
+
+ MockEdge edgeWithDifferentData(edgeVertices, differentEdgeData);
+ EXPECT_FALSE(edge == edgeWithDifferentData);
+ EXPECT_TRUE(edge != edgeWithDifferentData);
+
+ MockEdge edgeWithDifferentVerticesAndData(differentEdgeVertices, differentEdgeData);
+ EXPECT_FALSE(edge == edgeWithDifferentVerticesAndData);
+ EXPECT_TRUE(edge != edgeWithDifferentVerticesAndData);
+}
+
+TEST(MeshElementTest, TriangleTest)
+{
+ std::array<size_t, 3> triangleVertices = {{5, 2, 10}};
+ std::array<size_t, 3> triangleEdges = {{0, 1, 2}};
+ MockTriangleData triangleData(4, triangleEdges);
+ MockTriangle triangle(triangleVertices, triangleData);
+
+ EXPECT_EQ(triangleVertices, triangle.verticesId);
+ EXPECT_EQ(triangleData, triangle.data);
+
+ {
+ const MockTriangleData& data = triangle.data;
+ EXPECT_EQ(4u, data.getId());
+ EXPECT_EQ(triangleEdges, data.getEdges());
+ }
+
+ /// Check comparisons
+
+ MockTriangle sameTriangle(triangleVertices, triangleData);
+ EXPECT_TRUE(triangle == sameTriangle);
+ EXPECT_FALSE(triangle != sameTriangle);
+
+ std::array<size_t, 3> differentTriangleVertices = {{10, 5, 7}};
+ std::array<size_t, 3> differentTriangleEdges = {{2, 1, 3}};
+ MockTriangleData differentTriangleData(4, differentTriangleEdges);
+
+ MockTriangle triangleWithDifferentVertices(differentTriangleVertices, triangleData);
+ EXPECT_FALSE(triangle == triangleWithDifferentVertices);
+ EXPECT_TRUE(triangle != triangleWithDifferentVertices);
+
+ MockTriangle triangleWithDifferentData(triangleVertices, differentTriangleData);
+ EXPECT_FALSE(triangle == triangleWithDifferentData);
+ EXPECT_TRUE(triangle != triangleWithDifferentData);
+
+ MockTriangle triangleWithDifferentVerticesAndData(differentTriangleVertices, differentTriangleData);
+ EXPECT_FALSE(triangle == triangleWithDifferentVerticesAndData);
+ EXPECT_TRUE(triangle != triangleWithDifferentVerticesAndData);
+}
+
+TEST(MeshElementTest, TetrahedronTest)
+{
+ std::array<size_t, 4> tetrahedronVertices = {{5, 2, 10, 6}};
+ MockTetrahedron tetrahedron(tetrahedronVertices);
+
+ EXPECT_EQ(tetrahedronVertices, tetrahedron.verticesId);
+
+ /// Check comparisons
+
+ MockTetrahedron sameTetrahedron(tetrahedronVertices);
+ EXPECT_TRUE(tetrahedron == sameTetrahedron);
+ EXPECT_FALSE(tetrahedron != sameTetrahedron);
+
+ std::array<size_t, 4> differentTetrahedronVertices = {{10, 5, 7, 3}};
+
+ MockTetrahedron tetrahedronWithDifferentVertices(differentTetrahedronVertices);
+ EXPECT_FALSE(tetrahedron == tetrahedronWithDifferentVertices);
+ EXPECT_TRUE(tetrahedron != tetrahedronWithDifferentVertices);
+}
\ No newline at end of file
diff --git a/SurgSim/DataStructures/UnitTests/MeshTest.cpp b/SurgSim/DataStructures/UnitTests/MeshTest.cpp
new file mode 100644
index 0000000..52a2322
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/MeshTest.cpp
@@ -0,0 +1,274 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Mesh class.
+
+#include "gtest/gtest.h"
+
+#include "SurgSim/DataStructures/MeshElement.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/UnitTests/MockObjects.h"
+#include "SurgSim/DataStructures/Vertex.h"
+
+
+#include <random>
+
+using SurgSim::DataStructures::Vertices;
+using SurgSim::DataStructures::Vertex;
+using SurgSim::Math::Vector3d;
+
+
+class MeshTests : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // Set to true to print the test positions.
+ bool printPositions = false;
+ // Set to true to print the test normals.
+ bool printNormals = false;
+ // Set the number of test vertices
+ size_t numVertices = 10;
+
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> positionDistribution(-10.0, 10.0);
+ std::uniform_real_distribution<double> normalDistribution(-1.0, 1.0);
+
+ if (printPositions)
+ {
+ std::cout << "Test Positions:\n";
+ }
+
+ /// Generate random positions for each vertex
+ for (size_t i = 0; i < numVertices; ++i)
+ {
+ Vector3d position(positionDistribution(generator), positionDistribution(generator),
+ positionDistribution(generator));
+ testPositions.push_back(position);
+
+ if (printPositions)
+ {
+ std::cout << "\t" << i << ": (" << position.x() << ", " << position.y() << ", " << position.z()
+ << ")\n";
+ }
+ }
+
+ if (printNormals)
+ {
+ std::cout << "Test Normals:\n";
+ }
+
+ /// Generate random normals for each vertex
+ for (size_t i = 0; i < numVertices; ++i)
+ {
+ Vector3d normal(normalDistribution(generator), normalDistribution(generator),
+ normalDistribution(generator));
+ normal.normalize();
+ testNormals.push_back(normal);
+
+ if (printNormals)
+ {
+ std::cout << "\t" << i << ": (" << normal.x() << ", " << normal.y() << ", " << normal.z() << ")\n";
+ }
+ }
+
+ }
+
+ void TearDown()
+ {
+
+ }
+
+ /// Positions of test vertices
+ std::vector<Vector3d> testPositions;
+ /// Normals of test vertices
+ std::vector<Vector3d> testNormals;
+};
+
+
+TEST_F(MeshTests, InitTest)
+{
+ ASSERT_NO_THROW({MockMesh mesh;});
+
+ /// Check that we can also create a mesh with no data
+ ASSERT_NO_THROW({Vertices<void> mesh;});
+}
+
+TEST_F(MeshTests, CreateVerticesTest)
+{
+ MockMesh mesh;
+
+ EXPECT_EQ(0u, mesh.getNumVertices());
+ EXPECT_EQ(0u, mesh.getVertices().size());
+
+ /// Create the test vertices
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(testPositions[i], testNormals[i]));
+ EXPECT_EQ(i + 1, mesh.getNumVertices());
+
+ const std::vector<MockMesh::VertexType>& vertices = mesh.getVertices();
+ EXPECT_EQ(i + 1, vertices.size());
+
+ /// Make sure each vertex is set properly
+ for (size_t j = 0; j < mesh.getNumVertices(); ++j)
+ {
+ EXPECT_EQ(testPositions[j], vertices[j].position);
+
+ const MockVertexData& data = vertices[j].data;
+ EXPECT_EQ(j, data.getId());
+ EXPECT_EQ(testNormals[j], data.getNormal());
+ }
+ }
+}
+
+TEST_F(MeshTests, SetVertexPositionsTest)
+{
+ MockMesh mesh;
+
+ /// Create vertices with test normals, but all positions at (0,0,0)
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(Vector3d(0.0, 0.0, 0.0), testNormals[i]));
+ EXPECT_EQ(i + 1, mesh.getNumVertices());
+ }
+
+ mesh.setVertexPositions(testPositions);
+
+ EXPECT_EQ(1, mesh.getNumUpdates());
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+
+ const std::vector<MockMesh::VertexType>& vertices = mesh.getVertices();
+ EXPECT_EQ(testPositions.size(), vertices.size());
+
+ /// Make sure each vertex is set properly
+ for (size_t i = 0; i < mesh.getNumVertices(); ++i)
+ {
+ EXPECT_EQ(testPositions[i], vertices[i].position);
+
+ const MockVertexData& data = vertices[i].data;
+ EXPECT_EQ(testNormals[i], data.getNormal());
+ }
+
+ mesh.setVertexPositions(testPositions, false);
+
+ EXPECT_EQ(1, mesh.getNumUpdates());
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+ EXPECT_EQ(testPositions.size(), mesh.getVertices().size());
+
+ mesh.setVertexPositions(testPositions, true);
+
+ EXPECT_EQ(2, mesh.getNumUpdates());
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+ EXPECT_EQ(testPositions.size(), mesh.getVertices().size());
+
+ /// Test the individual set/get methods
+ mesh.setVertexPosition(5, Vector3d(0.0, 0.0, 0.0));
+
+ /// Make sure each vertex is set properly
+ for (size_t i = 0; i < mesh.getNumVertices(); ++i)
+ {
+ if (i == 5)
+ {
+ EXPECT_EQ(Vector3d(0.0, 0.0, 0.0), mesh.getVertexPosition(i));
+ EXPECT_EQ(testNormals[i], mesh.getVertexNormal(i));
+ }
+ else
+ {
+ EXPECT_EQ(testPositions[i], mesh.getVertexPosition(i));
+ EXPECT_EQ(testNormals[i], mesh.getVertexNormal(i));
+ }
+ }
+
+ /// Try setting with wrong number of vertices
+ mesh.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)); // create one more vertex
+
+ EXPECT_ANY_THROW(mesh.setVertexPositions(testPositions));
+}
+
+TEST_F(MeshTests, ClearTest)
+{
+ MockMesh mesh;
+
+ EXPECT_EQ(0, mesh.getNumUpdates());
+ EXPECT_EQ(0u, mesh.getNumVertices());
+ EXPECT_EQ(0u, mesh.getVertices().size());
+
+ /// Create vertices
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)));
+ EXPECT_EQ(i + 1, mesh.getNumVertices());
+ }
+
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+ EXPECT_EQ(testPositions.size(), mesh.getVertices().size());
+
+ /// Clear mesh
+ mesh.clear();
+
+ EXPECT_EQ(0u, mesh.getNumVertices());
+ EXPECT_EQ(0u, mesh.getVertices().size());
+}
+
+TEST_F(MeshTests, UpdateTest)
+{
+ MockMesh mesh;
+
+ EXPECT_EQ(0, mesh.getNumUpdates());
+
+ for (int i = 0; i < 10; ++i)
+ {
+ mesh.update();
+ EXPECT_EQ(i + 1, mesh.getNumUpdates());
+ }
+}
+
+TEST_F(MeshTests, ComparisonTest)
+{
+ MockMesh mesh;
+
+ /// Create vertices using test positions and normals
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(testPositions[i], testNormals[i]));
+ }
+
+ MockMesh sameMesh;
+
+ /// Create same mesh again
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, sameMesh.createVertex(testPositions[i], testNormals[i]));
+ }
+
+ MockMesh differentMesh;
+
+ /// Create vertices, each with position and normal of (0,0,0)
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, differentMesh.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)));
+ }
+
+ /// Test comparisons
+ EXPECT_TRUE(mesh == sameMesh);
+ EXPECT_FALSE(mesh != sameMesh);
+
+ EXPECT_FALSE(mesh == differentMesh);
+ EXPECT_TRUE(mesh != differentMesh);
+}
+
+
diff --git a/SurgSim/DataStructures/UnitTests/MeshVertexTest.cpp b/SurgSim/DataStructures/UnitTests/MeshVertexTest.cpp
new file mode 100644
index 0000000..b0b79b9
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/MeshVertexTest.cpp
@@ -0,0 +1,65 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Mesh class.
+
+#include "gtest/gtest.h"
+
+#include "SurgSim/DataStructures/Vertex.h"
+#include "SurgSim/DataStructures/UnitTests/MockObjects.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Vertex;
+using SurgSim::Math::Vector3d;
+
+/// Vertex with ID and normal data
+typedef SurgSim::DataStructures::Vertex<MockVertexData> MockVertex;
+/// Vertex with no data
+typedef SurgSim::DataStructures::Vertex<void> MockVertexNoData;
+
+TEST(MeshVertexTest, InitTest)
+{
+ Vector3d position(0.0, 0.0, 0.0);
+ MockVertexData data(0, Vector3d(1.0, 0.0, 0.0));
+ ASSERT_NO_THROW({MockVertex vertex(position, data);});
+
+ /// Check that we can also create a vertex with no data
+ ASSERT_NO_THROW({MockVertexNoData vertex(position);});
+}
+
+TEST(MeshVertexTest, VertexTest)
+{
+ Vector3d position(1.0, 2.0, 3.0);
+ MockVertexData data(2, Vector3d(1.0, 0.0, 0.0));
+ MockVertex vertex(position, data);
+
+ EXPECT_EQ(position, vertex.position);
+ EXPECT_EQ(data, vertex.data);
+
+ {
+ const MockVertexData& data = vertex.data;
+ EXPECT_EQ(2u, data.getId());
+ EXPECT_EQ(Vector3d(1.0, 0.0, 0.0), data.getNormal());
+ }
+}
+
+TEST(MeshVertexTest, VertexNoDataTest)
+{
+ Vector3d position(1.0, 2.0, 3.0);
+ MockVertexNoData vertex(position);
+
+ EXPECT_EQ(position, vertex.position);
+}
\ No newline at end of file
diff --git a/SurgSim/DataStructures/UnitTests/MockObjects.h b/SurgSim/DataStructures/UnitTests/MockObjects.h
new file mode 100644
index 0000000..26ec965
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/MockObjects.h
@@ -0,0 +1,524 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_UNITTESTS_MOCKOBJECTS_H
+#define SURGSIM_DATASTRUCTURES_UNITTESTS_MOCKOBJECTS_H
+
+#include "SurgSim/DataStructures/TetrahedronMesh.h"
+#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <array>
+#include <limits>
+
+template<typename T>
+class Mock3DData
+{
+public:
+ Mock3DData() :
+ m_height(0),
+ m_width(0),
+ m_depth(0),
+ m_buffer(0)
+ {
+ }
+
+ Mock3DData(int height, int width, int depth) :
+ m_height(height),
+ m_width(width),
+ m_depth(depth),
+ m_buffer(height*depth*width)
+ {
+ }
+
+ T get(int i, int j, int k)
+ {
+ return m_buffer[getIndex(i,j,k)];
+ }
+
+ void set(int i, int j, int k, T value)
+ {
+ m_buffer[getIndex(i,j,k)] = value;
+ }
+
+private:
+ int getIndex(int i, int j, int k)
+ {
+ return i + j*m_width + k*m_width*m_height;
+ }
+
+ int m_height;
+ int m_width;
+ int m_depth;
+ std::vector<T> m_buffer;
+};
+
+/// Vertex data for testing, storing ID and surface normal
+class MockVertexData
+{
+public:
+ /// Constructor
+ /// \param id Unique ID of the vertex in its mesh
+ /// \param normal Surface normal of the vertex
+ MockVertexData(size_t id, const SurgSim::Math::Vector3d& normal) :
+ m_id(id),
+ m_normal(normal)
+ {
+ }
+ /// Destructor
+ virtual ~MockVertexData()
+ {
+ }
+
+ /// Gets the vertex's unique ID in its mesh.
+ size_t getId() const
+ {
+ return m_id;
+ }
+
+ /// Gets the vertex surface normal
+ const SurgSim::Math::Vector3d& getNormal() const
+ {
+ return m_normal;
+ }
+
+ /// Compare the vertex data to another one (equality)
+ /// \param data The MockVertexData to compare it to
+ /// \return True if the two vertex data are equal, False otherwise
+ bool operator==(const MockVertexData& data) const
+ {
+ return m_id == data.m_id && (m_normal - data.m_normal).norm() < 1.0e-10;
+ }
+private:
+ /// Vertex's unique ID in its mesh
+ size_t m_id;
+ /// Vertex surface normal
+ SurgSim::Math::Vector3d m_normal;
+};
+
+/// Edge data for testing, storing ID
+class MockEdgeData
+{
+public:
+ /// Constructor
+ /// \param id Unique ID of the edge in its mesh
+ explicit MockEdgeData(size_t id) :
+ m_id(id)
+ {
+ }
+ /// Destructor
+ virtual ~MockEdgeData()
+ {
+ }
+
+ /// Gets the edge's unique ID in its mesh.
+ size_t getId() const
+ {
+ return m_id;
+ }
+
+ /// Compare the edge data to another one (equality)
+ /// \param data The MockEdgeData to compare it to
+ /// \return True if the two edge data are equal, False otherwise
+ bool operator==(const MockEdgeData& data) const
+ {
+ return m_id == data.m_id;
+ }
+private:
+ /// Edge's unique ID in its mesh
+ size_t m_id;
+};
+
+/// Triangle data for testing, storing ID and edge IDs
+class MockTriangleData
+{
+public:
+ /// Constructor
+ /// \param id Unique ID of the triangle in its mesh
+ /// \param edges IDs of the triangle's edges in its mesh
+ MockTriangleData(size_t id, const std::array<size_t, 3>& edges) :
+ m_id(id),
+ m_edges(edges)
+ {
+ }
+ /// Destructor
+ virtual ~MockTriangleData()
+ {
+ }
+
+ /// Gets the triangle's unique ID in its mesh.
+ size_t getId() const
+ {
+ return m_id;
+ }
+
+ /// Gets the IDs of the triangle's edges in its mesh.
+ const std::array<size_t, 3>& getEdges() const
+ {
+ return m_edges;
+ }
+
+ /// Compare the triangle data to another one (equality)
+ /// \param data The MockTriangleData to compare it to
+ /// \return True if the two triangle data are equal, False otherwise
+ bool operator==(const MockTriangleData& data) const
+ {
+ return m_id == data.m_id && m_edges == data.m_edges;
+ }
+private:
+ /// Triangle's unique ID in its mesh
+ size_t m_id;
+ /// The IDs of the triangle's edges in its mesh, in order: {vertex0->vertex1, vertex1->vertex2, vertex2->vertex3}
+ std::array<size_t, 3> m_edges;
+};
+
+/// Tetrahedron data for testing, storing ID and edge IDs, triangle IDs
+class MockTetrahedronData
+{
+public:
+ /// Constructor
+ /// \param id Unique ID of the tetrahedron in its mesh
+ /// \param edges IDs of the tetrahedron's edges in its mesh (6 edges)
+ /// \param triangles IDs of the tetrahedron's triangles in its mesh (4 triangles)
+ MockTetrahedronData(size_t id,
+ const std::array<size_t, 6>& edges,
+ const std::array<size_t, 4>& triangles) :
+ m_id(id), m_edges(edges), m_triangles(triangles)
+ {
+ }
+
+ /// Destructor
+ virtual ~MockTetrahedronData()
+ {
+ }
+
+ /// Gets the tetrahedron's unique ID in its mesh.
+ size_t getId() const
+ {
+ return m_id;
+ }
+
+ /// Gets the IDs of the tetrahedron's edges in its mesh.
+ const std::array<size_t, 6>& getEdges() const
+ {
+ return m_edges;
+ }
+
+ /// Gets the IDs of the tetrahedron's triangles in its mesh.
+ const std::array<size_t, 4>& getTriangles() const
+ {
+ return m_triangles;
+ }
+
+ /// Compare the tetrahedron data (equality)
+ /// \param data The MockTetrahedronData to compare it to
+ /// \return True if the two tetrahedron data are equals, False otherwise
+ bool operator==(const MockTetrahedronData& data) const
+ {
+ return m_id == data.m_id && m_edges == data.m_edges && m_triangles == data.m_triangles;
+ }
+private:
+ /// Tetrahedron's unique ID in its mesh
+ size_t m_id;
+
+ /// The IDs of the tetrahedron's edges in its mesh, in order:
+ /// {vertex0->vertex1, vertex0->vertex2, vertex0->vertex3, vertex1->vertex2, vertex1->vertex3, vertex2->vertex3}
+ std::array<size_t, 6> m_edges;
+
+ /// The IDs of the tetrahedron's triangles in its mesh, in order: {vertex012, vertex123, vertex230, vertex301}
+ std::array<size_t, 4> m_triangles;
+};
+
+/// Mesh for testing using MockVertexData
+class MockMesh : public SurgSim::DataStructures::Vertices<MockVertexData>
+{
+public:
+ /// Vertex type for convenience
+ typedef Vertices<MockVertexData>::VertexType VertexType;
+
+ /// Constructor. Start out with no vertices and 0 updates
+ MockMesh() : SurgSim::DataStructures::Vertices<MockVertexData>(),
+ m_numUpdates(0)
+ {
+ }
+ /// Destructor
+ virtual ~MockMesh()
+ {
+ }
+
+ /// Create a new vertex in the mesh
+ /// \param position Position of the vertex
+ /// \param normal Normal of the vertex
+ /// \return Unique ID of vertex in the mesh
+ size_t createVertex(const SurgSim::Math::Vector3d& position, const SurgSim::Math::Vector3d& normal)
+ {
+ VertexType vertex(position, MockVertexData(getNumVertices(), normal));
+
+ return addVertex(vertex);
+ }
+
+ /// Returns the normal of a vertex
+ const SurgSim::Math::Vector3d& getVertexNormal(size_t id) const
+ {
+ return getVertex(id).data.getNormal();
+ }
+
+ /// Returns the number of updates performed on the mesh
+ int getNumUpdates() const
+ {
+ return m_numUpdates;
+ }
+
+private:
+ /// Provides update functionality, which just increments the number of updates
+ virtual void doUpdate()
+ {
+ ++m_numUpdates;
+ }
+
+ /// Number of updates performed on the mesh
+ int m_numUpdates;
+};
+
+/// Triangle Mesh for testing using MockVertexData, MockEdgeData, and MockTriangleData
+class MockTriangleMeshBase : public SurgSim::DataStructures::TriangleMeshBase<MockVertexData,
+ MockEdgeData,
+ MockTriangleData>
+{
+public:
+ /// Vertex type for convenience
+ typedef TriangleMeshBase<MockVertexData, MockEdgeData, MockTriangleData>::VertexType VertexType;
+ /// Edge type for convenience
+ typedef TriangleMeshBase<MockVertexData, MockEdgeData, MockTriangleData>::EdgeType EdgeType;
+ /// Triangle type for convenience
+ typedef TriangleMeshBase<MockVertexData, MockEdgeData, MockTriangleData>::TriangleType TriangleType;
+
+ /// Constructor. Start out with no vertices and 0 updates
+ MockTriangleMeshBase() :SurgSim::DataStructures::TriangleMeshBase<MockVertexData, MockEdgeData, MockTriangleData>(),
+ m_numUpdates(0)
+ {
+ }
+ /// Destructor
+ virtual ~MockTriangleMeshBase()
+ {
+ }
+
+ /// Create a new vertex in the mesh
+ /// \param position Position of the vertex
+ /// \param normal Normal of the vertex
+ /// \return Unique ID of vertex in the mesh
+ size_t createVertex(const SurgSim::Math::Vector3d& position, const SurgSim::Math::Vector3d& normal)
+ {
+ VertexType vertex(position, MockVertexData(getNumVertices(), normal));
+
+ return addVertex(vertex);
+ }
+
+ /// Create a new edge in the mesh
+ /// \param vertices Edge vertices
+ /// \return Unique ID of vertex in the mesh
+ size_t createEdge(const std::array<size_t, 2>& vertices)
+ {
+ EdgeType edge(vertices, MockEdgeData(getNumEdges()));
+
+ return addEdge(edge);
+ }
+
+ /// Create a new triangle in the mesh
+ /// \param vertices The triangle vertices
+ /// \param edges The triangle edges
+ /// \return Unique ID of vertex in the mesh
+ size_t createTriangle(const std::array<size_t, 3>& vertices, const std::array<size_t, 3>& edges)
+ {
+ TriangleType triangle(vertices, MockTriangleData(getNumTriangles(), edges));
+
+ return addTriangle(triangle);
+ }
+
+ /// Returns the normal of a vertex
+ const SurgSim::Math::Vector3d& getVertexNormal(size_t id) const
+ {
+ return getVertex(id).data.getNormal();
+ }
+
+ /// Returns the number of updates performed on the mesh
+ int getNumUpdates() const
+ {
+ return m_numUpdates;
+ }
+
+private:
+ /// Provides update functionality, which just increments the number of updates
+ virtual void doUpdate()
+ {
+ ++m_numUpdates;
+ }
+
+ /// Number of updates performed on the mesh
+ int m_numUpdates;
+};
+
+/// Tetrahedron Mesh for testing using MockVertexData, MockEdgeData, MockTriangleData and MockTetrahedronData
+class MockTetrahedronMesh : public SurgSim::DataStructures::TetrahedronMesh<MockVertexData, MockEdgeData,
+ MockTriangleData, MockTetrahedronData>
+{
+public:
+ /// Vertex type for convenience
+ typedef TetrahedronMesh<MockVertexData, MockEdgeData, MockTriangleData, MockTetrahedronData>::VertexType
+ VertexType;
+ /// Edge type for convenience
+ typedef TetrahedronMesh<MockVertexData, MockEdgeData, MockTriangleData, MockTetrahedronData>::EdgeType
+ EdgeType;
+ /// Triangle type for convenience
+ typedef TetrahedronMesh<MockVertexData, MockEdgeData, MockTriangleData, MockTetrahedronData>::TriangleType
+ TriangleType;
+ /// Tetrahedron type for convenience
+ typedef TetrahedronMesh<MockVertexData, MockEdgeData, MockTriangleData, MockTetrahedronData>::TetrahedronType
+ TetrahedronType;
+
+ /// Constructor. Start out with no vertices and 0 updates
+ MockTetrahedronMesh() :
+ SurgSim::DataStructures::TetrahedronMesh<MockVertexData, MockEdgeData, MockTriangleData, MockTetrahedronData>(),
+ m_numUpdates(0)
+ {
+ }
+ /// Destructor
+ virtual ~MockTetrahedronMesh()
+ {
+ }
+
+ /// Create a new vertex in the mesh
+ /// \param position Position of the vertex
+ /// \param normal Normal of the vertex
+ /// \return Unique ID of vertex in the mesh
+ size_t createVertex(const SurgSim::Math::Vector3d& position, const SurgSim::Math::Vector3d& normal)
+ {
+ VertexType vertex(position, MockVertexData(getNumVertices(), normal));
+
+ return addVertex(vertex);
+ }
+
+ /// Create a new edge in the mesh
+ /// \param vertices Edge vertices (x2)
+ /// \return Unique ID of vertex in the mesh
+ size_t createEdge(const std::array<size_t, 2>& vertices)
+ {
+ EdgeType edge(vertices, MockEdgeData(getNumEdges()));
+
+ return addEdge(edge);
+ }
+
+ /// Create a new triangle in the mesh
+ /// \param vertices (x3)
+ /// \param edges (x3)
+ /// \return Unique ID of vertex in the mesh
+ size_t createTriangle(const std::array<size_t, 3>& vertices, const std::array<size_t, 3>& edges)
+ {
+ TriangleType triangle(vertices, MockTriangleData(getNumTriangles(), edges));
+
+ return addTriangle(triangle);
+ }
+
+ /// Create a new tetrahedron in the mesh
+ /// \param vertices connectivity (x4)
+ /// \param edges connectivity (x6)
+ /// \param triangles connectivity (x4)
+ /// \return Unique ID of vertex in the mesh
+ size_t createTetrahedron(const std::array<size_t, 4>& vertices,
+ const std::array<size_t, 6>& edges,
+ const std::array<size_t, 4>& triangles)
+ {
+ TetrahedronType tetrahedron(vertices, MockTetrahedronData(getNumTetrahedrons(), edges, triangles));
+
+ return addTetrahedron(tetrahedron);
+ }
+
+ /// Returns the normal of a vertex
+ const SurgSim::Math::Vector3d& getVertexNormal(size_t id) const
+ {
+ return getVertex(id).data.getNormal();
+ }
+
+ /// Returns the number of updates performed on the mesh
+ int getNumUpdates() const
+ {
+ return m_numUpdates;
+ }
+
+private:
+ /// Provides update functionality, which just increments the number of updates
+ virtual void doUpdate()
+ {
+ ++m_numUpdates;
+ }
+
+ /// Number of updates performed on the mesh
+ int m_numUpdates;
+};
+
+namespace wrappers
+{
+template <typename IteratorType>
+class formatIterator
+{
+public:
+ formatIterator(IteratorType begin, IteratorType end, const std::string& separator)
+ : m_begin(begin), m_end(end), m_separator(separator)
+ {
+ }
+
+ friend std::ostream& operator<<(std::ostream& stream, const formatIterator& formatIterator)
+ {
+ if (formatIterator.m_begin == formatIterator.m_end)
+ {
+ return stream;
+ }
+
+ IteratorType start = formatIterator.m_begin;
+ IteratorType penultimate = formatIterator.m_end - 1;
+ while (start != penultimate)
+ {
+ stream << *start++ << formatIterator.m_separator;
+ }
+ stream << *start;
+
+ return stream;
+ }
+
+private:
+ IteratorType m_begin;
+ IteratorType m_end;
+ const std::string& m_separator;
+};
+}
+
+template <typename IterableType>
+wrappers::formatIterator<typename IterableType::const_iterator> formatIterator(const IterableType& iterable,
+ const std::string& separator)
+{
+ return wrappers::formatIterator<typename IterableType::const_iterator>(
+ iterable.cbegin(), iterable.cend(), separator);
+}
+
+template <typename IteratorType>
+wrappers::formatIterator<IteratorType> formatIterator(IteratorType begin,
+ IteratorType end,
+ const std::string& separator)
+{
+ return wrappers::formatIterator<IteratorType>(begin, end, separator);
+}
+
+#endif // SURGSIM_DATASTRUCTURES_UNITTESTS_MOCKOBJECTS_H
diff --git a/SurgSim/DataStructures/UnitTests/NamedDataTests.cpp b/SurgSim/DataStructures/UnitTests/NamedDataTests.cpp
new file mode 100644
index 0000000..4af8ea0
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/NamedDataTests.cpp
@@ -0,0 +1,405 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 NamedData<T> class.
+
+#include "SurgSim/DataStructures/IndexDirectory.h"
+#include "SurgSim/DataStructures/NamedData.h"
+#include "SurgSim/DataStructures/NamedDataBuilder.h"
+#include "gtest/gtest.h"
+
+using SurgSim::DataStructures::NamedData;
+using SurgSim::DataStructures::NamedDataBuilder;
+
+namespace
+{
+const double EPSILON = 1e-9;
+};
+
+/// Creating a named data object.
+TEST(NamedDataTests, CanConstruct)
+{
+ NamedDataBuilder<float> builder;
+ builder.addEntry("test");
+ NamedData<float> data = builder.createData();
+
+ EXPECT_EQ(1, data.getNumEntries());
+ EXPECT_EQ(1u, data.size());
+
+ EXPECT_TRUE(data.isValid());
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("test"));
+ EXPECT_FALSE(data.hasData(0));
+ EXPECT_FALSE(data.hasData("test"));
+ EXPECT_EQ(0, data.getIndex("test"));
+ EXPECT_EQ("test", data.getName(0));
+
+ EXPECT_FALSE(data.hasEntry(1));
+ EXPECT_FALSE(data.hasEntry("missing"));
+ EXPECT_FALSE(data.hasData(1));
+ EXPECT_FALSE(data.hasData("missing"));
+ EXPECT_EQ(-1, data.getIndex("missing"));
+ EXPECT_EQ("", data.getName(1));
+}
+
+/// Creating a named data object by copy construction.
+TEST(NamedDataTests, CanCopyConstruct)
+{
+ NamedDataBuilder<float> builder;
+ builder.addEntry("test");
+ NamedData<float> preData = builder.createData();
+ NamedData<float> data = preData;
+ EXPECT_EQ(1, data.getNumEntries());
+ EXPECT_EQ(1u, data.size());
+
+ EXPECT_TRUE(data.isValid());
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("test"));
+ EXPECT_FALSE(data.hasData(0));
+ EXPECT_FALSE(data.hasData("test"));
+ EXPECT_EQ(0, data.getIndex("test"));
+ EXPECT_EQ("test", data.getName(0));
+
+ EXPECT_FALSE(data.hasEntry(1));
+ EXPECT_FALSE(data.hasEntry("missing"));
+ EXPECT_FALSE(data.hasData(1));
+ EXPECT_FALSE(data.hasData("missing"));
+ EXPECT_EQ(-1, data.getIndex("missing"));
+ EXPECT_EQ("", data.getName(1));
+
+}
+
+
+/// Creating a shared_ptr to a named data object.
+TEST(NamedDataTests, CanCreateShared)
+{
+ NamedDataBuilder<float> builder;
+ builder.addEntry("test");
+ std::shared_ptr<NamedData<float>> data = builder.createSharedData();
+
+ EXPECT_EQ(1, data->getNumEntries());
+ EXPECT_EQ(1u, data->size());
+
+ EXPECT_TRUE(data->isValid());
+ EXPECT_TRUE(data->hasEntry(0));
+ EXPECT_TRUE(data->hasEntry("test"));
+ EXPECT_FALSE(data->hasData(0));
+ EXPECT_FALSE(data->hasData("test"));
+ EXPECT_EQ(0, data->getIndex("test"));
+ EXPECT_EQ("test", data->getName(0));
+
+ EXPECT_FALSE(data->hasEntry(1));
+ EXPECT_FALSE(data->hasEntry("missing"));
+ EXPECT_FALSE(data->hasData(1));
+ EXPECT_FALSE(data->hasData("missing"));
+ EXPECT_EQ(-1, data->getIndex("missing"));
+ EXPECT_EQ("", data->getName(1));
+}
+
+
+/// Creating a named data object using a vector of names, without a builder.
+TEST(NamedDataTests, CanConstructFromNames)
+{
+ std::vector<std::string> names;
+ names.push_back("test");
+ NamedData<float> data(names);
+
+ EXPECT_EQ(1, data.getNumEntries());
+ EXPECT_EQ(1u, data.size());
+
+ EXPECT_TRUE(data.isValid());
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("test"));
+ EXPECT_FALSE(data.hasData(0));
+ EXPECT_FALSE(data.hasData("test"));
+ EXPECT_EQ(0, data.getIndex("test"));
+ EXPECT_EQ("test", data.getName(0));
+
+ EXPECT_FALSE(data.hasEntry(1));
+ EXPECT_FALSE(data.hasEntry("missing"));
+ EXPECT_FALSE(data.hasData(1));
+ EXPECT_FALSE(data.hasData("missing"));
+ EXPECT_EQ(-1, data.getIndex("missing"));
+ EXPECT_EQ("", data.getName(1));
+}
+
+/// Creating a named data object using an IndexDirectory, not from a builder.
+TEST(NamedDataTests, CanConstructFromIndexDirectory)
+{
+ NamedDataBuilder<float> builder;
+ builder.addEntry("test");
+ std::shared_ptr<const SurgSim::DataStructures::IndexDirectory> dir = builder.createData().getDirectory();
+ NamedData<float> data(dir);
+
+ EXPECT_EQ(1, data.getNumEntries());
+ EXPECT_EQ(1u, data.size());
+
+ EXPECT_TRUE(data.isValid());
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("test"));
+ EXPECT_FALSE(data.hasData(0));
+ EXPECT_FALSE(data.hasData("test"));
+ EXPECT_EQ(0, data.getIndex("test"));
+ EXPECT_EQ("test", data.getName(0));
+
+ EXPECT_FALSE(data.hasEntry(1));
+ EXPECT_FALSE(data.hasEntry("missing"));
+ EXPECT_FALSE(data.hasData(1));
+ EXPECT_FALSE(data.hasData("missing"));
+ EXPECT_EQ(-1, data.getIndex("missing"));
+ EXPECT_EQ("", data.getName(1));
+}
+
+/// Run a few tests against an empty NamedData structure.
+TEST(NamedDataTests, Empty)
+{
+ NamedDataBuilder<float> builder;
+
+ EXPECT_EQ(0, builder.getNumEntries());
+ EXPECT_EQ("", builder.getName(0));
+
+ EXPECT_EQ(-1, builder.getIndex("missing"));
+ EXPECT_FALSE(builder.hasEntry("missing"));
+
+ NamedData<float> data = builder.createData();
+
+ EXPECT_EQ(0, data.getNumEntries());
+ EXPECT_EQ("", data.getName(0));
+
+ EXPECT_EQ(-1, data.getIndex("missing"));
+ EXPECT_FALSE(data.hasEntry("missing"));
+}
+
+/// Creating an uninitialized data object.
+TEST(NamedDataTests, Uninitialized)
+{
+ NamedData<float> data;
+ EXPECT_FALSE(data.isValid());
+
+ EXPECT_EQ(0, data.getNumEntries());
+ EXPECT_EQ("", data.getName(0));
+
+ EXPECT_EQ(-1, data.getIndex("missing"));
+ EXPECT_FALSE(data.hasEntry("missing"));
+ EXPECT_FALSE(data.hasData("missing"));
+
+ float value = 9.87f;
+ EXPECT_FALSE(data.get("missing", &value));
+ EXPECT_NEAR(9.87f, value, 1e-9); // i.e. unchanged
+
+ EXPECT_FALSE(data.reset("missing"));
+
+ EXPECT_FALSE(data.set("missing", value));
+}
+
+/// Putting data into the container.
+TEST(NamedDataTests, Put)
+{
+ NamedDataBuilder<float> builder;
+ builder.addEntry("first");
+ builder.addEntry("second");
+ builder.addEntry("third");
+ NamedData<float> data = builder.createData();
+
+ data.set("first", 1.23f);
+ data.set(1, 4.56f);
+
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("first"));
+ EXPECT_TRUE(data.hasData(0));
+ EXPECT_TRUE(data.hasData("first"));
+ EXPECT_EQ(0, data.getIndex("first"));
+ EXPECT_EQ("first", data.getName(0));
+
+ EXPECT_TRUE(data.hasEntry(1));
+ EXPECT_TRUE(data.hasEntry("second"));
+ EXPECT_TRUE(data.hasData(1));
+ EXPECT_TRUE(data.hasData("second"));
+ EXPECT_EQ(1, data.getIndex("second"));
+ EXPECT_EQ("second", data.getName(1));
+
+ EXPECT_TRUE(data.hasEntry(2));
+ EXPECT_TRUE(data.hasEntry("third"));
+ EXPECT_FALSE(data.hasData(2));
+ EXPECT_FALSE(data.hasData("third"));
+ EXPECT_EQ(2, data.getIndex("third"));
+ EXPECT_EQ("third", data.getName(2));
+}
+
+/// Copying data between NamedData, when they cannot assign to each other.
+TEST(NamedDataTests, Copy)
+{
+ NamedDataBuilder<float> sourceBuilder;
+ sourceBuilder.addEntry("first");
+ sourceBuilder.addEntry("second");
+ NamedData<float> sourceData = sourceBuilder.createData();
+
+ NamedDataBuilder<float> targetBuilder;
+ targetBuilder.addEntry("second");
+ targetBuilder.addEntry("third");
+ NamedData<float> targetData = targetBuilder.createData();
+
+ const float secondFloat = 1.23f;
+ sourceData.set("second", secondFloat);
+ SurgSim::DataStructures::NamedDataCopyMap map;
+ map[sourceData.getIndex("second")] = targetData.getIndex("second");
+ targetData.copy(sourceData, map);
+
+ EXPECT_FALSE(targetData.hasEntry("first"));
+
+ float outSecondFloat;
+ ASSERT_TRUE(targetData.get("second", &outSecondFloat));
+ EXPECT_NEAR(secondFloat, outSecondFloat, EPSILON);
+
+ EXPECT_FALSE(targetData.hasData("third"));
+}
+
+/// Getting data into the container.
+TEST(NamedDataTests, Get)
+{
+ NamedDataBuilder<float> builder;
+ builder.addEntry("first");
+ builder.addEntry("second");
+ builder.addEntry("third");
+ NamedData<float> data = builder.createData();
+
+ data.set("first", 1.23f);
+ data.set(1, 4.56f);
+
+ {
+ float value = 9.87f;
+ EXPECT_TRUE(data.get(0, &value));
+ EXPECT_NEAR(1.23f, value, 1e-9);
+ }
+ {
+ float value = 9.87f;
+ EXPECT_TRUE(data.get("first", &value));
+ EXPECT_NEAR(1.23f, value, 1e-9);
+ }
+ {
+ float value = 9.87f;
+ EXPECT_TRUE(data.get(1, &value));
+ EXPECT_NEAR(4.56f, value, 1e-9);
+ }
+ {
+ float value = 9.87f;
+ EXPECT_TRUE(data.get("second", &value));
+ EXPECT_NEAR(4.56f, value, 1e-9);
+ }
+ {
+ float value = 9.87f;
+ EXPECT_FALSE(data.get(2, &value));
+ EXPECT_NEAR(9.87f, value, 1e-9); // i.e. unchanged
+ }
+ {
+ float value = 9.87f;
+ EXPECT_FALSE(data.get("third", &value));
+ EXPECT_NEAR(9.87f, value, 1e-9); // i.e. unchanged
+ }
+}
+
+/// Resetting the data in the container.
+TEST(NamedDataTests, ResetAll)
+{
+ NamedDataBuilder<float> builder;
+ builder.addEntry("first");
+ builder.addEntry("second");
+ builder.addEntry("third");
+ NamedData<float> data = builder.createData();
+
+ data.set("first", 1.23f);
+ data.set(1, 4.56f);
+
+ data.resetAll();
+
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("first"));
+ EXPECT_FALSE(data.hasData(0));
+ EXPECT_FALSE(data.hasData("first"));
+
+ EXPECT_TRUE(data.hasEntry(1));
+ EXPECT_TRUE(data.hasEntry("second"));
+ EXPECT_FALSE(data.hasData(1));
+ EXPECT_FALSE(data.hasData("second"));
+
+ EXPECT_TRUE(data.hasEntry(2));
+ EXPECT_TRUE(data.hasEntry("third"));
+ EXPECT_FALSE(data.hasData(2));
+ EXPECT_FALSE(data.hasData("third"));
+}
+
+/// Resetting one data entry at a time.
+TEST(NamedDataTests, ResetOne)
+{
+ NamedDataBuilder<float> builder;
+ builder.addEntry("first");
+ builder.addEntry("second");
+ builder.addEntry("third");
+ NamedData<float> data = builder.createData();
+
+ data.set("first", 1.23f);
+ data.set(1, 4.56f);
+
+ data.reset(0);
+
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("first"));
+ EXPECT_FALSE(data.hasData(0));
+ EXPECT_FALSE(data.hasData("first"));
+
+ EXPECT_TRUE(data.hasEntry(1));
+ EXPECT_TRUE(data.hasEntry("second"));
+ EXPECT_TRUE(data.hasData(1));
+ EXPECT_TRUE(data.hasData("second"));
+
+ EXPECT_TRUE(data.hasEntry(2));
+ EXPECT_TRUE(data.hasEntry("third"));
+ EXPECT_FALSE(data.hasData(2));
+ EXPECT_FALSE(data.hasData("third"));
+
+ data.reset("second");
+
+ EXPECT_TRUE(data.hasEntry(1));
+ EXPECT_TRUE(data.hasEntry("second"));
+ EXPECT_FALSE(data.hasData(1));
+ EXPECT_FALSE(data.hasData("second"));
+}
+
+TEST(NamedDataTests, CacheIndex)
+{
+ std::string name = "test";
+ NamedData<bool> data;
+ int index = -1;
+ // invalid NamedData
+ data.cacheIndex(name, &index);
+ EXPECT_EQ(-1, index);
+
+ NamedDataBuilder<bool> builder;
+ builder.addEntry(name);
+ data = builder.createData();
+ // valid NamedData, finds the string
+ data.cacheIndex(name, &index);
+ EXPECT_EQ(0, index);
+
+ // index is >= 0, so nothing happens
+ data.cacheIndex(name + "2", &index);
+ EXPECT_EQ(0, index);
+
+ int index2 = -1;
+ // valid NamedData, string not there
+ data.cacheIndex(name + "2", &index2);
+ EXPECT_EQ(-1, index2);
+}
diff --git a/SurgSim/DataStructures/UnitTests/NamedVariantDataTests.cpp b/SurgSim/DataStructures/UnitTests/NamedVariantDataTests.cpp
new file mode 100644
index 0000000..eb72c3c
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/NamedVariantDataTests.cpp
@@ -0,0 +1,176 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 NamedVariantData class.
+
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/DataStructures/NamedVariantData.h"
+#include "SurgSim/DataStructures/UnitTests/MockObjects.h"
+#include "gtest/gtest.h"
+
+using SurgSim::DataStructures::NamedVariantData;
+using SurgSim::DataStructures::NamedVariantDataBuilder;
+
+/// Creating a named variant data object.
+TEST(NamedVariantDataTests, CanConstruct)
+{
+ SurgSim::DataStructures::NamedVariantDataBuilder builder;
+ builder.addEntry("test");
+ SurgSim::DataStructures::NamedVariantData data = builder.createData();
+
+ EXPECT_EQ(1, data.getNumEntries());
+ EXPECT_EQ(1u, data.size());
+
+ EXPECT_TRUE(data.isValid());
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("test"));
+ EXPECT_FALSE(data.hasData(0));
+ EXPECT_FALSE(data.hasData("test"));
+ EXPECT_EQ(0, data.getIndex("test"));
+ EXPECT_EQ("test", data.getName(0));
+
+ EXPECT_FALSE(data.hasEntry(1));
+ EXPECT_FALSE(data.hasEntry("missing"));
+ EXPECT_FALSE(data.hasData(1));
+ EXPECT_FALSE(data.hasData("missing"));
+ EXPECT_EQ(-1, data.getIndex("missing"));
+ EXPECT_EQ("", data.getName(1));
+}
+
+/// Creating a named variant data object by copy construction.
+TEST(NamedVariantDataTests, CanCopyConstruct)
+{
+ SurgSim::DataStructures::NamedVariantDataBuilder builder;
+ builder.addEntry("test");
+ SurgSim::DataStructures::NamedVariantData preData = builder.createData();
+ SurgSim::DataStructures::NamedVariantData data = preData;
+
+ EXPECT_EQ(1, data.getNumEntries());
+ EXPECT_EQ(1u, data.size());
+
+ EXPECT_TRUE(data.isValid());
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("test"));
+ EXPECT_FALSE(data.hasData(0));
+ EXPECT_FALSE(data.hasData("test"));
+ EXPECT_EQ(0, data.getIndex("test"));
+ EXPECT_EQ("test", data.getName(0));
+
+ EXPECT_FALSE(data.hasEntry(1));
+ EXPECT_FALSE(data.hasEntry("missing"));
+ EXPECT_FALSE(data.hasData(1));
+ EXPECT_FALSE(data.hasData("missing"));
+ EXPECT_EQ(-1, data.getIndex("missing"));
+ EXPECT_EQ("", data.getName(1));
+}
+
+TEST(NamedVariantDataTests, Set)
+{
+ SurgSim::DataStructures::NamedVariantDataBuilder builder;
+ builder.addEntry("test");
+ SurgSim::DataStructures::NamedVariantData data = builder.createData();
+
+ Mock3DData<float> mockData(5, 5, 5);
+ mockData.set(1, 1, 1, 1.23f);
+ mockData.set(4, 3, 2, 4.56f);
+
+ data.set("test", mockData);
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("test"));
+ EXPECT_TRUE(data.hasData(0));
+ EXPECT_TRUE(data.hasData("test"));
+}
+
+TEST(NamedVariantDataTests, HasTypedData)
+{
+ SurgSim::DataStructures::NamedVariantDataBuilder builder;
+ builder.addEntry("test");
+ SurgSim::DataStructures::NamedVariantData data = builder.createData();
+
+ EXPECT_FALSE(data.hasTypedData<double>(0));
+ EXPECT_FALSE(data.hasTypedData<double>("test"));
+
+ Mock3DData<double> mockData(5, 5, 5);
+ mockData.set(1, 1, 1, 1.23);
+ mockData.set(4, 3, 2, 4.56);
+ data.set("test", mockData);
+ EXPECT_TRUE(data.hasTypedData<Mock3DData<double> >(0));
+ EXPECT_TRUE(data.hasTypedData<Mock3DData<double> >("test"));
+
+ EXPECT_FALSE(data.hasTypedData<double>(0));
+ EXPECT_FALSE(data.hasTypedData<double>("test"));
+ EXPECT_FALSE(data.hasTypedData<double>(5));
+ EXPECT_FALSE(data.hasTypedData<double>("missing"));
+}
+
+TEST(NamedVariantDataTests, Get)
+{
+ SurgSim::DataStructures::NamedVariantDataBuilder builder;
+ builder.addEntry("test");
+ SurgSim::DataStructures::NamedVariantData data = builder.createData();
+
+ Mock3DData<float> mockData(5, 5, 5);
+ mockData.set(1, 1, 1, 1.23f);
+ mockData.set(4, 3, 2, 4.56f);
+
+ data.set("test", mockData);
+ {
+ Mock3DData<float> value;
+ EXPECT_TRUE(data.get("test", &value));
+ EXPECT_EQ(1.23f, value.get(1, 1, 1));
+ EXPECT_EQ(4.56f, value.get(4, 3, 2));
+ }
+ {
+ Mock3DData<float> value;
+ EXPECT_TRUE(data.get(0, &value));
+ EXPECT_EQ(1.23f, value.get(1, 1, 1));
+ EXPECT_EQ(4.56f, value.get(4, 3, 2));
+ }
+ {
+ Mock3DData<float> value;
+ EXPECT_FALSE(data.get(5, &value));
+ EXPECT_FALSE(data.get("missing", &value));
+ }
+ {
+ Mock3DData<unsigned char> wrongType;
+ EXPECT_THROW(data.get("test", &wrongType), SurgSim::Framework::AssertionFailure);
+ }
+}
+
+TEST(NamedVariantDataTests, Reset)
+{
+ SurgSim::DataStructures::NamedVariantDataBuilder builder;
+ builder.addEntry("test");
+ SurgSim::DataStructures::NamedVariantData data = builder.createData();
+
+ Mock3DData<float> mockData(5, 5, 5);
+ mockData.set(1, 1, 1, 1.23f);
+ mockData.set(4, 3, 2, 4.56f);
+
+ data.set("test", mockData);
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("test"));
+ EXPECT_TRUE(data.hasData(0));
+ EXPECT_TRUE(data.hasData("test"));
+
+ data.resetAll();
+ EXPECT_TRUE(data.hasEntry(0));
+ EXPECT_TRUE(data.hasEntry("test"));
+ EXPECT_FALSE(data.hasData(0));
+ EXPECT_FALSE(data.hasData("test"));
+}
+
diff --git a/SurgSim/DataStructures/UnitTests/OctreeNodeTests.cpp b/SurgSim/DataStructures/UnitTests/OctreeNodeTests.cpp
new file mode 100644
index 0000000..ad3ed0f
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/OctreeNodeTests.cpp
@@ -0,0 +1,622 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OctreeNode class.
+
+#include "gtest/gtest.h"
+#include <array>
+#include <memory>
+#include <string>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/OctreeNode.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::DataStructures::OctreeNode;
+using SurgSim::Math::Vector3d;
+
+struct MockData
+{
+ int mockInt;
+ double mockDouble;
+ std::string mockString;
+};
+
+typedef OctreeNode<MockData> OctreeNodeType;
+
+namespace
+{
+const double epsilon = 1e-14;
+}
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+TEST(OctreeNodeTests, CanConstruct)
+{
+ SurgSim::Math::Aabbd boundingBox(Vector3d::Zero(), Vector3d::Ones());
+
+ EXPECT_NO_THROW({OctreeNodeType octree(boundingBox);});
+ EXPECT_NO_THROW(std::make_shared<OctreeNodeType>(boundingBox));
+}
+
+TEST(OctreeNodeTests, InitialValues)
+{
+ SurgSim::Math::Aabbd expectedBoundingBox(Vector3d::Zero(), Vector3d::Ones());
+ OctreeNodeType octree(expectedBoundingBox);
+
+ EXPECT_FALSE(octree.isActive());
+ EXPECT_FALSE(octree.hasChildren());
+ EXPECT_TRUE(expectedBoundingBox.isApprox(octree.getBoundingBox()));
+
+ auto children = octree.getChildren();
+ for (auto child = children.cbegin(); child != children.cend(); ++child)
+ {
+ EXPECT_EQ(nullptr, *child);
+ }
+}
+
+TEST(OctreeNodeTests, SetIsActive)
+{
+ SurgSim::Math::Aabbd expectedBoundingBox(Vector3d::Zero(), Vector3d::Ones());
+ OctreeNodeType octree(expectedBoundingBox);
+
+ octree.setIsActive(true);
+ EXPECT_TRUE(octree.isActive());
+
+ octree.setIsActive(false);
+ EXPECT_FALSE(octree.isActive());
+}
+
+TEST(OctreeNodeTests, Subdivide)
+{
+ SurgSim::Math::Aabbd boundingBox(Vector3d::Zero(), Vector3d::Ones() * 16.0);
+ OctreeNodeType octree(boundingBox);
+
+ EXPECT_FALSE(octree.hasChildren());
+ EXPECT_FALSE(octree.isActive());
+ octree.subdivide();
+ EXPECT_TRUE(octree.hasChildren());
+ EXPECT_FALSE(octree.isActive());
+
+ auto children = octree.getChildren();
+ for (auto child = children.cbegin(); child != children.cend(); ++child)
+ {
+ ASSERT_NE(nullptr, *child);
+ EXPECT_FALSE((*child)->isActive());
+ EXPECT_FALSE((*child)->hasChildren());
+ }
+
+ std::array<SurgSim::Math::Aabbd, 8> expectedBoxes = {{
+ SurgSim::Math::Aabbd(Vector3d(0.0, 0.0, 0.0), Vector3d(8.0, 8.0, 8.0)),
+ SurgSim::Math::Aabbd(Vector3d(0.0, 0.0, 8.0), Vector3d(8.0, 8.0, 16.0)),
+ SurgSim::Math::Aabbd(Vector3d(0.0, 8.0, 0.0), Vector3d(8.0, 16.0, 8.0)),
+ SurgSim::Math::Aabbd(Vector3d(0.0, 8.0, 8.0), Vector3d(8.0, 16.0, 16.0)),
+ SurgSim::Math::Aabbd(Vector3d(8.0, 0.0, 0.0), Vector3d(16.0, 8.0, 8.0)),
+ SurgSim::Math::Aabbd(Vector3d(8.0, 0.0, 8.0), Vector3d(16.0, 8.0, 16.0)),
+ SurgSim::Math::Aabbd(Vector3d(8.0, 8.0, 0.0), Vector3d(16.0, 16.0, 8.0)),
+ SurgSim::Math::Aabbd(Vector3d(8.0, 8.0, 8.0), Vector3d(16.0, 16.0, 16.0))
+ }
+ };
+ for (auto expectedBox = expectedBoxes.cbegin(); expectedBox != expectedBoxes.cend(); ++expectedBox)
+ {
+ bool boxFound = false;
+ for (auto child = children.cbegin(); child != children.cend(); ++child)
+ {
+ if (expectedBox->isApprox((*child)->getBoundingBox()))
+ {
+ boxFound = true;
+ break;
+ }
+ }
+ EXPECT_TRUE(boxFound);
+ }
+}
+
+int countOctreeLevels(std::shared_ptr<OctreeNodeType> node)
+{
+ if (node->hasChildren())
+ {
+ auto children = node->getChildren();
+ int maxLevels = 0;
+ for (auto child = children.cbegin(); child != children.cend(); ++child)
+ {
+ if ((*child)->isActive())
+ {
+ int levels = countOctreeLevels(*child);
+ if (levels > maxLevels)
+ {
+ maxLevels = levels;
+ }
+ }
+ }
+ return maxLevels + 1;
+ }
+ return 1;
+}
+
+TEST(OctreeNodeTests, AddNodes)
+{
+ SurgSim::Math::Aabbd boundingBox(Vector3d::Ones() * (-8.0), Vector3d::Ones() * 8.0);
+ std::shared_ptr<OctreeNodeType> octree = std::make_shared<OctreeNodeType>(boundingBox);
+
+ const int levels = 5;
+ MockData data = {1, 3.14, "string"};
+
+ EXPECT_FALSE(octree->hasChildren());
+ EXPECT_FALSE(octree->isActive());
+
+ EXPECT_TRUE(octree->addData(Vector3d(1.0, 1.0, 1.0), data, levels));
+ EXPECT_TRUE(octree->addData(Vector3d(-4.0, 5.0, -7.0), data, levels));
+
+ EXPECT_TRUE(octree->hasChildren());
+ EXPECT_TRUE(octree->isActive());
+ EXPECT_EQ(5, countOctreeLevels(octree));
+
+ int numActive = 0;
+ auto children = octree->getChildren();
+ for (auto child = children.cbegin(); child != children.cend(); ++child)
+ {
+ if ((*child)->isActive())
+ {
+ numActive++;
+ }
+ }
+ EXPECT_EQ(2, numActive);
+}
+
+TEST(OctreeNodeTests, Data)
+{
+ SurgSim::Math::Aabbd boundingBox(Vector3d::Ones() * (-8.0), Vector3d::Ones() * 8.0);
+ OctreeNodeType octree(boundingBox);
+
+ const int levels = 1;
+ MockData expectedData = {1, 3.14, "string"};
+
+ EXPECT_FALSE(octree.hasChildren());
+ EXPECT_FALSE(octree.isActive());
+ EXPECT_TRUE(octree.addData(Vector3d(1.0, 1.0, 1.0), expectedData, levels));
+ EXPECT_FALSE(octree.hasChildren());
+ EXPECT_TRUE(octree.isActive());
+
+ EXPECT_EQ(expectedData.mockInt, octree.data.mockInt);
+ EXPECT_NEAR(expectedData.mockDouble, octree.data.mockDouble, epsilon);
+ EXPECT_EQ(expectedData.mockString, octree.data.mockString);
+}
+
+TEST(OctreeNodeTests, OctreePath)
+{
+ SurgSim::Math::Aabbd boundingBox(Vector3d::Ones() * (-8.0), Vector3d::Ones() * 8.0);
+ std::shared_ptr<OctreeNodeType> octree = std::make_shared<OctreeNodeType>(boundingBox);
+
+ SurgSim::DataStructures::OctreePath path;
+ EXPECT_NO_THROW(octree->getNode(path));
+ EXPECT_EQ(octree, octree->getNode(path));
+
+ octree->subdivide();
+ path.push_back(3);
+ EXPECT_NO_THROW(octree->getNode(path));
+ EXPECT_NE(nullptr, octree->getNode(path));
+
+ auto previous = octree->getNode(path);
+
+ path.push_back(1);
+ EXPECT_THROW(octree->getNode(path), SurgSim::Framework::AssertionFailure);
+
+ // Should return the last valid node on the path
+ EXPECT_NO_THROW(octree->getNode(path, true));
+ EXPECT_EQ(previous, octree->getNode(path, true));
+}
+
+TEST(OctreeNodeTests, CopyConstructor)
+{
+ struct Data1
+ {
+ std::string name;
+ };
+ struct Data2
+ {
+ double value;
+ };
+
+ SurgSim::Math::Aabbd boundingBox(Vector3d::Zero(), 2 * Vector3d::Ones());
+ std::shared_ptr<OctreeNode<Data1>> octree1 = std::make_shared<OctreeNode<Data1>>(boundingBox);
+ Data1 dataRoot = {"root"};
+ octree1->addData(Vector3d(1.0, 1.0, 1.0), dataRoot, 1);
+ Data1 dataChild = {"child"};
+ octree1->addData(Vector3d(0.5, 0.5, 0.5), dataChild, 2);
+
+ {
+ SCOPED_TRACE("Copying with different Data Type");
+ std::shared_ptr<OctreeNode<Data2>> octree2 = std::make_shared<OctreeNode<Data2>>(*octree1);
+ ASSERT_NE(nullptr, octree2);
+ EXPECT_TRUE(octree1->getBoundingBox().isApprox(octree2->getBoundingBox()));
+ EXPECT_EQ(octree1->hasChildren(), octree2->hasChildren());
+ EXPECT_EQ(octree1->isActive(), octree2->isActive());
+ for (size_t i = 0; i < 8; i++)
+ {
+ if (octree1->getChild(i) == nullptr)
+ {
+ EXPECT_EQ(nullptr, octree2->getChild(i));
+ }
+ else
+ {
+ ASSERT_NE(nullptr, octree2->getChild(i));
+ EXPECT_TRUE(octree1->getChild(i)->getBoundingBox().isApprox(octree2->getChild(i)->getBoundingBox()));
+ EXPECT_EQ(octree1->getChild(i)->hasChildren(), octree2->getChild(i)->hasChildren());
+ EXPECT_EQ(octree1->getChild(i)->isActive(), octree2->getChild(i)->isActive());
+ }
+ }
+ }
+
+ {
+ SCOPED_TRACE("Copying with same Data Type");
+ std::shared_ptr<OctreeNode<Data1>> octree2 = std::make_shared<OctreeNode<Data1>>(*octree1);
+ ASSERT_NE(nullptr, octree2);
+ EXPECT_TRUE(octree1->getBoundingBox().isApprox(octree2->getBoundingBox()));
+ EXPECT_EQ(octree1->hasChildren(), octree2->hasChildren());
+ EXPECT_EQ(octree1->isActive(), octree2->isActive());
+ EXPECT_EQ(octree1->data.name, octree2->data.name);
+ for (size_t i = 0; i < 8; i++)
+ {
+ if (octree1->getChild(i) == nullptr)
+ {
+ EXPECT_EQ(nullptr, octree2->getChild(i));
+ }
+ else
+ {
+ ASSERT_NE(nullptr, octree2->getChild(i));
+ EXPECT_TRUE(octree1->getChild(i)->getBoundingBox().isApprox(octree2->getChild(i)->getBoundingBox()));
+ EXPECT_EQ(octree1->getChild(i)->hasChildren(), octree2->getChild(i)->hasChildren());
+ EXPECT_EQ(octree1->getChild(i)->isActive(), octree2->getChild(i)->isActive());
+ EXPECT_EQ(octree1->getChild(i)->data.name, octree2->getChild(i)->data.name);
+ }
+ }
+ }
+}
+
+TEST(OctreeNodeTests, EmptyData)
+{
+ SurgSim::Math::Aabbd boundingBox(Vector3d::Zero(), Vector3d::Ones());
+
+ EXPECT_NO_THROW({OctreeNode<EmptyData> octree(boundingBox);});
+ EXPECT_NO_THROW(std::make_shared<OctreeNode<EmptyData>>(boundingBox));
+}
+
+
+TEST(OctreeNodeTests, LoadOctree)
+{
+ std::shared_ptr<OctreeNode<SurgSim::DataStructures::EmptyData>> octree;
+ EXPECT_NO_THROW(octree = SurgSim::DataStructures::loadOctree("Data/OctreeShapeData/staple.vox"));
+
+ ASSERT_TRUE(nullptr != octree);
+ auto boundingBox = octree->getBoundingBox();
+
+ SurgSim::Math::Vector3d boundingMin(-0.00207699998282, -0.00532899983227, -0.000403999991249);
+ SurgSim::Math::Vector3d boundingMax(0.01392300001718, 0.01067100016773, 0.015596000008751);
+ EXPECT_TRUE(boundingMin.isApprox(boundingBox.min()));
+ EXPECT_TRUE(boundingMax.isApprox(boundingBox.max()));
+
+ EXPECT_TRUE(octree->isActive());
+ EXPECT_TRUE(octree->hasChildren());
+
+ EXPECT_TRUE(octree->getChild(0)->isActive());
+ EXPECT_TRUE(octree->getChild(0)->hasChildren());
+
+ EXPECT_TRUE(octree->getChild(0)->getChild(2)->isActive());
+ EXPECT_TRUE(octree->getChild(0)->getChild(2)->hasChildren());
+
+ EXPECT_TRUE(octree->getChild(0)->getChild(2)->getChild(2)->isActive());
+}
+
+TEST(OctreeNodeTests, DoLoadOctree)
+{
+ SurgSim::Framework::ApplicationData appData("config.txt");
+ auto octree = std::make_shared<OctreeNode<SurgSim::DataStructures::EmptyData>>();
+ EXPECT_NO_THROW(octree->load("OctreeShapeData/staple.vox", appData));
+
+ ASSERT_TRUE(nullptr != octree);
+ auto boundingBox = octree->getBoundingBox();
+
+ SurgSim::Math::Vector3d boundingMin(-0.00207699998282, -0.00532899983227, -0.000403999991249);
+ SurgSim::Math::Vector3d boundingMax(0.01392300001718, 0.01067100016773, 0.015596000008751);
+ EXPECT_TRUE(boundingMin.isApprox(boundingBox.min()));
+ EXPECT_TRUE(boundingMax.isApprox(boundingBox.max()));
+
+ EXPECT_TRUE(octree->isActive());
+ EXPECT_TRUE(octree->hasChildren());
+
+ EXPECT_TRUE(octree->getChild(0)->isActive());
+ EXPECT_TRUE(octree->getChild(0)->hasChildren());
+
+ EXPECT_TRUE(octree->getChild(0)->getChild(2)->isActive());
+ EXPECT_TRUE(octree->getChild(0)->getChild(2)->hasChildren());
+
+ EXPECT_TRUE(octree->getChild(0)->getChild(2)->getChild(2)->isActive());
+}
+
+TEST(OctreeNodeTests, NeighborhoodTestSimple)
+{
+ OctreePath path;
+ {
+ SCOPED_TRACE("No direction should not fail");
+ std::array<Symbol, 3> direction = { SYMBOL_HALT, SYMBOL_HALT, SYMBOL_HALT};
+ EXPECT_NO_THROW(getNeighbor(path, direction));
+ }
+
+ path.push_back(0);
+ {
+ SCOPED_TRACE("Right of 0 should be 1");
+ std::array<Symbol, 3> direction = { SYMBOL_RIGHT, SYMBOL_HALT, SYMBOL_HALT};
+ EXPECT_NO_THROW(getNeighbor(path, direction));
+ auto result = getNeighbor(path, direction);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(1, result[0]);
+ }
+
+ {
+ SCOPED_TRACE("Left of 0 with not levels is nothing");
+ std::array<Symbol, 3> direction = { SYMBOL_LEFT, SYMBOL_HALT, SYMBOL_HALT};
+ EXPECT_NO_THROW(getNeighbor(path, direction));
+ auto result = getNeighbor(path, direction);
+ ASSERT_EQ(0u, result.size());
+ }
+}
+
+
+// This verifies the basic non-boundary crossing neighborhoods
+TEST(OctreeNodeTests, NeigborhoodPlainFaces)
+{
+ int testValues[24][3] =
+ {
+ 0, SYMBOL_RIGHT, 1,
+ 0, SYMBOL_UP, 2,
+ 0, SYMBOL_FRONT, 4,
+
+ 1, SYMBOL_LEFT, 0,
+ 1, SYMBOL_UP, 3,
+ 1, SYMBOL_FRONT, 5,
+
+ 2, SYMBOL_RIGHT, 3,
+ 2, SYMBOL_DOWN, 0,
+ 2, SYMBOL_FRONT, 6,
+
+ 3, SYMBOL_LEFT, 2,
+ 3, SYMBOL_DOWN, 1,
+ 3, SYMBOL_FRONT, 7,
+
+ 4, SYMBOL_RIGHT, 5,
+ 4, SYMBOL_UP, 6,
+ 4, SYMBOL_BACK, 0,
+
+ 5, SYMBOL_LEFT, 4,
+ 5, SYMBOL_UP, 7,
+ 5, SYMBOL_BACK, 1,
+
+ 6, SYMBOL_RIGHT, 7,
+ 6, SYMBOL_DOWN, 4,
+ 6, SYMBOL_BACK, 2,
+
+ 7, SYMBOL_LEFT, 6,
+ 7, SYMBOL_DOWN, 5,
+ 7, SYMBOL_BACK, 3
+ };
+
+ for (size_t i = 0; i < 24; ++i)
+ {
+ OctreePath path(1);
+ path[0] = testValues[i][0];
+ std::array<Symbol, 3> direction = { static_cast<Symbol>(testValues[i][1]), SYMBOL_HALT, SYMBOL_HALT};
+
+ auto result = getNeighbor(path, direction);
+ ASSERT_EQ(1u, result.size()) << "For row " << i;
+ EXPECT_EQ(testValues[i][2], result[0]) << "For row " << i;
+ }
+}
+
+/// Verifies face neighbors for one set of second level nodes, this should exercise all of the state table,
+/// the expected data was calculated manually
+TEST(OctreeNodeTests, AllFaceNeighbors)
+{
+ size_t expected[8][7][2] =
+ {
+ // Node Down Up Right Left Back Front
+ 7, 0, 5, 2, 7, 2, 7, 1, 6, 1, 3, 4, 7, 4,
+ 6, 1, 4, 3, 6, 3, 7, 0, 6, 0, 2, 5, 6, 5,
+ 5, 2, 5, 0, 7, 0, 5, 3, 4, 3, 1, 6, 5, 6,
+ 4, 3, 6, 1, 4, 1, 5, 2, 4, 2, 0, 7, 4, 7,
+ 3, 4, 1, 6, 3, 6, 2, 5, 3, 5, 3, 0, 7, 0,
+ 2, 5, 0, 7, 2, 7, 3, 4, 2, 4, 2, 1, 6, 1,
+ 1, 6, 1, 4, 3, 4, 1, 7, 0, 7, 1, 2, 5, 2,
+ 0, 7, 0, 5, 2, 5, 1, 6, 0, 6, 0, 3, 4, 3
+ };
+
+
+ size_t checkSum = 0;
+ for (int i = 0; i < 8; ++i)
+ {
+ OctreePath node;
+ node.push_back(expected[i][0][0]);
+ node.push_back(expected[i][0][1]);
+
+ auto neighbors = getNeighbors(node, NEIGHBORHOOD_FACE);
+ checkSum += neighbors.size();
+
+ for (int j = 0; j < 6; ++j)
+ {
+ OctreePath neighbor;
+ neighbor.push_back(expected[i][1 + j][0]);
+ neighbor.push_back(expected[i][1 + j][1]);
+
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), neighbor) != neighbors.end())
+ << "Node # " << i << "Neighbor #" << j << " not found";
+
+ }
+ }
+
+ EXPECT_EQ(48u, checkSum);
+}
+
+
+// This verifies edge neighbours for one node
+TEST(OctreeNodeTests, EdgeNeighbors)
+{
+ OctreePath path;
+ path.push_back(1);
+ path.push_back(6);
+
+ auto neighbors = getNeighbors(path, NEIGHBORHOOD_EDGE);
+ const size_t expectedCount = 12;
+ EXPECT_EQ(expectedCount, neighbors.size());
+
+ // Expected coordinates of face neighbors for the node 1/6
+ size_t expected[expectedCount][2] =
+ {
+ // Upper Ring
+ 3, 5, // Up Right
+ 2, 5, // Up Left
+ 3, 0, // Up Back
+ 7, 0, // Up Front
+
+ // Lower Ring
+ 1, 5, // Down Right
+ 0, 5, // Down Left
+ 1, 0, // Down Back
+ 5, 0, // Down Front
+
+ // Horizontals
+ 1, 3, // Back Right
+ 0, 3, // Back Left
+ 5, 3, // Front Right
+ 4, 3, // Front Left
+ };
+
+ for (size_t i = 0; i < expectedCount; ++i)
+ {
+ OctreePath path;
+ path.push_back(expected[i][0]);
+ path.push_back(expected[i][1]);
+
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), path) != neighbors.end())
+ << "Item #" << i << " not found";
+ }
+
+}
+
+// This verifies corner neighbours for one node
+TEST(OctreeNodeTests, VertexNeighbors)
+{
+ OctreePath path;
+ path.push_back(1);
+ path.push_back(6);
+
+ auto neighbors = getNeighbors(path, NEIGHBORHOOD_VERTEX);
+ const size_t expectedCount = 8;
+
+ EXPECT_EQ(expectedCount, neighbors.size());
+
+ // Expected coordinates of face neighbors for the node 1/6
+ size_t expected[expectedCount][2] =
+ {
+ // Lower Ring
+ 5, 1, // Down Right Front
+ 1, 1, // Down Right Back
+ 4, 1, // Down Left Front
+ 0, 1, // Down Left Back
+
+ // Upper Ring
+ 7, 1, // Up Right Front
+ 3, 1, // Up Right Back
+ 6, 1, // Up Left Front
+ 2, 1, // Up Left Back
+ };
+
+ for (size_t i = 0; i < expectedCount; ++i)
+ {
+ OctreePath path;
+ path.push_back(expected[i][0]);
+ path.push_back(expected[i][1]);
+
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), path) != neighbors.end())
+ << "Item #" << i << " not found";
+ }
+}
+
+
+TEST(OctreeNodeTests, AllNeighbors)
+{
+ // All of these nodes are interior, they should return 26 neighbors
+ size_t nodes[8][2] =
+ {
+ 7, 0, 6, 1, 5, 2, 4, 3, 3, 4, 2, 5, 1, 6, 0, 7
+ };
+
+ for (int i = 0; i < 8; ++i)
+ {
+ OctreePath path;
+ path.push_back(nodes[i][0]);
+ path.push_back(nodes[i][1]);
+
+ auto neighbors = getNeighbors(path, NEIGHBORHOOD_FACE | NEIGHBORHOOD_EDGE | NEIGHBORHOOD_VERTEX);
+
+ EXPECT_EQ(26u, neighbors.size());
+ }
+}
+
+TEST(OctreeNodeTests, IncompleteNeighbors)
+{
+ static const size_t numNodes = 16;
+ size_t nodes[numNodes][3] =
+ {
+ //Node, Expected Vertex Neighbors
+ 0, 0, 1, // These are the cube corner pieces
+ 1, 1, 1,
+ 2, 2, 1,
+ 3, 3, 1,
+ 4, 4, 1,
+ 5, 5, 1,
+ 6, 6, 1,
+ 7, 7, 1,
+ 5, 4, 2, // These are on the outside cube edge
+ 3, 2, 2,
+ 1, 5, 2,
+ 0, 4, 2,
+ 3, 6, 4, // These are on an outside surface
+ 1, 4, 4,
+ 4, 2, 4,
+ 7, 2, 4
+ };
+
+ for (size_t i = 0; i < numNodes; ++i)
+ {
+ OctreePath path;
+ path.push_back(nodes[i][0]);
+ path.push_back(nodes[i][1]);
+
+ auto neighbors = getNeighbors(path, NEIGHBORHOOD_VERTEX);
+
+ EXPECT_EQ(nodes[i][2], neighbors.size())
+ << "Incorrect Neighbor Count for Node #" << i
+ << " [" << path[0] << ", " << path[1] << "].";
+ }
+}
+
+}
+}
diff --git a/SurgSim/DataStructures/UnitTests/OptionalValueTests.cpp b/SurgSim/DataStructures/UnitTests/OptionalValueTests.cpp
new file mode 100644
index 0000000..eabf34a
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/OptionalValueTests.cpp
@@ -0,0 +1,189 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <yaml-cpp/yaml.h>
+
+#include <memory>
+
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+TEST(OptionalValueTests, InitTest)
+{
+ EXPECT_NO_THROW({OptionalValue<int> a;});
+ EXPECT_NO_THROW({OptionalValue<double> b(10.0);});
+
+ OptionalValue<int> a;
+ EXPECT_FALSE(a.hasValue());
+}
+
+TEST(OptionalValueTests, AssertTest)
+{
+ OptionalValue<std::shared_ptr<int>> a;
+
+ EXPECT_ANY_THROW({ auto i = a.getValue();});
+}
+
+
+TEST(OptionalValueTests, SetValueTest)
+{
+ OptionalValue<double> a;
+ EXPECT_FALSE(a.hasValue());
+ a.setValue(10.0);
+
+ EXPECT_TRUE(a.hasValue());
+ EXPECT_EQ(10.0, a.getValue());
+
+ a.invalidate();
+
+ EXPECT_FALSE(a.hasValue());
+}
+
+TEST(OptionalValueTests, ComparatorTest)
+{
+ OptionalValue<int> a;
+ OptionalValue<int> b;
+
+ EXPECT_TRUE(a == b);
+ EXPECT_FALSE(a != b);
+
+ a.setValue(10);
+ EXPECT_FALSE(a == b);
+ EXPECT_TRUE(a != b);
+
+ b.setValue(10);
+ EXPECT_TRUE(a == b);
+ EXPECT_FALSE(a != b);
+
+ b.setValue(20);
+ EXPECT_FALSE(a == b);
+ EXPECT_TRUE(a != b);
+
+ a.invalidate();
+ EXPECT_FALSE(a == b);
+ EXPECT_TRUE(a != b);
+
+ EXPECT_FALSE(a == 1); // NOLINT
+
+ a = 2;
+ EXPECT_FALSE(a == 1); // NOLINT
+ EXPECT_TRUE(a != 1); // NOLINT
+ EXPECT_TRUE(a == 2); // NOLINT
+}
+
+TEST(OptionalValueTests, CopyConstructorTest)
+{
+ OptionalValue<int> one;
+ OptionalValue<int> copyOfOne(one);
+
+ EXPECT_EQ(one, copyOfOne);
+
+ OptionalValue<int> two(10);
+ OptionalValue<int> copyOfTwo(two);
+
+ EXPECT_EQ(two, copyOfTwo);
+}
+
+TEST(OptionalValueTests, AssignmentOperatorTest)
+{
+ OptionalValue<int> one;
+ OptionalValue<int> two(10);
+ OptionalValue<int> target(100);
+
+ EXPECT_NE(one, target);
+ EXPECT_NE(two, target);
+
+ target = one;
+ EXPECT_EQ(one, target);
+ EXPECT_NE(two, target);
+
+ target = two;
+ EXPECT_NE(one, target);
+ EXPECT_EQ(two, target);
+
+ one = 1;
+ EXPECT_TRUE(one.hasValue());
+ EXPECT_EQ(1, one.getValue());
+}
+
+template <typename Type>
+void testOptionalValueSerialization(Type value)
+{
+ {
+ OptionalValue<Type> optionalValue;
+ optionalValue.setValue(value);
+
+ // Encode
+ YAML::Node node;
+ EXPECT_NO_THROW(node = optionalValue;);
+
+ // Decode
+ OptionalValue<Type> newOptionalValue;
+ EXPECT_NO_THROW(newOptionalValue = node.as<OptionalValue<Type>>());
+
+ // Verify
+ EXPECT_TRUE(newOptionalValue.hasValue());
+ EXPECT_NO_THROW(newOptionalValue.getValue());
+ EXPECT_EQ(optionalValue.getValue(), newOptionalValue.getValue());
+ }
+
+ // Test for an empty node
+ {
+ OptionalValue<Type> optionalValue;
+
+ // Encode
+ YAML::Node node;
+ EXPECT_NO_THROW(node = optionalValue;);
+
+ // Decode
+ OptionalValue<Type> newOptionalValue;
+ newOptionalValue.setValue(value);
+ EXPECT_NO_THROW(newOptionalValue = node.as<OptionalValue<Type>>());
+
+ // Verify
+ EXPECT_FALSE(newOptionalValue.hasValue());
+ }
+}
+
+TEST(OptionalValueTests, Serialization)
+{
+ testOptionalValueSerialization<bool>(true);
+ testOptionalValueSerialization<bool>(false);
+ testOptionalValueSerialization<unsigned int>(144);
+ testOptionalValueSerialization<int>(37451);
+ testOptionalValueSerialization<float>(921.457f);
+ testOptionalValueSerialization<double>(3.1415);
+ testOptionalValueSerialization<char>('f');
+ testOptionalValueSerialization<std::string>("TestString");
+}
+
+TEST(OptionalValueTests, DereferenceAccess)
+{
+ OptionalValue<int> one;
+ OptionalValue<int> two(10);
+
+ EXPECT_ANY_THROW(*one);
+ EXPECT_NO_THROW(*two);
+ EXPECT_EQ(10, *two);
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/UnitTests/PlyReaderTests.cpp b/SurgSim/DataStructures/UnitTests/PlyReaderTests.cpp
new file mode 100644
index 0000000..3ae45d9
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/PlyReaderTests.cpp
@@ -0,0 +1,278 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+std::string findFile(std::string filename)
+{
+ std::vector<std::string> paths;
+ paths.push_back("Data/PlyReaderTests");
+ SurgSim::Framework::ApplicationData data(paths);
+
+ return data.findFile(filename);
+}
+
+typedef SurgSim::DataStructures::TriangleMesh MeshType;
+
+}
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+class PlyReaderTests : public ::testing::Test
+{
+};
+
+TEST_F(PlyReaderTests, InitTest)
+{
+ ASSERT_NO_THROW(PlyReader("xxx"));
+ ASSERT_NO_THROW(PlyReader(findFile("Cube.ply")));
+
+ PlyReader reader(findFile("Cube.ply"));
+ EXPECT_TRUE(reader.isValid());
+
+ PlyReader reader2(findFile("xxx"));
+ EXPECT_FALSE(reader2.isValid());
+}
+
+TEST_F(PlyReaderTests, FindElementsAndProperties)
+{
+ PlyReader reader(findFile("Cube.ply"));
+
+ EXPECT_TRUE(reader.hasElement("vertex"));
+ EXPECT_TRUE(reader.hasElement("face"));
+ EXPECT_FALSE(reader.hasElement("xxx"));
+
+ EXPECT_TRUE(reader.hasProperty("vertex", "x"));
+ EXPECT_TRUE(reader.hasProperty("vertex", "y"));
+ EXPECT_TRUE(reader.hasProperty("face", "vertex_indices"));
+ EXPECT_FALSE(reader.hasProperty("xxx", "vertex_indices"));
+ EXPECT_FALSE(reader.hasProperty("vertex", "vertex_indices"));
+}
+
+TEST_F(PlyReaderTests, IsScalar)
+{
+ PlyReader reader(findFile("Testdata.ply"));
+
+ EXPECT_TRUE(reader.isScalar("vertex", "x"));
+ EXPECT_FALSE(reader.isScalar("face", "vertex_indices"));
+ EXPECT_TRUE(reader.isScalar("face", "extra"));
+
+ EXPECT_FALSE(reader.isScalar("xxx", "xxx"));
+ EXPECT_FALSE(reader.isScalar("vertex", "xxx"));
+ EXPECT_FALSE(reader.isScalar("face", "xxx"));
+}
+
+class TestData
+{
+public:
+ void* beginVertices(const std::string& elementName, size_t vertices)
+ {
+ vertexData.overrun = 0;
+ vertexInitCount = vertices;
+ vertexRunningCount = 0;
+ endVerticesCalled = false;
+ return &vertexData;
+ }
+
+ void newVertex(const std::string& elementName)
+ {
+ ++vertexRunningCount;
+ vertices.push_back(Vector3d(vertexData.x, vertexData.y, vertexData.z));
+ }
+
+ void endVertices(const std::string& elementName)
+ {
+ endVerticesCalled = true;
+ }
+
+ void* beginFaces(const std::string& elementName, size_t faces)
+ {
+ faceInitCount = faces;
+ faceRunningCount = 0;
+ faceData.overrun = 0;
+ faceData.faceCount = 0;
+ faceData.faces = nullptr;
+ return &faceData;
+ }
+
+ void newFace(const std::string& elementName)
+ {
+ ++faceRunningCount;
+ std::vector<unsigned int> face;
+ for (unsigned int i = 0; i < faceData.faceCount; ++i)
+ {
+ face.push_back(faceData.faces[i]);
+ }
+ faces.push_back(face);
+ extras.push_back(faceData.extra);
+ }
+
+
+ struct VertexData
+ {
+ double x;
+ double y;
+ double z;
+ int64_t overrun;
+ };
+
+ struct FaceData
+ {
+ unsigned int faceCount;
+ unsigned int* faces;
+ int extra;
+ int64_t overrun;
+ };
+
+ VertexData vertexData;
+ size_t vertexInitCount;
+ int vertexRunningCount;
+ bool endVerticesCalled;
+
+ FaceData faceData;
+ size_t faceInitCount;
+ int faceRunningCount;
+
+ std::vector<Vector3d> vertices;
+ std::vector<std::vector<unsigned int>> faces;
+ std::vector<int> extras;
+
+};
+
+TEST_F(PlyReaderTests, ScalarReadTest)
+{
+ TestData testData;
+ PlyReader reader(findFile("Testdata.ply"));
+ EXPECT_TRUE(reader.requestElement("vertex",
+ std::bind(&TestData::beginVertices, &testData,
+ std::placeholders::_1, std::placeholders::_2),
+ std::bind(&TestData::newVertex, &testData, std::placeholders::_1),
+ std::bind(&TestData::endVertices, &testData, std::placeholders::_1)));
+
+ /// Should not be able to register the element twice
+ EXPECT_FALSE(reader.requestElement("vertex",
+ std::bind(&TestData::beginVertices, &testData,
+ std::placeholders::_1, std::placeholders::_2),
+ std::bind(&TestData::newVertex, &testData, std::placeholders::_1),
+ std::bind(&TestData::endVertices, &testData, std::placeholders::_1)));
+
+ EXPECT_TRUE(reader.requestScalarProperty(
+ "vertex", "x", PlyReader::TYPE_DOUBLE, offsetof(TestData::VertexData, x)));
+ EXPECT_FALSE(reader.requestScalarProperty(
+ "vertex", "x", PlyReader::TYPE_DOUBLE, offsetof(TestData::VertexData, x)));
+
+ EXPECT_TRUE(reader.requestScalarProperty(
+ "vertex", "y", PlyReader::TYPE_DOUBLE, offsetof(TestData::VertexData, y)));
+ EXPECT_TRUE(reader.requestScalarProperty(
+ "vertex", "z", PlyReader::TYPE_DOUBLE, offsetof(TestData::VertexData, z)));
+
+ ASSERT_NO_THROW(reader.parseFile());
+ EXPECT_EQ(0L, testData.vertexData.overrun);
+ EXPECT_EQ(4, testData.vertexInitCount);
+ EXPECT_EQ(4, testData.vertexRunningCount);
+
+ EXPECT_EQ(4u, testData.vertices.size());
+ for (size_t i = 0; i < testData.vertices.size(); ++i)
+ {
+ double sign = (i % 2 == 0) ? 1 : -1;
+ Vector3d expected(static_cast<double>(sign * (i + 1)),
+ static_cast<double>(sign * (i + 2)),
+ static_cast<double>(sign * (i + 3)));
+ EXPECT_TRUE(expected.isApprox(testData.vertices[i])) << expected << testData.vertices[i];
+ }
+}
+
+
+TEST_F(PlyReaderTests, ListReadTest)
+{
+ TestData testData;
+ PlyReader reader(findFile("Testdata.ply"));
+ EXPECT_TRUE(reader.requestElement("face",
+ std::bind(&TestData::beginFaces, &testData,
+ std::placeholders::_1, std::placeholders::_2),
+ std::bind(&TestData::newFace, &testData, std::placeholders::_1),
+ nullptr));
+ EXPECT_TRUE(reader.requestListProperty("face", "vertex_indices",
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(TestData::FaceData, faces),
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(TestData::FaceData, faceCount)));
+ EXPECT_TRUE(reader.requestScalarProperty(
+ "face", "extra", PlyReader::TYPE_INT, offsetof(TestData::FaceData, extra)));
+
+ ASSERT_NO_THROW(reader.parseFile());
+ EXPECT_EQ(0L, testData.faceData.overrun);
+ EXPECT_EQ(4, testData.faceInitCount);
+ EXPECT_EQ(4, testData.faceRunningCount);
+
+ EXPECT_EQ(4u, testData.faces.size());
+ EXPECT_EQ(4u, testData.extras.size());
+
+ unsigned int expected = 0;
+ for (size_t i = 0; i < testData.faces.size(); ++i)
+ {
+ std::vector<unsigned int> face = testData.faces[i];
+ EXPECT_EQ(i + 1, face.size());
+ EXPECT_EQ(-static_cast<int>(i), testData.extras[i]);
+
+ for (size_t j = 0; j < face.size(); ++j)
+ {
+ EXPECT_EQ(expected, face[j]);
+ ++expected;
+ }
+ }
+}
+
+TEST_F(PlyReaderTests, TriangleMeshDelegateTest)
+{
+ PlyReader reader(findFile("Cube.ply"));
+ auto delegate = std::make_shared<TriangleMeshPlyReaderDelegate<MeshType>>();
+
+ // parseWithDeletegate() will first call setDelegate(), then parseFile() if previous step successed.
+ EXPECT_NO_THROW(EXPECT_TRUE(reader.parseWithDelegate(delegate)));
+
+ auto mesh = delegate->getMesh();
+ EXPECT_EQ(26u, mesh->getNumVertices());
+ EXPECT_EQ(12u, mesh->getNumTriangles());
+
+ // The first and last vertices from the file
+ Vector3d vertex0(1.0, 1.0, -1.0);
+ Vector3d vertex25(-1.0, -1.0, 1.0);
+
+ EXPECT_TRUE(vertex0.isApprox(mesh->getVertex(0).position));
+ EXPECT_TRUE(vertex25.isApprox(mesh->getVertex(25).position));
+
+ std::array<size_t, 3> triangle0 = {0, 1, 2};
+ std::array<size_t, 3> triangle11 = {10, 25, 11};
+
+ EXPECT_EQ(triangle0, mesh->getTriangle(0).verticesId);
+ EXPECT_EQ(triangle11, mesh->getTriangle(11).verticesId);
+}
+
+} // DataStructures
+} // SurgSim
\ No newline at end of file
diff --git a/SurgSim/DataStructures/UnitTests/TetrahedronMeshTest.cpp b/SurgSim/DataStructures/UnitTests/TetrahedronMeshTest.cpp
new file mode 100644
index 0000000..34e753b
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/TetrahedronMeshTest.cpp
@@ -0,0 +1,663 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Tetrahedron class.
+
+#include "gtest/gtest.h"
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/Vertex.h"
+#include "SurgSim/DataStructures/MeshElement.h"
+#include "SurgSim/DataStructures/UnitTests/MockObjects.h"
+
+#include <random>
+
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::DataStructures::TetrahedronMesh;
+using SurgSim::Math::Vector3d;
+
+class TetrahedronMeshTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // Set to true to print the test positions.
+ bool printPositions = false;
+ // Set to true to print the test normals.
+ bool printNormals = false;
+ // Set to true to print the test edges.
+ bool printEdges = false;
+ // Set to true to print the test triangles.
+ bool printTriangles = false;
+ // Set to true to print the test tetrahedrons.
+ bool printTetrahedrons = false;
+ // Set the number of test vertices
+ size_t numVertices = 10;
+ // Set the number of test tetrahedrons
+ size_t numTetrahedrons = 15;
+
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> positionDistribution(-10.0, 10.0);
+ std::uniform_real_distribution<double> normalDistribution(-1.0, 1.0);
+ std::uniform_int_distribution<size_t> vertexIdDistribution(0, numVertices-1);
+
+ if (printPositions)
+ {
+ std::cout << "Test Vertex Positions:\n";
+ }
+
+ /// Generate random positions for each vertex
+ for (size_t i = 0; i < numVertices; ++i)
+ {
+ Vector3d position(positionDistribution(generator), positionDistribution(generator),
+ positionDistribution(generator));
+ testPositions.push_back(position);
+
+ if (printPositions)
+ {
+ std::cout << "\t" << i << ": (" << position.x() << ", " << position.y() << ", " << position.z()
+ << ")\n";
+ }
+ }
+
+ if (printNormals)
+ {
+ std::cout << "Test Vertex Normals:\n";
+ }
+
+ /// Generate random normals for each vertex
+ for (size_t i = 0; i < numVertices; ++i)
+ {
+ Vector3d normal(normalDistribution(generator), normalDistribution(generator),
+ normalDistribution(generator));
+ normal.normalize();
+ testNormals.push_back(normal);
+
+ if (printNormals)
+ {
+ std::cout << "\t" << i << ": (" << normal.x() << ", " << normal.y() << ", " << normal.z() << ")\n";
+ }
+ }
+
+ if (printTetrahedrons)
+ {
+ std::cout << "Test Tetrahedrons:\n";
+ }
+
+ /// Generate random vertex IDs within [0, numVertices) in quadruplets for mesh tetrahedrons
+ for (size_t i = 0; i < numTetrahedrons; ++i)
+ {
+ std::array<size_t, 4> tetrahedronVertices = {{ vertexIdDistribution(generator),
+ vertexIdDistribution(generator), vertexIdDistribution(generator), vertexIdDistribution(generator) }};
+ testTetrahedronsVerticesId.push_back(tetrahedronVertices);
+
+ /// Create 6 vertex ID pairs for each tetrahedron edge (not worrying about duplicates for these tests)
+ std::array<size_t, 6> tetrahedronEdges;
+ int edgeIDs[6][2] = { {0, 1}, {0, 2}, {0, 3}, {1, 2}, {1, 3}, {2, 3} };
+ for (int j = 0; j < 6; ++j)
+ {
+ std::array<size_t, 2> edgeVertices =
+ {
+ {
+ tetrahedronVertices[edgeIDs[j][0]],
+ tetrahedronVertices[edgeIDs[j][1]]
+ }
+ };
+ testEdgesVerticesId.push_back(edgeVertices);
+
+ tetrahedronEdges[j] = testEdgesVerticesId.size() - 1;
+ }
+ testTetrahedronsEdgesId.push_back(tetrahedronEdges);
+
+ /// Create 4 vertex ID pairs for each tetrahedron triangle (not worrying about duplicates for these tests)
+ int vertexIDs[4][3] = { {0, 1, 2}, {0, 1, 3}, {0, 2, 3}, {1, 2, 3} };
+ int tetTriangleEdgeIds[4][3] = { {0, 1, 3}, {0, 2, 4}, {1, 2, 5}, {3, 4, 5} };
+ std::array<size_t, 4> tetrahedronTriangles;
+ for (int j = 0; j < 4; ++j)
+ {
+ std::array<size_t, 3> triangleVertices =
+ {
+ {
+ tetrahedronVertices[vertexIDs[j][0]],
+ tetrahedronVertices[vertexIDs[j][1]],
+ tetrahedronVertices[vertexIDs[j][2]]
+ }
+ };
+ testTrianglesVerticesId.push_back(triangleVertices);
+
+ tetrahedronTriangles[j] = testTrianglesVerticesId.size() - 1;
+
+ std::array<size_t, 3> triangleEdges;
+ for (int k = 0; k < 3; k++)
+ {
+ triangleEdges[k] = tetrahedronEdges[tetTriangleEdgeIds[j][k]];
+ }
+ testTrianglesEdgesId.push_back(triangleEdges);
+ }
+ testTetrahedronsTrianglesId.push_back(tetrahedronTriangles);
+
+ if (printTetrahedrons)
+ {
+ std::cout << "\t" << i
+ << ": Vertices (" << formatIterator(tetrahedronVertices, ", ")
+ << "), Edges (" << formatIterator(tetrahedronEdges, ", ")
+ << "), Triangles (" << formatIterator(tetrahedronTriangles, ", ") << ")\n";
+ }
+ }
+
+ if (printTriangles)
+ {
+ std::cout << "Test Triangles:\n";
+
+ for (size_t i = 0; i < testTrianglesVerticesId.size(); ++i)
+ {
+ const std::array<size_t, 3>& triangleVertices = testTrianglesVerticesId[i];
+ const std::array<size_t, 3>& triangleEdges = testTrianglesEdgesId[i];
+
+ std::cout << "\t" << i << ": Vertices (" << formatIterator(triangleVertices, ", ")
+ << ") - Edges (" << formatIterator(triangleEdges, ", ") << ")\n";
+ }
+ }
+
+ if (printEdges)
+ {
+ std::cout << "Test Edges:\n";
+
+ for (size_t i = 0; i < testEdgesVerticesId.size(); ++i)
+ {
+ const std::array<size_t, 2>& edgeVertices = testEdgesVerticesId[i];
+ std::cout << "\t" << i << ": (" << formatIterator(edgeVertices, ", ") << ")\n";
+ }
+ }
+ }
+
+ void TearDown()
+ {
+
+ }
+
+ /// Positions of test vertices
+ std::vector<Vector3d> testPositions;
+ /// Normals of test vertices
+ std::vector<Vector3d> testNormals;
+
+ /// Vertices Id for all edges
+ std::vector<std::array<size_t, 2>> testEdgesVerticesId;
+
+ /// Vertices Id for all triangles
+ std::vector<std::array<size_t, 3>> testTrianglesVerticesId;
+ /// Edges Id for all triangles
+ std::vector<std::array<size_t, 3>> testTrianglesEdgesId;
+
+ /// Vertices Id for all tetrahedrons
+ std::vector<std::array<size_t, 4>> testTetrahedronsVerticesId;
+ /// Edges Id for all tetrahedrons
+ std::vector<std::array<size_t, 6>> testTetrahedronsEdgesId;
+ /// Triangles Id for all tetrahedrons
+ std::vector<std::array<size_t, 4>> testTetrahedronsTrianglesId;
+};
+
+
+TEST_F(TetrahedronMeshTest, InitTest)
+{
+ ASSERT_NO_THROW({MockTetrahedronMesh mesh;});
+
+ /// Make sure we can create triangle meshes with each possible combination of void data.
+ /// One void entry
+ typedef TetrahedronMesh<EmptyData, MockEdgeData, MockTriangleData, MockTetrahedronData> TetrahedronMeshNoVertexData;
+ typedef TetrahedronMesh<MockVertexData, EmptyData, MockTriangleData, MockTetrahedronData> TetrahedronMeshNoEdgeData;
+ typedef TetrahedronMesh<MockVertexData, MockEdgeData, EmptyData, MockTetrahedronData> TetrahedronMeshNoTriangleData;
+ typedef TetrahedronMesh<MockVertexData, MockEdgeData, MockTriangleData, EmptyData> TetrahedronMeshNoTetrahedronData;
+ ASSERT_NO_THROW({TetrahedronMeshNoVertexData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshNoEdgeData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshNoTriangleData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshNoTetrahedronData mesh;});
+
+ /// Two void entries
+ typedef TetrahedronMesh<EmptyData, EmptyData, MockTriangleData, MockTetrahedronData>
+ TetrahedronMeshNoVertexOrEdgeData;
+ typedef TetrahedronMesh<EmptyData, MockEdgeData, EmptyData, MockTetrahedronData>
+ TetrahedronMeshNoVertexOrTriangleData;
+ typedef TetrahedronMesh<EmptyData, MockEdgeData, MockTriangleData, EmptyData>
+ TetrahedronMeshNoVertexOrTetrahedronData;
+ typedef TetrahedronMesh<MockVertexData, EmptyData, EmptyData, MockTetrahedronData>
+ TetrahedronMeshNoEdgeOrTriangleData;
+ typedef TetrahedronMesh<MockVertexData, EmptyData, MockTriangleData, EmptyData>
+ TetrahedronMeshNoEdgeOrTetrahedronData;
+ typedef TetrahedronMesh<MockVertexData, MockEdgeData, EmptyData, EmptyData>
+ TetrahedronMeshNoTriangleOrTetrahedronData;
+ ASSERT_NO_THROW({TetrahedronMeshNoVertexOrEdgeData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshNoVertexOrTriangleData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshNoVertexOrTetrahedronData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshNoEdgeOrTriangleData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshNoEdgeOrTetrahedronData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshNoTriangleOrTetrahedronData mesh;});
+
+ /// Three void entries
+ typedef TetrahedronMesh<EmptyData, EmptyData, EmptyData, MockTetrahedronData> TetrahedronMeshOnlyTetrahedronData;
+ typedef TetrahedronMesh<EmptyData, EmptyData, MockTriangleData, EmptyData> TetrahedronMeshOnlyTriangleData;
+ typedef TetrahedronMesh<EmptyData, MockEdgeData, EmptyData, EmptyData> TetrahedronMeshOnlyEdgeData;
+ typedef TetrahedronMesh<MockVertexData, EmptyData, EmptyData, EmptyData> TetrahedronMeshOnlyVertexData;
+ ASSERT_NO_THROW({TetrahedronMeshOnlyTetrahedronData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshOnlyTriangleData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshOnlyEdgeData mesh;});
+ ASSERT_NO_THROW({TetrahedronMeshOnlyVertexData mesh;});
+
+ /// Four void entries
+ typedef TetrahedronMesh<EmptyData, EmptyData, EmptyData, EmptyData> TetrahedronMeshNoData;
+ ASSERT_NO_THROW({TetrahedronMeshNoData mesh;});
+}
+
+TEST_F(TetrahedronMeshTest, CreateVerticesTest)
+{
+ MockTetrahedronMesh mesh;
+
+ EXPECT_EQ(0u, mesh.getNumVertices());
+ EXPECT_EQ(0u, mesh.getVertices().size());
+ EXPECT_EQ(0u, mesh.getNumEdges());
+ EXPECT_EQ(0u, mesh.getEdges().size());
+ EXPECT_EQ(0u, mesh.getNumTriangles());
+ EXPECT_EQ(0u, mesh.getTriangles().size());
+ EXPECT_EQ(0u, mesh.getNumTetrahedrons());
+ EXPECT_EQ(0u, mesh.getTetrahedrons().size());
+
+ EXPECT_EQ(0, mesh.getNumUpdates());
+
+ /// Create the test vertices
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(testPositions[i], testNormals[i]));
+ EXPECT_EQ(i + 1, mesh.getNumVertices());
+
+ const std::vector<MockTriangleMeshBase::VertexType>& vertices = mesh.getVertices();
+ EXPECT_EQ(i + 1, vertices.size());
+
+ /// Make sure each vertex is set properly
+ for (size_t j = 0; j < mesh.getNumVertices(); ++j)
+ {
+ EXPECT_EQ(testPositions[j], vertices[j].position);
+
+ const MockVertexData& data = vertices[j].data;
+ EXPECT_EQ(j, data.getId());
+ EXPECT_EQ(testNormals[j], data.getNormal());
+ }
+ }
+
+ /// Create the test edges
+ for (size_t i = 0; i < testEdgesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createEdge(testEdgesVerticesId[i]));
+ EXPECT_EQ(i + 1, mesh.getNumEdges());
+
+ const std::vector<MockTriangleMeshBase::EdgeType>& edges = mesh.getEdges();
+ EXPECT_EQ(i + 1, edges.size());
+
+ /// Make sure each vertex is set properly
+ for (size_t j = 0; j < mesh.getNumEdges(); ++j)
+ {
+ EXPECT_EQ(testEdgesVerticesId[j], edges[j].verticesId);
+
+ const MockEdgeData& data = edges[j].data;
+ EXPECT_EQ(j, data.getId());
+ }
+ }
+
+ /// Create the test triangles
+ for (size_t i = 0; i < testTrianglesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createTriangle(testTrianglesVerticesId[i], testTrianglesEdgesId[i]));
+ EXPECT_EQ(i + 1, mesh.getNumTriangles());
+
+ const std::vector<MockTriangleMeshBase::TriangleType>& triangles = mesh.getTriangles();
+ EXPECT_EQ(i + 1, triangles.size());
+
+ /// Make sure each vertex is set properly
+ for (size_t j = 0; j < mesh.getNumTriangles(); ++j)
+ {
+ EXPECT_EQ(testTrianglesVerticesId[j], triangles[j].verticesId);
+
+ const MockTriangleData& data = triangles[j].data;
+ EXPECT_EQ(j, data.getId());
+ EXPECT_EQ(testTrianglesEdgesId[j], data.getEdges());
+ }
+ }
+
+ /// Create the test tetrahedrons
+ for (size_t i = 0; i < testTetrahedronsVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createTetrahedron(testTetrahedronsVerticesId[i], \
+ testTetrahedronsEdgesId[i], testTetrahedronsTrianglesId[i]));
+ EXPECT_EQ(i + 1, mesh.getNumTetrahedrons());
+
+ const std::vector<MockTetrahedronMesh::TetrahedronType>& tetrahedrons = mesh.getTetrahedrons();
+ EXPECT_EQ(i + 1, tetrahedrons.size());
+
+ /// Make sure each tetrahedron is set properly
+ for (size_t j = 0; j < mesh.getNumTetrahedrons(); ++j)
+ {
+ EXPECT_EQ(testTetrahedronsVerticesId[j], tetrahedrons[j].verticesId);
+
+ const MockTetrahedronData& data = tetrahedrons[j].data;
+ EXPECT_EQ(j, data.getId());
+ EXPECT_EQ(testTetrahedronsEdgesId[j], data.getEdges());
+ EXPECT_EQ(testTetrahedronsTrianglesId[j], data.getTriangles());
+ }
+ }
+}
+
+TEST_F(TetrahedronMeshTest, isValidTest)
+{
+ MockTetrahedronMesh mesh;
+
+ EXPECT_TRUE(mesh.isValid());
+
+ /// Create the edges (no vertices yet => the mesh is NOT valid)
+ for (size_t i = 0; i < testEdgesVerticesId.size(); ++i)
+ {
+ mesh.createEdge(testEdgesVerticesId[i]);
+ }
+
+ EXPECT_FALSE(mesh.isValid());
+
+ /// Create the triangles (no vertices yet => the mesh is NOT valid)
+ for (size_t i = 0; i < testTrianglesVerticesId.size(); ++i)
+ {
+ mesh.createTriangle(testTrianglesVerticesId[i], testTrianglesEdgesId[i]);
+ }
+
+ EXPECT_FALSE(mesh.isValid());
+
+ /// Create the tetrahedrons (no vertices yet => the mesh is NOT valid)
+ for (size_t i = 0; i < testTetrahedronsVerticesId.size(); ++i)
+ {
+ mesh.createTetrahedron(testTetrahedronsVerticesId[i], testTetrahedronsEdgesId[i], \
+ testTetrahedronsTrianglesId[i]);
+ }
+
+ EXPECT_FALSE(mesh.isValid());
+
+ /// Create the vertices
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ mesh.createVertex(testPositions[i], testNormals[i]);
+ }
+
+ EXPECT_TRUE(mesh.isValid());
+}
+
+TEST_F(TetrahedronMeshTest, SetVertexPositionsTest)
+{
+ MockTetrahedronMesh mesh;
+
+ /// Create vertices with test normals, but all positions at (0,0,0)
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(Vector3d(0.0, 0.0, 0.0), testNormals[i]));
+ EXPECT_EQ(i + 1, mesh.getNumVertices());
+ }
+
+ mesh.setVertexPositions(testPositions);
+
+ EXPECT_EQ(1, mesh.getNumUpdates());
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+
+ const std::vector<MockMesh::VertexType>& vertices = mesh.getVertices();
+ EXPECT_EQ(testPositions.size(), vertices.size());
+
+ /// Make sure each vertex is set properly
+ for (size_t i = 0; i < mesh.getNumVertices(); ++i)
+ {
+ EXPECT_EQ(testPositions[i], vertices[i].position);
+
+ const MockVertexData& data = vertices[i].data;
+ EXPECT_EQ(testNormals[i], data.getNormal());
+ }
+
+ mesh.setVertexPositions(testPositions, false);
+
+ EXPECT_EQ(1, mesh.getNumUpdates());
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+ EXPECT_EQ(testPositions.size(), mesh.getVertices().size());
+
+ mesh.setVertexPositions(testPositions, true);
+
+ EXPECT_EQ(2, mesh.getNumUpdates());
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+ EXPECT_EQ(testPositions.size(), mesh.getVertices().size());
+
+ /// Test the individual set/get methods
+ mesh.setVertexPosition(5, Vector3d(0.0, 0.0, 0.0));
+
+ /// Make sure each vertex is set properly
+ for (size_t i = 0; i < mesh.getNumVertices(); ++i)
+ {
+ if (i == 5)
+ {
+ EXPECT_EQ(Vector3d(0.0, 0.0, 0.0), mesh.getVertexPosition(i));
+ EXPECT_EQ(testNormals[i], mesh.getVertexNormal(i));
+ }
+ else
+ {
+ EXPECT_EQ(testPositions[i], mesh.getVertexPosition(i));
+ EXPECT_EQ(testNormals[i], mesh.getVertexNormal(i));
+ }
+ }
+
+ /// Try setting with wrong number of vertices
+ mesh.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)); // create one more vertex
+
+ EXPECT_ANY_THROW(mesh.setVertexPositions(testPositions));
+}
+
+TEST_F(TetrahedronMeshTest, ClearTest)
+{
+ MockTetrahedronMesh mesh;
+
+ EXPECT_EQ(0, mesh.getNumUpdates());
+
+ EXPECT_EQ(0u, mesh.getNumVertices());
+ EXPECT_EQ(0u, mesh.getVertices().size());
+
+ EXPECT_EQ(0u, mesh.getNumEdges());
+ EXPECT_EQ(0u, mesh.getEdges().size());
+
+ EXPECT_EQ(0u, mesh.getNumTriangles());
+ EXPECT_EQ(0u, mesh.getTriangles().size());
+
+ EXPECT_EQ(0u, mesh.getNumTetrahedrons());
+ EXPECT_EQ(0u, mesh.getTetrahedrons().size());
+
+ /// Create mesh using test data
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)));
+ EXPECT_EQ(i + 1, mesh.getNumVertices());
+ }
+ for (size_t i = 0; i < testEdgesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createEdge(testEdgesVerticesId[i]));
+ EXPECT_EQ(i + 1, mesh.getNumEdges());
+ }
+ for (size_t i = 0; i < testTrianglesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createTriangle(testTrianglesVerticesId[i], testTrianglesEdgesId[i]));
+ EXPECT_EQ(i + 1, mesh.getNumTriangles());
+ }
+ for (size_t i = 0; i < testTetrahedronsVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createTetrahedron(testTetrahedronsVerticesId[i], \
+ testTetrahedronsEdgesId[i], testTetrahedronsTrianglesId[i]));
+ EXPECT_EQ(i + 1, mesh.getNumTetrahedrons());
+ }
+
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+ EXPECT_EQ(testPositions.size(), mesh.getVertices().size());
+
+ EXPECT_EQ(testEdgesVerticesId.size(), mesh.getNumEdges());
+ EXPECT_EQ(testEdgesVerticesId.size(), mesh.getEdges().size());
+
+ EXPECT_EQ(testTrianglesVerticesId.size(), mesh.getNumTriangles());
+ EXPECT_EQ(testTrianglesVerticesId.size(), mesh.getTriangles().size());
+
+ EXPECT_EQ(testTetrahedronsVerticesId.size(), mesh.getNumTetrahedrons());
+ EXPECT_EQ(testTetrahedronsVerticesId.size(), mesh.getTetrahedrons().size());
+
+ /// Clear mesh
+ mesh.clear();
+
+ EXPECT_EQ(0u, mesh.getNumVertices());
+ EXPECT_EQ(0u, mesh.getVertices().size());
+
+ EXPECT_EQ(0u, mesh.getNumEdges());
+ EXPECT_EQ(0u, mesh.getEdges().size());
+
+ EXPECT_EQ(0u, mesh.getNumTriangles());
+ EXPECT_EQ(0u, mesh.getTriangles().size());
+
+ EXPECT_EQ(0u, mesh.getNumTetrahedrons());
+ EXPECT_EQ(0u, mesh.getTetrahedrons().size());
+}
+
+TEST_F(TetrahedronMeshTest, UpdateTest)
+{
+ MockTetrahedronMesh mesh;
+
+ EXPECT_EQ(0, mesh.getNumUpdates());
+
+ for (int i = 0; i < 10; ++i)
+ {
+ mesh.update();
+ EXPECT_EQ(i + 1, mesh.getNumUpdates());
+ }
+}
+
+TEST_F(TetrahedronMeshTest, ComparisonTest)
+{
+ /// Create mesh using test data
+ MockTetrahedronMesh mesh;
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(testPositions[i], testNormals[i]));
+ }
+ for (size_t i = 0; i < testEdgesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createEdge(testEdgesVerticesId[i]));
+ }
+ for (size_t i = 0; i < testTrianglesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createTriangle(testTrianglesVerticesId[i], testTrianglesEdgesId[i]));
+ }
+ for (size_t i = 0; i < testTetrahedronsVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createTetrahedron(testTetrahedronsVerticesId[i], \
+ testTetrahedronsEdgesId[i], testTetrahedronsTrianglesId[i]));
+ }
+
+ /// Create same mesh again
+ MockTetrahedronMesh sameMesh;
+
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, sameMesh.createVertex(testPositions[i], testNormals[i]));
+ }
+ for (size_t i = 0; i < testEdgesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, sameMesh.createEdge(testEdgesVerticesId[i]));
+ }
+ for (size_t i = 0; i < testTrianglesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, sameMesh.createTriangle(testTrianglesVerticesId[i], testTrianglesEdgesId[i]));
+ }
+ for (size_t i = 0; i < testTetrahedronsVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, sameMesh.createTetrahedron(testTetrahedronsVerticesId[i], \
+ testTetrahedronsEdgesId[i], testTetrahedronsTrianglesId[i]));
+ }
+
+ /// Create mesh with test data, but each vertex has position and normal of (0,0,0) to make them different
+ MockTetrahedronMesh meshWithDifferentVertices;
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentVertices.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)));
+ }
+ for (size_t i = 0; i < testEdgesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentVertices.createEdge(testEdgesVerticesId[i]));
+ }
+ for (size_t i = 0; i < testTrianglesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentVertices.createTriangle(testTrianglesVerticesId[i], testTrianglesEdgesId[i]));
+ }
+ for (size_t i = 0; i < testTetrahedronsVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentVertices.createTetrahedron(testTetrahedronsVerticesId[i], \
+ testTetrahedronsEdgesId[i], testTetrahedronsTrianglesId[i]));
+ }
+
+ /// Create mesh with test data, but reverse each edge's vertex order to make them different
+ MockTetrahedronMesh meshWithDifferentEdges;
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentEdges.createVertex(testPositions[i], testNormals[i]));
+ }
+ for (size_t i = 0; i < testEdgesVerticesId.size(); ++i)
+ {
+ std::array<size_t, 2> edge = {{ testEdgesVerticesId[i][1], testEdgesVerticesId[i][0] }};
+ EXPECT_EQ(i, meshWithDifferentEdges.createEdge(edge));
+ }
+ for (size_t i = 0; i < testTrianglesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentEdges.createTriangle(testTrianglesVerticesId[i], testTrianglesEdgesId[i]));
+ }
+ for (size_t i = 0; i < testTetrahedronsVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentEdges.createTetrahedron(testTetrahedronsVerticesId[i], \
+ testTetrahedronsEdgesId[i], testTetrahedronsTrianglesId[i]));
+ }
+
+ /// Create mesh with test data, but only create half of the triangles to make the list different.
+ MockTetrahedronMesh meshWithDifferentTriangles;
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentTriangles.createVertex(testPositions[i], testNormals[i]));
+ }
+ for (size_t i = 0; i < testEdgesVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentTriangles.createEdge(testEdgesVerticesId[i]));
+ }
+ for (size_t i = 0; i < testTrianglesVerticesId.size() / 2; ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentTriangles.createTriangle(testTrianglesVerticesId[i], testTrianglesEdgesId[i]));
+ }
+ for (size_t i = 0; i < testTetrahedronsVerticesId.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentTriangles.createTetrahedron(testTetrahedronsVerticesId[i], \
+ testTetrahedronsEdgesId[i], testTetrahedronsTrianglesId[i]));
+ }
+
+ /// Test comparisons
+ EXPECT_TRUE(mesh == sameMesh);
+ EXPECT_FALSE(mesh != sameMesh);
+
+ EXPECT_FALSE(mesh == meshWithDifferentVertices);
+ EXPECT_TRUE(mesh != meshWithDifferentVertices);
+
+ EXPECT_FALSE(mesh == meshWithDifferentEdges);
+ EXPECT_TRUE(mesh != meshWithDifferentEdges);
+
+ EXPECT_FALSE(mesh == meshWithDifferentTriangles);
+ EXPECT_TRUE(mesh != meshWithDifferentTriangles);
+}
diff --git a/SurgSim/DataStructures/UnitTests/TriangleMeshBaseTest.cpp b/SurgSim/DataStructures/UnitTests/TriangleMeshBaseTest.cpp
new file mode 100644
index 0000000..bb34586
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/TriangleMeshBaseTest.cpp
@@ -0,0 +1,655 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Mesh class.
+
+#include "gtest/gtest.h"
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/MeshElement.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+#include "SurgSim/DataStructures/UnitTests/MockObjects.h"
+#include "SurgSim/DataStructures/Vertex.h"
+
+#include <random>
+
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::DataStructures::TriangleMeshBase;
+using SurgSim::DataStructures::TriangleMeshPlain;
+using SurgSim::Math::Vector3d;
+
+class TriangleMeshBaseTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // Set to true to print the test positions.
+ bool printPositions = false;
+ // Set to true to print the test normals.
+ bool printNormals = false;
+ // Set to true to print the test triangles.
+ bool printTriangles = false;
+ // Set to true to print the test edges.
+ bool printEdges = false;
+ // Set the number of test vertices
+ size_t numVertices = 10;
+ // Set the number of test triangles
+ size_t numTriangles = 20;
+
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> positionDistribution(-10.0, 10.0);
+ std::uniform_real_distribution<double> normalDistribution(-1.0, 1.0);
+ std::uniform_int_distribution<size_t> vertexIdDistribution(0, numVertices - 1);
+
+ if (printPositions)
+ {
+ std::cout << "Test Vertex Positions:\n";
+ }
+
+ /// Generate random positions for each vertex
+ for (size_t i = 0; i < numVertices; ++i)
+ {
+ Vector3d position(positionDistribution(generator), positionDistribution(generator),
+ positionDistribution(generator));
+ testPositions.push_back(position);
+
+ if (printPositions)
+ {
+ std::cout << "\t" << i << ": (" << position.x() << ", " << position.y() << ", " << position.z()
+ << ")\n";
+ }
+ }
+
+ if (printNormals)
+ {
+ std::cout << "Test Vertex Normals:\n";
+ }
+
+ /// Generate random normals for each vertex
+ for (size_t i = 0; i < numVertices; ++i)
+ {
+ Vector3d normal(normalDistribution(generator), normalDistribution(generator),
+ normalDistribution(generator));
+ normal.normalize();
+ testNormals.push_back(normal);
+
+ if (printNormals)
+ {
+ std::cout << "\t" << i << ": (" << normal.x() << ", " << normal.y() << ", " << normal.z() << ")\n";
+ }
+ }
+
+ if (printTriangles)
+ {
+ std::cout << "Test Triangles:\n";
+ }
+
+ /// Generate random vertex IDs within [0, numVertices) in triplets for mesh triangles
+ for (size_t i = 0; i < numTriangles; ++i)
+ {
+ std::array<size_t, 3> triangleVertices = {{
+ vertexIdDistribution(generator),
+ vertexIdDistribution(generator), vertexIdDistribution(generator)
+ }
+ };
+ testTriangleVertices.push_back(triangleVertices);
+
+ /// Create 3 vertex ID pairs for each triangle edge (not worrying about duplicates for these tests)
+ std::array<size_t, 3> triangleEdges;
+ for (int j = 0; j < 3; ++j)
+ {
+ std::array<size_t, 2> edgeVertices = {{ triangleVertices[0], triangleVertices[1] }};
+ testEdgeVertices.push_back(edgeVertices);
+
+ triangleEdges[j] = testEdgeVertices.size() - 1;
+ }
+ testTriangleEdges.push_back(triangleEdges);
+
+ if (printTriangles)
+ {
+ std::cout << "\t" << i << ": Vertices (" << formatIterator(triangleVertices, ", ")
+ << "), Edges (" << formatIterator(triangleEdges, ", ") << ")\n";
+ }
+ }
+
+ if (printEdges)
+ {
+ std::cout << "Test Edges:\n";
+
+ for (size_t i = 0; i < testEdgeVertices.size(); ++i)
+ {
+ const std::array<size_t, 2>& edgeVertices = testEdgeVertices[i];
+ std::cout << "\t" << i << ": (" << formatIterator(edgeVertices, ", ") << ")\n";
+ }
+ }
+ }
+
+ void TearDown()
+ {
+
+ }
+
+ /// Positions of test vertices
+ std::vector<Vector3d> testPositions;
+ /// Normals of test vertices
+ std::vector<Vector3d> testNormals;
+
+ /// Vertices of test edges
+ std::vector<std::array<size_t, 2>> testEdgeVertices;
+
+ /// Vertices of test triangles
+ std::vector<std::array<size_t, 3>> testTriangleVertices;
+ /// Edges of test triangles
+ std::vector<std::array<size_t, 3>> testTriangleEdges;
+};
+
+
+TEST_F(TriangleMeshBaseTest, InitTest)
+{
+ ASSERT_NO_THROW({MockTriangleMeshBase mesh;});
+
+ /// Make sure we can create triangle meshes with each possible combination of EmptyData data.
+ typedef TriangleMeshBase<EmptyData, MockEdgeData, MockTriangleData> TriangleMeshNoVertexData;
+ typedef TriangleMeshBase<MockVertexData, EmptyData, MockTriangleData> TriangleMeshNoEdgeData;
+ typedef TriangleMeshBase<MockVertexData, MockEdgeData, EmptyData> TriangleMeshNoTriangleData;
+
+ typedef TriangleMeshBase<MockVertexData, EmptyData, EmptyData> TriangleMeshNoEdgeOrTriangleData;
+ typedef TriangleMeshBase<EmptyData, MockEdgeData, EmptyData> TriangleMeshNoVertexOrTriangleData;
+ typedef TriangleMeshBase<EmptyData, EmptyData, MockTriangleData> TriangleMeshNoVertexOrEdgeData;
+
+ ASSERT_NO_THROW({TriangleMeshNoVertexData mesh;});
+ ASSERT_NO_THROW({TriangleMeshNoEdgeData mesh;});
+ ASSERT_NO_THROW({TriangleMeshNoTriangleData mesh;});
+
+ ASSERT_NO_THROW({TriangleMeshNoEdgeOrTriangleData mesh;});
+ ASSERT_NO_THROW({TriangleMeshNoVertexOrTriangleData mesh;});
+ ASSERT_NO_THROW({TriangleMeshNoVertexOrEdgeData mesh;});
+
+ ASSERT_NO_THROW({TriangleMeshPlain mesh;});
+}
+
+TEST_F(TriangleMeshBaseTest, CreateVerticesTest)
+{
+ MockTriangleMeshBase mesh;
+
+ EXPECT_EQ(0u, mesh.getNumVertices());
+ EXPECT_EQ(0u, mesh.getVertices().size());
+
+ /// Create the test vertices
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(testPositions[i], testNormals[i]));
+ EXPECT_EQ(i + 1, mesh.getNumVertices());
+
+ const std::vector<MockTriangleMeshBase::VertexType>& vertices = mesh.getVertices();
+ EXPECT_EQ(i + 1, vertices.size());
+
+ /// Make sure each vertex is set properly
+ for (size_t j = 0; j < mesh.getNumVertices(); ++j)
+ {
+ EXPECT_EQ(testPositions[j], vertices[j].position);
+
+ const MockVertexData& data = vertices[j].data;
+ EXPECT_EQ(j, data.getId());
+ EXPECT_EQ(testNormals[j], data.getNormal());
+ }
+ }
+
+ /// Create the test edges
+ for (size_t i = 0; i < testEdgeVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createEdge(testEdgeVertices[i]));
+ EXPECT_EQ(i + 1, mesh.getNumEdges());
+
+ const std::vector<MockTriangleMeshBase::EdgeType>& edges = mesh.getEdges();
+ EXPECT_EQ(i + 1, edges.size());
+
+ /// Make sure each vertex is set properly
+ for (size_t j = 0; j < mesh.getNumEdges(); ++j)
+ {
+ EXPECT_EQ(testEdgeVertices[j], edges[j].verticesId);
+
+ const MockEdgeData& data = edges[j].data;
+ EXPECT_EQ(j, data.getId());
+ }
+ }
+
+ /// Create the test triangles
+ for (size_t i = 0; i < testTriangleVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createTriangle(testTriangleVertices[i], testTriangleEdges[i]));
+ EXPECT_EQ(i + 1, mesh.getNumTriangles());
+
+ const std::vector<MockTriangleMeshBase::TriangleType>& triangles = mesh.getTriangles();
+ EXPECT_EQ(i + 1, triangles.size());
+
+ /// Make sure each vertex is set properly
+ for (size_t j = 0; j < mesh.getNumTriangles(); ++j)
+ {
+ EXPECT_EQ(testTriangleVertices[j], triangles[j].verticesId);
+
+ const MockTriangleData& data = triangles[j].data;
+ EXPECT_EQ(j, data.getId());
+ EXPECT_EQ(testTriangleEdges[j], data.getEdges());
+ }
+ }
+}
+
+TEST_F(TriangleMeshBaseTest, isValidTest)
+{
+ MockTriangleMeshBase mesh;
+
+ EXPECT_TRUE(mesh.isValid());
+
+ /// Create the edges (no vertices yet => the mesh is NOT valid)
+ for (size_t i = 0; i < testEdgeVertices.size(); ++i)
+ {
+ mesh.createEdge(testEdgeVertices[i]);
+ }
+
+ EXPECT_FALSE(mesh.isValid());
+
+ /// Create the triangles (no vertices yet => the mesh is NOT valid)
+ for (size_t i = 0; i < testTriangleVertices.size(); ++i)
+ {
+ mesh.createTriangle(testTriangleVertices[i], testTriangleEdges[i]);
+ }
+
+ EXPECT_FALSE(mesh.isValid());
+
+ /// Create the vertices
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ mesh.createVertex(testPositions[i], testNormals[i]);
+ }
+
+ EXPECT_TRUE(mesh.isValid());
+}
+
+TEST_F(TriangleMeshBaseTest, SetVertexPositionsTest)
+{
+ MockTriangleMeshBase mesh;
+
+ /// Create vertices with test normals, but all positions at (0,0,0)
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(Vector3d(0.0, 0.0, 0.0), testNormals[i]));
+ EXPECT_EQ(i + 1, mesh.getNumVertices());
+ }
+
+ mesh.setVertexPositions(testPositions);
+
+ EXPECT_EQ(1, mesh.getNumUpdates());
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+
+ const std::vector<MockMesh::VertexType>& vertices = mesh.getVertices();
+ EXPECT_EQ(testPositions.size(), vertices.size());
+
+ /// Make sure each vertex is set properly
+ for (size_t i = 0; i < mesh.getNumVertices(); ++i)
+ {
+ EXPECT_EQ(testPositions[i], vertices[i].position);
+
+ const MockVertexData& data = vertices[i].data;
+ EXPECT_EQ(testNormals[i], data.getNormal());
+ }
+
+ mesh.setVertexPositions(testPositions, false);
+
+ EXPECT_EQ(1, mesh.getNumUpdates());
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+ EXPECT_EQ(testPositions.size(), mesh.getVertices().size());
+
+ mesh.setVertexPositions(testPositions, true);
+
+ EXPECT_EQ(2, mesh.getNumUpdates());
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+ EXPECT_EQ(testPositions.size(), mesh.getVertices().size());
+
+ /// Test the individual set/get methods
+ mesh.setVertexPosition(5, Vector3d(0.0, 0.0, 0.0));
+
+ /// Make sure each vertex is set properly
+ for (size_t i = 0; i < mesh.getNumVertices(); ++i)
+ {
+ if (i == 5)
+ {
+ EXPECT_EQ(Vector3d(0.0, 0.0, 0.0), mesh.getVertexPosition(i));
+ EXPECT_EQ(testNormals[i], mesh.getVertexNormal(i));
+ }
+ else
+ {
+ EXPECT_EQ(testPositions[i], mesh.getVertexPosition(i));
+ EXPECT_EQ(testNormals[i], mesh.getVertexNormal(i));
+ }
+ }
+
+ /// Try setting with wrong number of vertices
+ mesh.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)); // create one more vertex
+
+ EXPECT_ANY_THROW(mesh.setVertexPositions(testPositions));
+}
+
+TEST_F(TriangleMeshBaseTest, ClearTest)
+{
+ MockTriangleMeshBase mesh;
+
+ EXPECT_EQ(0, mesh.getNumUpdates());
+
+ EXPECT_EQ(0u, mesh.getNumVertices());
+ EXPECT_EQ(0u, mesh.getVertices().size());
+
+ EXPECT_EQ(0u, mesh.getNumEdges());
+ EXPECT_EQ(0u, mesh.getEdges().size());
+
+ EXPECT_EQ(0u, mesh.getNumTriangles());
+ EXPECT_EQ(0u, mesh.getTriangles().size());
+
+ /// Create mesh using test data
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)));
+ EXPECT_EQ(i + 1, mesh.getNumVertices());
+ }
+ for (size_t i = 0; i < testEdgeVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createEdge(testEdgeVertices[i]));
+ EXPECT_EQ(i + 1, mesh.getNumEdges());
+ }
+ for (size_t i = 0; i < testTriangleVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createTriangle(testTriangleVertices[i], testTriangleEdges[i]));
+ EXPECT_EQ(i + 1, mesh.getNumTriangles());
+ }
+
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+ EXPECT_EQ(testPositions.size(), mesh.getVertices().size());
+
+ EXPECT_EQ(testEdgeVertices.size(), mesh.getNumEdges());
+ EXPECT_EQ(testEdgeVertices.size(), mesh.getEdges().size());
+
+ EXPECT_EQ(testTriangleVertices.size(), mesh.getNumTriangles());
+ EXPECT_EQ(testTriangleVertices.size(), mesh.getTriangles().size());
+
+ /// Clear mesh
+ mesh.clear();
+
+ EXPECT_EQ(0u, mesh.getNumVertices());
+ EXPECT_EQ(0u, mesh.getVertices().size());
+
+ EXPECT_EQ(0u, mesh.getNumEdges());
+ EXPECT_EQ(0u, mesh.getEdges().size());
+
+ EXPECT_EQ(0u, mesh.getNumTriangles());
+ EXPECT_EQ(0u, mesh.getTriangles().size());
+}
+
+TEST_F(TriangleMeshBaseTest, UpdateTest)
+{
+ MockTriangleMeshBase mesh;
+
+ EXPECT_EQ(0, mesh.getNumUpdates());
+
+ for (int i = 0; i < 10; ++i)
+ {
+ mesh.update();
+ EXPECT_EQ(i + 1, mesh.getNumUpdates());
+ }
+}
+
+TEST_F(TriangleMeshBaseTest, ComparisonTest)
+{
+ /// Create mesh using test data
+ MockTriangleMeshBase mesh;
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(testPositions[i], testNormals[i]));
+ }
+ for (size_t i = 0; i < testEdgeVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createEdge(testEdgeVertices[i]));
+ }
+ for (size_t i = 0; i < testTriangleVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createTriangle(testTriangleVertices[i], testTriangleEdges[i]));
+ }
+
+ /// Create same mesh again
+ MockTriangleMeshBase sameMesh;
+
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, sameMesh.createVertex(testPositions[i], testNormals[i]));
+ }
+ for (size_t i = 0; i < testEdgeVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, sameMesh.createEdge(testEdgeVertices[i]));
+ }
+ for (size_t i = 0; i < testTriangleVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, sameMesh.createTriangle(testTriangleVertices[i], testTriangleEdges[i]));
+ }
+
+ /// Create mesh with test data, but each vertex has position and normal of (0,0,0) to make them different
+ MockTriangleMeshBase meshWithDifferentVertices;
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentVertices.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)));
+ }
+ for (size_t i = 0; i < testEdgeVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentVertices.createEdge(testEdgeVertices[i]));
+ }
+ for (size_t i = 0; i < testTriangleVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentVertices.createTriangle(testTriangleVertices[i], testTriangleEdges[i]));
+ }
+
+ /// Create mesh with test data, but reverse each edge's vertex order to make them different
+ MockTriangleMeshBase meshWithDifferentEdges;
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentEdges.createVertex(testPositions[i], testNormals[i]));
+ }
+ for (size_t i = 0; i < testEdgeVertices.size(); ++i)
+ {
+ std::array<size_t, 2> edge = {{ testEdgeVertices[i][1], testEdgeVertices[i][0] }};
+ EXPECT_EQ(i, meshWithDifferentEdges.createEdge(edge));
+ }
+ for (size_t i = 0; i < testTriangleVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentEdges.createTriangle(testTriangleVertices[i], testTriangleEdges[i]));
+ }
+
+ /// Create mesh with test data, but only create half of the triangles to make the list different.
+ MockTriangleMeshBase meshWithDifferentTriangles;
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentTriangles.createVertex(testPositions[i], testNormals[i]));
+ }
+ for (size_t i = 0; i < testEdgeVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentTriangles.createEdge(testEdgeVertices[i]));
+ }
+ for (size_t i = 0; i < testTriangleVertices.size() / 2; ++i)
+ {
+ EXPECT_EQ(i, meshWithDifferentTriangles.createTriangle(testTriangleVertices[i], testTriangleEdges[i]));
+ }
+
+ /// Test comparisons
+ EXPECT_TRUE(mesh == sameMesh);
+ EXPECT_FALSE(mesh != sameMesh);
+
+ EXPECT_FALSE(mesh == meshWithDifferentVertices);
+ EXPECT_TRUE(mesh != meshWithDifferentVertices);
+
+ EXPECT_FALSE(mesh == meshWithDifferentEdges);
+ EXPECT_TRUE(mesh != meshWithDifferentEdges);
+
+ EXPECT_FALSE(mesh == meshWithDifferentTriangles);
+ EXPECT_TRUE(mesh != meshWithDifferentTriangles);
+}
+
+
+TEST_F(TriangleMeshBaseTest, CopyConstructorTest)
+{
+ MockTriangleMeshBase mesh;
+
+ /// Create mesh using test data
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)));
+ EXPECT_EQ(i + 1, mesh.getNumVertices());
+ }
+ for (size_t i = 0; i < testEdgeVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createEdge(testEdgeVertices[i]));
+ EXPECT_EQ(i + 1, mesh.getNumEdges());
+ }
+ for (size_t i = 0; i < testTriangleVertices.size(); ++i)
+ {
+ EXPECT_EQ(i, mesh.createTriangle(testTriangleVertices[i], testTriangleEdges[i]));
+ EXPECT_EQ(i + 1, mesh.getNumTriangles());
+ }
+
+ SurgSim::DataStructures::TriangleMeshPlain mesh2(mesh);
+
+ for (size_t i = 0; i < mesh.getNumVertices(); ++i)
+ {
+ EXPECT_EQ(mesh.getVertexPosition(i), mesh2.getVertexPosition(i));
+ }
+ for (size_t i = 0; i < mesh.getNumEdges(); ++i)
+ {
+ EXPECT_EQ(mesh.getEdge(i).verticesId, mesh2.getEdge(i).verticesId);
+ }
+ for (size_t i = 0; i < mesh.getNumTriangles(); ++i)
+ {
+ EXPECT_EQ(mesh.getTriangle(i).verticesId, mesh2.getTriangle(i).verticesId);
+ }
+}
+
+TEST_F(TriangleMeshBaseTest, loadTriangleMeshTest)
+{
+ auto mesh = SurgSim::DataStructures::loadTriangleMesh("TriangleMeshBaseTests/Cube.ply");
+
+ EXPECT_EQ(26u, mesh->getNumVertices());
+ EXPECT_EQ(12u, mesh->getNumTriangles());
+
+ // The first and last vertices from the file
+ Vector3d vertex0(1.0, 1.0, -1.0);
+ Vector3d vertex25(-1.0, -1.0, 1.0);
+
+ EXPECT_TRUE(vertex0.isApprox(mesh->getVertex(0).position));
+ EXPECT_TRUE(vertex25.isApprox(mesh->getVertex(25).position));
+
+ std::array<size_t, 3> triangle0 = {0, 1, 2};
+ std::array<size_t, 3> triangle11 = {10, 25, 11};
+
+ EXPECT_EQ(triangle0, mesh->getTriangle(0).verticesId);
+ EXPECT_EQ(triangle11, mesh->getTriangle(11).verticesId);
+}
+
+TEST_F(TriangleMeshBaseTest, GetTrianglePositions)
+{
+ MockTriangleMeshBase mesh;
+
+ // Initialization
+ auto normal = testNormals.begin();
+ for (auto position = testPositions.begin(); position != testPositions.end(); ++position)
+ {
+ mesh.createVertex(*position, *normal++);
+ }
+
+ auto edges = testTriangleEdges.begin();
+ for (auto vertices = testTriangleVertices.begin(); vertices != testTriangleVertices.end(); ++vertices)
+ {
+ mesh.createTriangle(*vertices, *edges++);
+ }
+
+ // Testing
+ for (size_t id = 0; id < mesh.getTriangles().size(); ++id)
+ {
+ auto verticesPositions = mesh.getTrianglePositions(id);
+
+ auto& vertexIds = mesh.getTriangle(id).verticesId;
+
+ EXPECT_TRUE(mesh.getVertex(vertexIds[0]).position.isApprox(verticesPositions[0]));
+ EXPECT_TRUE(mesh.getVertex(vertexIds[1]).position.isApprox(verticesPositions[1]));
+ EXPECT_TRUE(mesh.getVertex(vertexIds[2]).position.isApprox(verticesPositions[2]));
+ }
+}
+
+TEST_F(TriangleMeshBaseTest, TriangleDeletionTest)
+{
+ typedef TriangleMeshPlain::TriangleType TriangleType;
+
+ TriangleMeshPlain mesh;
+
+ mesh.addVertex(testPositions[0]);
+ mesh.addVertex(testPositions[0]);
+ mesh.addVertex(testPositions[0]);
+
+ TriangleType::IdType ids = {0, 1, 2};
+ mesh.addTriangle(TriangleType(ids));
+ mesh.addTriangle(TriangleType(ids));
+ mesh.addTriangle(TriangleType(ids));
+
+ EXPECT_TRUE(mesh.isValid());
+
+ EXPECT_NO_THROW(mesh.removeTriangle(1));
+ EXPECT_TRUE(mesh.isValid());
+
+ // Basic checks
+ EXPECT_EQ(2u, mesh.getNumTriangles());
+ EXPECT_ANY_THROW(mesh.getTriangle(1));
+ EXPECT_EQ(3u, mesh.getTriangles().size());
+
+ // Should not be able to remove the same triangle twice
+ EXPECT_ANY_THROW(mesh.removeTriangle(1));
+
+ // Remove all other triangles to check boundary conditions
+ mesh.removeTriangle(0);
+ EXPECT_EQ(1u, mesh.getNumTriangles());
+
+ mesh.removeTriangle(2);
+ EXPECT_EQ(0u, mesh.getNumTriangles());
+
+ EXPECT_EQ(3u, mesh.getTriangles().size());
+
+ // Adding a new triangle we should get an id from the old ids
+ EXPECT_GT(3u, mesh.addTriangle(TriangleType(ids)));
+ EXPECT_EQ(1u, mesh.getNumTriangles());
+
+ // The array size should not have been change
+ EXPECT_EQ(3u, mesh.getTriangles().size());
+
+ EXPECT_GT(3u, mesh.addTriangle(TriangleType(ids)));
+ EXPECT_GT(3u, mesh.addTriangle(TriangleType(ids)));
+
+ // That is a new triangle
+ EXPECT_EQ(3u, mesh.addTriangle(TriangleType(ids)));
+ EXPECT_EQ(4u, mesh.getNumTriangles());
+ EXPECT_EQ(4u, mesh.getTriangles().size());
+
+ // Test clear with deleted triangles
+ mesh.removeTriangle(3);
+ EXPECT_NO_THROW(mesh.clear());
+ EXPECT_EQ(0u, mesh.getNumTriangles());
+ EXPECT_EQ(0u, mesh.addTriangle(TriangleType(ids)));
+ EXPECT_EQ(1u, mesh.addTriangle(TriangleType(ids)));
+
+}
diff --git a/SurgSim/DataStructures/UnitTests/TriangleMeshTest.cpp b/SurgSim/DataStructures/UnitTests/TriangleMeshTest.cpp
new file mode 100644
index 0000000..3c1137c
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/TriangleMeshTest.cpp
@@ -0,0 +1,141 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/MeshElement.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+#include "SurgSim/DataStructures/UnitTests/MockObjects.h"
+#include "SurgSim/DataStructures/Vertex.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <random>
+
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+TEST(TriangleMeshTest, NormalTest)
+{
+ auto mesh = std::make_shared<TriangleMeshPlain>();
+
+ // Add vertex
+ TriangleMeshPlain::VertexType v0(Vector3d(-1.0, -1.0, -1.0));
+ TriangleMeshPlain::VertexType v1(Vector3d(1.0, -1.0, -1.0));
+ TriangleMeshPlain::VertexType v2(Vector3d(-1.0, 1.0, -1.0));
+
+ mesh->addVertex(v0);
+ mesh->addVertex(v1);
+ mesh->addVertex(v2);
+
+ // Add edges
+ std::array<size_t, 2> edgePoints01;
+ std::array<size_t, 2> edgePoints02;
+ std::array<size_t, 2> edgePoints12;
+
+ TriangleMeshPlain::EdgeType e01(edgePoints01);
+ TriangleMeshPlain::EdgeType e02(edgePoints02);
+ TriangleMeshPlain::EdgeType e12(edgePoints12);
+
+ mesh->addEdge(e01);
+ mesh->addEdge(e02);
+ mesh->addEdge(e12);
+
+ // Add triangle
+ std::array<size_t, 3> trianglePoints = {0, 1, 2};
+
+ TriangleMeshPlain::TriangleType t(trianglePoints);
+ mesh->addTriangle(t);
+
+ std::shared_ptr<TriangleMesh> meshWithNormal = std::make_shared<TriangleMesh>(*mesh);
+
+ Vector3d expectedZNormal(0.0, 0.0, 1.0);
+ EXPECT_EQ(expectedZNormal, meshWithNormal->getNormal(0));
+
+ // Update new vertex location of v2 to v3
+ Vector3d v3(-1.0, -1.0, 1.0);
+ SurgSim::DataStructures::Vertex<SurgSim::DataStructures::EmptyData>& v2p = meshWithNormal->getVertex(2);
+ v2p = v3;
+
+ // Recompute normals for meshWithNormal
+ meshWithNormal->calculateNormals();
+ Vector3d expectedXNormal(0.0, -1.0, 0.0);
+ EXPECT_EQ(expectedXNormal, meshWithNormal->getNormal(0));
+}
+
+TEST(TriangleMeshTest, CopyWithTransformTest)
+{
+ const std::string fileName = "MeshShapeData/staple_collision.ply";
+ auto originalMesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(*loadTriangleMesh(fileName));
+ auto expectedMesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(*originalMesh);
+ auto actualMesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(*originalMesh);
+
+ RigidTransform3d transform = SurgSim::Math::makeRigidTransform(
+ Vector3d(4.3, 2.1, 6.5), Vector3d(-1.5, 7.5, -2.5), Vector3d(8.7, -4.7, -3.1));
+
+ for (auto it = expectedMesh->getVertices().begin(); it != expectedMesh->getVertices().end(); ++it)
+ {
+ it->position = transform * it->position;
+ }
+
+ for (auto it = expectedMesh->getTriangles().begin(); it != expectedMesh->getTriangles().end(); ++it)
+ {
+ it->data.normal = transform.linear() * it->data.normal;
+ }
+
+ actualMesh->copyWithTransform(transform, *originalMesh);
+
+ EXPECT_EQ(expectedMesh->getVertices(), actualMesh->getVertices());
+ EXPECT_EQ(expectedMesh->getTriangles(), actualMesh->getTriangles());
+}
+
+TEST(TriangleMeshTest, DoLoadTest)
+{
+ SurgSim::Framework::ApplicationData appData("config.txt");
+
+ {
+ SCOPED_TRACE("Load nonexistent file should throw");
+ // Nonexistent file
+ const std::string fileName = "Nonexistent file";
+ auto mesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>();
+ EXPECT_ANY_THROW(mesh->load(fileName, appData));
+ }
+
+ {
+ SCOPED_TRACE("Load existent file which contains invalid mesh should throw");
+ // File exists, but contains an invalid Mesh
+ const std::string fileName = "MeshShapeData/InvalidMesh.ply";
+ auto mesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>();
+ EXPECT_ANY_THROW(mesh->load(fileName, appData));
+ }
+
+ {
+ SCOPED_TRACE("Load existent file which contains valid mesh should not throw");
+ // File exists, and contains a valid Mesh
+ const std::string fileName = "MeshShapeData/staple_collision.ply";
+ auto mesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>();
+ EXPECT_NO_THROW(mesh->load(fileName, appData));
+ }
+}
+
+};
+};
diff --git a/SurgSim/DataStructures/UnitTests/config.txt.in b/SurgSim/DataStructures/UnitTests/config.txt.in
new file mode 100644
index 0000000..507c4e7
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/config.txt.in
@@ -0,0 +1,2 @@
+${CMAKE_CURRENT_BINARY_DIR}/Data/
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data
\ No newline at end of file
diff --git a/SurgSim/DataStructures/Vertex.h b/SurgSim/DataStructures/Vertex.h
new file mode 100644
index 0000000..423a55e
--- /dev/null
+++ b/SurgSim/DataStructures/Vertex.h
@@ -0,0 +1,112 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_VERTEX_H
+#define SURGSIM_DATASTRUCTURES_VERTEX_H
+
+#include "SurgSim/Math/Vector.h"
+
+#include <memory>
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+/// Vertex structure for meshes. Vertices are the lowest level of structure in a Mesh, providing a position and can
+/// store extra per-vertex data. MeshElements combine MeshVertices to form the structure of the mesh.
+///
+/// Vertex is to be used purely as a data structure and not provide implementation of algorithms.
+/// For example, a physics FEM's nodes are not subclasses of Vertex if they provide code that is part of the FEM
+/// algorithm, but they may used with a Mesh to store the structure of the FEM.
+///
+/// The extra Data is left up to the particular use of Mesh to specify. For example, for use collision detection,
+/// a vertex may need a normal and adjacent triangle information, which could be stored in a struct.
+///
+/// If no extra Data is needed, a specialization exists for void, in which case the constructor takes no data.
+///
+/// \tparam Data Type of extra data stored in the vertex (void for no data)
+/// \sa Vertices
+template <class Data>
+struct Vertex
+{
+ /// Constructor
+ /// \param position Position of the vertex
+ /// \param data Extra data to be stored in the vertex
+ Vertex(const SurgSim::Math::Vector3d& position, const Data& data = Data()) :
+ position(position),
+ data(data)
+ {
+ }
+
+ /// Position of the vertex.
+ SurgSim::Math::Vector3d position;
+ /// Extra vertex data.
+ Data data;
+
+ /// Compare the vertex to another one (equality)
+ /// \param vertex The Vertex to compare it to
+ /// \return True if the two vertices are equal, false otherwise.
+ bool operator==(const Vertex<Data>& vertex) const
+ {
+ return data == vertex.data && position == vertex.position;
+ }
+
+ /// Compare the vertex to another one (inequality)
+ /// \param vertex The Vertex to compare it to
+ /// \return False if the two vertices are equal, true otherwise.
+ bool operator!=(const Vertex<Data>& vertex) const
+ {
+ return ! ((*this) == vertex);
+ }
+};
+
+/// Specialization of Vertex with no data.
+/// \sa Vertex
+template <>
+struct Vertex<void>
+{
+ /// Constructor
+ /// \param position Position of the vertex
+ explicit Vertex(const SurgSim::Math::Vector3d& position) : position(position)
+ {
+ }
+
+ /// Position of the vertex.
+ SurgSim::Math::Vector3d position;
+
+ /// Compare the vertex to another one (equality)
+ /// \param vertex The Vertex to compare it to
+ /// \return True if the two vertices are equal, false otherwise.
+ bool operator==(const Vertex<void>& vertex) const
+ {
+ return position == vertex.position;
+ }
+
+ /// Compare the vertex to another one (inequality)
+ /// \param vertex The Vertex to compare it to
+ /// \return False if the two vertices are equal, true otherwise.
+ bool operator!=(const Vertex<void>& vertex) const
+ {
+ return ! ((*this) == vertex);
+ }
+};
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_VERTEX_H
diff --git a/SurgSim/DataStructures/Vertices.h b/SurgSim/DataStructures/Vertices.h
new file mode 100644
index 0000000..f52ba4a
--- /dev/null
+++ b/SurgSim/DataStructures/Vertices.h
@@ -0,0 +1,205 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DATASTRUCTURES_VERTICES_H
+#define SURGSIM_DATASTRUCTURES_VERTICES_H
+
+#include "SurgSim/DataStructures/Vertex.h"
+#include "SurgSim/Framework/Assert.h"
+
+#include <array>
+#include <typeinfo>
+#include <vector>
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+/// Base class for mesh structures, handling basic vertex functionality.
+///
+/// Vertices is to be used purely as a data structure and not provide implementation of algorithms.
+/// For example, a physics FEM is not a subclass of Vertices, but may use a Mesh for storing the structure of the FEM.
+///
+/// Subclasses of this class should handle the elements required for a specific type of mesh (as simple as just a
+/// generic triangle mesh or as specific as a triangle mesh for collision detection, which might also specify the data
+/// types for the vertex and elements).
+///
+/// It is recommended that subclasses of this class also provide convenience methods for creation of vertices and
+/// elements, and the data each contains. A method such as createVertex(position, other data...) simplifies the creation
+/// of vertices and the data required. This method would use the addVertex() method to add the created vertices to the
+/// Mesh.
+///
+/// \tparam VertexData Type of extra data stored in each vertex (void for no data)
+/// \sa Vertex
+/// \sa MeshElement
+template <class VertexData>
+class Vertices
+{
+public:
+ /// Vertex type for convenience
+ typedef Vertex<VertexData> VertexType;
+
+ /// Constructor. The mesh is initially empty (no vertices).
+ Vertices()
+ {
+ }
+ /// Destructor
+ virtual ~Vertices()
+ {
+ }
+
+ /// Clear mesh to return to an empty state (no vertices).
+ void clear()
+ {
+ doClear();
+ }
+
+ /// Performs any updates that are required when the vertices are modified.
+ /// Calls doUpdate() to perform the updates.
+ void update()
+ {
+ doUpdate();
+ }
+
+ /// Adds a vertex to the mesh.
+ /// Recommend that subclasses with a specific purpose (such as for use in collision detection), have a
+ /// createVertex(position, other data...) method which performs any checking desired and sets up the vertex data
+ /// based on the other parameters.
+ /// \param vertex Vertex to add to the mesh
+ /// \return Unique ID of the new vertex.
+ size_t addVertex(const VertexType& vertex)
+ {
+ m_vertices.push_back(vertex);
+ return m_vertices.size() - 1;
+ }
+
+ /// Returns the number of vertices in this mesh.
+ size_t getNumVertices() const
+ {
+ return m_vertices.size();
+ }
+
+ /// Returns the specified vertex.
+ const VertexType& getVertex(size_t id) const
+ {
+ return m_vertices[id];
+ }
+ /// Returns the specified vertex (non const version).
+ VertexType& getVertex(size_t id)
+ {
+ return m_vertices[id];
+ }
+
+ /// Returns a vector containing the position of each vertex.
+ const std::vector<VertexType>& getVertices() const
+ {
+ return m_vertices;
+ }
+ /// Returns a vector containing the position of each vertex (non const version).
+ std::vector<VertexType>& getVertices()
+ {
+ return m_vertices;
+ }
+
+ /// Sets the position of a vertex.
+ /// \param id Unique ID of the vertex
+ /// \param position Position of the vertex
+ void setVertexPosition(size_t id, const SurgSim::Math::Vector3d& position)
+ {
+ m_vertices[id].position = position;
+ }
+ /// Returns the position of a vertex.
+ /// \param id Unique ID of the vertex
+ /// \return Position of the vertex
+ const SurgSim::Math::Vector3d& getVertexPosition(size_t id) const
+ {
+ return m_vertices[id].position;
+ }
+
+ /// Sets the position of each vertex.
+ /// \param positions Vector containing new position for each vertex
+ /// \param doUpdate True to perform an update after setting the vertices, false to skip update; default is true.
+ void setVertexPositions(const std::vector<SurgSim::Math::Vector3d>& positions, bool doUpdate = true)
+ {
+ SURGSIM_ASSERT(m_vertices.size() == positions.size()) << "Number of positions must match number of vertices.";
+
+ for (size_t i = 0; i < m_vertices.size(); ++i)
+ {
+ m_vertices[i].position = positions[i];
+ }
+
+ if (doUpdate)
+ {
+ update();
+ }
+ }
+
+ /// Compares the mesh with another one (equality)
+ /// \param mesh The Vertices to compare it to
+ /// \return True if the two vertices are equals, False otherwise
+ bool operator==(const Vertices& mesh) const
+ {
+ return (typeid(*this) == typeid(mesh)) && isEqual(mesh);
+ }
+
+ /// Compares the mesh with another one (inequality)
+ /// \param mesh The Vertices to compare it to
+ /// \return False if the two vertices are equals, True otherwise
+ bool operator!=(const Vertices& mesh) const
+ {
+ return (typeid(*this) != typeid(mesh)) || ! isEqual(mesh);
+ }
+
+protected:
+ /// Remove all vertices from the mesh.
+ virtual void doClearVertices()
+ {
+ m_vertices.clear();
+ }
+
+ /// Internal comparison of meshes of the same type: returns true if equal, false if not equal.
+ /// Override this method to provide custom comparison. Base Mesh implementation compares vertices:
+ /// the order of vertices must also match to be considered equal.
+ /// \param mesh Mesh must be of the same type as that which it is compared against
+ virtual bool isEqual(const Vertices& mesh) const
+ {
+ return m_vertices == mesh.m_vertices;
+ }
+
+private:
+ /// Clear mesh to return to an empty state (no vertices).
+ virtual void doClear()
+ {
+ doClearVertices();
+ }
+
+ /// Performs any updates that are required when the vertices are modified.
+ /// Override this method to implement update functionality.
+ /// For example, this could be overridden to calculate normals for each Vertex.
+ virtual void doUpdate()
+ {
+ }
+
+ /// Vertices
+ std::vector<VertexType> m_vertices;
+};
+
+}; // namespace DataStructures
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_VERTICES_H
diff --git a/SurgSim/DataStructures/ply.c b/SurgSim/DataStructures/ply.c
new file mode 100644
index 0000000..273bc9c
--- /dev/null
+++ b/SurgSim/DataStructures/ply.c
@@ -0,0 +1,2589 @@
+/*
+
+The interface routines for reading and writing PLY polygon files.
+
+Greg Turk, February 1994
+
+---------------------------------------------------------------
+
+A PLY file contains a single polygonal _object_.
+
+An object is composed of lists of _elements_. Typical elements are
+vertices, faces, edges and materials.
+
+Each type of element for a given object has one or more _properties_
+associated with the element type. For instance, a vertex element may
+have as properties the floating-point values x,y,z and the three unsigned
+chars representing red, green and blue.
+
+---------------------------------------------------------------
+
+Copyright (c) 1994 The Board of Trustees of The Leland Stanford
+Junior University. All rights reserved.
+
+Permission to use, copy, modify and distribute this software and its
+documentation for any purpose is hereby granted without fee, provided
+that the above copyright notice and this permission notice appear in
+all copies of this software and that you do not sell the software.
+
+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.
+
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <string.h>
+#include "SurgSim/DataStructures/ply.h"
+
+
+// HS-2014-apr-03 There are a few warnings from gcc regarding these
+// in the spirit of changing this file as little as possible we decided
+// to ignore these. The variables concerned are:
+// line 1621 'item'
+// line 1736 'item_size'
+// line 2378 'item'
+// line 1717 'other_data'
+
+// JL-2014-may-23 Upgrade get_words() to handle Windows line ending properly.
+// Minimal changes as been made from lines 1843 to 1846 and comment updated on line 1830.
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4996)
+#elif defined(__GNUC__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif
+
+char *type_names[] = {
+"invalid",
+"char", "short", "int",
+"uchar", "ushort", "uint",
+"float", "double"
+};
+
+int ply_type_size[] = {
+ 0,
+ sizeof(char), sizeof(short), sizeof(int),
+ sizeof(unsigned char), sizeof(unsigned short), sizeof(unsigned int),
+ sizeof(float), sizeof(double)
+};
+
+#define NO_OTHER_PROPS -1
+
+#define DONT_STORE_PROP 0
+#define STORE_PROP 1
+
+#define OTHER_PROP 0
+#define NAMED_PROP 1
+
+
+/* returns 1 if strings are equal, 0 if not */
+int equal_strings(const char *,const char *);
+
+/* find an element in a plyfile's list */
+PlyElement *find_element(PlyFile *,const char *);
+
+/* find a property in an element's list */
+PlyProperty *find_property(PlyElement *,const char *, int *);
+
+/* write to a file the word describing a PLY file data type */
+void write_scalar_type (FILE *, int);
+
+/* read a line from a file and break it up into separate words */
+char **get_words(FILE *, int *, char **);
+char **old_get_words(FILE *, int *);
+
+/* write an item to a file */
+void write_binary_item(FILE *, int, unsigned int, double, int);
+void write_ascii_item(FILE *, int, unsigned int, double, int);
+double old_write_ascii_item(FILE *, char *, int);
+
+/* add information to a PLY file descriptor */
+void add_element(PlyFile *, char **, int);
+void add_property(PlyFile *, char **, int);
+void add_comment(PlyFile *, char *);
+void add_obj_info(PlyFile *, char *);
+
+/* copy a property */
+void copy_property(PlyProperty *, PlyProperty *);
+
+/* store a value into where a pointer and a type specify */
+void store_item(char *, int, int, unsigned int, double);
+
+/* return the value of a stored item */
+void get_stored_item( void *, int, int *, unsigned int *, double *);
+
+/* return the value stored in an item, given ptr to it and its type */
+double get_item_value(char *, int);
+
+/* get binary or ascii item and store it according to ptr and type */
+void get_ascii_item(char *, int, int *, unsigned int *, double *);
+void get_binary_item(FILE *, int, int *, unsigned int *, double *);
+
+/* get a bunch of elements from a file */
+void ascii_get_element(PlyFile *, char *);
+void binary_get_element(PlyFile *, char *);
+
+// /* memory allocation */
+// static char *my_alloc(int, int, char *);
+
+
+/*************/
+/* Writing */
+/*************/
+
+
+/******************************************************************************
+Given a file pointer, get ready to write PLY data to the file.
+
+Entry:
+ fp - the given file pointer
+ nelems - number of elements in object
+ elem_names - list of element names
+ file_type - file type, either ascii or binary
+
+Exit:
+ returns a pointer to a PlyFile, used to refer to this file, or NULL if error
+******************************************************************************/
+
+PlyFile *ply_write(
+ FILE *fp,
+ int nelems,
+ char **elem_names,
+ int file_type
+)
+{
+ int i;
+ PlyFile *plyfile;
+ PlyElement *elem;
+
+ /* check for NULL file pointer */
+ if (fp == NULL)
+ return (NULL);
+
+ /* create a record for this object */
+
+ plyfile = (PlyFile *) myalloc (sizeof (PlyFile));
+ plyfile->file_type = file_type;
+ plyfile->num_comments = 0;
+ plyfile->num_obj_info = 0;
+ plyfile->nelems = nelems;
+ plyfile->version = 1.0;
+ plyfile->fp = fp;
+ plyfile->other_elems = NULL;
+
+ /* tuck aside the names of the elements */
+
+ plyfile->elems = (PlyElement **) myalloc (sizeof (PlyElement *) * nelems);
+ for (i = 0; i < nelems; i++) {
+ elem = (PlyElement *) myalloc (sizeof (PlyElement));
+ plyfile->elems[i] = elem;
+ elem->name = strdup (elem_names[i]);
+ elem->num = 0;
+ elem->nprops = 0;
+ }
+
+ /* return pointer to the file descriptor */
+ return (plyfile);
+}
+
+
+/******************************************************************************
+Open a polygon file for writing.
+
+Entry:
+ filename - name of file to read from
+ nelems - number of elements in object
+ elem_names - list of element names
+ file_type - file type, either ascii or binary
+
+Exit:
+ version - version number of PLY file
+ returns a file identifier, used to refer to this file, or NULL if error
+******************************************************************************/
+
+PlyFile *ply_open_for_writing(
+ const char *filename,
+ int nelems,
+ char **elem_names,
+ int file_type,
+ float *version
+)
+{
+ PlyFile *plyfile;
+ char *name;
+ FILE *fp;
+
+ /* tack on the extension .ply, if necessary */
+
+ name = (char *) myalloc (sizeof (char) * (strlen (filename) + 5));
+ strcpy (name, filename);
+ if (strlen (name) < 4 ||
+ strcmp (name + strlen (name) - 4, ".ply") != 0)
+ strcat (name, ".ply");
+
+ /* open the file for writing */
+
+ fp = fopen (name, "w");
+ if (fp == NULL) {
+ return (NULL);
+ }
+
+ /* create the actual PlyFile structure */
+
+ plyfile = ply_write (fp, nelems, elem_names, file_type);
+ if (plyfile == NULL)
+ return (NULL);
+
+ /* say what PLY file version number we're writing */
+ *version = plyfile->version;
+
+ /* return pointer to the file descriptor */
+ return (plyfile);
+}
+
+
+/******************************************************************************
+Describe an element, including its properties and how many will be written
+to the file.
+
+Entry:
+ plyfile - file identifier
+ elem_name - name of element that information is being specified about
+ nelems - number of elements of this type to be written
+ nprops - number of properties contained in the element
+ prop_list - list of properties
+******************************************************************************/
+
+void ply_describe_element(
+ PlyFile *plyfile,
+ char *elem_name,
+ int nelems,
+ int nprops,
+ PlyProperty *prop_list
+)
+{
+ int i;
+ PlyElement *elem;
+ PlyProperty *prop;
+
+ /* look for appropriate element */
+ elem = find_element (plyfile, elem_name);
+ if (elem == NULL) {
+ fprintf(stderr,"ply_describe_element: can't find element '%s'\n",elem_name);
+ exit (-1);
+ }
+
+ elem->num = nelems;
+
+ /* copy the list of properties */
+
+ elem->nprops = nprops;
+ elem->props = (PlyProperty **) myalloc (sizeof (PlyProperty *) * nprops);
+ elem->store_prop = (char *) myalloc (sizeof (char) * nprops);
+
+ for (i = 0; i < nprops; i++) {
+ prop = (PlyProperty *) myalloc (sizeof (PlyProperty));
+ elem->props[i] = prop;
+ elem->store_prop[i] = NAMED_PROP;
+ copy_property (prop, &prop_list[i]);
+ }
+}
+
+
+/******************************************************************************
+Describe a property of an element.
+
+Entry:
+ plyfile - file identifier
+ elem_name - name of element that information is being specified about
+ prop - the new property
+******************************************************************************/
+
+void ply_describe_property(
+ PlyFile *plyfile,
+ char *elem_name,
+ PlyProperty *prop
+)
+{
+ PlyElement *elem;
+ PlyProperty *elem_prop;
+
+ /* look for appropriate element */
+ elem = find_element (plyfile, elem_name);
+ if (elem == NULL) {
+ fprintf(stderr, "ply_describe_property: can't find element '%s'\n",
+ elem_name);
+ return;
+ }
+
+ /* create room for new property */
+
+ if (elem->nprops == 0) {
+ elem->props = (PlyProperty **) myalloc (sizeof (PlyProperty *));
+ elem->store_prop = (char *) myalloc (sizeof (char));
+ elem->nprops = 1;
+ }
+ else {
+ elem->nprops++;
+ elem->props = (PlyProperty **)
+ realloc (elem->props, sizeof (PlyProperty *) * elem->nprops);
+ elem->store_prop = (char *)
+ realloc (elem->store_prop, sizeof (char) * elem->nprops);
+ }
+
+ /* copy the new property */
+
+ elem_prop = (PlyProperty *) myalloc (sizeof (PlyProperty));
+ elem->props[elem->nprops - 1] = elem_prop;
+ elem->store_prop[elem->nprops - 1] = NAMED_PROP;
+ copy_property (elem_prop, prop);
+}
+
+
+/******************************************************************************
+Describe what the "other" properties are that are to be stored, and where
+they are in an element.
+******************************************************************************/
+
+void ply_describe_other_properties(
+ PlyFile *plyfile,
+ PlyOtherProp *other,
+ int offset
+)
+{
+ int i;
+ PlyElement *elem;
+ PlyProperty *prop;
+
+ /* look for appropriate element */
+ elem = find_element (plyfile, other->name);
+ if (elem == NULL) {
+ fprintf(stderr, "ply_describe_other_properties: can't find element '%s'\n",
+ other->name);
+ return;
+ }
+
+ /* create room for other properties */
+
+ if (elem->nprops == 0) {
+ elem->props = (PlyProperty **)
+ myalloc (sizeof (PlyProperty *) * other->nprops);
+ elem->store_prop = (char *) myalloc (sizeof (char) * other->nprops);
+ elem->nprops = 0;
+ }
+ else {
+ int newsize;
+ newsize = elem->nprops + other->nprops;
+ elem->props = (PlyProperty **)
+ realloc (elem->props, sizeof (PlyProperty *) * newsize);
+ elem->store_prop = (char *)
+ realloc (elem->store_prop, sizeof (char) * newsize);
+ }
+
+ /* copy the other properties */
+
+ for (i = 0; i < other->nprops; i++) {
+ prop = (PlyProperty *) myalloc (sizeof (PlyProperty));
+ copy_property (prop, other->props[i]);
+ elem->props[elem->nprops] = prop;
+ elem->store_prop[elem->nprops] = OTHER_PROP;
+ elem->nprops++;
+ }
+
+ /* save other info about other properties */
+ elem->other_size = other->size;
+ elem->other_offset = offset;
+}
+
+
+/******************************************************************************
+State how many of a given element will be written.
+
+Entry:
+ plyfile - file identifier
+ elem_name - name of element that information is being specified about
+ nelems - number of elements of this type to be written
+******************************************************************************/
+
+void ply_element_count(
+ PlyFile *plyfile,
+ char *elem_name,
+ int nelems
+)
+{
+ PlyElement *elem;
+
+ /* look for appropriate element */
+ elem = find_element (plyfile, elem_name);
+ if (elem == NULL) {
+ fprintf(stderr,"ply_element_count: can't find element '%s'\n",elem_name);
+ exit (-1);
+ }
+
+ elem->num = nelems;
+}
+
+
+/******************************************************************************
+Signal that we've described everything a PLY file's header and that the
+header should be written to the file.
+
+Entry:
+ plyfile - file identifier
+******************************************************************************/
+
+void ply_header_complete(PlyFile *plyfile)
+{
+ int i,j;
+ FILE *fp = plyfile->fp;
+ PlyElement *elem;
+ PlyProperty *prop;
+
+ fprintf (fp, "ply\n");
+
+ switch (plyfile->file_type) {
+ case PLY_ASCII:
+ fprintf (fp, "format ascii 1.0\n");
+ break;
+ case PLY_BINARY_BE:
+ fprintf (fp, "format binary_big_endian 1.0\n");
+ break;
+ case PLY_BINARY_LE:
+ fprintf (fp, "format binary_little_endian 1.0\n");
+ break;
+ default:
+ fprintf (stderr, "ply_header_complete: bad file type = %d\n",
+ plyfile->file_type);
+ exit (-1);
+ }
+
+ /* write out the comments */
+
+ for (i = 0; i < plyfile->num_comments; i++)
+ fprintf (fp, "comment %s\n", plyfile->comments[i]);
+
+ /* write out object information */
+
+ for (i = 0; i < plyfile->num_obj_info; i++)
+ fprintf (fp, "obj_info %s\n", plyfile->obj_info[i]);
+
+ /* write out information about each element */
+
+ for (i = 0; i < plyfile->nelems; i++) {
+
+ elem = plyfile->elems[i];
+ fprintf (fp, "element %s %d\n", elem->name, elem->num);
+
+ /* write out each property */
+ for (j = 0; j < elem->nprops; j++) {
+ prop = elem->props[j];
+ if (prop->is_list) {
+ fprintf (fp, "property list ");
+ write_scalar_type (fp, prop->count_external);
+ fprintf (fp, " ");
+ write_scalar_type (fp, prop->external_type);
+ fprintf (fp, " %s\n", prop->name);
+ }
+ else {
+ fprintf (fp, "property ");
+ write_scalar_type (fp, prop->external_type);
+ fprintf (fp, " %s\n", prop->name);
+ }
+ }
+ }
+
+ fprintf (fp, "end_header\n");
+}
+
+
+/******************************************************************************
+Specify which elements are going to be written. This should be called
+before a call to the routine ply_put_element().
+
+Entry:
+ plyfile - file identifier
+ elem_name - name of element we're talking about
+******************************************************************************/
+
+void ply_put_element_setup(PlyFile *plyfile, char *elem_name)
+{
+ PlyElement *elem;
+
+ elem = find_element (plyfile, elem_name);
+ if (elem == NULL) {
+ fprintf(stderr, "ply_elements_setup: can't find element '%s'\n", elem_name);
+ exit (-1);
+ }
+
+ plyfile->which_elem = elem;
+}
+
+
+/******************************************************************************
+Write an element to the file. This routine assumes that we're
+writing the type of element specified in the last call to the routine
+ply_put_element_setup().
+
+Entry:
+ plyfile - file identifier
+ elem_ptr - pointer to the element
+******************************************************************************/
+
+void ply_put_element(PlyFile *plyfile, void *elem_ptr)
+{
+ int j,k;
+ FILE *fp = plyfile->fp;
+ PlyElement *elem;
+ PlyProperty *prop;
+ char *elem_data,*item;
+ char **item_ptr;
+ int list_count;
+ int item_size;
+ int int_val;
+ unsigned int uint_val;
+ double double_val;
+ char **other_ptr;
+
+ elem = plyfile->which_elem;
+ elem_data = elem_ptr;
+ other_ptr = (char **) (((char *) elem_ptr) + elem->other_offset);
+
+ /* write out either to an ascii or binary file */
+
+ if (plyfile->file_type == PLY_ASCII) {
+
+ /* write an ascii file */
+
+ /* write out each property of the element */
+ for (j = 0; j < elem->nprops; j++) {
+ prop = elem->props[j];
+ if (elem->store_prop[j] == OTHER_PROP)
+ elem_data = *other_ptr;
+ else
+ elem_data = elem_ptr;
+ if (prop->is_list) {
+ item = elem_data + prop->count_offset;
+ get_stored_item ((void *) item, prop->count_internal,
+ &int_val, &uint_val, &double_val);
+ write_ascii_item (fp, int_val, uint_val, double_val,
+ prop->count_external);
+ list_count = uint_val;
+ item_ptr = (char **) (elem_data + prop->offset);
+ item = item_ptr[0];
+ item_size = ply_type_size[prop->internal_type];
+ for (k = 0; k < list_count; k++) {
+ get_stored_item ((void *) item, prop->internal_type,
+ &int_val, &uint_val, &double_val);
+ write_ascii_item (fp, int_val, uint_val, double_val,
+ prop->external_type);
+ item += item_size;
+ }
+ }
+ else {
+ item = elem_data + prop->offset;
+ get_stored_item ((void *) item, prop->internal_type,
+ &int_val, &uint_val, &double_val);
+ write_ascii_item (fp, int_val, uint_val, double_val,
+ prop->external_type);
+ }
+ }
+
+ fprintf (fp, "\n");
+ }
+ else {
+
+ /* write a binary file */
+
+ /* write out each property of the element */
+ for (j = 0; j < elem->nprops; j++) {
+ prop = elem->props[j];
+ if (elem->store_prop[j] == OTHER_PROP)
+ elem_data = *other_ptr;
+ else
+ elem_data = elem_ptr;
+ if (prop->is_list) {
+ item = elem_data + prop->count_offset;
+ item_size = ply_type_size[prop->count_internal];
+ get_stored_item ((void *) item, prop->count_internal,
+ &int_val, &uint_val, &double_val);
+ write_binary_item (fp, int_val, uint_val, double_val,
+ prop->count_external);
+ list_count = uint_val;
+ item_ptr = (char **) (elem_data + prop->offset);
+ item = item_ptr[0];
+ item_size = ply_type_size[prop->internal_type];
+ for (k = 0; k < list_count; k++) {
+ get_stored_item ((void *) item, prop->internal_type,
+ &int_val, &uint_val, &double_val);
+ write_binary_item (fp, int_val, uint_val, double_val,
+ prop->external_type);
+ item += item_size;
+ }
+ }
+ else {
+ item = elem_data + prop->offset;
+ item_size = ply_type_size[prop->internal_type];
+ get_stored_item ((void *) item, prop->internal_type,
+ &int_val, &uint_val, &double_val);
+ write_binary_item (fp, int_val, uint_val, double_val,
+ prop->external_type);
+ }
+ }
+
+ }
+}
+
+
+/******************************************************************************
+Specify a comment that will be written in the header.
+
+Entry:
+ plyfile - file identifier
+ comment - the comment to be written
+******************************************************************************/
+
+void ply_put_comment(PlyFile *plyfile, char *comment)
+{
+ /* (re)allocate space for new comment */
+ if (plyfile->num_comments == 0)
+ plyfile->comments = (char **) myalloc (sizeof (char *));
+ else
+ plyfile->comments = (char **) realloc (plyfile->comments,
+ sizeof (char *) * (plyfile->num_comments + 1));
+
+ /* add comment to list */
+ plyfile->comments[plyfile->num_comments] = strdup (comment);
+ plyfile->num_comments++;
+}
+
+
+/******************************************************************************
+Specify a piece of object information (arbitrary text) that will be written
+in the header.
+
+Entry:
+ plyfile - file identifier
+ obj_info - the text information to be written
+******************************************************************************/
+
+void ply_put_obj_info(PlyFile *plyfile, char *obj_info)
+{
+ /* (re)allocate space for new info */
+ if (plyfile->num_obj_info == 0)
+ plyfile->obj_info = (char **) myalloc (sizeof (char *));
+ else
+ plyfile->obj_info = (char **) realloc (plyfile->obj_info,
+ sizeof (char *) * (plyfile->num_obj_info + 1));
+
+ /* add info to list */
+ plyfile->obj_info[plyfile->num_obj_info] = strdup (obj_info);
+ plyfile->num_obj_info++;
+}
+
+
+
+
+
+
+
+/*************/
+/* Reading */
+/*************/
+
+
+
+/******************************************************************************
+Given a file pointer, get ready to read PLY data from the file.
+
+Entry:
+ fp - the given file pointer
+
+Exit:
+ nelems - number of elements in object
+ elem_names - list of element names
+ returns a pointer to a PlyFile, used to refer to this file, or NULL if error
+******************************************************************************/
+
+PlyFile *ply_read(FILE *fp, int *nelems, char ***elem_names)
+{
+ int i,j;
+ PlyFile *plyfile;
+ int nwords;
+ char **words;
+ char **elist;
+ PlyElement *elem;
+ char *orig_line;
+
+ /* check for NULL file pointer */
+ if (fp == NULL)
+ return (NULL);
+
+ /* create record for this object */
+
+ plyfile = (PlyFile *) myalloc (sizeof (PlyFile));
+ plyfile->nelems = 0;
+ plyfile->comments = NULL;
+ plyfile->num_comments = 0;
+ plyfile->obj_info = NULL;
+ plyfile->num_obj_info = 0;
+ plyfile->fp = fp;
+ plyfile->other_elems = NULL;
+
+ /* read and parse the file's header */
+
+ words = get_words (plyfile->fp, &nwords, &orig_line);
+ if (!words || !equal_strings (words[0], "ply"))
+ {
+ if (words) free(words);
+ return (NULL);
+ }
+
+ while (words) {
+
+ /* parse words */
+
+ if (equal_strings (words[0], "format")) {
+ if (nwords != 3)
+ return (NULL);
+ if (equal_strings (words[1], "ascii"))
+ plyfile->file_type = PLY_ASCII;
+ else if (equal_strings (words[1], "binary_big_endian"))
+ plyfile->file_type = PLY_BINARY_BE;
+ else if (equal_strings (words[1], "binary_little_endian"))
+ plyfile->file_type = PLY_BINARY_LE;
+ else
+ {
+ free(words);
+ return (NULL);
+ }
+
+ plyfile->version = (float)atof (words[2]);
+ }
+ else if (equal_strings (words[0], "element"))
+ add_element (plyfile, words, nwords);
+ else if (equal_strings (words[0], "property"))
+ add_property (plyfile, words, nwords);
+ else if (equal_strings (words[0], "comment"))
+ add_comment (plyfile, orig_line);
+ else if (equal_strings (words[0], "obj_info"))
+ add_obj_info (plyfile, orig_line);
+ else if (equal_strings (words[0], "end_header"))
+ {
+ free(words);
+ break;
+ }
+ /* free up words space */
+ free (words);
+
+ words = get_words (plyfile->fp, &nwords, &orig_line);
+ }
+
+ /* create tags for each property of each element, to be used */
+ /* later to say whether or not to store each property for the user */
+
+ for (i = 0; i < plyfile->nelems; i++) {
+ elem = plyfile->elems[i];
+ elem->store_prop = (char *) myalloc (sizeof (char) * elem->nprops);
+ for (j = 0; j < elem->nprops; j++)
+ elem->store_prop[j] = DONT_STORE_PROP;
+ elem->other_offset = NO_OTHER_PROPS; /* no "other" props by default */
+ }
+
+ /* set return values about the elements */
+
+ elist = (char **) myalloc (sizeof (char *) * plyfile->nelems);
+ for (i = 0; i < plyfile->nelems; i++)
+ elist[i] = strdup (plyfile->elems[i]->name);
+
+ *elem_names = elist;
+ *nelems = plyfile->nelems;
+
+ /* return a pointer to the file's information */
+
+ return (plyfile);
+}
+
+
+/******************************************************************************
+Open a polygon file for reading.
+
+Entry:
+ filename - name of file to read from
+
+Exit:
+ nelems - number of elements in object
+ elem_names - list of element names
+ file_type - file type, either ascii or binary
+ version - version number of PLY file
+ returns a file identifier, used to refer to this file, or NULL if error
+******************************************************************************/
+
+PlyFile *ply_open_for_reading(
+ const char *filename,
+ int *nelems,
+ char ***elem_names,
+ int *file_type,
+ float *version
+)
+{
+ FILE *fp;
+ PlyFile *plyfile;
+
+ /* open the file for reading */
+
+ fp = fopen (filename, "r");
+ if (fp == NULL)
+ return (NULL);
+
+ /* create the PlyFile data structure */
+
+ plyfile = ply_read (fp, nelems, elem_names);
+
+ /* determine the file type and version */
+ if (plyfile != NULL)
+ {
+ *file_type = plyfile->file_type;
+ *version = plyfile->version;
+ }
+
+ /* return a pointer to the file's information */
+ return (plyfile);
+}
+
+
+/******************************************************************************
+Get information about a particular element.
+
+Entry:
+ plyfile - file identifier
+ elem_name - name of element to get information about
+
+Exit:
+ nelems - number of elements of this type in the file
+ nprops - number of properties
+ returns a list of properties, or NULL if the file doesn't contain that elem
+******************************************************************************/
+
+PlyProperty **ply_get_element_description(
+ PlyFile *plyfile,
+ char *elem_name,
+ int *nelems,
+ int *nprops
+)
+{
+ int i;
+ PlyElement *elem;
+ PlyProperty *prop;
+ PlyProperty **prop_list;
+
+ /* find information about the element */
+ elem = find_element (plyfile, elem_name);
+ if (elem == NULL)
+ return (NULL);
+
+ *nelems = elem->num;
+ *nprops = elem->nprops;
+
+ /* make a copy of the element's property list */
+ prop_list = (PlyProperty **) myalloc (sizeof (PlyProperty *) * elem->nprops);
+ for (i = 0; i < elem->nprops; i++) {
+ prop = (PlyProperty *) myalloc (sizeof (PlyProperty));
+ copy_property (prop, elem->props[i]);
+ prop_list[i] = prop;
+ }
+
+ /* return this duplicate property list */
+ return (prop_list);
+}
+
+
+/******************************************************************************
+Specify which properties of an element are to be returned. This should be
+called before a call to the routine ply_get_element().
+
+Entry:
+ plyfile - file identifier
+ elem_name - which element we're talking about
+ nprops - number of properties
+ prop_list - list of properties
+******************************************************************************/
+
+void ply_get_element_setup(
+ PlyFile *plyfile,
+ char *elem_name,
+ int nprops,
+ PlyProperty *prop_list
+)
+{
+ int i;
+ PlyElement *elem;
+ PlyProperty *prop;
+ int index;
+
+ /* find information about the element */
+ elem = find_element (plyfile, elem_name);
+ plyfile->which_elem = elem;
+
+ /* deposit the property information into the element's description */
+ for (i = 0; i < nprops; i++) {
+
+ /* look for actual property */
+ prop = find_property (elem, prop_list[i].name, &index);
+ if (prop == NULL) {
+ fprintf (stderr, "Warning: Can't find property '%s' in element '%s'\n",
+ prop_list[i].name, elem_name);
+ continue;
+ }
+
+ /* store its description */
+ prop->internal_type = prop_list[i].internal_type;
+ prop->offset = prop_list[i].offset;
+ prop->count_internal = prop_list[i].count_internal;
+ prop->count_offset = prop_list[i].count_offset;
+
+ /* specify that the user wants this property */
+ elem->store_prop[index] = STORE_PROP;
+ }
+}
+
+
+/******************************************************************************
+Specify a property of an element that is to be returned. This should be
+called (usually multiple times) before a call to the routine ply_get_element().
+This routine should be used in preference to the less flexible old routine
+called ply_get_element_setup().
+
+Entry:
+ plyfile - file identifier
+ elem_name - which element we're talking about
+ prop - property to add to those that will be returned
+******************************************************************************/
+
+void ply_get_property(
+ PlyFile *plyfile,
+ char *elem_name,
+ PlyProperty *prop
+)
+{
+ PlyElement *elem;
+ PlyProperty *prop_ptr;
+ int index;
+
+ /* find information about the element */
+ elem = find_element (plyfile, elem_name);
+ plyfile->which_elem = elem;
+
+ /* deposit the property information into the element's description */
+
+ prop_ptr = find_property (elem, prop->name, &index);
+ if (prop_ptr == NULL) {
+ fprintf (stderr, "Warning: Can't find property '%s' in element '%s'\n",
+ prop->name, elem_name);
+ return;
+ }
+ prop_ptr->internal_type = prop->internal_type;
+ prop_ptr->offset = prop->offset;
+ prop_ptr->count_internal = prop->count_internal;
+ prop_ptr->count_offset = prop->count_offset;
+
+ /* specify that the user wants this property */
+ elem->store_prop[index] = STORE_PROP;
+}
+
+
+/******************************************************************************
+Read one element from the file. This routine assumes that we're reading
+the type of element specified in the last call to the routine
+ply_get_element_setup().
+
+Entry:
+ plyfile - file identifier
+ elem_ptr - pointer to location where the element information should be put
+******************************************************************************/
+
+void ply_get_element(PlyFile *plyfile, void *elem_ptr)
+{
+ if (plyfile->file_type == PLY_ASCII)
+ ascii_get_element (plyfile, (char *) elem_ptr);
+ else
+ binary_get_element (plyfile, (char *) elem_ptr);
+}
+
+
+/******************************************************************************
+Extract the comments from the header information of a PLY file.
+
+Entry:
+ plyfile - file identifier
+
+Exit:
+ num_comments - number of comments returned
+ returns a pointer to a list of comments
+******************************************************************************/
+
+char **ply_get_comments(PlyFile *plyfile, int *num_comments)
+{
+ *num_comments = plyfile->num_comments;
+ return (plyfile->comments);
+}
+
+
+/******************************************************************************
+Extract the object information (arbitrary text) from the header information
+of a PLY file.
+
+Entry:
+ plyfile - file identifier
+
+Exit:
+ num_obj_info - number of lines of text information returned
+ returns a pointer to a list of object info lines
+******************************************************************************/
+
+char **ply_get_obj_info(PlyFile *plyfile, int *num_obj_info)
+{
+ *num_obj_info = plyfile->num_obj_info;
+ return (plyfile->obj_info);
+}
+
+
+/******************************************************************************
+Make ready for "other" properties of an element-- those properties that
+the user has not explicitly asked for, but that are to be stashed away
+in a special structure to be carried along with the element's other
+information.
+
+Entry:
+ plyfile - file identifier
+ elem - element for which we want to save away other properties
+******************************************************************************/
+
+void setup_other_props(PlyFile *plyfile, PlyElement *elem)
+{
+ int i;
+ PlyProperty *prop;
+ int size = 0;
+ int type_size;
+
+ /* Examine each property in decreasing order of size. */
+ /* We do this so that all data types will be aligned by */
+ /* word, half-word, or whatever within the structure. */
+
+ for (type_size = 8; type_size > 0; type_size /= 2) {
+
+ /* add up the space taken by each property, and save this information */
+ /* away in the property descriptor */
+
+ for (i = 0; i < elem->nprops; i++) {
+
+ /* don't bother with properties we've been asked to store explicitly */
+ if (elem->store_prop[i])
+ continue;
+
+ prop = elem->props[i];
+
+ /* internal types will be same as external */
+ prop->internal_type = prop->external_type;
+ prop->count_internal = prop->count_external;
+
+ /* check list case */
+ if (prop->is_list) {
+
+ /* pointer to list */
+ if (type_size == sizeof (void *)) {
+ prop->offset = size;
+ size += sizeof (void *); /* always use size of a pointer here */
+ }
+
+ /* count of number of list elements */
+ if (type_size == ply_type_size[prop->count_external]) {
+ prop->count_offset = size;
+ size += ply_type_size[prop->count_external];
+ }
+ }
+ /* not list */
+ else if (type_size == ply_type_size[prop->external_type]) {
+ prop->offset = size;
+ size += ply_type_size[prop->external_type];
+ }
+ }
+
+ }
+
+ /* save the size for the other_props structure */
+ elem->other_size = size;
+}
+
+
+/******************************************************************************
+Specify that we want the "other" properties of an element to be tucked
+away within the user's structure. The user needn't be concerned for how
+these properties are stored.
+
+Entry:
+ plyfile - file identifier
+ elem_name - name of element that we want to store other_props in
+ offset - offset to where other_props will be stored inside user's structure
+
+Exit:
+ returns pointer to structure containing description of other_props
+******************************************************************************/
+
+PlyOtherProp *ply_get_other_properties(
+ PlyFile *plyfile,
+ char *elem_name,
+ int offset
+)
+{
+ int i;
+ PlyElement *elem;
+
+ PlyOtherProp *other;
+ PlyProperty *prop;
+ int nprops;
+
+ /* find information about the element */
+ elem = find_element (plyfile, elem_name);
+ if (elem == NULL) {
+ fprintf (stderr, "ply_get_other_properties: Can't find element '%s'\n",
+ elem_name);
+ return (NULL);
+ }
+
+ /* remember that this is the "current" element */
+ plyfile->which_elem = elem;
+
+ /* save the offset to where to store the other_props */
+ elem->other_offset = offset;
+
+ /* place the appropriate pointers, etc. in the element's property list */
+ setup_other_props (plyfile, elem);
+
+ /* create structure for describing other_props */
+ other = (PlyOtherProp *) myalloc (sizeof (PlyOtherProp));
+ other->name = strdup (elem_name);
+#if 0
+ if (elem->other_offset == NO_OTHER_PROPS) {
+ other->size = 0;
+ other->props = NULL;
+ other->nprops = 0;
+ return (other);
+ }
+#endif
+ other->size = elem->other_size;
+ other->props = (PlyProperty **) myalloc (sizeof(PlyProperty) * elem->nprops);
+
+ /* save descriptions of each "other" property */
+ nprops = 0;
+ for (i = 0; i < elem->nprops; i++) {
+ if (elem->store_prop[i])
+ continue;
+ prop = (PlyProperty *) myalloc (sizeof (PlyProperty));
+ copy_property (prop, elem->props[i]);
+ other->props[nprops] = prop;
+ nprops++;
+ }
+ other->nprops = nprops;
+
+#if 1
+ /* set other_offset pointer appropriately if there are NO other properties */
+ if (other->nprops == 0) {
+ elem->other_offset = NO_OTHER_PROPS;
+ }
+#endif
+
+ /* return structure */
+ return (other);
+}
+
+
+
+
+/*************************/
+/* Other Element Stuff */
+/*************************/
+
+
+
+
+/******************************************************************************
+Grab all the data for an element that a user does not want to explicitly
+read in.
+
+Entry:
+ plyfile - pointer to file
+ elem_name - name of element whose data is to be read in
+ elem_count - number of instances of this element stored in the file
+
+Exit:
+ returns pointer to ALL the "other" element data for this PLY file
+******************************************************************************/
+
+PlyOtherElems *ply_get_other_element (
+ PlyFile *plyfile,
+ char *elem_name,
+ int elem_count
+)
+{
+ int i;
+ PlyElement *elem;
+ PlyOtherElems *other_elems;
+ OtherElem *other;
+
+ /* look for appropriate element */
+ elem = find_element (plyfile, elem_name);
+ if (elem == NULL) {
+ fprintf (stderr,
+ "ply_get_other_element: can't find element '%s'\n", elem_name);
+ exit (-1);
+ }
+
+ /* create room for the new "other" element, initializing the */
+ /* other data structure if necessary */
+
+ if (plyfile->other_elems == NULL) {
+ plyfile->other_elems = (PlyOtherElems *) myalloc (sizeof (PlyOtherElems));
+ other_elems = plyfile->other_elems;
+ other_elems->other_list = (OtherElem *) myalloc (sizeof (OtherElem));
+ other = &(other_elems->other_list[0]);
+ other_elems->num_elems = 1;
+ }
+ else {
+ other_elems = plyfile->other_elems;
+ other_elems->other_list = (OtherElem *) realloc (other_elems->other_list,
+ sizeof (OtherElem) * (other_elems->num_elems + 1));
+ other = &(other_elems->other_list[other_elems->num_elems]);
+ other_elems->num_elems++;
+ }
+
+ /* count of element instances in file */
+ other->elem_count = elem_count;
+
+ /* save name of element */
+ other->elem_name = strdup (elem_name);
+
+ /* create a list to hold all the current elements */
+ other->other_data = (OtherData **)
+ malloc (sizeof (OtherData *) * other->elem_count);
+
+ /* set up for getting elements */
+ other->other_props = ply_get_other_properties (plyfile, elem_name,
+ offsetof(OtherData,other_props));
+
+ /* grab all these elements */
+ for (i = 0; i < other->elem_count; i++) {
+ /* grab and element from the file */
+ other->other_data[i] = (OtherData *) malloc (sizeof (OtherData));
+ ply_get_element (plyfile, (void *) other->other_data[i]);
+ }
+
+ /* return pointer to the other elements data */
+ return (other_elems);
+}
+
+
+/******************************************************************************
+Pass along a pointer to "other" elements that we want to save in a given
+PLY file. These other elements were presumably read from another PLY file.
+
+Entry:
+ plyfile - file pointer in which to store this other element info
+ other_elems - info about other elements that we want to store
+******************************************************************************/
+
+void ply_describe_other_elements (
+ PlyFile *plyfile,
+ PlyOtherElems *other_elems
+)
+{
+ int i;
+ OtherElem *other;
+
+ /* ignore this call if there is no other element */
+ if (other_elems == NULL)
+ return;
+
+ /* save pointer to this information */
+ plyfile->other_elems = other_elems;
+
+ /* describe the other properties of this element */
+
+ for (i = 0; i < other_elems->num_elems; i++) {
+ other = &(other_elems->other_list[i]);
+ ply_element_count (plyfile, other->elem_name, other->elem_count);
+ ply_describe_other_properties (plyfile, other->other_props,
+ offsetof(OtherData,other_props));
+ }
+}
+
+
+/******************************************************************************
+Write out the "other" elements specified for this PLY file.
+
+Entry:
+ plyfile - pointer to PLY file to write out other elements for
+******************************************************************************/
+
+void ply_put_other_elements (PlyFile *plyfile)
+{
+ int i,j;
+ OtherElem *other;
+
+ /* make sure we have other elements to write */
+ if (plyfile->other_elems == NULL)
+ return;
+
+ /* write out the data for each "other" element */
+
+ for (i = 0; i < plyfile->other_elems->num_elems; i++) {
+
+ other = &(plyfile->other_elems->other_list[i]);
+ ply_put_element_setup (plyfile, other->elem_name);
+
+ /* write out each instance of the current element */
+ for (j = 0; j < other->elem_count; j++)
+ ply_put_element (plyfile, (void *) other->other_data[j]);
+ }
+}
+
+
+/******************************************************************************
+Free up storage used by an "other" elements data structure.
+
+Entry:
+ other_elems - data structure to free up
+******************************************************************************/
+
+void ply_free_other_elements (PlyOtherElems *other_elems)
+{
+
+}
+
+
+
+/*******************/
+/* Miscellaneous */
+/*******************/
+
+
+
+/******************************************************************************
+Close a PLY file.
+
+Entry:
+ plyfile - identifier of file to close
+******************************************************************************/
+
+void ply_close(PlyFile *plyfile)
+{
+ int i;
+ int j;
+ PlyElement* elem;
+
+ fclose (plyfile->fp);
+ for (i=0; i<plyfile->nelems; i++)
+ {
+ elem = plyfile->elems[i];
+ free(elem->name);
+ for (j=0; j<elem->nprops; j++)
+ {
+ free((elem->props[j]->name));
+ free (elem->props[j]);
+ }
+ free (elem->props);
+ free (elem->store_prop);
+ free (elem);
+ }
+ free(plyfile->elems);
+
+ for (i=0; i<plyfile->num_comments; i++)
+ {
+ free (plyfile->comments[i]);
+ }
+ free (plyfile->comments);
+
+ for (i=0; i<plyfile->num_obj_info; i++)
+ {
+ free (plyfile->obj_info[i]);
+ }
+ free (plyfile->obj_info);
+
+ if (plyfile->other_elems != NULL)
+ {
+ for (i=0; i<plyfile->other_elems->num_elems; ++i)
+ {
+ OtherElem* other = &(plyfile->other_elems->other_list[i]);
+ free(other->elem_name);
+ for (j=0; j<other->elem_count; ++j)
+ {
+ free(other->other_data[j]);
+ }
+ free(other->other_data);
+ free(other->other_props->name);
+ for (j=0; j<other->other_props->nprops; ++j)
+ {
+ free(other->other_props->props[j]->name);
+ free(other->other_props->props[j]);
+ }
+ free(other->other_props->props);
+ free(other->other_props);
+ }
+ free(plyfile->other_elems->other_list);
+ free(plyfile->other_elems);
+ }
+
+ free (plyfile);
+
+}
+
+
+/******************************************************************************
+Get version number and file type of a PlyFile.
+
+Entry:
+ ply - pointer to PLY file
+
+Exit:
+ version - version of the file
+ file_type - PLY_ASCII, PLY_BINARY_BE, or PLY_BINARY_LE
+******************************************************************************/
+
+void ply_get_info(PlyFile *ply, float *version, int *file_type)
+{
+ if (ply == NULL)
+ return;
+
+ *version = ply->version;
+ *file_type = ply->file_type;
+}
+
+
+/******************************************************************************
+Compare two strings. Returns 1 if they are the same, 0 if not.
+******************************************************************************/
+
+int equal_strings(const char *s1,const char *s2)
+{
+
+ while (*s1 && *s2)
+ if (*s1++ != *s2++)
+ return (0);
+
+ if (*s1 != *s2)
+ return (0);
+ else
+ return (1);
+}
+
+
+/******************************************************************************
+Find an element from the element list of a given PLY object.
+
+Entry:
+ plyfile - file id for PLY file
+ element - name of element we're looking for
+
+Exit:
+ returns the element, or NULL if not found
+******************************************************************************/
+
+PlyElement *find_element(PlyFile *plyfile,const char *element)
+{
+ int i;
+
+ for (i = 0; i < plyfile->nelems; i++)
+ if (equal_strings (element, plyfile->elems[i]->name))
+ return (plyfile->elems[i]);
+
+ return (NULL);
+}
+
+
+/******************************************************************************
+Find a property in the list of properties of a given element.
+
+Entry:
+ elem - pointer to element in which we want to find the property
+ prop_name - name of property to find
+
+Exit:
+ index - index to position in list
+ returns a pointer to the property, or NULL if not found
+******************************************************************************/
+
+PlyProperty *find_property(PlyElement *elem, const char *prop_name, int *index)
+{
+ int i;
+
+ for (i = 0; i < elem->nprops; i++)
+ if (equal_strings (prop_name, elem->props[i]->name)) {
+ *index = i;
+ return (elem->props[i]);
+ }
+
+ *index = -1;
+ return (NULL);
+}
+
+
+/******************************************************************************
+Read an element from an ascii file.
+
+Entry:
+ plyfile - file identifier
+ elem_ptr - pointer to element
+******************************************************************************/
+
+void ascii_get_element(PlyFile *plyfile, char *elem_ptr)
+{
+ int j,k;
+ PlyElement *elem;
+ PlyProperty *prop;
+ char **words;
+ int nwords;
+ int which_word;
+ FILE *fp = plyfile->fp;
+ char *elem_data,*item;
+ char *item_ptr;
+ int item_size;
+ int int_val;
+ unsigned int uint_val;
+ double double_val;
+ int list_count;
+ int store_it;
+ char **store_array;
+ char *orig_line;
+ char *other_data = NULL;
+ int other_flag;
+
+ /* the kind of element we're reading currently */
+ elem = plyfile->which_elem;
+
+ /* do we need to setup for other_props? */
+
+// NOTE HS-2014-02-12 This leaks, it is easier to disable the 'other' functionality than to
+// fix the leak, vtk solves this by allocating on an internal heap rather than the global heap
+// if (elem->other_offset != NO_OTHER_PROPS) {
+// char **ptr;
+// other_flag = 1;
+// /* make room for other_props */
+// other_data = (char *) myalloc (elem->other_size);
+// /* store pointer in user's structure to the other_props */
+// ptr = (char **) (elem_ptr + elem->other_offset);
+// *ptr = other_data;
+// }
+// else
+ other_flag = 0;
+
+ /* read in the element */
+
+ words = get_words (fp, &nwords, &orig_line);
+ if (words == NULL) {
+ fprintf (stderr, "ply_get_element: unexpected end of file\n");
+ exit (-1);
+ }
+
+ which_word = 0;
+
+ for (j = 0; j < elem->nprops; j++) {
+
+ prop = elem->props[j];
+ store_it = (elem->store_prop[j] | other_flag);
+
+ /* store either in the user's structure or in other_props */
+ if (elem->store_prop[j])
+ elem_data = elem_ptr;
+ else
+ elem_data = other_data;
+
+ if (prop->is_list) { /* a list */
+
+ /* get and store the number of items in the list */
+ get_ascii_item (words[which_word++], prop->count_external,
+ &int_val, &uint_val, &double_val);
+ if (store_it) {
+ item = elem_data + prop->count_offset;
+ store_item(item, prop->count_internal, int_val, uint_val, double_val);
+ }
+
+ /* allocate space for an array of items and store a ptr to the array */
+ list_count = int_val;
+ item_size = ply_type_size[prop->internal_type];
+ store_array = (char **) (elem_data + prop->offset);
+
+ if (list_count == 0) {
+ if (store_it)
+ *store_array = NULL;
+ }
+ else {
+ if (store_it) {
+ item_ptr = (char *) myalloc (sizeof (char) * item_size * list_count);
+ item = item_ptr;
+ *store_array = item_ptr;
+ }
+
+ /* read items and store them into the array */
+ for (k = 0; k < list_count; k++) {
+ get_ascii_item (words[which_word++], prop->external_type,
+ &int_val, &uint_val, &double_val);
+ if (store_it) {
+ store_item (item, prop->internal_type,
+ int_val, uint_val, double_val);
+ item += item_size;
+ }
+ }
+ }
+
+ }
+ else { /* not a list */
+ get_ascii_item (words[which_word++], prop->external_type,
+ &int_val, &uint_val, &double_val);
+ if (store_it) {
+ item = elem_data + prop->offset;
+ store_item (item, prop->internal_type, int_val, uint_val, double_val);
+ }
+ }
+
+ }
+
+ free (words);
+}
+
+
+/******************************************************************************
+Read an element from a binary file.
+
+Entry:
+ plyfile - file identifier
+ elem_ptr - pointer to an element
+******************************************************************************/
+
+void binary_get_element(PlyFile *plyfile, char *elem_ptr)
+{
+ int j,k;
+ PlyElement *elem;
+ PlyProperty *prop;
+ FILE *fp = plyfile->fp;
+ char *elem_data,*item;
+ char *item_ptr;
+ int item_size;
+ int int_val;
+ unsigned int uint_val;
+ double double_val;
+ int list_count;
+ int store_it;
+ char **store_array;
+ char *other_data;
+ int other_flag;
+
+ /* the kind of element we're reading currently */
+ elem = plyfile->which_elem;
+
+ /* do we need to setup for other_props? */
+
+ if (elem->other_offset != NO_OTHER_PROPS) {
+ char **ptr;
+ other_flag = 1;
+ /* make room for other_props */
+ other_data = (char *) myalloc (elem->other_size);
+ /* store pointer in user's structure to the other_props */
+ ptr = (char **) (elem_ptr + elem->other_offset);
+ *ptr = other_data;
+ }
+ else
+ other_flag = 0;
+
+ /* read in a number of elements */
+
+ for (j = 0; j < elem->nprops; j++) {
+
+ prop = elem->props[j];
+ store_it = (elem->store_prop[j] | other_flag);
+
+ /* store either in the user's structure or in other_props */
+ if (elem->store_prop[j])
+ elem_data = elem_ptr;
+ else
+ elem_data = other_data;
+
+ if (prop->is_list) { /* a list */
+
+ /* get and store the number of items in the list */
+ get_binary_item (fp, prop->count_external,
+ &int_val, &uint_val, &double_val);
+ if (store_it) {
+ item = elem_data + prop->count_offset;
+ store_item(item, prop->count_internal, int_val, uint_val, double_val);
+ }
+
+ /* allocate space for an array of items and store a ptr to the array */
+ list_count = int_val;
+ /* The "if" was added by Afra Zomorodian 8/22/95
+ * so that zipper won't crash reading plies that have additional
+ * properties.
+ */
+ if (store_it) {
+ item_size = ply_type_size[prop->internal_type];
+ }
+ store_array = (char **) (elem_data + prop->offset);
+ if (list_count == 0) {
+ if (store_it)
+ *store_array = NULL;
+ }
+ else {
+ if (store_it) {
+ item_ptr = (char *) myalloc (sizeof (char) * item_size * list_count);
+ item = item_ptr;
+ *store_array = item_ptr;
+ }
+
+ /* read items and store them into the array */
+ for (k = 0; k < list_count; k++) {
+ get_binary_item (fp, prop->external_type,
+ &int_val, &uint_val, &double_val);
+ if (store_it) {
+ store_item (item, prop->internal_type,
+ int_val, uint_val, double_val);
+ item += item_size;
+ }
+ }
+ }
+
+ }
+ else { /* not a list */
+ get_binary_item (fp, prop->external_type,
+ &int_val, &uint_val, &double_val);
+ if (store_it) {
+ item = elem_data + prop->offset;
+ store_item (item, prop->internal_type, int_val, uint_val, double_val);
+ }
+ }
+
+ }
+}
+
+
+/******************************************************************************
+Write to a file the word that represents a PLY data type.
+
+Entry:
+ fp - file pointer
+ code - code for type
+******************************************************************************/
+
+void write_scalar_type (FILE *fp, int code)
+{
+ /* make sure this is a valid code */
+
+ if (code <= PLY_START_TYPE || code >= PLY_END_TYPE) {
+ fprintf (stderr, "write_scalar_type: bad data code = %d\n", code);
+ exit (-1);
+ }
+
+ /* write the code to a file */
+
+ fprintf (fp, "%s", type_names[code]);
+}
+
+
+/******************************************************************************
+Get a text line from a file and break it up into words.
+
+IMPORTANT: The calling routine call "free" on the returned pointer once
+finished with it.
+
+Entry:
+ fp - file to read from
+
+Exit:
+ nwords - number of words returned
+ orig_line - the original line of characters
+ returns a list of words from the line, or NULL if end-of-file
+******************************************************************************/
+
+char **get_words(FILE *fp, int *nwords, char **orig_line)
+{
+#define BIG_STRING 4096
+ static char str[BIG_STRING];
+ static char str_copy[BIG_STRING];
+ char **words;
+ int max_words = 10;
+ int num_words = 0;
+ char *ptr,*ptr2;
+ char *result;
+
+
+ /* read in a line */
+ result = fgets (str, BIG_STRING, fp);
+ if (result == NULL) {
+ *nwords = 0;
+ *orig_line = NULL;
+ return (NULL);
+ }
+
+ words = (char **) myalloc (sizeof (char *) * max_words);
+
+ /* convert carriage-return, line-feed and tabs into spaces */
+ /* (this guarentees that there will be a space before the */
+ /* null character at the end of the string) */
+
+ str[BIG_STRING-2] = ' ';
+ str[BIG_STRING-1] = '\0';
+
+ for (ptr = str, ptr2 = str_copy; *ptr != '\0'; ptr++, ptr2++) {
+ *ptr2 = *ptr;
+ if (*ptr == '\t') {
+ *ptr = ' ';
+ *ptr2 = ' ';
+ }
+ else if (*ptr == '\r' || *ptr == '\n') {
+ // In Linux line ending, the line would end by "\n\0", which originally was changed into " \0".
+ // In Windows line ending, the line would end by "\r\n\0", which needs to change into " \0" as well.
+ *ptr++ = ' '; *ptr = '\0';
+ *ptr2 = '\0';
+ break;
+ }
+ }
+
+ /* find the words in the line */
+
+ ptr = str;
+ while (*ptr != '\0') {
+
+ /* jump over leading spaces */
+ while (*ptr == ' ')
+ ptr++;
+
+ /* break if we reach the end */
+ if (*ptr == '\0')
+ break;
+
+ /* save pointer to beginning of word */
+ if (num_words >= max_words) {
+ max_words += 10;
+ words = (char **) realloc (words, sizeof (char *) * max_words);
+ }
+ words[num_words++] = ptr;
+
+ /* jump over non-spaces */
+ while (*ptr != ' ')
+ ptr++;
+
+ /* place a null character here to mark the end of the word */
+ *ptr++ = '\0';
+ }
+
+ /* return the list of words */
+ *nwords = num_words;
+ *orig_line = str_copy;
+ return (words);
+}
+
+
+/******************************************************************************
+Return the value of an item, given a pointer to it and its type.
+
+Entry:
+ item - pointer to item
+ type - data type that "item" points to
+
+Exit:
+ returns a double-precision float that contains the value of the item
+******************************************************************************/
+
+double get_item_value(char *item, int type)
+{
+ unsigned char *puchar;
+ char *pchar;
+ short int *pshort;
+ unsigned short int *pushort;
+ int *pint;
+ unsigned int *puint;
+ float *pfloat;
+ double *pdouble;
+ int int_value;
+ unsigned int uint_value;
+ double double_value;
+
+ switch (type) {
+ case PLY_CHAR:
+ pchar = (char *) item;
+ int_value = *pchar;
+ return ((double) int_value);
+ case PLY_UCHAR:
+ puchar = (unsigned char *) item;
+ int_value = *puchar;
+ return ((double) int_value);
+ case PLY_SHORT:
+ pshort = (short int *) item;
+ int_value = *pshort;
+ return ((double) int_value);
+ case PLY_USHORT:
+ pushort = (unsigned short int *) item;
+ int_value = *pushort;
+ return ((double) int_value);
+ case PLY_INT:
+ pint = (int *) item;
+ int_value = *pint;
+ return ((double) int_value);
+ case PLY_UINT:
+ puint = (unsigned int *) item;
+ uint_value = *puint;
+ return ((double) uint_value);
+ case PLY_FLOAT:
+ pfloat = (float *) item;
+ double_value = *pfloat;
+ return (double_value);
+ case PLY_DOUBLE:
+ pdouble = (double *) item;
+ double_value = *pdouble;
+ return (double_value);
+ default:
+ fprintf (stderr, "get_item_value: bad type = %d\n", type);
+ exit (-1);
+ }
+}
+
+
+/******************************************************************************
+Write out an item to a file as raw binary bytes.
+
+Entry:
+ fp - file to write to
+ int_val - integer version of item
+ uint_val - unsigned integer version of item
+ double_val - double-precision float version of item
+ type - data type to write out
+******************************************************************************/
+
+void write_binary_item(
+ FILE *fp,
+ int int_val,
+ unsigned int uint_val,
+ double double_val,
+ int type
+)
+{
+ unsigned char uchar_val;
+ char char_val;
+ unsigned short ushort_val;
+ short short_val;
+ float float_val;
+
+ switch (type) {
+ case PLY_CHAR:
+ char_val = int_val;
+ fwrite (&char_val, 1, 1, fp);
+ break;
+ case PLY_SHORT:
+ short_val = int_val;
+ fwrite (&short_val, 2, 1, fp);
+ break;
+ case PLY_INT:
+ fwrite (&int_val, 4, 1, fp);
+ break;
+ case PLY_UCHAR:
+ uchar_val = uint_val;
+ fwrite (&uchar_val, 1, 1, fp);
+ break;
+ case PLY_USHORT:
+ ushort_val = uint_val;
+ fwrite (&ushort_val, 2, 1, fp);
+ break;
+ case PLY_UINT:
+ fwrite (&uint_val, 4, 1, fp);
+ break;
+ case PLY_FLOAT:
+ float_val = (float)double_val;
+ fwrite (&float_val, 4, 1, fp);
+ break;
+ case PLY_DOUBLE:
+ fwrite (&double_val, 8, 1, fp);
+ break;
+ default:
+ fprintf (stderr, "write_binary_item: bad type = %d\n", type);
+ exit (-1);
+ }
+}
+
+
+/******************************************************************************
+Write out an item to a file as ascii characters.
+
+Entry:
+ fp - file to write to
+ int_val - integer version of item
+ uint_val - unsigned integer version of item
+ double_val - double-precision float version of item
+ type - data type to write out
+******************************************************************************/
+
+void write_ascii_item(
+ FILE *fp,
+ int int_val,
+ unsigned int uint_val,
+ double double_val,
+ int type
+)
+{
+ switch (type) {
+ case PLY_CHAR:
+ case PLY_SHORT:
+ case PLY_INT:
+ fprintf (fp, "%d ", int_val);
+ break;
+ case PLY_UCHAR:
+ case PLY_USHORT:
+ case PLY_UINT:
+ fprintf (fp, "%u ", uint_val);
+ break;
+ case PLY_FLOAT:
+ case PLY_DOUBLE:
+ fprintf (fp, "%g ", double_val);
+ break;
+ default:
+ fprintf (stderr, "write_ascii_item: bad type = %d\n", type);
+ exit (-1);
+ }
+}
+
+
+/******************************************************************************
+Write out an item to a file as ascii characters.
+
+Entry:
+ fp - file to write to
+ item - pointer to item to write
+ type - data type that "item" points to
+
+Exit:
+ returns a double-precision float that contains the value of the written item
+******************************************************************************/
+
+double old_write_ascii_item(FILE *fp, char *item, int type)
+{
+ unsigned char *puchar;
+ char *pchar;
+ short int *pshort;
+ unsigned short int *pushort;
+ int *pint;
+ unsigned int *puint;
+ float *pfloat;
+ double *pdouble;
+ int int_value;
+ unsigned int uint_value;
+ double double_value;
+
+ switch (type) {
+ case PLY_CHAR:
+ pchar = (char *) item;
+ int_value = *pchar;
+ fprintf (fp, "%d ", int_value);
+ return ((double) int_value);
+ case PLY_UCHAR:
+ puchar = (unsigned char *) item;
+ int_value = *puchar;
+ fprintf (fp, "%d ", int_value);
+ return ((double) int_value);
+ case PLY_SHORT:
+ pshort = (short int *) item;
+ int_value = *pshort;
+ fprintf (fp, "%d ", int_value);
+ return ((double) int_value);
+ case PLY_USHORT:
+ pushort = (unsigned short int *) item;
+ int_value = *pushort;
+ fprintf (fp, "%d ", int_value);
+ return ((double) int_value);
+ case PLY_INT:
+ pint = (int *) item;
+ int_value = *pint;
+ fprintf (fp, "%d ", int_value);
+ return ((double) int_value);
+ case PLY_UINT:
+ puint = (unsigned int *) item;
+ uint_value = *puint;
+ fprintf (fp, "%u ", uint_value);
+ return ((double) uint_value);
+ case PLY_FLOAT:
+ pfloat = (float *) item;
+ double_value = *pfloat;
+ fprintf (fp, "%g ", double_value);
+ return (double_value);
+ case PLY_DOUBLE:
+ pdouble = (double *) item;
+ double_value = *pdouble;
+ fprintf (fp, "%g ", double_value);
+ return (double_value);
+ default:
+ fprintf (stderr, "old_write_ascii_item: bad type = %d\n", type);
+ exit (-1);
+ }
+}
+
+
+/******************************************************************************
+Get the value of an item that is in memory, and place the result
+into an integer, an unsigned integer and a double.
+
+Entry:
+ ptr - pointer to the item
+ type - data type supposedly in the item
+
+Exit:
+ int_val - integer value
+ uint_val - unsigned integer value
+ double_val - double-precision floating point value
+******************************************************************************/
+
+void get_stored_item(
+ void *ptr,
+ int type,
+ int *int_val,
+ unsigned int *uint_val,
+ double *double_val
+)
+{
+ switch (type) {
+ case PLY_CHAR:
+ *int_val = *((char *) ptr);
+ *uint_val = *int_val;
+ *double_val = *int_val;
+ break;
+ case PLY_UCHAR:
+ *uint_val = *((unsigned char *) ptr);
+ *int_val = *uint_val;
+ *double_val = *uint_val;
+ break;
+ case PLY_SHORT:
+ *int_val = *((short int *) ptr);
+ *uint_val = *int_val;
+ *double_val = *int_val;
+ break;
+ case PLY_USHORT:
+ *uint_val = *((unsigned short int *) ptr);
+ *int_val = *uint_val;
+ *double_val = *uint_val;
+ break;
+ case PLY_INT:
+ *int_val = *((int *) ptr);
+ *uint_val = *int_val;
+ *double_val = *int_val;
+ break;
+ case PLY_UINT:
+ *uint_val = *((unsigned int *) ptr);
+ *int_val = *uint_val;
+ *double_val = *uint_val;
+ break;
+ case PLY_FLOAT:
+ *double_val = *((float *) ptr);
+ *int_val = (int)(*double_val);
+ *uint_val = (unsigned int)*double_val;
+ break;
+ case PLY_DOUBLE:
+ *double_val = *((double *) ptr);
+ *int_val = (int)(*double_val);
+ *uint_val = (unsigned int)(*double_val);
+ break;
+ default:
+ fprintf (stderr, "get_stored_item: bad type = %d\n", type);
+ exit (-1);
+ }
+}
+
+
+/******************************************************************************
+Get the value of an item from a binary file, and place the result
+into an integer, an unsigned integer and a double.
+
+Entry:
+ fp - file to get item from
+ type - data type supposedly in the word
+
+Exit:
+ int_val - integer value
+ uint_val - unsigned integer value
+ double_val - double-precision floating point value
+******************************************************************************/
+
+void get_binary_item(
+ FILE *fp,
+ int type,
+ int *int_val,
+ unsigned int *uint_val,
+ double *double_val
+)
+{
+ char c[8];
+ void *ptr;
+
+ ptr = (void *) c;
+
+ switch (type) {
+ case PLY_CHAR:
+ fread (ptr, 1, 1, fp);
+ *int_val = *((char *) ptr);
+ *uint_val = *int_val;
+ *double_val = *int_val;
+ break;
+ case PLY_UCHAR:
+ fread (ptr, 1, 1, fp);
+ *uint_val = *((unsigned char *) ptr);
+ *int_val = *uint_val;
+ *double_val = *uint_val;
+ break;
+ case PLY_SHORT:
+ fread (ptr, 2, 1, fp);
+ *int_val = *((short int *) ptr);
+ *uint_val = *int_val;
+ *double_val = *int_val;
+ break;
+ case PLY_USHORT:
+ fread (ptr, 2, 1, fp);
+ *uint_val = *((unsigned short int *) ptr);
+ *int_val = *uint_val;
+ *double_val = *uint_val;
+ break;
+ case PLY_INT:
+ fread (ptr, 4, 1, fp);
+ *int_val = *((int *) ptr);
+ *uint_val = *int_val;
+ *double_val = *int_val;
+ break;
+ case PLY_UINT:
+ fread (ptr, 4, 1, fp);
+ *uint_val = *((unsigned int *) ptr);
+ *int_val = *uint_val;
+ *double_val = *uint_val;
+ break;
+ case PLY_FLOAT:
+ fread (ptr, 4, 1, fp);
+ *double_val = *((float *) ptr);
+ *int_val = (int)(*double_val);
+ *uint_val = (unsigned int)(*double_val);
+ break;
+ case PLY_DOUBLE:
+ fread (ptr, 8, 1, fp);
+ *double_val = *((double *) ptr);
+ *int_val = (int)(*double_val);
+ *uint_val = (unsigned int)(*double_val);
+ break;
+ default:
+ fprintf (stderr, "get_binary_item: bad type = %d\n", type);
+ exit (-1);
+ }
+}
+
+
+/******************************************************************************
+Extract the value of an item from an ascii word, and place the result
+into an integer, an unsigned integer and a double.
+
+Entry:
+ word - word to extract value from
+ type - data type supposedly in the word
+
+Exit:
+ int_val - integer value
+ uint_val - unsigned integer value
+ double_val - double-precision floating point value
+******************************************************************************/
+
+void get_ascii_item(
+ char *word,
+ int type,
+ int *int_val,
+ unsigned int *uint_val,
+ double *double_val
+)
+{
+ switch (type) {
+ case PLY_CHAR:
+ case PLY_UCHAR:
+ case PLY_SHORT:
+ case PLY_USHORT:
+ case PLY_INT:
+ *int_val = atoi (word);
+ *uint_val = *int_val;
+ *double_val = *int_val;
+ break;
+
+ case PLY_UINT:
+ *uint_val = strtoul (word, (char **) NULL, 10);
+ *int_val = *uint_val;
+ *double_val = *uint_val;
+ break;
+
+ case PLY_FLOAT:
+ case PLY_DOUBLE:
+ *double_val = atof (word);
+ *int_val = (int) *double_val;
+ *uint_val = (unsigned int) *double_val;
+ break;
+
+ default:
+ fprintf (stderr, "get_ascii_item: bad type = %d\n", type);
+ exit (-1);
+ }
+}
+
+
+/******************************************************************************
+Store a value into a place being pointed to, guided by a data type.
+
+Entry:
+ item - place to store value
+ type - data type
+ int_val - integer version of value
+ uint_val - unsigned integer version of value
+ double_val - double version of value
+
+Exit:
+ item - pointer to stored value
+******************************************************************************/
+
+void store_item (
+ char *item,
+ int type,
+ int int_val,
+ unsigned int uint_val,
+ double double_val
+)
+{
+ unsigned char *puchar;
+ short int *pshort;
+ unsigned short int *pushort;
+ int *pint;
+ unsigned int *puint;
+ float *pfloat;
+ double *pdouble;
+
+ switch (type) {
+ case PLY_CHAR:
+ *item = int_val;
+ break;
+ case PLY_UCHAR:
+ puchar = (unsigned char *) item;
+ *puchar = uint_val;
+ break;
+ case PLY_SHORT:
+ pshort = (short *) item;
+ *pshort = int_val;
+ break;
+ case PLY_USHORT:
+ pushort = (unsigned short *) item;
+ *pushort = uint_val;
+ break;
+ case PLY_INT:
+ pint = (int *) item;
+ *pint = int_val;
+ break;
+ case PLY_UINT:
+ puint = (unsigned int *) item;
+ *puint = uint_val;
+ break;
+ case PLY_FLOAT:
+ pfloat = (float *) item;
+ *pfloat = (float)double_val;
+ break;
+ case PLY_DOUBLE:
+ pdouble = (double *) item;
+ *pdouble = double_val;
+ break;
+ default:
+ fprintf (stderr, "store_item: bad type = %d\n", type);
+ exit (-1);
+ }
+}
+
+
+/******************************************************************************
+Add an element to a PLY file descriptor.
+
+Entry:
+ plyfile - PLY file descriptor
+ words - list of words describing the element
+ nwords - number of words in the list
+******************************************************************************/
+
+void add_element (PlyFile *plyfile, char **words, int nwords)
+{
+ PlyElement *elem;
+
+ /* create the new element */
+ elem = (PlyElement *) myalloc (sizeof (PlyElement));
+ elem->name = strdup (words[1]);
+ elem->num = atoi (words[2]);
+ elem->nprops = 0;
+
+ /* make room for new element in the object's list of elements */
+ if (plyfile->nelems == 0)
+ plyfile->elems = (PlyElement **) myalloc (sizeof (PlyElement *));
+ else
+ plyfile->elems = (PlyElement **) realloc (plyfile->elems,
+ sizeof (PlyElement *) * (plyfile->nelems + 1));
+
+ /* add the new element to the object's list */
+ plyfile->elems[plyfile->nelems] = elem;
+ plyfile->nelems++;
+}
+
+
+/******************************************************************************
+Return the type of a property, given the name of the property.
+
+Entry:
+ name - name of property type
+
+Exit:
+ returns integer code for property, or 0 if not found
+******************************************************************************/
+
+int get_prop_type(char *type_name)
+{
+ int i;
+
+ for (i = PLY_START_TYPE + 1; i < PLY_END_TYPE; i++)
+ if (equal_strings (type_name, type_names[i]))
+ return (i);
+
+ /* if we get here, we didn't find the type */
+ return (0);
+}
+
+
+/******************************************************************************
+Add a property to a PLY file descriptor.
+
+Entry:
+ plyfile - PLY file descriptor
+ words - list of words describing the property
+ nwords - number of words in the list
+******************************************************************************/
+
+void add_property (PlyFile *plyfile, char **words, int nwords)
+{
+ PlyProperty *prop;
+ PlyElement *elem;
+
+ /* create the new property */
+
+ prop = (PlyProperty *) myalloc (sizeof (PlyProperty));
+
+ if (equal_strings (words[1], "list")) { /* is a list */
+ prop->count_external = get_prop_type (words[2]);
+ prop->external_type = get_prop_type (words[3]);
+ prop->name = strdup (words[4]);
+ prop->is_list = 1;
+ }
+ else { /* not a list */
+ prop->external_type = get_prop_type (words[1]);
+ prop->name = strdup (words[2]);
+ prop->is_list = 0;
+ }
+
+ /* add this property to the list of properties of the current element */
+
+ elem = plyfile->elems[plyfile->nelems - 1];
+
+ if (elem->nprops == 0)
+ elem->props = (PlyProperty **) myalloc (sizeof (PlyProperty *));
+ else
+ elem->props = (PlyProperty **) realloc (elem->props,
+ sizeof (PlyProperty *) * (elem->nprops + 1));
+
+ elem->props[elem->nprops] = prop;
+ elem->nprops++;
+}
+
+
+/******************************************************************************
+Add a comment to a PLY file descriptor.
+
+Entry:
+ plyfile - PLY file descriptor
+ line - line containing comment
+******************************************************************************/
+
+void add_comment (PlyFile *plyfile, char *line)
+{
+ int i;
+
+ /* skip over "comment" and leading spaces and tabs */
+ i = 7;
+ while (line[i] == ' ' || line[i] == '\t')
+ i++;
+
+ ply_put_comment (plyfile, &line[i]);
+}
+
+
+/******************************************************************************
+Add a some object information to a PLY file descriptor.
+
+Entry:
+ plyfile - PLY file descriptor
+ line - line containing text info
+******************************************************************************/
+
+void add_obj_info (PlyFile *plyfile, char *line)
+{
+ int i;
+
+ /* skip over "obj_info" and leading spaces and tabs */
+ i = 8;
+ while (line[i] == ' ' || line[i] == '\t')
+ i++;
+
+ ply_put_obj_info (plyfile, &line[i]);
+}
+
+
+/******************************************************************************
+Copy a property.
+******************************************************************************/
+
+void copy_property(PlyProperty *dest, PlyProperty *src)
+{
+ dest->name = strdup (src->name);
+ dest->external_type = src->external_type;
+ dest->internal_type = src->internal_type;
+ dest->offset = src->offset;
+
+ dest->is_list = src->is_list;
+ dest->count_external = src->count_external;
+ dest->count_internal = src->count_internal;
+ dest->count_offset = src->count_offset;
+}
+
+
+/******************************************************************************
+Allocate some memory.
+
+Entry:
+ size - amount of memory requested (in bytes)
+ lnum - line number from which memory was requested
+ fname - file name from which memory was requested
+******************************************************************************/
+
+char *my_alloc(int size, int lnum, char *fname)
+{
+ char *ptr;
+
+ ptr = (char *) malloc (size);
+
+ if (ptr == 0) {
+ fprintf(stderr, "Memory allocation bombed on line %d in %s\n", lnum, fname);
+ }
+
+ return (ptr);
+}
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
diff --git a/SurgSim/DataStructures/ply.h b/SurgSim/DataStructures/ply.h
new file mode 100644
index 0000000..39ddef9
--- /dev/null
+++ b/SurgSim/DataStructures/ply.h
@@ -0,0 +1,183 @@
+/* Copyright (c) 1994 The Board of Trustees of The Leland Stanford
+Junior University. All rights reserved.
+
+Header for PLY polygon files.
+
+- Greg Turk, March 1994
+
+A PLY file contains a single polygonal _object_.
+
+An object is composed of lists of _elements_. Typical elements are
+vertices, faces, edges and materials.
+
+Each type of element for a given object has one or more _properties_
+associated with the element type. For instance, a vertex element may
+have as properties three floating-point values x,y,z and three unsigned
+chars for red, green and blue.
+
+---------------------------------------------------------------
+
+Copyright (c) 1994 The Board of Trustees of The Leland Stanford
+Junior University. All rights reserved.
+
+Permission to use, copy, modify and distribute this software and its
+documentation for any purpose is hereby granted without fee, provided
+that the above copyright notice and this permission notice appear in
+all copies of this software and that you do not sell the software.
+
+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.
+
+*/
+
+#ifndef SURGSIM_DATASTRUCTURES_PLY_H
+#define SURGSIM_DATASTRUCTURES_PLY_H
+
+#ifdef __cplusplus
+#include <cstddef>
+
+namespace SurgSim {
+namespace DataStructures {
+extern "C" {
+#endif
+
+#include <stdio.h>
+#include <stddef.h>
+
+#define PLY_ASCII 1 /* ascii PLY file */
+#define PLY_BINARY_BE 2 /* binary PLY file, big endian */
+#define PLY_BINARY_LE 3 /* binary PLY file, little endian */
+
+#define PLY_OKAY 0 /* ply routine worked okay */
+#define PLY_ERROR -1 /* error in ply routine */
+
+/* scalar data types supported by PLY format */
+
+#define PLY_START_TYPE 0
+#define PLY_CHAR 1
+#define PLY_SHORT 2
+#define PLY_INT 3
+#define PLY_UCHAR 4
+#define PLY_USHORT 5
+#define PLY_UINT 6
+#define PLY_FLOAT 7
+#define PLY_DOUBLE 8
+#define PLY_END_TYPE 9
+
+#define PLY_SCALAR 0
+#define PLY_LIST 1
+
+
+typedef struct PlyProperty { /* description of a property */
+
+ char *name; /* property name */
+ int external_type; /* file's data type */
+ int internal_type; /* program's data type */
+ int offset; /* offset bytes of prop in a struct */
+
+ int is_list; /* 1 = list, 0 = scalar */
+ int count_external; /* file's count type */
+ int count_internal; /* program's count type */
+ int count_offset; /* offset byte for list count */
+
+} PlyProperty;
+
+typedef struct PlyElement { /* description of an element */
+ char *name; /* element name */
+ int num; /* number of elements in this object */
+ int size; /* size of element (bytes) or -1 if variable */
+ int nprops; /* number of properties for this element */
+ PlyProperty **props; /* list of properties in the file */
+ char *store_prop; /* flags: property wanted by user? */
+ int other_offset; /* offset to un-asked-for props, or -1 if none*/
+ int other_size; /* size of other_props structure */
+} PlyElement;
+
+typedef struct PlyOtherProp { /* describes other properties in an element */
+ char *name; /* element name */
+ int size; /* size of other_props */
+ int nprops; /* number of properties in other_props */
+ PlyProperty **props; /* list of properties in other_props */
+} PlyOtherProp;
+
+typedef struct OtherData { /* for storing other_props for an other element */
+ void *other_props;
+} OtherData;
+
+typedef struct OtherElem { /* data for one "other" element */
+ char *elem_name; /* names of other elements */
+ int elem_count; /* count of instances of each element */
+ OtherData **other_data; /* actual property data for the elements */
+ PlyOtherProp *other_props; /* description of the property data */
+} OtherElem;
+
+typedef struct PlyOtherElems { /* "other" elements, not interpreted by user */
+ int num_elems; /* number of other elements */
+ OtherElem *other_list; /* list of data for other elements */
+} PlyOtherElems;
+
+typedef struct PlyFile { /* description of PLY file */
+ FILE *fp; /* file pointer */
+ int file_type; /* ascii or binary */
+ float version; /* version number of file */
+ int nelems; /* number of elements of object */
+ PlyElement **elems; /* list of elements */
+ int num_comments; /* number of comments */
+ char **comments; /* list of comments */
+ int num_obj_info; /* number of items of object information */
+ char **obj_info; /* list of object info items */
+ PlyElement *which_elem; /* which element we're currently writing */
+ PlyOtherElems *other_elems; /* "other" elements from a PLY file */
+} PlyFile;
+
+/* memory allocation */
+extern char *my_alloc();
+#define myalloc(mem_size) my_alloc((mem_size), __LINE__, __FILE__)
+
+
+/*** delcaration of routines ***/
+
+extern PlyFile *ply_write(FILE *, int, char **, int);
+extern PlyFile *ply_open_for_writing(const char *, int, char **, int, float *);
+extern void ply_describe_element(PlyFile *, char *, int, int, PlyProperty *);
+extern void ply_describe_property(PlyFile *, char *, PlyProperty *);
+extern void ply_element_count(PlyFile *, char *, int);
+extern void ply_header_complete(PlyFile * plyfile);
+extern void ply_put_element_setup(PlyFile *, char *);
+extern void ply_put_element(PlyFile *, void *);
+extern void ply_put_comment(PlyFile *, char *);
+extern void ply_put_obj_info(PlyFile *, char *);
+extern PlyFile *ply_read(FILE *, int *, char ***);
+extern PlyFile *ply_open_for_reading(const char *, int *, char ***, int *, float *);
+extern PlyProperty **ply_get_element_description(PlyFile *, char *, int*, int*);
+extern void ply_get_element_setup( PlyFile *, char *, int, PlyProperty *);
+extern void ply_get_property(PlyFile *, char *, PlyProperty *);
+extern PlyOtherProp *ply_get_other_properties(PlyFile *, char *, int);
+extern void ply_get_element(PlyFile *, void *);
+extern char **ply_get_comments(PlyFile *, int *);
+extern char **ply_get_obj_info(PlyFile *, int *);
+extern void ply_close(PlyFile * plyfile);
+extern void ply_get_info(PlyFile *, float *, int *);
+extern PlyOtherElems *ply_get_other_element (PlyFile *, char *, int);
+extern void ply_describe_other_elements ( PlyFile *, PlyOtherElems *);
+extern void ply_put_other_elements (PlyFile *plyfile);
+extern void ply_free_other_elements (PlyOtherElems *elements);
+
+extern int equal_strings(const char *,const char *);
+
+/* find an element in a plyfile's list */
+PlyElement *find_element(PlyFile *, const char *);
+
+/* find a property in an element's list */
+PlyProperty *find_property(PlyElement *, const char *, int *);
+
+
+#ifdef __cplusplus
+}
+}
+}
+#endif
+
+#endif
+
diff --git a/SurgSim/Devices/CMakeLists.txt b/SurgSim/Devices/CMakeLists.txt
new file mode 100644
index 0000000..db76bde
--- /dev/null
+++ b/SurgSim/Devices/CMakeLists.txt
@@ -0,0 +1,44 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+add_subdirectory(DeviceFilters)
+add_subdirectory(IdentityPoseDevice)
+add_subdirectory(Keyboard)
+add_subdirectory(Mouse)
+
+set(DEVICES
+ LabJack
+ MultiAxis
+ Novint
+ Phantom
+ Sixense
+ TrackIR
+)
+
+set(SURGSIM_DEVICES_DOCUMENTATION
+ devices.dox
+)
+
+foreach(DEVICE ${DEVICES})
+ string(TOUPPER ${DEVICE} DEVICE_UPPER_CASE)
+ option(BUILD_DEVICE_${DEVICE_UPPER_CASE} "Build ${DEVICE} device." OFF)
+ if(BUILD_DEVICE_${DEVICE_UPPER_CASE})
+ add_subdirectory(${DEVICE})
+ list(APPEND SURGSIM_DEVICES_DOCUMENTATION ${DEVICE}/${DEVICE}.dox)
+ endif(BUILD_DEVICE_${DEVICE_UPPER_CASE})
+endforeach(DEVICE)
+
+add_custom_target(SurgSimDevicesDocumentation SOURCES ${SURGSIM_DEVICES_DOCUMENTATION})
+set_target_properties(SurgSimDevicesDocumentation PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/DeviceFilters/CMakeLists.txt b/SurgSim/Devices/DeviceFilters/CMakeLists.txt
new file mode 100644
index 0000000..e4e1448
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/CMakeLists.txt
@@ -0,0 +1,49 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+link_directories(${Boost_LIBRARY_DIRS})
+set(LIBS
+ ${Boost_LIBRARIES}
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+)
+
+set(DEVICE_FILTERS_SOURCES
+ ForceScale.cpp
+ PoseIntegrator.cpp
+ PoseTransform.cpp
+)
+
+set(DEVICE_FILTERS_HEADERS
+ ForceScale.h
+ PoseIntegrator.h
+ PoseTransform.h
+)
+
+surgsim_add_library(
+ SurgSimDeviceFilters
+ "${DEVICE_FILTERS_SOURCES}" "${DEVICE_FILTERS_HEADERS}"
+ SurgSim/Devices/DeviceFilters
+)
+
+target_link_libraries(SurgSimDeviceFilters ${LIBS})
+
+if(BUILD_TESTING)
+ # The unit tests will be built whenever the library is built.
+ add_subdirectory(UnitTests)
+endif()
+
+set_target_properties(SurgSimDeviceFilters PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/DeviceFilters/ForceScale.cpp b/SurgSim/Devices/DeviceFilters/ForceScale.cpp
new file mode 100644
index 0000000..101c370
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/ForceScale.cpp
@@ -0,0 +1,141 @@
+// This filrgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <boost/thread/locks.hpp>
+
+#include "SurgSim/Devices/DeviceFilters/ForceScale.h"
+
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Device
+{
+
+ForceScale::ForceScale(const std::string& name) :
+ CommonDevice(name),
+ m_forceScale(1.0),
+ m_torqueScale(1.0),
+ m_forceIndex(-1),
+ m_torqueIndex(-1),
+ m_springJacobianIndex(-1),
+ m_damperJacobianIndex(-1),
+ m_cachedOutputIndices(false)
+{
+}
+
+ForceScale::~ForceScale()
+{
+ finalize();
+}
+
+bool ForceScale::initialize()
+{
+ return true;
+}
+
+bool ForceScale::finalize()
+{
+ return true;
+}
+
+void ForceScale::initializeInput(const std::string& device, const DataGroup& inputData)
+{
+ getInputData() = inputData;
+}
+
+void ForceScale::handleInput(const std::string& device, const DataGroup& inputData)
+{
+ getInputData() = inputData;
+ pushInput();
+}
+
+bool ForceScale::requestOutput(const std::string& device, DataGroup* outputData)
+{
+ bool state = pullOutput();
+ if (state)
+ {
+ if (!m_cachedOutputIndices)
+ {
+ const DataGroup& initialOutputData = getOutputData();
+ m_forceIndex = initialOutputData.vectors().getIndex(SurgSim::DataStructures::Names::FORCE);
+ m_torqueIndex = initialOutputData.vectors().getIndex(SurgSim::DataStructures::Names::TORQUE);
+ m_springJacobianIndex =
+ initialOutputData.matrices().getIndex(SurgSim::DataStructures::Names::SPRING_JACOBIAN);
+ m_damperJacobianIndex =
+ initialOutputData.matrices().getIndex(SurgSim::DataStructures::Names::DAMPER_JACOBIAN);
+ m_cachedOutputIndices = true;
+ }
+ outputFilter(getOutputData(), outputData);
+ }
+ return state;
+}
+
+void ForceScale::outputFilter(const DataGroup& dataToFilter, DataGroup* result)
+{
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+
+ *result = dataToFilter; // Pass on all the data entries.
+
+ Vector3d force = Vector3d::Zero();
+ if (dataToFilter.vectors().get(m_forceIndex, &force))
+ {
+ force *= m_forceScale;
+ result->vectors().set(m_forceIndex, force);
+ }
+
+ Vector3d torque = Vector3d::Zero();
+ if (dataToFilter.vectors().get(m_torqueIndex, &torque))
+ {
+ torque *= m_torqueScale;
+ result->vectors().set(m_torqueIndex, torque);
+ }
+
+ // Scale the spring's contribution to the force and torque. First three rows calculate force, last three for torque.
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType springJacobian;
+ if (dataToFilter.matrices().get(m_springJacobianIndex, &springJacobian))
+ {
+ springJacobian.block<3,6>(0, 0) *= m_forceScale;
+ springJacobian.block<3,6>(3, 0) *= m_torqueScale;
+ result->matrices().set(m_springJacobianIndex, springJacobian);
+ }
+
+ // Scale the damper's contribution to the force and torque. First three rows calculate force, last three for torque.
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType damperJacobian;
+ if (dataToFilter.matrices().get(m_damperJacobianIndex, &damperJacobian))
+ {
+ damperJacobian.block<3,6>(0, 0) *= m_forceScale;
+ damperJacobian.block<3,6>(3, 0) *= m_torqueScale;
+ result->matrices().set(m_damperJacobianIndex, damperJacobian);
+ }
+}
+
+void ForceScale::setForceScale(double forceScale)
+{
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ m_forceScale = forceScale;
+}
+
+void ForceScale::setTorqueScale(double torqueScale)
+{
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ m_torqueScale = torqueScale;
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/DeviceFilters/ForceScale.h b/SurgSim/Devices/DeviceFilters/ForceScale.h
new file mode 100644
index 0000000..3492d29
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/ForceScale.h
@@ -0,0 +1,122 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_DEVICEFILTERS_FORCESCALE_H
+#define SURGSIM_DEVICES_DEVICEFILTERS_FORCESCALE_H
+
+#include <boost/thread/mutex.hpp>
+#include <string>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+
+namespace SurgSim
+{
+
+namespace Device
+{
+/// An output device filter that scales forces and/or torques. Any other entries in the DataGroup are passed through.
+/// For convenience, it is also an InputConsumerInterface that does no filtering of the input data. Thus it can be
+/// added as an input consumer to the raw device, and set as the output producer for the raw device, after which other
+/// device filters, input components, and output components only need access to the ForceScale instance, not the raw
+/// device.
+class ForceScale : public SurgSim::Input::CommonDevice,
+ public SurgSim::Input::InputConsumerInterface, public SurgSim::Input::OutputProducerInterface
+{
+public:
+ /// Constructor.
+ /// \param name Name of this device filter.
+ explicit ForceScale(const std::string& name);
+
+ /// Destructor.
+ virtual ~ForceScale();
+
+ /// Fully initialize the device.
+ /// When the manager object creates the device, the internal state of the device usually isn't fully
+ /// initialized yet. This method performs any needed initialization.
+ /// \return True on success.
+ virtual bool initialize() override;
+
+ /// Set the initial input data. Passes through all data unchanged.
+ /// \param device The name of the device that is producing the input. This should only be used to identify
+ /// the device (e.g. if the consumer is listening to several devices at once).
+ /// \param inputData The application input state coming from the device.
+ virtual void initializeInput(const std::string& device,
+ const SurgSim::DataStructures::DataGroup& inputData) override;
+
+ /// Notifies the consumer that the application input coming from the device has been updated.
+ /// Passes through all data unchanged.
+ /// \param device The name of the device that is producing the input. This should only be used to identify
+ /// the device (e.g. if the consumer is listening to several devices at once).
+ /// \param inputData The application input state coming from the device.
+ virtual void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override;
+
+ /// Asks the producer to provide output state to the device. Passes through all data, modifying certain data
+ /// entries used by haptic devices to calculate force and torque. Note that devices may never call this method,
+ /// e.g., because the device doesn't actually have any output capability.
+ /// \param device The name of the device that is requesting the output. This should only be used to identify
+ /// the device (e.g. if the producer is listening to several devices at once).
+ /// \param [out] outputData The data being sent to the device.
+ /// \return True if the producer has provided output data. A producer that returns false should leave outputData
+ /// unmodified.
+ virtual bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) override;
+
+ /// Set the force scale factor so that each direction has the same scale.
+ /// \param forceScale The scalar scaling factor.
+ void setForceScale(double forceScale);
+
+ /// Set the torque scale factor so that each direction has the same scale.
+ /// \param torqueScale The scalar scaling factor.
+ void setTorqueScale(double torqueScale);
+
+private:
+ /// Finalize (de-initialize) the device.
+ /// \return True on success.
+ virtual bool finalize() override;
+
+ /// Filter the output data, scaling the forces and torques.
+ /// \param dataToFilter The data that will be filtered.
+ /// \param [in,out] result A pointer to a DataGroup object that must be assignable to by the dataToFilter object.
+ /// Will contain the filtered data.
+ void outputFilter(const SurgSim::DataStructures::DataGroup& dataToFilter,
+ SurgSim::DataStructures::DataGroup* result);
+
+ /// The mutex that protects the scaling factors.
+ boost::mutex m_mutex;
+
+ /// The scaling factor applied to each direction of the force.
+ double m_forceScale;
+
+ /// The scaling factor applied to each direction of the torque.
+ double m_torqueScale;
+
+ ///@{
+ /// The indices into the DataGroups.
+ int m_forceIndex;
+ int m_torqueIndex;
+ int m_springJacobianIndex;
+ int m_damperJacobianIndex;
+ ///@}
+
+ /// True if the output DataGroup indices have been cached.
+ bool m_cachedOutputIndices;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_DEVICEFILTERS_FORCESCALE_H
diff --git a/SurgSim/Devices/DeviceFilters/PoseIntegrator.cpp b/SurgSim/Devices/DeviceFilters/PoseIntegrator.cpp
new file mode 100644
index 0000000..74ea9dd
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/PoseIntegrator.cpp
@@ -0,0 +1,178 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/DeviceFilters/PoseIntegrator.h"
+
+#include <boost/math/special_functions/fpclassify.hpp>
+
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/DataStructures/DataGroupCopier.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Device
+{
+
+PoseIntegrator::PoseIntegrator(const std::string& name) :
+ CommonDevice(name),
+ m_poseResult(PoseType::Identity()),
+ m_firstInput(true),
+ m_poseIndex(-1),
+ m_linearVelocityIndex(-1),
+ m_angularVelocityIndex(-1),
+ m_resetIndex(-1)
+{
+}
+
+const PoseIntegrator::PoseType& PoseIntegrator::integrate(const PoseType& pose)
+{
+ // Note: we apply translation and rotation separately. This is NOT the same as (m_poseResult * pose)!
+ m_poseResult.pretranslate(pose.translation());
+ m_poseResult.rotate(pose.rotation());
+ return m_poseResult;
+}
+
+bool PoseIntegrator::initialize()
+{
+ return true;
+}
+
+bool PoseIntegrator::finalize()
+{
+ return true;
+}
+
+void PoseIntegrator::initializeInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData)
+{
+ if (m_firstInput)
+ {
+ m_firstInput = false;
+
+ if (!inputData.vectors().hasEntry(SurgSim::DataStructures::Names::LINEAR_VELOCITY) ||
+ !inputData.vectors().hasEntry(SurgSim::DataStructures::Names::ANGULAR_VELOCITY))
+ {
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addEntriesFrom(inputData);
+ builder.addVector(SurgSim::DataStructures::Names::LINEAR_VELOCITY);
+ builder.addVector(SurgSim::DataStructures::Names::ANGULAR_VELOCITY);
+ getInputData() = builder.createData();
+ m_copier = std::make_shared<SurgSim::DataStructures::DataGroupCopier>(inputData, getInputData());
+ }
+ }
+
+ if (m_copier == nullptr)
+ {
+ getInputData() = inputData;
+ }
+ else
+ {
+ m_copier->copy();
+ }
+
+ m_poseIndex = inputData.poses().getIndex(SurgSim::DataStructures::Names::POSE);
+ m_linearVelocityIndex = getInputData().vectors().getIndex(SurgSim::DataStructures::Names::LINEAR_VELOCITY);
+ m_angularVelocityIndex = getInputData().vectors().getIndex(SurgSim::DataStructures::Names::ANGULAR_VELOCITY);
+ m_resetIndex = inputData.booleans().getIndex(m_resetName);
+
+ SurgSim::Math::RigidTransform3d pose;
+ if (inputData.poses().get(SurgSim::DataStructures::Names::POSE, &pose))
+ {
+ m_poseResult = pose;
+ }
+}
+
+void PoseIntegrator::handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData)
+{
+ if (m_copier == nullptr)
+ {
+ getInputData() = inputData;
+ }
+ else
+ {
+ m_copier->copy();
+ }
+
+ if (m_poseIndex >= 0)
+ {
+ SurgSim::Math::RigidTransform3d pose;
+ if (inputData.poses().get(m_poseIndex, &pose))
+ {
+ m_timer.markFrame();
+ double rate = m_timer.getAverageFrameRate();
+ if (m_timer.getNumberOfClockFails() > 0)
+ {
+ m_timer.start();
+ rate = 0.0;
+ SURGSIM_LOG_DEBUG(SurgSim::Framework::Logger::getLogger("Devices/Filters/PoseIntegrator")) <<
+ "The Timer used by " << getName() <<
+ " had a clock fail. The calculated velocities will be zero this update.";
+ }
+
+ if (!boost::math::isnormal(rate))
+ {
+ rate = 0.0;
+ }
+
+ if (m_resetIndex >= 0)
+ {
+ bool reset = false;
+ inputData.booleans().get(m_resetName, &reset);
+ if (reset)
+ {
+ pose.translation() = -m_poseResult.translation();
+ pose.linear() = m_poseResult.linear().transpose();
+ }
+ }
+
+ // Before updating the current pose, use it to calculate the angular velocity.
+ Vector3d rotationAxis;
+ double angle;
+ SurgSim::Math::computeAngleAndAxis(pose.rotation(), &angle, &rotationAxis);
+ rotationAxis = m_poseResult.rotation() * rotationAxis; // rotate the axis into global space
+ // The angular and linear indices must exist because the entries were added in initializeInput.
+ getInputData().vectors().set(m_angularVelocityIndex, rotationAxis * angle * rate);
+
+ getInputData().poses().set(m_poseIndex, integrate(pose));
+
+ getInputData().vectors().set(m_linearVelocityIndex, pose.translation() * rate);
+ }
+ }
+ pushInput();
+}
+
+bool PoseIntegrator::requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData)
+{
+ bool state = pullOutput();
+ if (state)
+ {
+ *outputData = getOutputData();
+ }
+ return state;
+}
+
+void PoseIntegrator::setReset(const std::string& name)
+{
+ SURGSIM_ASSERT(m_firstInput) <<
+ "PoseIntegrator::setReset cannot be called after the first call to initializeInput.";
+ m_resetName = name;
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/DeviceFilters/PoseIntegrator.h b/SurgSim/Devices/DeviceFilters/PoseIntegrator.h
new file mode 100644
index 0000000..5c71c8a
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/PoseIntegrator.h
@@ -0,0 +1,113 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_DEVICEFILTERS_POSEINTEGRATOR_H
+#define SURGSIM_DEVICES_DEVICEFILTERS_POSEINTEGRATOR_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Framework/Timer.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class DataGroupCopier;
+};
+
+namespace Device
+{
+/// A device filter that integrates the pose, turning a relative device into an absolute one.
+/// Also provides the instantaneous linear and angular velocities.
+/// \sa SurgSim::Input::CommonDevice
+/// \sa SurgSim::Input::InputConsumerInterface
+/// \sa SurgSim::Input::OutputProducerInterface
+class PoseIntegrator : public SurgSim::Input::CommonDevice,
+ public SurgSim::Input::InputConsumerInterface, public SurgSim::Input::OutputProducerInterface
+{
+public:
+ /// The type used for poses.
+ typedef SurgSim::Math::RigidTransform3d PoseType;
+
+ /// Constructor.
+ /// \param name Name of this device filter.
+ explicit PoseIntegrator(const std::string& name);
+
+ /// Integrates the pose.
+ /// \param pose The latest differential pose.
+ /// \return The integrated pose.
+ const PoseType& integrate(const PoseType& pose);
+
+ virtual bool initialize() override;
+
+ virtual bool finalize() override;
+
+ virtual void initializeInput(const std::string& device,
+ const SurgSim::DataStructures::DataGroup& inputData) override;
+
+ /// Notifies the consumer that the application input coming from the device has been updated.
+ /// Treats the pose coming from the input device as a delta pose and integrates it to get the output pose.
+ /// \param device The name of the device that is producing the input. This should only be used to identify
+ /// the device (e.g. if the consumer is listening to several devices at once).
+ /// \param inputData The application input state coming from the device.
+ virtual void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override;
+
+ virtual bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) override;
+
+ /// Sets the string name of the boolean entry that will reset the pose to its initial value. Such a reset can be
+ /// useful if the integrated pose is used to position an object and the integration takes the object out of view.
+ /// \param name The name of the NamedData<bool> entry.
+ /// \warning A pose reset may generate high velocities, and if the pose is the input to a VirtualToolCoupler then
+ /// large forces will likely be generated.
+ /// \exception Asserts if called after initialize.
+ void setReset(const std::string& name);
+
+private:
+ /// The result of integrating the input poses.
+ PoseType m_poseResult;
+
+ /// A timer for the update rate needed for calculating velocity.
+ SurgSim::Framework::Timer m_timer;
+
+ /// true if the input DataGroup should be created.
+ bool m_firstInput;
+
+ /// A copier into the input DataGroup, if needed.
+ std::shared_ptr<SurgSim::DataStructures::DataGroupCopier> m_copier;
+
+ /// The name of the reset boolean (if any).
+ std::string m_resetName;
+
+ ///@{
+ /// The indices into the DataGroups.
+ int m_poseIndex;
+ int m_linearVelocityIndex;
+ int m_angularVelocityIndex;
+ int m_resetIndex;
+ ///@}
+};
+
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_DEVICEFILTERS_POSEINTEGRATOR_H
diff --git a/SurgSim/Devices/DeviceFilters/PoseTransform.cpp b/SurgSim/Devices/DeviceFilters/PoseTransform.cpp
new file mode 100644
index 0000000..b7035b2
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/PoseTransform.cpp
@@ -0,0 +1,279 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <boost/thread/locks.hpp>
+
+#include "SurgSim/Devices/DeviceFilters/PoseTransform.h"
+
+#include "SurgSim/Math/Matrix.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Device
+{
+
+PoseTransform::PoseTransform(const std::string& name) :
+ CommonDevice(name),
+ m_transform(RigidTransform3d::Identity()),
+ m_transformInverse(RigidTransform3d::Identity()),
+ m_translationScale(1.0),
+ m_poseIndex(-1),
+ m_linearVelocityIndex(-1),
+ m_angularVelocityIndex(-1),
+ m_forceIndex(-1),
+ m_torqueIndex(-1),
+ m_springJacobianIndex(-1),
+ m_inputPoseIndex(-1),
+ m_damperJacobianIndex(-1),
+ m_inputLinearVelocityIndex(-1),
+ m_inputAngularVelocityIndex(-1),
+ m_cachedOutputIndices(false)
+{
+}
+
+PoseTransform::~PoseTransform()
+{
+ finalize();
+}
+
+bool PoseTransform::initialize()
+{
+ return true;
+}
+
+bool PoseTransform::finalize()
+{
+ return true;
+}
+
+void PoseTransform::initializeInput(const std::string& device, const DataGroup& inputData)
+{
+ m_poseIndex = inputData.poses().getIndex(SurgSim::DataStructures::Names::POSE);
+ m_linearVelocityIndex = inputData.vectors().getIndex(SurgSim::DataStructures::Names::LINEAR_VELOCITY);
+ m_angularVelocityIndex = inputData.vectors().getIndex(SurgSim::DataStructures::Names::ANGULAR_VELOCITY);
+
+ inputFilter(inputData, &getInputData());
+}
+
+void PoseTransform::handleInput(const std::string& device, const DataGroup& inputData)
+{
+ inputFilter(inputData, &getInputData());
+ pushInput();
+}
+
+bool PoseTransform::requestOutput(const std::string& device, DataGroup* outputData)
+{
+ bool state = pullOutput();
+ if (state)
+ {
+ if (!m_cachedOutputIndices)
+ {
+ const DataGroup& initialOutputData = getOutputData();
+ m_forceIndex = initialOutputData.vectors().getIndex(SurgSim::DataStructures::Names::FORCE);
+ m_torqueIndex = initialOutputData.vectors().getIndex(SurgSim::DataStructures::Names::TORQUE);
+ m_springJacobianIndex =
+ initialOutputData.matrices().getIndex(SurgSim::DataStructures::Names::SPRING_JACOBIAN);
+ m_inputPoseIndex = initialOutputData.poses().getIndex(SurgSim::DataStructures::Names::INPUT_POSE);
+ m_damperJacobianIndex =
+ initialOutputData.matrices().getIndex(SurgSim::DataStructures::Names::DAMPER_JACOBIAN);
+ m_inputLinearVelocityIndex =
+ initialOutputData.vectors().getIndex(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY);
+ m_inputAngularVelocityIndex =
+ initialOutputData.vectors().getIndex(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY);
+ m_cachedOutputIndices = true;
+ }
+ outputFilter(getOutputData(), outputData);
+ }
+ return state;
+}
+
+void PoseTransform::inputFilter(const DataGroup& dataToFilter, DataGroup* result)
+{
+ boost::lock_guard<boost::mutex> lock(m_mutex); // Prevent the transform or scaling from being set simultaneously.
+
+ *result = dataToFilter; // Pass on all the data entries.
+
+ if (m_poseIndex >= 0)
+ {
+ RigidTransform3d pose; // If there is a pose, scale the translation, then transform the result.
+ if (dataToFilter.poses().get(m_poseIndex, &pose))
+ {
+ pose.translation() *= m_translationScale;
+ pose = m_transform * pose;
+ result->poses().set(m_poseIndex, pose);
+ }
+ }
+
+ if (m_linearVelocityIndex >= 0)
+ {
+ // If there is a linear velocity, scale then rotate it. The linear velocity is scaled because it is the change
+ // in translation over time, and the translation is being scaled.
+ Vector3d linearVelocity;
+ if (dataToFilter.vectors().get(m_linearVelocityIndex, &linearVelocity))
+ {
+ linearVelocity *= m_translationScale;
+ linearVelocity = m_transform.linear() * linearVelocity;
+ result->vectors().set(m_linearVelocityIndex, linearVelocity);
+ }
+ }
+
+ if (m_angularVelocityIndex >= 0)
+ {
+ Vector3d angularVelocity; // If there is an angular velocity, rotate it.
+ if (dataToFilter.vectors().get(m_angularVelocityIndex, &angularVelocity))
+ {
+ angularVelocity = m_transform.linear() * angularVelocity;
+ result->vectors().set(m_angularVelocityIndex, angularVelocity);
+ }
+ }
+}
+
+void PoseTransform::outputFilter(const DataGroup& dataToFilter, DataGroup* result)
+{
+ boost::lock_guard<boost::mutex> lock(m_mutex); // Prevent the transform or scaling from being set simultaneously.
+
+ *result = dataToFilter; // Pass on all the data entries.
+
+ // Since the haptic devices will compare the data in the output DataGroup to a raw input pose, the filter must
+ // perform the reverse transform and scaling to data used by the haptic devices.
+
+ // The force and torque must be transformed into device space. In order to reliably display the desired
+ // forces as calculated by the simulation, the nominal forces and torques are not scaled by the translation scaling.
+ // The benefit is that increasing the translation scaling does not result in larger penetrations for the
+ // same force, but the downside is that as the translation scaling increases it becomes more likely that the haptic
+ // feedback loop at a surface will become "active" (smaller motions penetrating another object are sufficient to
+ // create forces ejecting the device's collision representation). Therefore, a device that is having its
+ // translation scaled may required a force scaling filter to reduce the forces.
+ if (m_forceIndex >= 0)
+ {
+ Vector3d force;
+ if (dataToFilter.vectors().get(m_forceIndex, &force))
+ {
+ force = m_transformInverse.linear() * force;
+ result->vectors().set(m_forceIndex, force);
+ }
+ }
+
+ if (m_torqueIndex >= 0)
+ {
+ Vector3d torque;
+ if (dataToFilter.vectors().get(m_torqueIndex, &torque))
+ {
+ torque = m_transformInverse.linear() * torque;
+ result->vectors().set(m_torqueIndex, torque);
+ }
+ }
+
+ // The Jacobians must be transformed into device space. The Jacobians are scaled based on the translation scaling,
+ // so that the forces and torques displayed by the device will be correct for the scene-space motions, not for the
+ // device-space motions. Let R be the linear rotation portion of our transform, s be the translation scaling
+ // factor, and J be the 3x3 upper-left corner of the spring Jacobian. Then:
+ // delta-translation-in-scene = s * R * delta-translation-in-device
+ // delta-force-in-scene = J * delta-translation-in-scene
+ // So to transform J into the device space, we left-multiply by R^-1, and right-multiply by R.
+ // Then, because we want the forces to be scaled as in scene-space, we scale J by s. The bottom-left 3x3 block
+ // in springJacobian (or damperJacobian) also transforms from delta-translation and is treated the same, while the
+ // two 3x3 blocks on the right half (of springJacobian or damperJacobian) will be multiplied by the delta-rotation
+ // (not delta-translation) and so should not be scaled.
+ if (m_springJacobianIndex >= 0)
+ {
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType springJacobian;
+ if (dataToFilter.matrices().get(m_springJacobianIndex, &springJacobian))
+ {
+ springJacobian.block<3,3>(0, 0).applyOnTheLeft(m_transformInverse.linear());
+ springJacobian.block<3,3>(0, 0).applyOnTheRight(m_transform.linear());
+ springJacobian.block<3,3>(3, 0).applyOnTheLeft(m_transformInverse.linear());
+ springJacobian.block<3,3>(3, 0).applyOnTheRight(m_transform.linear());
+ springJacobian.block<3,3>(0, 3).applyOnTheLeft(m_transformInverse.linear());
+ springJacobian.block<3,3>(0, 3).applyOnTheRight(m_transform.linear());
+ springJacobian.block<3,3>(3, 3).applyOnTheLeft(m_transformInverse.linear());
+ springJacobian.block<3,3>(3, 3).applyOnTheRight(m_transform.linear());
+ springJacobian.block<6,3>(0, 0) *= m_translationScale;
+ result->matrices().set(m_springJacobianIndex, springJacobian);
+ }
+
+ // The haptic scaffolds only use the input pose if they receive a springJacobian.
+ if (m_inputPoseIndex >= 0)
+ {
+ RigidTransform3d inputPose;
+ if (dataToFilter.poses().get(m_inputPoseIndex, &inputPose))
+ {
+ inputPose = m_transformInverse * inputPose;
+ inputPose.translation() /= m_translationScale;
+ result->poses().set(m_inputPoseIndex, inputPose);
+ }
+ }
+ }
+
+ if (m_damperJacobianIndex >= 0)
+ {
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType damperJacobian;
+ if (dataToFilter.matrices().get(m_damperJacobianIndex, &damperJacobian))
+ {
+ damperJacobian.block<3,3>(0, 0).applyOnTheLeft(m_transformInverse.linear());
+ damperJacobian.block<3,3>(0, 0).applyOnTheRight(m_transform.linear());
+ damperJacobian.block<3,3>(3, 0).applyOnTheLeft(m_transformInverse.linear());
+ damperJacobian.block<3,3>(3, 0).applyOnTheRight(m_transform.linear());
+ damperJacobian.block<3,3>(0, 3).applyOnTheLeft(m_transformInverse.linear());
+ damperJacobian.block<3,3>(0, 3).applyOnTheRight(m_transform.linear());
+ damperJacobian.block<3,3>(3, 3).applyOnTheLeft(m_transformInverse.linear());
+ damperJacobian.block<3,3>(3, 3).applyOnTheRight(m_transform.linear());
+ damperJacobian.block<6,3>(0, 0) *= m_translationScale;
+ result->matrices().set(m_damperJacobianIndex, damperJacobian);
+ }
+
+ // The haptic scaffolds only use the velocities if they receive a damperJacobian.
+ if (m_inputLinearVelocityIndex >= 0)
+ {
+ Vector3d inputLinearVelocity;
+ if (dataToFilter.vectors().get(m_inputLinearVelocityIndex, &inputLinearVelocity))
+ {
+ inputLinearVelocity = m_transformInverse.linear() * inputLinearVelocity;
+ inputLinearVelocity /= m_translationScale;
+ result->vectors().set(m_inputLinearVelocityIndex, inputLinearVelocity);
+ }
+ }
+
+ if (m_inputAngularVelocityIndex >= 0)
+ {
+ Vector3d inputAngularVelocity;
+ if (dataToFilter.vectors().get(m_inputAngularVelocityIndex, &inputAngularVelocity))
+ {
+ inputAngularVelocity = m_transformInverse.linear() * inputAngularVelocity;
+ result->vectors().set(m_inputAngularVelocityIndex, inputAngularVelocity);
+ }
+ }
+ }
+}
+
+void PoseTransform::setTranslationScale(double translationScale)
+{
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ m_translationScale = translationScale;
+}
+
+void PoseTransform::setTransform(const RigidTransform3d& transform)
+{
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ m_transform = transform;
+ m_transformInverse = m_transform.inverse();
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/DeviceFilters/PoseTransform.h b/SurgSim/Devices/DeviceFilters/PoseTransform.h
new file mode 100644
index 0000000..b89b386
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/PoseTransform.h
@@ -0,0 +1,161 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_DEVICEFILTERS_POSETRANSFORM_H
+#define SURGSIM_DEVICES_DEVICEFILTERS_POSETRANSFORM_H
+
+#include <boost/thread/mutex.hpp>
+#include <memory>
+#include <string>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Device
+{
+/// A device filter that transforms the input pose. It can scale the translation, and/or apply a constant transform.
+/// Any other data in the DataGroup is passed through. For an input/output device (e.g., a haptic device), the filter
+/// should be added as one of the device's input consumers and set as the device's output producer. For a purely input
+/// device, the filter can just be added as an input consumer. If it is used for both input and output, the input data
+/// is filtered using the offset(s) and scaling, and the output data is un-filtered so the device does not need to know
+/// about the filtering.
+/// For haptic devices, so that changing the translation scaling does not alter the relationship between displayed
+/// forces and collision penetrations, the output filter does not scale the nominal forces and torques, and it does
+/// scale the Jacobians. Thereby the displayed forces and torques are appropriate for the scene (not the device-space
+/// motions). In other words, a 1 m motion by the device's scene representation generates forces according to that 1 m
+/// motion, instead of the original device motion before scaling. This means the device displays forces and torques
+/// that are "true" to the scene, with the consequence that increasing the translation scaling can negatively impact the
+/// haptic stability. As the scaling increases, the same motion would cause larger forces, until at great enough
+/// scaling the system becomes unstable (either when colliding with another scene element, or just due to over-damping).
+/// Consider chaining this device filter with a force scaling device filter to improve system stability.
+/// \sa SurgSim::Input::CommonDevice
+/// \sa SurgSim::Input::InputConsumerInterface
+/// \sa SurgSim::Input::OutputProducerInterface
+class PoseTransform : public SurgSim::Input::CommonDevice,
+ public SurgSim::Input::InputConsumerInterface, public SurgSim::Input::OutputProducerInterface
+{
+public:
+ /// Constructor.
+ /// \param name Name of this device filter.
+ explicit PoseTransform(const std::string& name);
+
+ /// Destructor.
+ virtual ~PoseTransform();
+
+ /// Fully initialize the device.
+ /// When the manager object creates the device, the internal state of the device usually isn't fully
+ /// initialized yet. This method performs any needed initialization.
+ /// \return True on success.
+ virtual bool initialize() override;
+
+ /// Set the initial input data. Used when transforming the pose coming from an input device.
+ /// \param device The name of the device that is producing the input. This should only be used to identify
+ /// the device (e.g. if the consumer is listening to several devices at once).
+ /// \param inputData The application input state coming from the device.
+ virtual void initializeInput(const std::string& device,
+ const SurgSim::DataStructures::DataGroup& inputData) override;
+
+ /// Notifies the consumer that the application input coming from the device has been updated.
+ /// Used when transforming the pose coming from an input device.
+ /// \param device The name of the device that is producing the input. This should only be used to identify
+ /// the device (e.g. if the consumer is listening to several devices at once).
+ /// \param inputData The application input state coming from the device.
+ virtual void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override;
+
+ /// Asks the producer to provide output state to the device. Passes through all data, modifying the data entries
+ /// used by haptic devices. Note that devices may never call this method, e.g. because the device doesn't actually
+ /// have any output capability.
+ /// \param device The name of the device that is requesting the output. This should only be used to identify
+ /// the device (e.g. if the producer is listening to several devices at once).
+ /// \param [out] outputData The data being sent to the device.
+ /// \return True if the producer has provided output data. A producer that returns false should leave outputData
+ /// unmodified.
+ virtual bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) override;
+
+ /// Set the translation scale factor so that each direction has the same scale.
+ /// \param translationScale The scalar scaling factor.
+ /// \warning This setter is thread-safe, but after calling this function the output filter will use the new
+ /// transform even if the following output data is based off input data that used the old transform.
+ void setTranslationScale(double translationScale);
+
+ /// Set the constant transform. The transform is pre-applied to the input pose.
+ /// \param transform The transform, which must be invertible.
+ /// \warning This setter is thread-safe, but after calling this function the output filter will use the new
+ /// transform even if the following output data is based off input data that used the old transform.
+ void setTransform(const SurgSim::Math::RigidTransform3d& transform);
+
+private:
+ /// Finalize (de-initialize) the device.
+ /// \return True on success.
+ virtual bool finalize() override;
+
+ /// Filter the input data.
+ /// \param dataToFilter The data that will be filtered.
+ /// \param [in,out] result A pointer to a DataGroup object that must be assignable to by the dataToFilter object.
+ /// Will contain the filtered data.
+ void inputFilter(const SurgSim::DataStructures::DataGroup& dataToFilter,
+ SurgSim::DataStructures::DataGroup* result);
+
+ /// Filter the output data.
+ /// If this device filter offsets/scales input data from a haptic device, then when output data (forces, torques,
+ /// Jacobians) are created for output to that device, this function incorporates the transform/scaling to correct
+ /// the displayed outputs.
+ /// \param dataToFilter The data that will be filtered.
+ /// \param [in,out] result A pointer to a DataGroup object that must be assignable to by the dataToFilter object.
+ /// Will contain the filtered data.
+ void outputFilter(const SurgSim::DataStructures::DataGroup& dataToFilter,
+ SurgSim::DataStructures::DataGroup* result);
+
+ /// The mutex that protects the transform and scaling factor.
+ boost::mutex m_mutex;
+
+ /// The constant pre-transform.
+ SurgSim::Math::RigidTransform3d m_transform;
+
+ /// The inverse of the pre-transform.
+ SurgSim::Math::RigidTransform3d m_transformInverse;
+
+ /// The scaling factor applied to each direction of the translation.
+ double m_translationScale;
+
+ ///@{
+ /// The indices into the DataGroups.
+ int m_poseIndex;
+ int m_linearVelocityIndex;
+ int m_angularVelocityIndex;
+ int m_forceIndex;
+ int m_torqueIndex;
+ int m_springJacobianIndex;
+ int m_inputPoseIndex;
+ int m_damperJacobianIndex;
+ int m_inputLinearVelocityIndex;
+ int m_inputAngularVelocityIndex;
+ ///@}
+
+ /// True if the output DataGroup indices have been cached.
+ bool m_cachedOutputIndices;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_DEVICEFILTERS_POSETRANSFORM_H
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/CMakeLists.txt b/SurgSim/Devices/DeviceFilters/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..2546057
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/CMakeLists.txt
@@ -0,0 +1,33 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ ForceScaleTest.cpp
+ PoseIntegratorTest.cpp
+ PoseTransformTest.cpp
+)
+
+set(LIBS
+ IdentityPoseDevice
+ SurgSimDeviceFilters
+)
+
+surgsim_add_unit_tests(SurgSimDeviceFiltersTest)
+
+set_target_properties(SurgSimDeviceFiltersTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/ForceScaleTest.cpp b/SurgSim/Devices/DeviceFilters/UnitTests/ForceScaleTest.cpp
new file mode 100644
index 0000000..229d086
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/ForceScaleTest.cpp
@@ -0,0 +1,240 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 forceScale class.
+
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/DeviceFilters/ForceScale.h"
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Device::ForceScale;
+using SurgSim::Input::CommonDevice;
+using SurgSim::Math::Matrix66d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Testing::MockInputOutput;
+
+namespace
+{
+const double ERROR_EPSILON = 1e-7;
+}
+
+/// Exposes protected members of CommonDevice.
+class MockForceScale : public ForceScale
+{
+public:
+ explicit MockForceScale(const std::string& name) : ForceScale(name)
+ {
+ }
+
+ SurgSim::DataStructures::DataGroup& doGetInputData()
+ {
+ return getInputData();
+ }
+};
+
+TEST(ForceScaleDeviceFilterTest, InputDataFilter)
+{
+ auto forceScaler = std::make_shared<MockForceScale>("ForceScaleFilter");
+ ASSERT_TRUE(forceScaler->initialize());
+
+ DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::LINEAR_VELOCITY);
+
+ DataGroup data = builder.createData();
+ data.vectors().set(SurgSim::DataStructures::Names::LINEAR_VELOCITY, Vector3d(5.0, 6.0, 7.0));
+
+ // Normally the input device's initial input data would be set by the constructor or scaffold, then
+ // initializeInput would be called in addInputConsumer.
+ forceScaler->initializeInput("device", data);
+
+ // The ForceScale device filter should pass through the input data unchanged.
+ DataGroup actualInputData = forceScaler->doGetInputData();
+ Vector3d actualLinearVelocity;
+ ASSERT_TRUE(actualInputData.vectors().get(SurgSim::DataStructures::Names::LINEAR_VELOCITY,
+ &actualLinearVelocity));
+ EXPECT_TRUE(actualLinearVelocity.isApprox(Vector3d(5.0, 6.0, 7.0), ERROR_EPSILON));
+}
+
+TEST(ForceScaleDeviceFilterTest, OutputDataFilterDefaultScaling)
+{
+ auto forceScaler = std::make_shared<MockForceScale>("ForceScaleFilter");
+ ASSERT_TRUE(forceScaler->initialize());
+
+ DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::FORCE);
+ builder.addVector(SurgSim::DataStructures::Names::TORQUE);
+ builder.addMatrix(SurgSim::DataStructures::Names::SPRING_JACOBIAN);
+ builder.addMatrix(SurgSim::DataStructures::Names::DAMPER_JACOBIAN);
+ builder.addBoolean("extraData");
+
+ DataGroup data = builder.createData();
+ const Vector3d initialForce(1.0, 2.5, -13.8);
+ data.vectors().set(SurgSim::DataStructures::Names::FORCE, initialForce);
+ const Vector3d initialTorque(-7.0, 5.0, -1.2);
+ data.vectors().set(SurgSim::DataStructures::Names::TORQUE, initialTorque);
+
+ Matrix66d initialSpringJacobian;
+ initialSpringJacobian << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6,
+ 0.7, 0.8, 0.9, 1.0, 1.1, 1.2,
+ 1.3, 1.4, 1.5, 1.6, 1.7, 1.8,
+ 0.1, 0.2, 0.3, 0.4, 0.5, 0.6,
+ 0.7, 0.8, 0.9, 1.0, 1.1, 1.2,
+ 1.3, 1.4, 1.5, 1.6, 1.7, 1.8;
+ data.matrices().set(SurgSim::DataStructures::Names::SPRING_JACOBIAN, initialSpringJacobian);
+
+ Matrix66d initialDamperJacobian;
+ initialDamperJacobian << 0.91, 0.82, 0.73, 0.64, 0.55, 0.46,
+ 0.37, 0.28, 0.19, 11.0, 21.1, 31.2,
+ 41.3, 51.4, 61.5, 71.6, 81.7, 91.8,
+ 0.91, 0.82, 0.73, 0.64, 0.55, 0.46,
+ 0.37, 0.28, 0.19, 11.0, 21.1, 31.2,
+ 41.3, 51.4, 61.5, 71.6, 81.7, 91.8;
+ data.matrices().set(SurgSim::DataStructures::Names::DAMPER_JACOBIAN, initialDamperJacobian);
+
+ const bool initialBoolean = true;
+ data.booleans().set("extraData", initialBoolean);
+
+ // Normally the data would be set by a behavior, then the output device scaffold would call requestOutput on the
+ // filter, which would call requestOutput on the OutputComponent.
+ auto producer = std::make_shared<MockInputOutput>();
+ producer->m_output.setValue(data);
+
+ // The OutputProducer sends data out to the filter, which sends data out to the device.
+ forceScaler->setOutputProducer(producer);
+
+ DataGroup actualData;
+ ASSERT_TRUE(forceScaler->requestOutput("device", &actualData));
+
+ // Check the default scaling. Should be identity.
+ Vector3d actualForce;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::FORCE, &actualForce));
+ EXPECT_TRUE(actualForce.isApprox(initialForce, ERROR_EPSILON));
+
+ Vector3d actualTorque;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::TORQUE, &actualTorque));
+ EXPECT_TRUE(actualTorque.isApprox(initialTorque, ERROR_EPSILON));
+
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType actualSpringJacobian;
+ ASSERT_TRUE(actualData.matrices().get(SurgSim::DataStructures::Names::SPRING_JACOBIAN, &actualSpringJacobian));
+ EXPECT_TRUE(actualSpringJacobian.isApprox(initialSpringJacobian, ERROR_EPSILON));
+
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType actualDamperJacobian;
+ ASSERT_TRUE(actualData.matrices().get(SurgSim::DataStructures::Names::DAMPER_JACOBIAN, &actualDamperJacobian));
+ EXPECT_TRUE(actualDamperJacobian.isApprox(initialDamperJacobian, ERROR_EPSILON));
+
+ // Other data should pass through.
+ bool actualBoolean;
+ ASSERT_TRUE(actualData.booleans().get("extraData", &actualBoolean));
+ EXPECT_EQ(actualBoolean, initialBoolean);
+}
+
+TEST(ForceScaleDeviceFilterTest, OutputDataFilter)
+{
+ auto forceScaler = std::make_shared<MockForceScale>("ForceScaleFilter");
+ ASSERT_TRUE(forceScaler->initialize());
+
+ DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::FORCE);
+ builder.addVector(SurgSim::DataStructures::Names::TORQUE);
+ builder.addMatrix(SurgSim::DataStructures::Names::SPRING_JACOBIAN);
+ builder.addMatrix(SurgSim::DataStructures::Names::DAMPER_JACOBIAN);
+ builder.addBoolean("extraData");
+
+ DataGroup data = builder.createData();
+ data.vectors().set(SurgSim::DataStructures::Names::FORCE, Vector3d(1.0, 2.5, -13.8));
+ data.vectors().set(SurgSim::DataStructures::Names::TORQUE, Vector3d(-7.0, 5.0, -1.2));
+
+ Matrix66d springJacobian;
+ springJacobian << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6,
+ 0.7, 0.8, 0.9, 1.0, 1.1, 1.2,
+ 1.3, 1.4, 1.5, 1.6, 1.7, 1.8,
+ 0.1, 0.2, 0.3, 0.4, 0.5, 0.6,
+ 0.7, 0.8, 0.9, 1.0, 1.1, 1.2,
+ 1.3, 1.4, 1.5, 1.6, 1.7, 1.8;
+ data.matrices().set(SurgSim::DataStructures::Names::SPRING_JACOBIAN, springJacobian);
+
+ Matrix66d damperJacobian;
+ damperJacobian << 0.91, 0.82, 0.73, 0.64, 0.55, 0.46,
+ 0.37, 0.28, 0.19, 11.0, 21.1, 31.2,
+ 41.3, 51.4, 61.5, 71.6, 81.7, 91.8,
+ 0.91, 0.82, 0.73, 0.64, 0.55, 0.46,
+ 0.37, 0.28, 0.19, 11.0, 21.1, 31.2,
+ 41.3, 51.4, 61.5, 71.6, 81.7, 91.8;
+ data.matrices().set(SurgSim::DataStructures::Names::DAMPER_JACOBIAN, damperJacobian);
+
+ data.booleans().set("extraData", true);
+
+ // Normally the data would be set by a behavior, then the output device scaffold would call requestOutput on the
+ // filter, which would call requestOutput on the OutputComponent.
+ auto producer = std::make_shared<MockInputOutput>();
+ producer->m_output.setValue(data);
+
+ // The OutputProducer sends data out to the filter, which sends data out to the device.
+ forceScaler->setForceScale(10.0);
+ forceScaler->setTorqueScale(0.1);
+ forceScaler->setOutputProducer(producer);
+
+ DataGroup actualData;
+ ASSERT_TRUE(forceScaler->requestOutput("device", &actualData));
+
+ // Check the scaling.
+ Vector3d actualForce;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::FORCE, &actualForce));
+ Vector3d expectedForce;
+ expectedForce << 10.0, 25.0, -138.0;
+ EXPECT_TRUE(actualForce.isApprox(expectedForce, ERROR_EPSILON));
+
+ Vector3d actualTorque;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::TORQUE, &actualTorque));
+ Vector3d expectedTorque;
+ expectedTorque << -0.7, 0.5, -0.12;
+ EXPECT_TRUE(actualTorque.isApprox(expectedTorque, ERROR_EPSILON));
+
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType actualSpringJacobian;
+ ASSERT_TRUE(actualData.matrices().get(SurgSim::DataStructures::Names::SPRING_JACOBIAN, &actualSpringJacobian));
+ Matrix66d expectedSpringJacobian;
+ expectedSpringJacobian << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0,
+ 7.0, 8.0, 9.0, 10.0, 11.0, 12.0,
+ 13.0, 14.0, 15.0, 16.0, 17.0, 18.0,
+ 0.01, 0.02, 0.03, 0.04, 0.05, 0.06,
+ 0.07, 0.08, 0.09, 0.1, 0.11, 0.12,
+ 0.13, 0.14, 0.15, 0.16, 0.17, 0.18;
+ EXPECT_TRUE(actualSpringJacobian.isApprox(expectedSpringJacobian, ERROR_EPSILON));
+
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType actualDamperJacobian;
+ ASSERT_TRUE(actualData.matrices().get(SurgSim::DataStructures::Names::DAMPER_JACOBIAN, &actualDamperJacobian));
+ Matrix66d expectedDamperJacobian;
+ expectedDamperJacobian << 9.1, 8.2, 7.3, 6.4, 5.5, 4.6,
+ 3.7, 2.8, 1.9, 110.0, 211.0, 312.0,
+ 413.0, 514.0, 615.0, 716.0, 817.0, 918.0,
+ 0.091, 0.082, 0.073, 0.064, 0.055, 0.046,
+ 0.037, 0.028, 0.019, 1.10, 2.11, 3.12,
+ 4.13, 5.14, 6.15, 7.16, 8.17, 9.18;
+ EXPECT_TRUE(actualDamperJacobian.isApprox(expectedDamperJacobian, ERROR_EPSILON));
+
+ // Other data should pass through.
+ bool actualBoolean;
+ ASSERT_TRUE(actualData.booleans().get("extraData", &actualBoolean));
+ EXPECT_EQ(actualBoolean, true);
+}
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/PoseIntegratorTest.cpp b/SurgSim/Devices/DeviceFilters/UnitTests/PoseIntegratorTest.cpp
new file mode 100644
index 0000000..62cfb90
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/PoseIntegratorTest.cpp
@@ -0,0 +1,287 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 PoseIntegrator class.
+
+#include <boost/thread.hpp>
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/DeviceFilters/PoseIntegrator.h"
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Device::PoseIntegrator;
+using SurgSim::Input::CommonDevice;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Testing::MockInputOutput;
+
+namespace
+{
+const double ERROR_EPSILON = 1e-7;
+
+/// Exposes protected members of CommonDevice.
+class MockPoseIntegrator : public PoseIntegrator
+{
+public:
+ explicit MockPoseIntegrator(const std::string& name) : PoseIntegrator(name)
+ {
+ }
+
+ SurgSim::DataStructures::DataGroup& doGetInputData()
+ {
+ return getInputData();
+ }
+};
+
+void TestInputDataGroup(const DataGroup& actualData, const DataGroup& expectedData)
+{
+ RigidTransform3d actualPose;
+ ASSERT_TRUE(actualData.poses().get(SurgSim::DataStructures::Names::POSE, &actualPose));
+ RigidTransform3d expectedPose;
+ ASSERT_TRUE(expectedData.poses().get(SurgSim::DataStructures::Names::POSE, &expectedPose));
+ EXPECT_TRUE(actualPose.isApprox(expectedPose, ERROR_EPSILON));
+
+ Vector3d actualLinearVelocity;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::LINEAR_VELOCITY, &actualLinearVelocity));
+ Vector3d expectedLinearVelocity;
+ ASSERT_TRUE(expectedData.vectors().get(SurgSim::DataStructures::Names::LINEAR_VELOCITY,
+ &expectedLinearVelocity));
+ EXPECT_TRUE(actualLinearVelocity.isApprox(expectedLinearVelocity, ERROR_EPSILON));
+
+ Vector3d actualAngularVelocity;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::ANGULAR_VELOCITY, &actualAngularVelocity));
+ Vector3d expectedAngularVelocity;
+ ASSERT_TRUE(expectedData.vectors().get(SurgSim::DataStructures::Names::ANGULAR_VELOCITY,
+ &expectedAngularVelocity));
+ EXPECT_TRUE(actualAngularVelocity.isApprox(expectedAngularVelocity, ERROR_EPSILON));
+
+ bool actualBoolean;
+ ASSERT_TRUE(actualData.booleans().get("extraData", &actualBoolean));
+ bool expectedBoolean;
+ ASSERT_TRUE(expectedData.booleans().get("extraData", &expectedBoolean));
+ EXPECT_EQ(expectedBoolean, actualBoolean);
+}
+};
+
+// If the Device being filtered does not provide the linear and/or angular velocity vectors in its DataGroup, the
+// PoseIntegrator adds them.
+TEST(PoseIntegratorDeviceFilterTest, AddVelocityDataEntries)
+{
+ auto integrator = std::make_shared<MockPoseIntegrator>("PoseIntegratorFilter");
+ ASSERT_TRUE(integrator->initialize());
+
+ DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ builder.addVector("someVector");
+ builder.addBoolean("extraData");
+
+ DataGroup data = builder.createData();
+ const double rotationAngle = 0.1;
+ const Vector3d rotationAxis = Vector3d::UnitX();
+ const Vector3d translation(2.0, 3.0, 4.0);
+ const RigidTransform3d pose = makeRigidTransform(makeRotationQuaternion(rotationAngle, rotationAxis), translation);
+ data.poses().set(SurgSim::DataStructures::Names::POSE, pose);
+ const Vector3d expectedVector(24.0, -3.2, 0.0);
+ data.vectors().set("someVector", expectedVector);
+ const bool expectedBoolean = true;
+ data.booleans().set("extraData", expectedBoolean);
+
+ { // Test the code-path in initializeInput when the filter has to add entries.
+ // Normally the input device's initial input data would be set by the constructor or scaffold, then
+ // initializeInput would be called in addInputConsumer.
+ integrator->initializeInput("device", data);
+ const DataGroup actualInputData = integrator->doGetInputData();
+
+ // Test that the velocity entries were added
+ EXPECT_TRUE(actualInputData.vectors().hasEntry(SurgSim::DataStructures::Names::LINEAR_VELOCITY));
+ EXPECT_TRUE(actualInputData.vectors().hasEntry(SurgSim::DataStructures::Names::ANGULAR_VELOCITY));
+
+ RigidTransform3d actualPose;
+ ASSERT_TRUE(actualInputData.poses().get(SurgSim::DataStructures::Names::POSE, &actualPose));
+ EXPECT_TRUE(actualPose.isApprox(pose, ERROR_EPSILON));
+
+ Vector3d actualVector;
+ ASSERT_TRUE(actualInputData.vectors().get("someVector", &actualVector));
+ EXPECT_TRUE(actualVector.isApprox(expectedVector, ERROR_EPSILON));
+
+ bool actualBoolean;
+ ASSERT_TRUE(actualInputData.booleans().get("extraData", &actualBoolean));
+ EXPECT_EQ(expectedBoolean, actualBoolean);
+ }
+
+ const RigidTransform3d newPose = pose.inverse();
+ data.poses().set(SurgSim::DataStructures::Names::POSE, newPose);
+ const Vector3d newVector = Vector3d(-0.77, 3.9, 99.0);
+ data.vectors().set("someVector", newVector);
+ const bool newBoolean = !expectedBoolean;
+ data.booleans().set("extraData", newBoolean);
+
+ { // Test that the code-path through handleInput if the filter had to add entries...did it correctly pass data.
+ // The InputDataFilter test checks for correctness of the pose and velocity entries.
+ integrator->handleInput("device", data);
+ const DataGroup actualInputData = integrator->doGetInputData();
+
+ Vector3d actualVector;
+ ASSERT_TRUE(actualInputData.vectors().get("someVector", &actualVector));
+ EXPECT_TRUE(actualVector.isApprox(newVector, ERROR_EPSILON));
+
+ bool actualBoolean;
+ ASSERT_TRUE(actualInputData.booleans().get("extraData", &actualBoolean));
+ EXPECT_EQ(newBoolean, actualBoolean);
+ }
+};
+
+TEST(PoseIntegratorDeviceFilterTest, InputDataFilter)
+{
+ auto integrator = std::make_shared<MockPoseIntegrator>("PoseIntegratorFilter");
+ EXPECT_NO_THROW(integrator->setReset(SurgSim::DataStructures::Names::BUTTON_1));
+ ASSERT_TRUE(integrator->initialize());
+
+ DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ builder.addVector(SurgSim::DataStructures::Names::LINEAR_VELOCITY);
+ builder.addVector(SurgSim::DataStructures::Names::ANGULAR_VELOCITY);
+ builder.addBoolean("extraData");
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_1);
+
+ DataGroup data = builder.createData();
+ const double rotationAngle = 0.1;
+ const Vector3d rotationAxis = Vector3d::UnitX();
+ const Vector3d translation(2.0, 3.0, 4.0);
+ const RigidTransform3d pose = makeRigidTransform(makeRotationQuaternion(rotationAngle, rotationAxis), translation);
+ data.poses().set(SurgSim::DataStructures::Names::POSE, pose);
+ data.vectors().set(SurgSim::DataStructures::Names::LINEAR_VELOCITY, Vector3d(5.0, 6.0, 7.0));
+ data.vectors().set(SurgSim::DataStructures::Names::ANGULAR_VELOCITY, Vector3d(8.0, 9.0, 10.0));
+ data.booleans().set("extraData", true);
+
+ // Normally the input device's initial input data would be set by the constructor or scaffold, then
+ // initializeInput would be called in addInputConsumer.
+ integrator->initializeInput("device", data);
+
+ EXPECT_ANY_THROW(integrator->setReset(SurgSim::DataStructures::Names::BUTTON_2));
+
+ // After initialization, before the first handleInput, the initial and current input data should be the same.
+ // There is no DataGroup::operator==, so we just test both DataGroups.
+ {
+ SCOPED_TRACE("Testing Initial Input Data.");
+ // Normally the InputComponent (or another device filter) would have its handleInput called with the
+ // PoseIntegrator's input data.
+ DataGroup actualInputData = integrator->doGetInputData();
+ TestInputDataGroup(actualInputData, data);
+ }
+
+ {
+ SCOPED_TRACE("Testing Input Data, before first HandleInput.");
+ DataGroup actualInputData = integrator->doGetInputData();
+ TestInputDataGroup(actualInputData, data);
+ }
+
+ // Now test integration and velocity calculation.
+ // Normally the input device would PushInput, which would call the filter's handleInput.
+ boost::this_thread::sleep(boost::posix_time::millisec(100));
+ integrator->handleInput("device", data);
+
+ DataGroup expectedData = builder.createData();
+
+ // The "pose" data should have incremented its angle of rotation and its translation.
+ RigidTransform3d expectedPose = makeRigidTransform(makeRotationQuaternion(2.0 * rotationAngle, rotationAxis),
+ 2.0 * translation);
+ expectedData.poses().set(SurgSim::DataStructures::Names::POSE, expectedPose);
+
+ // The linearVelocity should be the translation (as a delta) times the rate. We don't know the rate, so we'll
+ // back-calculate it from the linear velocity...then we'll make sure the same rate was used for both linear and
+ // angular velocities.
+ const DataGroup actualTransformedInputData = integrator->doGetInputData();
+ Vector3d actualLinearVelocity;
+ ASSERT_TRUE(actualTransformedInputData.vectors().get(SurgSim::DataStructures::Names::LINEAR_VELOCITY,
+ &actualLinearVelocity));
+ const double rate = actualLinearVelocity.norm() / translation.norm();
+
+ Vector3d actualAngularVelocity;
+ ASSERT_TRUE(actualTransformedInputData.vectors().get(SurgSim::DataStructures::Names::ANGULAR_VELOCITY,
+ &actualAngularVelocity));
+ ASSERT_NEAR(rate, actualAngularVelocity.norm() / rotationAngle, ERROR_EPSILON);
+
+ Vector3d expectedLinearVelocity = translation * rate;
+ expectedData.vectors().set(SurgSim::DataStructures::Names::LINEAR_VELOCITY, expectedLinearVelocity);
+
+ // The angularVelocity should be the angleAxis (as a delta) times the rate.
+ Vector3d expectedAngularVelocity = rotationAxis * rotationAngle * rate;
+ expectedData.vectors().set(SurgSim::DataStructures::Names::ANGULAR_VELOCITY, expectedAngularVelocity);
+
+ expectedData.booleans().set("extraData", true);
+
+ {
+ SCOPED_TRACE("Testing Input Data, after HandleInput.");
+ TestInputDataGroup(actualTransformedInputData, expectedData);
+ }
+
+ // Reset the pose
+ data.booleans().set(SurgSim::DataStructures::Names::BUTTON_1, true);
+ integrator->handleInput("device", data);
+ const DataGroup resetInputData = integrator->doGetInputData();
+ RigidTransform3d actualPose;
+ ASSERT_TRUE(resetInputData.poses().get(SurgSim::DataStructures::Names::POSE, &actualPose));
+ EXPECT_TRUE(actualPose.isApprox(RigidTransform3d::Identity(), ERROR_EPSILON));
+}
+
+TEST(PoseIntegratorDeviceFilterTest, OutputDataFilter)
+{
+ auto integrator = std::make_shared<MockPoseIntegrator>("PoseIntegratorFilter");
+ ASSERT_TRUE(integrator->initialize());
+
+ DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ builder.addBoolean("extraData");
+
+ DataGroup data = builder.createData();
+ const double rotationAngle = 0.1;
+ const Vector3d rotationAxis = Vector3d::UnitX();
+ const Vector3d translation(2.0, 3.0, 4.0);
+ const RigidTransform3d pose = makeRigidTransform(makeRotationQuaternion(rotationAngle, rotationAxis), translation);
+ data.poses().set(SurgSim::DataStructures::Names::POSE, pose);
+ data.booleans().set("extraData", true);
+
+ // Normally the data would be set by a behavior, then the output device scaffold would call requestOutput on the
+ // filter, which would call requestOutput on the OutputComponent.
+ auto producer = std::make_shared<MockInputOutput>();
+ producer->m_output.setValue(data);
+ integrator->setOutputProducer(producer);
+
+ DataGroup actualData;
+ integrator->requestOutput("device", &actualData);
+
+ // The PoseIntegrator should not alter output data.
+ RigidTransform3d actualPose;
+ ASSERT_TRUE(actualData.poses().get(SurgSim::DataStructures::Names::POSE, &actualPose));
+ EXPECT_TRUE(actualPose.isApprox(pose, ERROR_EPSILON));
+
+ bool actualBoolean;
+ ASSERT_TRUE(actualData.booleans().get("extraData", &actualBoolean));
+ const bool expectedBoolean = true;
+ EXPECT_EQ(expectedBoolean, actualBoolean);
+}
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/PoseTransformTest.cpp b/SurgSim/Devices/DeviceFilters/UnitTests/PoseTransformTest.cpp
new file mode 100644
index 0000000..d63257a
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/PoseTransformTest.cpp
@@ -0,0 +1,290 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 PoseTransform class.
+
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/DeviceFilters/PoseTransform.h"
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Device::PoseTransform;
+using SurgSim::Input::CommonDevice;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+using SurgSim::Math::Matrix66d;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Testing::MockInputOutput;
+
+namespace
+{
+const double ERROR_EPSILON = 1e-7;
+}
+
+/// Exposes protected members of CommonDevice.
+class MockPoseTransform : public PoseTransform
+{
+public:
+ explicit MockPoseTransform(const std::string& name) : PoseTransform(name)
+ {
+ }
+
+ SurgSim::DataStructures::DataGroup& doGetInputData()
+ {
+ return getInputData();
+ }
+};
+
+void TestInputDataGroup(const DataGroup& actualData, const DataGroup& expectedData)
+{
+ RigidTransform3d actualPose;
+ ASSERT_TRUE(actualData.poses().get(SurgSim::DataStructures::Names::POSE, &actualPose));
+ RigidTransform3d expectedPose;
+ ASSERT_TRUE(expectedData.poses().get(SurgSim::DataStructures::Names::POSE, &expectedPose));
+ EXPECT_TRUE(actualPose.isApprox(expectedPose, ERROR_EPSILON));
+
+ Vector3d actualLinearVelocity;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::LINEAR_VELOCITY, &actualLinearVelocity));
+ Vector3d expectedLinearVelocity;
+ ASSERT_TRUE(expectedData.vectors().get(SurgSim::DataStructures::Names::LINEAR_VELOCITY,
+ &expectedLinearVelocity));
+ EXPECT_TRUE(actualLinearVelocity.isApprox(expectedLinearVelocity, ERROR_EPSILON));
+
+ Vector3d actualAngularVelocity;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::ANGULAR_VELOCITY, &actualAngularVelocity));
+ Vector3d expectedAngularVelocity;
+ ASSERT_TRUE(expectedData.vectors().get(SurgSim::DataStructures::Names::ANGULAR_VELOCITY,
+ &expectedAngularVelocity));
+ EXPECT_TRUE(actualAngularVelocity.isApprox(expectedAngularVelocity, ERROR_EPSILON));
+
+ bool actualBoolean;
+ ASSERT_TRUE(actualData.booleans().get("extraData", &actualBoolean));
+ bool expectedBoolean;
+ ASSERT_TRUE(expectedData.booleans().get("extraData", &expectedBoolean));
+ EXPECT_EQ(expectedBoolean, actualBoolean);
+}
+
+TEST(PoseTransformDeviceFilterTest, InputDataFilter)
+{
+ auto poseTransformer = std::make_shared<MockPoseTransform>("PoseTransformFilter");
+ ASSERT_TRUE(poseTransformer->initialize());
+
+ DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ builder.addVector(SurgSim::DataStructures::Names::LINEAR_VELOCITY);
+ builder.addVector(SurgSim::DataStructures::Names::ANGULAR_VELOCITY);
+ builder.addBoolean("extraData");
+
+ DataGroup data = builder.createData();
+ RigidTransform3d pose = makeRigidTransform(makeRotationQuaternion(1.5, Vector3d(1.2, 5.6, 0.7).normalized()),
+ Vector3d(2.0, 3.0, 4.0));
+ data.poses().set(SurgSim::DataStructures::Names::POSE, pose);
+ data.vectors().set(SurgSim::DataStructures::Names::LINEAR_VELOCITY, Vector3d(5.0, 6.0, 7.0));
+ data.vectors().set(SurgSim::DataStructures::Names::ANGULAR_VELOCITY, Vector3d(8.0, 9.0, 10.0));
+ data.booleans().set("extraData", true);
+
+ // Normally the input device's initial input data would be set by the constructor or scaffold, then
+ // initializeInput would be called in addInputConsumer.
+ poseTransformer->initializeInput("device", data);
+
+ // After initialization, before the first handleInput, the input data should be unchanged.
+ {
+ SCOPED_TRACE("Testing Input Data, no transform or scaling.");
+ // Normally the InputComponent (or another device filter) would have its handleInput called with the
+ // PoseTransform's input data.
+ DataGroup actualInputData = poseTransformer->doGetInputData();
+ TestInputDataGroup(actualInputData, data);
+ }
+
+ // Now test setting transform and scaling.
+ RigidTransform3d transform = makeRigidTransform(makeRotationQuaternion(0.9, Vector3d(10.8, -7.6, 5.4)).normalized(),
+ Vector3d(18.3, -12.6, 1.0));
+ poseTransformer->setTransform(transform);
+ poseTransformer->setTranslationScale(3.7);
+
+ // Normally the input device would PushInput, which would call the filter's handleInput.
+ poseTransformer->handleInput("device", data);
+
+ DataGroup expectedData = builder.createData();
+
+ // The "pose" data should have its translation scaled and be pre-transformed.
+ RigidTransform3d expectedPose = makeRigidTransform(makeRotationQuaternion(2.4972022984476547,
+ Vector3d(0.29211414245268102, -0.31582037986877071, 0.90273297017372756)),
+ Vector3d(15.6146599514, -31.1502325138, -5.75927677405));
+
+ expectedData.poses().set(SurgSim::DataStructures::Names::POSE, expectedPose);
+
+ // The linearVelocity should be scaled and rotated.
+ Vector3d expectedLinearVelocity(-6.28155746782, -37.3676130859, -8.37278496308);
+ expectedData.vectors().set(SurgSim::DataStructures::Names::LINEAR_VELOCITY, expectedLinearVelocity);
+
+ // The angularVelocity should be rotated.
+ Vector3d expectedAngularVelocity(-2.66966888839, -15.1851334211, -2.69899814922);
+ expectedData.vectors().set(SurgSim::DataStructures::Names::ANGULAR_VELOCITY, expectedAngularVelocity);
+
+ expectedData.booleans().set("extraData", true);
+
+ {
+ SCOPED_TRACE("Testing Input Data, with transform or scaling.");
+ DataGroup actualTransformedInputData = poseTransformer->doGetInputData();
+ TestInputDataGroup(actualTransformedInputData, expectedData);
+ }
+
+ // A new initializeInput should run the new input data through the filter with the new parameters.
+ poseTransformer->initializeInput("device", data);
+ {
+ SCOPED_TRACE("Testing Input Data after initializeInput, with transform or scaling.");
+ DataGroup actualInputData = poseTransformer->doGetInputData();
+ TestInputDataGroup(actualInputData, expectedData);
+ }
+}
+
+TEST(PoseTransformDeviceFilterTest, OutputDataFilter)
+{
+ auto poseTransformer = std::make_shared<MockPoseTransform>("PoseTransformFilter");
+ ASSERT_TRUE(poseTransformer->initialize());
+
+ DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::FORCE);
+ builder.addVector(SurgSim::DataStructures::Names::TORQUE);
+ builder.addMatrix(SurgSim::DataStructures::Names::SPRING_JACOBIAN);
+ builder.addPose(SurgSim::DataStructures::Names::INPUT_POSE);
+ builder.addMatrix(SurgSim::DataStructures::Names::DAMPER_JACOBIAN);
+ builder.addVector(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY);
+ builder.addVector(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY);
+ builder.addBoolean("extraData");
+
+ DataGroup data = builder.createData();
+ data.vectors().set(SurgSim::DataStructures::Names::FORCE, Vector3d(-6.0, 8.0, -10.0));
+ data.vectors().set(SurgSim::DataStructures::Names::TORQUE, Vector3d(8.0, -4.0, 2.0));
+
+ Matrix66d springJacobian;
+ springJacobian << 2.0, 4.0, 6.0, 8.0, 10.0, 12.0,
+ 14.0, 16.0, 18.0, 20.0, 22.0, 24.0,
+ 26.0, 28.0, 30.0, 32.0, 34.0, 36.0,
+ -2.0, -4.0, -6.0, -8.0, -10.0, -12.0,
+ -14.0, -16.0, -18.0, -20.0, -22.0, -24.0,
+ -26.0, -28.0, -30.0, -32.0, -34.0, -36.0;
+ data.matrices().set(SurgSim::DataStructures::Names::SPRING_JACOBIAN, springJacobian);
+
+ RigidTransform3d inputPose = makeRigidTransform(makeRotationQuaternion(M_PI_2, Vector3d::UnitX().eval()),
+ Vector3d(3., 5., 7.));
+ data.poses().set(SurgSim::DataStructures::Names::INPUT_POSE, inputPose);
+
+ Matrix66d damperJacobian;
+ damperJacobian << 6.0, 10.0, 14.0, 18.0, 22.0, 26.0,
+ 30.0, 34.0, 38.0, 42.0, 46.0, 50.0,
+ 54.0, 58.0, 62.0, 66.0, 70.0, 74.0,
+ -6.0, -10.0, -14.0, -18.0, -22.0, -26.0,
+ -30.0, -34.0, -38.0, -42.0, -46.0, -50.0,
+ -54.0, -58.0, -62.0, -66.0, -70.0, -74.0;
+ data.matrices().set(SurgSim::DataStructures::Names::DAMPER_JACOBIAN, damperJacobian);
+
+ data.vectors().set(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY, Vector3d(10.0, 6.0, 14.0));
+ data.vectors().set(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY, Vector3d(8.0, 9.0, 10.0));
+ data.booleans().set("extraData", true);
+
+ // Normally the data would be set by a behavior, then the output device scaffold would call requestOutput on the
+ // filter, which would call requestOutput on the OutputComponent.
+ auto producer = std::make_shared<MockInputOutput>();
+ producer->m_output.setValue(data);
+ poseTransformer->setOutputProducer(producer);
+
+ RigidTransform3d transform = makeRigidTransform(makeRotationQuaternion(M_PI_2, Vector3d::UnitY().eval()),
+ Vector3d(11.0, 12.0, 13.0));
+ poseTransformer->setTransform(transform);
+ poseTransformer->setTranslationScale(2.0);
+
+ DataGroup actualData;
+ poseTransformer->requestOutput("device", &actualData);
+
+ // The force should be anti-rotated.
+ Vector3d actualForce;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::FORCE, &actualForce));
+ Vector3d expectedForce(10.0, 8.0, -6.0);
+ EXPECT_TRUE(actualForce.isApprox(expectedForce, ERROR_EPSILON));
+
+ // The torque should be anti-rotated.
+ Vector3d actualtorque;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::TORQUE, &actualtorque));
+ Vector3d expectedtorque(-2.0, -4.0, 8.0);
+ EXPECT_TRUE(actualtorque.isApprox(expectedtorque, ERROR_EPSILON));
+
+ // The springJacobian should be transformed into device space, so each 3x3 block should be left-multiplied by
+ // the rotation, and right-multiplied by the anti-rotation, then the first three columns of the 6x6 should
+ // be scaled by the translation scaling factor so that the forces & torques are the right magnitude for
+ // the scene.
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType actualSpringJacobian;
+ ASSERT_TRUE(actualData.matrices().get(SurgSim::DataStructures::Names::SPRING_JACOBIAN, &actualSpringJacobian));
+ Matrix66d expectedSpringJacobian;
+ expectedSpringJacobian << 60.0, -56.0, -52.0, 36.0, -34.0, -32.0,
+ -36.0, 32.0, 28.0, -24.0, 22.0, 20.0,
+ -12.0, 8.0, 4.0, -12.0, 10.0, 8.0,
+ -60.0, 56.0, 52.0, -36.0, 34.0, 32.0,
+ 36.0, -32.0, -28.0, 24.0, -22.0, -20.0,
+ 12.0, -8.0, -4.0, 12.0, -10.0, -8.0;
+ EXPECT_TRUE(actualSpringJacobian.isApprox(expectedSpringJacobian, ERROR_EPSILON));
+
+ // The inputPose should be anti-transformed, and have its translation un-scaled.
+ RigidTransform3d actualInputPose;
+ ASSERT_TRUE(actualData.poses().get(SurgSim::DataStructures::Names::INPUT_POSE, &actualInputPose));
+ RigidTransform3d expectedInputPose = makeRigidTransform(makeRotationQuaternion(-M_PI_2, Vector3d::UnitY().eval()) *
+ makeRotationQuaternion(M_PI_2, Vector3d::UnitX().eval()),
+ Vector3d(3.0, -3.5, -4.0));
+ EXPECT_TRUE(actualInputPose.isApprox(expectedInputPose, ERROR_EPSILON));
+
+ // The damperJacobian should be transformed the same as the springJacobian.
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType actualDamperJacobian;
+ ASSERT_TRUE(actualData.matrices().get(SurgSim::DataStructures::Names::DAMPER_JACOBIAN, &actualDamperJacobian));
+ Matrix66d expectedDamperJacobian;
+ expectedDamperJacobian << 124.0, -116.0, -108.0, 74.0, -70.0, -66.0,
+ -76.0, 68.0, 60.0, -50.0, 46.0, 42.0,
+ -28.0, 20.0, 12.0, -26.0, 22.0, 18.0,
+ -124.0, 116.0, 108.0, -74.0, 70.0, 66.0,
+ 76.0, -68.0, -60.0, 50.0, -46.0, -42.0,
+ 28.0, -20.0, -12.0, 26.0, -22.0, -18.0;
+ EXPECT_TRUE(actualDamperJacobian.isApprox(expectedDamperJacobian, ERROR_EPSILON));
+
+ // The inputLinearVelocity should be anti-rotated and un-scaled.
+ Vector3d actualInputLinearVelocity;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY,
+ &actualInputLinearVelocity));
+ Vector3d expectedInputLinearVelocity(-7.0, 3.0, 5.0);
+ EXPECT_TRUE(actualInputLinearVelocity.isApprox(expectedInputLinearVelocity, ERROR_EPSILON));
+
+ // The inputAngularVelocity should be anti-rotated.
+ Vector3d actualInputAngularVelocity;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY,
+ &actualInputAngularVelocity));
+ Vector3d expectedInputAngularVelocity(-10.0, 9.0, 8.0);
+ EXPECT_TRUE(actualInputAngularVelocity.isApprox(expectedInputAngularVelocity, ERROR_EPSILON));
+
+ // Other data should pass through unchanged.
+ bool actualBoolean;
+ ASSERT_TRUE(actualData.booleans().get("extraData", &actualBoolean));
+ bool expectedBoolean = true;
+ EXPECT_EQ(expectedBoolean, actualBoolean);
+}
diff --git a/SurgSim/Devices/IdentityPoseDevice/CMakeLists.txt b/SurgSim/Devices/IdentityPoseDevice/CMakeLists.txt
new file mode 100644
index 0000000..4d3af9c
--- /dev/null
+++ b/SurgSim/Devices/IdentityPoseDevice/CMakeLists.txt
@@ -0,0 +1,49 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+
+link_directories(${Boost_LIBRARY_DIRS})
+
+include_directories("${CMAKE_CURRENT_SOURCE_DIR}")
+
+set(IDENTITY_POSE_DEVICE_SOURCES
+ IdentityPoseDevice.cpp
+)
+
+set(IDENTITY_POSE_DEVICE_HEADERS
+ IdentityPoseDevice.h
+)
+
+surgsim_add_library(
+ IdentityPoseDevice
+ "${IDENTITY_POSE_DEVICE_SOURCES}"
+ "${IDENTITY_POSE_DEVICE_HEADERS}"
+ "SurgSim/Devices/IdentityPoseDevice"
+)
+
+set(LIBS
+ SurgSimInput
+)
+
+target_link_libraries(IdentityPoseDevice ${LIBS}
+)
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+endif()
+
+# Put IdentityPoseDevice into folder "Devices"
+set_target_properties(IdentityPoseDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.cpp b/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.cpp
new file mode 100644
index 0000000..b973683
--- /dev/null
+++ b/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.cpp
@@ -0,0 +1,84 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::RigidTransform3d;
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+IdentityPoseDevice::IdentityPoseDevice(const std::string& uniqueName) :
+ SurgSim::Input::CommonDevice(uniqueName, buildInputData())
+{
+}
+
+bool IdentityPoseDevice::initialize()
+{
+ // required by the DeviceInterface API
+ return true;
+}
+
+bool IdentityPoseDevice::finalize()
+{
+ // required by the DeviceInterface API
+ return true;
+}
+
+DataGroup IdentityPoseDevice::buildInputData()
+{
+ DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_0);
+ return builder.createData();
+}
+
+bool IdentityPoseDevice::addInputConsumer(std::shared_ptr<SurgSim::Input::InputConsumerInterface> inputConsumer)
+{
+ if (! CommonDevice::addInputConsumer(std::move(inputConsumer)))
+ {
+ return false;
+ }
+
+ // The IdentityPoseDevice doesn't have any input events; it just sits there.
+ // So we push the output to all the consumers, including the new one, right away after we add a consumer.
+ // This ensures that all consumers always see the identity pose.
+ getInputData().poses().set(SurgSim::DataStructures::Names::POSE, RigidTransform3d::Identity());
+ getInputData().booleans().set(SurgSim::DataStructures::Names::BUTTON_0, false);
+ pushInput();
+
+ return true;
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h b/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h
new file mode 100644
index 0000000..c933958
--- /dev/null
+++ b/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_IDENTITYPOSEDEVICE_IDENTITYPOSEDEVICE_H
+#define SURGSIM_DEVICES_IDENTITYPOSEDEVICE_IDENTITYPOSEDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+/// A class implementing the identity pose device, which is a pretend device that doesn't move.
+///
+/// The identity pose device produces a pose that's always the identity transform (no translation from the origin
+/// and no rotation from the model orientation).
+/// This can be useful not only for writing tests, but also as a way to replace real hardware devices in situations
+/// where the simulator needs to be run but the hardware is not currently available.
+///
+/// \sa SurgSim::Input::DeviceInterface
+class IdentityPoseDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ explicit IdentityPoseDevice(const std::string& uniqueName);
+
+ virtual bool addInputConsumer(std::shared_ptr<SurgSim::Input::InputConsumerInterface> inputConsumer);
+
+protected:
+ virtual bool initialize();
+
+ virtual bool finalize();
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static SurgSim::DataStructures::DataGroup buildInputData();
+};
+
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_IDENTITYPOSEDEVICE_IDENTITYPOSEDEVICE_H
diff --git a/SurgSim/Devices/IdentityPoseDevice/UnitTests/CMakeLists.txt b/SurgSim/Devices/IdentityPoseDevice/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..dce17dc
--- /dev/null
+++ b/SurgSim/Devices/IdentityPoseDevice/UnitTests/CMakeLists.txt
@@ -0,0 +1,31 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ IdentityPoseDeviceTest.cpp
+)
+
+set(LIBS
+ IdentityPoseDevice
+)
+
+surgsim_add_unit_tests(IdentityPoseDeviceTest)
+
+set_target_properties(IdentityPoseDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/IdentityPoseDevice/UnitTests/IdentityPoseDeviceTest.cpp b/SurgSim/Devices/IdentityPoseDevice/UnitTests/IdentityPoseDeviceTest.cpp
new file mode 100644
index 0000000..3859f50
--- /dev/null
+++ b/SurgSim/Devices/IdentityPoseDevice/UnitTests/IdentityPoseDeviceTest.cpp
@@ -0,0 +1,129 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 IdentityPoseDevice class.
+
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Testing::MockInputOutput;
+
+
+TEST(IdentityPoseDeviceTest, CanConstruct)
+{
+ EXPECT_NO_THROW({IdentityPoseDevice device("MyIdentityPoseDevice");});
+}
+
+TEST(IdentityPoseDeviceTest, Name)
+{
+ IdentityPoseDevice device("MyIdentityPoseDevice");
+ EXPECT_EQ("MyIdentityPoseDevice", device.getName());
+}
+
+TEST(IdentityPoseDeviceTest, AddInputConsumer)
+{
+ IdentityPoseDevice device("MyIdentityPoseDevice");
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device.addInputConsumer(consumer));
+
+ // IdentityPoseDevice is supposed to shove an identity pose (and a button) at every consumer when it's added.
+ EXPECT_EQ(1, consumer->m_numTimesReceivedInput);
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData(SurgSim::DataStructures::Names::POSE));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_0));
+
+ // Check the data.
+ RigidTransform3d pose = SurgSim::Math::makeRigidTransform(SurgSim::Math::Vector3d(1.3, 32.0, 68.0),
+ SurgSim::Math::Vector3d(13.2, 2.8, 8.0), SurgSim::Math::Vector3d(273.0, -32.0, -6.0));
+ ASSERT_TRUE(consumer->m_lastReceivedInput.poses().get(SurgSim::DataStructures::Names::POSE, &pose));
+ EXPECT_NEAR(0, (pose.matrix() - Matrix44d::Identity()).norm(), 1e-6);
+ bool button0 = false;
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().get(SurgSim::DataStructures::Names::BUTTON_0, &button0));
+ EXPECT_FALSE(button0);
+
+ // Adding the same input consumer again should fail.
+ EXPECT_FALSE(device.addInputConsumer(consumer));
+ EXPECT_EQ(1, consumer->m_numTimesReceivedInput);
+
+ // Adding a different device should push to it.
+ std::shared_ptr<MockInputOutput> consumer2 = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer2->m_numTimesReceivedInput);
+ EXPECT_TRUE(device.addInputConsumer(consumer2));
+ EXPECT_EQ(1, consumer2->m_numTimesReceivedInput);
+ // We don't care if the first consumer was updated again or not.
+ EXPECT_TRUE((consumer->m_numTimesReceivedInput >= 1) && (consumer->m_numTimesReceivedInput <= 2)) <<
+ "consumer->m_numTimesReceivedInput = " << consumer->m_numTimesReceivedInput << std::endl;
+}
+
+TEST(IdentityPoseDeviceTest, RemoveInputConsumer)
+{
+ IdentityPoseDevice device("MyIdentityPoseDevice");
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device.removeInputConsumer(consumer));
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device.addInputConsumer(consumer));
+ // IdentityPoseDevice is supposed to shove an identity pose (and a button) at every consumer when it's added.
+ EXPECT_EQ(1, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device.removeInputConsumer(consumer));
+ EXPECT_EQ(1, consumer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device.removeInputConsumer(consumer));
+ EXPECT_EQ(1, consumer->m_numTimesReceivedInput);
+}
+
+TEST(IdentityPoseDeviceTest, SetOutputProducer)
+{
+ IdentityPoseDevice device("MyIdentityPoseDevice");
+ std::shared_ptr<MockInputOutput> producer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_TRUE(device.setOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+}
+
+TEST(IdentityPoseDeviceTest, RemoveOutputProducer)
+{
+ IdentityPoseDevice device("MyIdentityPoseDevice");
+ std::shared_ptr<MockInputOutput> producer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_FALSE(device.removeOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device.setOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device.removeOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device.removeOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesReceivedInput);
+}
diff --git a/SurgSim/Devices/Keyboard/CMakeLists.txt b/SurgSim/Devices/Keyboard/CMakeLists.txt
new file mode 100644
index 0000000..fc4743c
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/CMakeLists.txt
@@ -0,0 +1,55 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${OPENSCENEGRAPH_INCLUDE_DIR}"
+)
+
+set(KEYBOARD_DEVICE_SOURCES
+ KeyboardDevice.cpp
+ KeyboardScaffold.cpp
+ OsgKeyboardHandler.cpp
+)
+
+set(KEYBOARD_DEVICE_HEADERS
+ KeyboardDevice.h
+ KeyboardScaffold.h
+ KeyCode.h
+ OsgKeyboardHandler.h
+)
+
+surgsim_add_library(
+ KeyboardDevice
+ "${KEYBOARD_DEVICE_SOURCES}" "${KEYBOARD_DEVICE_HEADERS}"
+ SurgSim/Devices/Keyboard
+)
+
+set(LIBS
+ SurgSimInput
+ ${Boost_LIBRARIES}
+)
+
+target_link_libraries(KeyboardDevice ${LIBS}
+)
+
+if(BUILD_TESTING)
+ # The unit tests will be built whenever the library is built.
+ add_subdirectory(UnitTests)
+ add_subdirectory(VisualTests)
+endif()
+
+# Put KeyboardDevice into folder "Devices"
+set_target_properties(KeyboardDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Keyboard/KeyCode.h b/SurgSim/Devices/Keyboard/KeyCode.h
new file mode 100644
index 0000000..30b590e
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/KeyCode.h
@@ -0,0 +1,249 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_KEYBOARD_KEYCODE_H
+#define SURGSIM_DEVICES_KEYBOARD_KEYCODE_H
+
+namespace SurgSim
+{
+namespace Device
+{
+ // KeyCode pulled out from osgGA/GUIEventAdapter
+ enum KeyCode
+ {
+ NONE = -1,
+ KEY_SPACE = 0x20,
+ KEY_0 = '0',
+ KEY_1 = '1',
+ KEY_2 = '2',
+ KEY_3 = '3',
+ KEY_4 = '4',
+ KEY_5 = '5',
+ KEY_6 = '6',
+ KEY_7 = '7',
+ KEY_8 = '8',
+ KEY_9 = '9',
+ KEY_A = 'a',
+ KEY_B = 'b',
+ KEY_C = 'c',
+ KEY_D = 'd',
+ KEY_E = 'e',
+ KEY_F = 'f',
+ KEY_G = 'g',
+ KEY_H = 'h',
+ KEY_I = 'i',
+ KEY_J = 'j',
+ KEY_K = 'k',
+ KEY_L = 'l',
+ KEY_M = 'm',
+ KEY_N = 'n',
+ KEY_O = 'o',
+ KEY_P = 'p',
+ KEY_Q = 'q',
+ KEY_R = 'r',
+ KEY_S = 's',
+ KEY_T = 't',
+ KEY_U = 'u',
+ KEY_V = 'v',
+ KEY_W = 'w',
+ KEY_X = 'x',
+ KEY_Y = 'y',
+ KEY_Z = 'z',
+ KEY_EXCLAIM = 0x21,
+ KEY_QUOTEDBL = 0x22,
+ KEY_HASH = 0x23,
+ KEY_DOLLAR = 0x24,
+ KEY_AMPERSAND = 0x26,
+ KEY_QUOTE = 0x27,
+ KEY_LEFTPAREN = 0x28,
+ KEY_RIGHTPAREN = 0x29,
+ KEY_ASTERISK = 0x2A,
+ KEY_PLUS = 0x2B,
+ KEY_COMMA = 0x2C,
+ KEY_MINUS = 0x2D,
+ KEY_PERIOD = 0x2E,
+ KEY_SLASH = 0x2F,
+ KEY_COLON = 0x3A,
+ KEY_SEMICOLON = 0x3B,
+ KEY_LESS = 0x3C,
+ KEY_EQUALS = 0x3D,
+ KEY_GREATER = 0x3E,
+ KEY_QUESTION = 0x3F,
+ KEY_AT = 0x40,
+ KEY_LEFTBRACKET = 0x5B,
+ KEY_BACKSLASH = 0x5C,
+ KEY_RIGHTBRACKET = 0x5D,
+ KEY_CARET = 0x5E,
+ KEY_UNDERSCORE = 0x5F,
+ KEY_BACKQUOTE = 0x60,
+ KEY_BACKSPACE = 0xFF08, /* back space, back char */
+ KEY_TAB = 0xFF09,
+ KEY_LINEFEED = 0xFF0A, /* Linefeed, LF */
+ KEY_CLEAR = 0xFF0B,
+ KEY_RETURN = 0xFF0D, /* Return, enter */
+ KEY_PAUSE = 0xFF13, /* Pause, hold */
+ KEY_SCROLL_LOCK = 0xFF14,
+ KEY_SYS_REQ = 0xFF15,
+ KEY_ESCAPE = 0xFF1B,
+ KEY_DELETE = 0xFFFF, /* Delete, rubout */
+ KEY_HOME = 0xFF50,
+ KEY_LEFT = 0xFF51, /* Move left, left arrow */
+ KEY_UP = 0xFF52, /* Move up, up arrow */
+ KEY_RIGHT = 0xFF53, /* Move right, right arrow */
+ KEY_DOWN = 0xFF54, /* Move down, down arrow */
+ KEY_PRIOR = 0xFF55, /* Prior, previous */
+ KEY_PAGE_UP = 0xFF55,
+ KEY_NEXT = 0xFF56, /* Next */
+ KEY_PAGE_DOWN = 0xFF56,
+ KEY_END = 0xFF57, /* EOL */
+ KEY_BEGIN = 0xFF58, /* BOL */
+ KEY_SELECT = 0xFF60, /* Select, mark */
+ KEY_PRINT = 0xFF61,
+ KEY_EXECUTE = 0xFF62, /* Execute, run, do */
+ KEY_INSERT = 0xFF63, /* Insert, insert here */
+ KEY_UNDO = 0xFF65, /* Undo, oops */
+ KEY_REDO = 0xFF66, /* redo, again */
+ KEY_MENU = 0xFF67, /* On Windows, this is VK_APPS, the context-menu key */
+ KEY_FIND = 0xFF68, /* Find, search */
+ KEY_CANCEL = 0xFF69, /* Cancel, stop, abort, exit */
+ KEY_HELP = 0xFF6A, /* Help */
+ KEY_BREAK = 0xFF6B,
+ KEY_MODE_SWITCH = 0xFF7E, /* Character set switch */
+ KEY_SCRIPT_SWITCH = 0xFF7E, /* Alias for mode_switch */
+ KEY_NUM_LOCK = 0xFF7F,
+ KEY_KP_SPACE = 0xFF80, /* space */
+ KEY_KP_TAB = 0xFF89,
+ KEY_KP_ENTER = 0xFF8D, /* enter */
+ KEY_KP_F1 = 0xFF91, /* PF1, KP_A, ... */
+ KEY_KP_F2 = 0xFF92,
+ KEY_KP_F3 = 0xFF93,
+ KEY_KP_F4 = 0xFF94,
+ KEY_KP_HOME = 0xFF95,
+ KEY_KP_LEFT = 0xFF96,
+ KEY_KP_UP = 0xFF97,
+ KEY_KP_RIGHT = 0xFF98,
+ KEY_KP_DOWN = 0xFF99,
+ KEY_KP_PRIOR = 0xFF9A,
+ KEY_KP_PAGE_UP = 0xFF9A,
+ KEY_KP_NEXT = 0xFF9B,
+ KEY_KP_PAGE_DOWN = 0xFF9B,
+ KEY_KP_END = 0xFF9C,
+ KEY_KP_BEGIN = 0xFF9D,
+ KEY_KP_INSERT = 0xFF9E,
+ KEY_KP_DELETE = 0xFF9F,
+ KEY_KP_EQUAL = 0xFFBD, /* equals */
+ KEY_KP_MULTIPLY = 0xFFAA,
+ KEY_KP_ADD = 0xFFAB,
+ KEY_KP_SEPARATOR = 0xFFAC, /* separator, often comma */
+ KEY_KP_SUBTRACT = 0xFFAD,
+ KEY_KP_DECIMAL = 0xFFAE,
+ KEY_KP_DIVIDE = 0xFFAF,
+ KEY_KP_0 = 0xFFB0,
+ KEY_KP_1 = 0xFFB1,
+ KEY_KP_2 = 0xFFB2,
+ KEY_KP_3 = 0xFFB3,
+ KEY_KP_4 = 0xFFB4,
+ KEY_KP_5 = 0xFFB5,
+ KEY_KP_6 = 0xFFB6,
+ KEY_KP_7 = 0xFFB7,
+ KEY_KP_8 = 0xFFB8,
+ KEY_KP_9 = 0xFFB9,
+ KEY_F1 = 0xFFBE,
+ KEY_F2 = 0xFFBF,
+ KEY_F3 = 0xFFC0,
+ KEY_F4 = 0xFFC1,
+ KEY_F5 = 0xFFC2,
+ KEY_F6 = 0xFFC3,
+ KEY_F7 = 0xFFC4,
+ KEY_F8 = 0xFFC5,
+ KEY_F9 = 0xFFC6,
+ KEY_F10 = 0xFFC7,
+ KEY_F11 = 0xFFC8,
+ KEY_F12 = 0xFFC9,
+ KEY_F13 = 0xFFCA,
+ KEY_F14 = 0xFFCB,
+ KEY_F15 = 0xFFCC,
+ KEY_F16 = 0xFFCD,
+ KEY_F17 = 0xFFCE,
+ KEY_F18 = 0xFFCF,
+ KEY_F19 = 0xFFD0,
+ KEY_F20 = 0xFFD1,
+ KEY_F21 = 0xFFD2,
+ KEY_F22 = 0xFFD3,
+ KEY_F23 = 0xFFD4,
+ KEY_F24 = 0xFFD5,
+ KEY_F25 = 0xFFD6,
+ KEY_F26 = 0xFFD7,
+ KEY_F27 = 0xFFD8,
+ KEY_F28 = 0xFFD9,
+ KEY_F29 = 0xFFDA,
+ KEY_F30 = 0xFFDB,
+ KEY_F31 = 0xFFDC,
+ KEY_F32 = 0xFFDD,
+ KEY_F33 = 0xFFDE,
+ KEY_F34 = 0xFFDF,
+ KEY_F35 = 0xFFE0,
+ /* Modifiers */
+ KEY_SHIFT_L = 0xFFE1, /* Left shift */
+ KEY_SHIFT_R = 0xFFE2, /* Right shift */
+ KEY_CONTROL_L = 0xFFE3, /* Left control */
+ KEY_CONTROL_R = 0xFFE4, /* Right control */
+ KEY_CAPS_LOCK = 0xFFE5, /* Caps lock */
+ KEY_SHIFT_LOCK = 0xFFE6, /* Shift lock */
+ KEY_META_L = 0xFFE7, /* Left meta */
+ KEY_META_R = 0xFFE8, /* Right meta */
+ KEY_ALT_L = 0xFFE9, /* Left alt */
+ KEY_ALT_R = 0xFFEA, /* Right alt */
+ KEY_SUPER_L = 0xFFEB, /* Left super */
+ KEY_SUPER_R = 0xFFEC, /* Right super */
+ KEY_HYPER_L = 0xFFED, /* Left hyper */
+ KEY_HYPER_R = 0xFFEE /* Right hyper */
+ };
+
+ // Masks pulled out from osgGA/GUIEventAdapter
+ enum ModKeyMask
+ {
+ MODKEY_NONE = 0,
+ MODKEY_LEFT_SHIFT = 0x0001,
+ MODKEY_RIGHT_SHIFT = 0x0002,
+ MODKEY_LEFT_CTRL = 0x0004,
+ MODKEY_RIGHT_CTRL = 0x0008,
+ MODKEY_LEFT_ALT = 0x0010,
+ MODKEY_RIGHT_ALT = 0x0020,
+ MODKEY_LEFT_META = 0x0040,
+ MODKEY_RIGHT_META = 0x0080,
+ MODKEY_LEFT_SUPER = 0x0100,
+ MODKEY_RIGHT_SUPER = 0x0200,
+ MODKEY_LEFT_HYPER = 0x0400,
+ MODKEY_RIGHT_HYPER = 0x0800,
+ MODKEY_NUM_LOCK = 0x1000,
+ MODKEY_CAPS_LOCK = 0x2000,
+ MODKEY_CTRL = (MODKEY_LEFT_CTRL|MODKEY_RIGHT_CTRL),
+ MODKEY_SHIFT = (MODKEY_LEFT_SHIFT|MODKEY_RIGHT_SHIFT),
+ MODKEY_ALT = (MODKEY_LEFT_ALT|MODKEY_RIGHT_ALT),
+ MODKEY_META = (MODKEY_LEFT_META|MODKEY_RIGHT_META),
+ MODKEY_SUPER = (MODKEY_LEFT_SUPER|MODKEY_RIGHT_SUPER),
+ MODKEY_HYPER = (MODKEY_LEFT_HYPER|MODKEY_RIGHT_HYPER),
+ MODKEY_CAPS_SHIFT_L = (MODKEY_CAPS_LOCK|MODKEY_LEFT_SHIFT),
+ MODKEY_CAPS_SHIFT_R = (MODKEY_CAPS_LOCK|MODKEY_RIGHT_SHIFT),
+ MODKEY_CAPS_CONTROL_L = (MODKEY_CAPS_LOCK|MODKEY_LEFT_CTRL),
+ MODKEY_CAPS_CONTROL_R = (MODKEY_CAPS_LOCK|MODKEY_RIGHT_CTRL),
+ MODKEY_CAPS_ALT_L = (MODKEY_CAPS_LOCK|MODKEY_LEFT_ALT),
+ MODKEY_CAPS_ALT_R = (MODKEY_CAPS_LOCK|MODKEY_RIGHT_ALT)
+ };
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_KEYBOARD_KEYCODE_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Keyboard/KeyboardDevice.cpp b/SurgSim/Devices/Keyboard/KeyboardDevice.cpp
new file mode 100644
index 0000000..eb65afc
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/KeyboardDevice.cpp
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Keyboard/KeyboardDevice.h"
+
+#include "SurgSim/Devices/Keyboard/KeyboardScaffold.h"
+#include "SurgSim/Devices/Keyboard/OsgKeyboardHandler.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+KeyboardDevice::KeyboardDevice(const std::string& deviceName) :
+ SurgSim::Input::CommonDevice(deviceName, KeyboardScaffold::buildDeviceInputData())
+{
+}
+
+KeyboardDevice::~KeyboardDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+bool KeyboardDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized());
+
+ m_scaffold = KeyboardScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(m_scaffold);
+
+ m_scaffold->registerDevice(this);
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
+
+ return true;
+}
+
+bool KeyboardDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized());
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ m_scaffold.reset();
+ return true;
+}
+
+bool KeyboardDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+OsgKeyboardHandler* KeyboardDevice::getKeyboardHandler() const
+{
+ return m_scaffold->getKeyboardHandler();
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Keyboard/KeyboardDevice.h b/SurgSim/Devices/Keyboard/KeyboardDevice.h
new file mode 100644
index 0000000..4f00326
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/KeyboardDevice.h
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_KEYBOARD_KEYBOARDDEVICE_H
+#define SURGSIM_DEVICES_KEYBOARD_KEYBOARDDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+class KeyboardScaffold;
+class OsgKeyboardHandler;
+
+/// A class implementing the communication with a keyboard
+///
+/// \par Application input provided from the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | int | "key" | %Key code for the pressed key, if any. Default: -1 |
+/// | int | "modifierMask" | %Mask for the pressed modifier, if any. Default: 0 |
+///
+/// \par Application output used by the device:
+/// NONE
+///
+/// \note Key 'Fn' is not currently captured (No key code is assigned to it).
+/// \sa SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class KeyboardDevice : public SurgSim::Input::CommonDevice
+{
+ friend class KeyboardScaffold;
+ friend class KeyboardDeviceTest;
+
+public:
+ /// Constructor
+ /// \param deviceName Name for keyboard device
+ explicit KeyboardDevice(const std::string& deviceName);
+ /// Destructor
+ virtual ~KeyboardDevice();
+
+ /// Initialize this device and register it with corresponding scaffold.
+ /// \return True on success; false otherwise.
+ virtual bool initialize() override;
+ /// "De"-initialize this device and unregister from the scaffold.
+ /// \return True on success; false, otherwise.
+ virtual bool finalize() override;
+
+ /// Check if the scaffold of this device is initialized.
+ /// \return True if this the scaffold of this device is initialized; Otherwise, false.
+ bool isInitialized() const;
+
+ /// Get keyboard handler
+ /// \return The keyboard handler associated with this device
+ OsgKeyboardHandler* getKeyboardHandler() const;
+
+private:
+ /// Communication with hardware is handled by scaffold.
+ std::shared_ptr<KeyboardScaffold> m_scaffold;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif //SURGSIM_DEVICES_KEYBOARD_KEYBOARDDEVICE_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Keyboard/KeyboardScaffold.cpp b/SurgSim/Devices/Keyboard/KeyboardScaffold.cpp
new file mode 100644
index 0000000..0e84fa2
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/KeyboardScaffold.cpp
@@ -0,0 +1,138 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Keyboard/KeyboardScaffold.h"
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/Keyboard/KeyboardDevice.h"
+#include "SurgSim/Devices/Keyboard/OsgKeyboardHandler.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+
+/// Struct to hold a KeyboardDevice object, a KeyboardHandler object, and a mutex for data passing.
+struct KeyboardScaffold::DeviceData
+{
+ /// Constructor
+ /// \param device Device to be managed by this scaffold
+ explicit DeviceData(KeyboardDevice* device) : deviceObject(device)
+ {
+ keyboardHandler = new OsgKeyboardHandler();
+ }
+
+ /// Device object managed by this scaffold.
+ KeyboardDevice* const deviceObject;
+ /// Keyboard Handler to communicate with underneath API.
+ osg::ref_ptr<OsgKeyboardHandler> keyboardHandler;
+ /// The mutex that protects the externally modifiable parameters.
+ boost::mutex mutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ DeviceData(const DeviceData&) /*= delete*/;
+ DeviceData& operator=(const DeviceData&) /*= delete*/;
+};
+
+KeyboardScaffold::KeyboardScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) : m_logger(logger)
+{
+ if (!m_logger)
+ {
+ m_logger = SurgSim::Framework::Logger::getLogger("Keyboard device");
+ m_logger->setThreshold(m_defaultLogLevel);
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Keyboard: Shared scaffold created.";
+}
+
+KeyboardScaffold::~KeyboardScaffold()
+{
+ unregisterDevice();
+}
+
+bool KeyboardScaffold::registerDevice(KeyboardDevice* device)
+{
+ m_device.reset(new DeviceData(device));
+ if (m_device == nullptr)
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "KeyboardScaffold::registerDevice(): failed to create a DeviceData";
+ return false;
+ }
+ return true;
+}
+
+bool KeyboardScaffold::unregisterDevice()
+{
+ m_device.reset();
+ if (m_device == nullptr)
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "Keyboard: Shared scaffold unregistered.";
+ return true;
+ }
+ return false;
+}
+
+bool KeyboardScaffold::updateDevice(int key, int modifierMask)
+{
+ boost::lock_guard<boost::mutex> lock(m_device->mutex);
+ SurgSim::DataStructures::DataGroup& inputData = m_device->deviceObject->getInputData();
+ inputData.integers().set("key", key);
+ inputData.integers().set("modifierMask", modifierMask);
+
+ m_device->deviceObject->pushInput();
+ return true;
+}
+
+OsgKeyboardHandler* KeyboardScaffold::getKeyboardHandler() const
+{
+ return m_device->keyboardHandler.get();
+}
+
+
+/// Builds the data layout for the application input (i.e. device output).
+SurgSim::DataStructures::DataGroup KeyboardScaffold::buildDeviceInputData()
+{
+ DataGroupBuilder builder;
+ builder.addInteger("key");
+ builder.addInteger("modifierMask");
+ return builder.createData();
+}
+
+std::shared_ptr<KeyboardScaffold> KeyboardScaffold::getOrCreateSharedInstance()
+{
+ static SurgSim::Framework::SharedInstance<KeyboardScaffold> sharedInstance;
+ return sharedInstance.get();
+}
+
+void KeyboardScaffold::setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel)
+{
+ m_defaultLogLevel = logLevel;
+}
+
+std::shared_ptr<SurgSim::Framework::Logger> KeyboardScaffold::getLogger() const
+{
+ return m_logger;
+}
+
+SurgSim::Framework::LogLevel KeyboardScaffold::m_defaultLogLevel = SurgSim::Framework::LOG_LEVEL_INFO;
+
+}; // namespace Device
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/Keyboard/KeyboardScaffold.h b/SurgSim/Devices/Keyboard/KeyboardScaffold.h
new file mode 100644
index 0000000..70e4700
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/KeyboardScaffold.h
@@ -0,0 +1,105 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_KEYBOARD_KEYBOARDSCAFFOLD_H
+#define SURGSIM_DEVICES_KEYBOARD_KEYBOARDSCAFFOLD_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Logger.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+class DataGroup;
+}
+
+namespace Device
+{
+class KeyboardDevice;
+class OsgKeyboardHandler;
+
+/// A class that implements the behavior of KeyboardDevice objects.
+/// \sa SurgSim::Device::KeyboardDevice
+class KeyboardScaffold
+{
+ friend class KeyboardDevice;
+ friend class KeyboardDeviceTest;
+ friend class OsgKeyboardHandler;
+
+public:
+ /// Constructor.
+ /// \param logger (optional) The logger to be used by the scaffold object and the devices it manages.
+ /// If unspecified or empty, a console logger will be created and used.
+ explicit KeyboardScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger = nullptr);
+ /// Destructor
+ ~KeyboardScaffold();
+
+ /// Gets the logger used by this object and the devices it manages.
+ /// \return The logger.
+ std::shared_ptr<SurgSim::Framework::Logger> getLogger() const;
+
+ /// Gets or creates the scaffold shared by all KeyboardDevice instances.
+ /// The scaffold is managed using a SingleInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<KeyboardScaffold> getOrCreateSharedInstance();
+
+ /// Sets the default log level.
+ /// Has no effect unless called before a scaffold is created (i.e. before the first device).
+ /// \param logLevel The log level.
+ static void setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel);
+
+private:
+ /// Internal per-device information.
+ struct DeviceData;
+
+ /// Registers the specified device object.
+ /// If successful, the device object will become connected to an hardware device.
+ /// \param device The device object to be used, which should have a unique name.
+ /// \return True if the initialization succeeds, false if it fails.
+ bool registerDevice(KeyboardDevice* device);
+ /// Unregisters the specified device object.
+ /// The corresponding controller will become unused, and can be re-registered later.
+ /// \return True on success, false on failure.
+ bool unregisterDevice();
+
+ /// Updates the device information for a single device.
+ /// \param key Unmodified OSG key code.
+ /// \param modifierMask Modifier mask.
+ /// \return True on success.
+ bool updateDevice(int key, int modifierMask);
+
+ /// Get keyboard handler
+ /// \return The keyboard handler associated with this device
+ OsgKeyboardHandler* getKeyboardHandler() const;
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static SurgSim::DataStructures::DataGroup buildDeviceInputData();
+
+
+ /// Logger used by the scaffold and all devices.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+ /// The default logging level.
+ static SurgSim::Framework::LogLevel m_defaultLogLevel;
+ /// The keyboard device managed by this scaffold
+ std::unique_ptr<DeviceData> m_device;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_KEYBOARD_KEYBOARDSCAFFOLD_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Keyboard/OsgKeyboardHandler.cpp b/SurgSim/Devices/Keyboard/OsgKeyboardHandler.cpp
new file mode 100644
index 0000000..a66f006
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/OsgKeyboardHandler.cpp
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Keyboard/KeyCode.h"
+#include "SurgSim/Devices/Keyboard/KeyboardScaffold.h"
+#include "SurgSim/Devices/Keyboard/OsgKeyboardHandler.h"
+
+#include <memory>
+
+#include <osgGA/GUIEventHandler>
+
+namespace SurgSim
+{
+namespace Device
+{
+
+OsgKeyboardHandler::OsgKeyboardHandler() : m_keyboardScaffold(KeyboardScaffold::getOrCreateSharedInstance())
+{
+}
+
+bool OsgKeyboardHandler::handle(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter&)
+{
+ bool result = false;
+
+ switch(ea.getEventType())
+ {
+ case(osgGA::GUIEventAdapter::KEYDOWN) :
+ {// Note that we are setting the modifier mask here instead of the modifier itself
+ m_keyboardScaffold.lock()->updateDevice(ea.getUnmodifiedKey(), ea.getModKeyMask());
+ result = true;
+ break;
+ }
+ case(osgGA::GUIEventAdapter::KEYUP) :
+ {
+ m_keyboardScaffold.lock()->updateDevice(KeyCode::NONE, ModKeyMask::MODKEY_NONE);
+ result = true;
+ break;
+ }
+ default:
+ result = false;
+ break;
+ }
+
+ return result;
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Keyboard/OsgKeyboardHandler.h b/SurgSim/Devices/Keyboard/OsgKeyboardHandler.h
new file mode 100644
index 0000000..3ef6541
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/OsgKeyboardHandler.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_KEYBOARD_OSGKEYBOARDHANDLER_H
+#define SURGSIM_DEVICES_KEYBOARD_OSGKEYBOARDHANDLER_H
+
+#include <memory>
+
+#include <osgGA/GUIEventHandler>
+
+namespace SurgSim
+{
+namespace Device
+{
+
+class KeyboardScaffold;
+
+class OsgKeyboardHandler : public osgGA::GUIEventHandler
+{
+public:
+ /// Constructor
+ OsgKeyboardHandler();
+
+ /// Method to handle GUI event
+ /// \param ea A osgGA::GUIEventAdapter
+ /// \param aa A osgGA::GUIActionAdapter (required by this virtual method)
+ /// \return True if the event has been handled by this method; Otherwise, false.
+ virtual bool handle(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& aa) override;
+
+private:
+ /// A back pointer to the scaffold which owns this handle
+ std::weak_ptr<KeyboardScaffold> m_keyboardScaffold;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_KEYBOARD_OSGKEYBOARDHANDLER_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Keyboard/UnitTests/CMakeLists.txt b/SurgSim/Devices/Keyboard/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..a8e45d8
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/UnitTests/CMakeLists.txt
@@ -0,0 +1,36 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ KeyboardDeviceTest.cpp
+ KeyboardScaffoldTest.cpp
+)
+
+set(LIBS
+ KeyboardDevice
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ ${Boost_LIBRARIES}
+ ${OPENSCENEGRAPH_LIBRARIES}
+)
+
+surgsim_add_unit_tests(KeyboardDeviceTest)
+
+set_target_properties(KeyboardDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Keyboard/UnitTests/KeyboardDeviceTest.cpp b/SurgSim/Devices/Keyboard/UnitTests/KeyboardDeviceTest.cpp
new file mode 100644
index 0000000..3a251f6
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/UnitTests/KeyboardDeviceTest.cpp
@@ -0,0 +1,83 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 KeyboardDevice class.
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Keyboard/KeyboardDevice.h"
+#include "SurgSim/Devices/Keyboard/KeyboardScaffold.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+using SurgSim::Device::KeyboardDevice;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Testing::MockInputOutput;
+
+class KeyboardDeviceTest : public ::testing::Test
+{
+public:
+ static void update(std::shared_ptr<KeyboardDevice> device)
+ {
+ device->m_scaffold->updateDevice(0, 0);
+ }
+};
+
+
+TEST_F(KeyboardDeviceTest, CreateInitializeAndDestroyDevice)
+{
+ std::shared_ptr<KeyboardDevice> device = std::make_shared<KeyboardDevice>("TestKeyboard");
+
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Keyboard device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+
+ ASSERT_TRUE(device->finalize()) << "Device finalization failed";
+ EXPECT_FALSE(device->isInitialized());
+
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Keyboard device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+
+ ASSERT_TRUE(device->finalize()) << "Device finalization failed";
+ EXPECT_FALSE(device->isInitialized());
+}
+
+TEST_F(KeyboardDeviceTest, InputConsumer)
+{
+ std::shared_ptr<KeyboardDevice> device = std::make_shared<KeyboardDevice>("TestKeyboard");
+ device->initialize();
+
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_TRUE(device->addInputConsumer(consumer));
+
+ KeyboardDeviceTest::update(device);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.integers().hasData("key"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.integers().hasData("modifierMask"));
+
+ device->finalize();
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Keyboard/UnitTests/KeyboardScaffoldTest.cpp b/SurgSim/Devices/Keyboard/UnitTests/KeyboardScaffoldTest.cpp
new file mode 100644
index 0000000..b9a7a7c
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/UnitTests/KeyboardScaffoldTest.cpp
@@ -0,0 +1,55 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgKeyboardScaffold class and its device interactions.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Devices/Keyboard/KeyboardScaffold.h"
+
+using SurgSim::Device::KeyboardScaffold;
+
+TEST(KeyboardScaffoldTest, CreateAndDestroyScaffold)
+{
+ std::shared_ptr<KeyboardScaffold> scaffold = KeyboardScaffold::getOrCreateSharedInstance();
+ ASSERT_TRUE(nullptr != scaffold) << "The scaffold was not created!";
+ std::weak_ptr<KeyboardScaffold> scaffold1 = scaffold;
+ {
+ std::shared_ptr<KeyboardScaffold> stillHaveScaffold = scaffold1.lock();
+ EXPECT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ EXPECT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+ {
+ std::shared_ptr<KeyboardScaffold> sameScaffold = KeyboardScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, sameScaffold) << "Unable to get scaffold from class";
+ EXPECT_EQ(scaffold, sameScaffold) << "Scaffold mismatch!";
+ }
+
+ scaffold.reset();
+ {
+ std::shared_ptr<KeyboardScaffold> dontHaveScaffold = scaffold1.lock();
+ EXPECT_TRUE(nullptr == dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ }
+
+ scaffold = KeyboardScaffold::getOrCreateSharedInstance();
+ ASSERT_TRUE(nullptr != scaffold) << "The scaffold was not created the 2nd time!";
+ std::weak_ptr<KeyboardScaffold> scaffold2 = scaffold;
+ {
+ std::shared_ptr<KeyboardScaffold> stillHaveScaffold = scaffold2.lock();
+ ASSERT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ ASSERT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+}
\ No newline at end of file
diff --git a/SurgSim/Devices/Keyboard/VisualTests/CMakeLists.txt b/SurgSim/Devices/Keyboard/VisualTests/CMakeLists.txt
new file mode 100644
index 0000000..ace14f4
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/VisualTests/CMakeLists.txt
@@ -0,0 +1,35 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+set(UNIT_TEST_SOURCES
+ KeyboardVisualTests.cpp
+)
+
+set(UNIT_TEST_HEADERS
+)
+
+add_executable(KeyboardVisualTest
+ ${UNIT_TEST_SOURCES} ${UNIT_TEST_HEADERS})
+
+set(LIBS KeyboardDevice
+ SurgSimDataStructures
+ SurgSimInput
+ ${OPENSCENEGRAPH_LIBRARIES}
+)
+
+target_link_libraries(KeyboardVisualTest ${LIBS})
+
+# Put KeyboardVisualTest into folder "Devices"
+set_target_properties(KeyboardVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Keyboard/VisualTests/KeyboardVisualTests.cpp b/SurgSim/Devices/Keyboard/VisualTests/KeyboardVisualTests.cpp
new file mode 100644
index 0000000..f02f34a
--- /dev/null
+++ b/SurgSim/Devices/Keyboard/VisualTests/KeyboardVisualTests.cpp
@@ -0,0 +1,325 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <iostream>
+#include <map>
+#include <memory>
+#include <string>
+
+#include <osg/Camera>
+#include <osg/Geode>
+#include <osgText/Text>
+#include <osgViewer/Viewer>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Keyboard/KeyboardDevice.h"
+#include "SurgSim/Devices/Keyboard/OsgKeyboardHandler.h"
+#include "SurgSim/Devices/Keyboard/KeyCode.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+
+
+
+using SurgSim::DataStructures::DataGroup;
+
+struct TestListener : public SurgSim::Input::InputConsumerInterface
+{
+ TestListener()
+ {
+ creatKeyMap();
+ createModifierMap();
+ }
+ virtual void initializeInput(const std::string& device, const DataGroup& inputData) override {}
+ virtual void handleInput(const std::string& device, const DataGroup& inputData) override
+ {
+ int key, modifierMask;
+ inputData.integers().get("key", &key);
+ inputData.integers().get("modifierMask", &modifierMask);
+
+ if (key != SurgSim::Device::KeyCode::NONE)
+ {
+ std::cerr << "Key pressed :" << keyMap[key] << std::endl;
+ if (modifierMask != SurgSim::Device::ModKeyMask::MODKEY_NONE)
+ {
+ if (modifierMap[modifierMask] != "")
+ {
+ std::cerr << "Modifier:" << modifierMap[modifierMask] << std::endl;
+ }
+ else
+ {
+ std::cerr << "Modifier: UNDEFINED" << std::endl;
+ }
+ }
+ }
+ }
+
+ // keyMap is used to output the corresponding key name for a given key code.
+ std::map<int, std::string> keyMap;
+
+ // modifierMap is used to output the corresponding modifier name for a given modifier mask.
+ std::map<int, std::string> modifierMap;
+
+ void creatKeyMap()
+ {
+ keyMap[SurgSim::Device::KeyCode::KEY_SPACE] = "KEY_SPACE";
+ keyMap[SurgSim::Device::KeyCode::KEY_0] = "0";
+ keyMap[SurgSim::Device::KeyCode::KEY_1] = "1";
+ keyMap[SurgSim::Device::KeyCode::KEY_2] = "2";
+ keyMap[SurgSim::Device::KeyCode::KEY_3] = "3";
+ keyMap[SurgSim::Device::KeyCode::KEY_4] = "4";
+ keyMap[SurgSim::Device::KeyCode::KEY_5] = "5";
+ keyMap[SurgSim::Device::KeyCode::KEY_6] = "6";
+ keyMap[SurgSim::Device::KeyCode::KEY_7] = "7";
+ keyMap[SurgSim::Device::KeyCode::KEY_8] = "8";
+ keyMap[SurgSim::Device::KeyCode::KEY_9] = "9";
+ keyMap[SurgSim::Device::KeyCode::KEY_A] = "a";
+ keyMap[SurgSim::Device::KeyCode::KEY_B] = "b";
+ keyMap[SurgSim::Device::KeyCode::KEY_C] = "c";
+ keyMap[SurgSim::Device::KeyCode::KEY_D] = "d";
+ keyMap[SurgSim::Device::KeyCode::KEY_E] = "e";
+ keyMap[SurgSim::Device::KeyCode::KEY_F] = "f";
+ keyMap[SurgSim::Device::KeyCode::KEY_G] = "g";
+ keyMap[SurgSim::Device::KeyCode::KEY_H] = "h";
+ keyMap[SurgSim::Device::KeyCode::KEY_I] = "i";
+ keyMap[SurgSim::Device::KeyCode::KEY_J] = "j";
+ keyMap[SurgSim::Device::KeyCode::KEY_K] = "k";
+ keyMap[SurgSim::Device::KeyCode::KEY_L] = "l";
+ keyMap[SurgSim::Device::KeyCode::KEY_M] = "m";
+ keyMap[SurgSim::Device::KeyCode::KEY_N] = "n";
+ keyMap[SurgSim::Device::KeyCode::KEY_O] = "o";
+ keyMap[SurgSim::Device::KeyCode::KEY_P] = "p";
+ keyMap[SurgSim::Device::KeyCode::KEY_Q] = "q";
+ keyMap[SurgSim::Device::KeyCode::KEY_R] = "r";
+ keyMap[SurgSim::Device::KeyCode::KEY_S] = "s";
+ keyMap[SurgSim::Device::KeyCode::KEY_T] = "t";
+ keyMap[SurgSim::Device::KeyCode::KEY_U] = "u";
+ keyMap[SurgSim::Device::KeyCode::KEY_V] = "v";
+ keyMap[SurgSim::Device::KeyCode::KEY_W] = "w";
+ keyMap[SurgSim::Device::KeyCode::KEY_X] = "x";
+ keyMap[SurgSim::Device::KeyCode::KEY_Y] = "y";
+ keyMap[SurgSim::Device::KeyCode::KEY_Z] = "z";
+ keyMap[SurgSim::Device::KeyCode::KEY_EXCLAIM] = "KEY_EXCLAIM";
+ keyMap[SurgSim::Device::KeyCode::KEY_QUOTEDBL] = "KEY_QUOTEDBL";
+ keyMap[SurgSim::Device::KeyCode::KEY_HASH] = "KEY_HASH";
+ keyMap[SurgSim::Device::KeyCode::KEY_DOLLAR] = "KEY_DOLLAR";
+ keyMap[SurgSim::Device::KeyCode::KEY_AMPERSAND] = "KEY_AMPERSAND";
+ keyMap[SurgSim::Device::KeyCode::KEY_QUOTE] = "KEY_QUOTE";
+ keyMap[SurgSim::Device::KeyCode::KEY_LEFTPAREN] = "KEY_LEFTPAREN";
+ keyMap[SurgSim::Device::KeyCode::KEY_RIGHTPAREN]= "KEY_RIGHTPAREN";
+ keyMap[SurgSim::Device::KeyCode::KEY_ASTERISK] = "KEY_ASTERISK";
+ keyMap[SurgSim::Device::KeyCode::KEY_PLUS] = "KEY_PLUS";
+ keyMap[SurgSim::Device::KeyCode::KEY_COMMA] = "KEY_COMMA";
+ keyMap[SurgSim::Device::KeyCode::KEY_MINUS] = "KEY_MINUS";
+ keyMap[SurgSim::Device::KeyCode::KEY_PERIOD] = "KEY_PERIOD";
+ keyMap[SurgSim::Device::KeyCode::KEY_SLASH] = "KEY_SLASH";
+ keyMap[SurgSim::Device::KeyCode::KEY_COLON] = "KEY_COLON";
+ keyMap[SurgSim::Device::KeyCode::KEY_SEMICOLON] = "KEY_SEMICOLON";
+ keyMap[SurgSim::Device::KeyCode::KEY_LESS] = "KEY_LESS";
+ keyMap[SurgSim::Device::KeyCode::KEY_EQUALS] = "KEY_EQUALS";
+ keyMap[SurgSim::Device::KeyCode::KEY_GREATER] = "KEY_GREATER";
+ keyMap[SurgSim::Device::KeyCode::KEY_QUESTION] = "KEY_QUESTION";
+ keyMap[SurgSim::Device::KeyCode::KEY_AT] = "KEY_AT";
+ keyMap[SurgSim::Device::KeyCode::KEY_LEFTBRACKET] = "KEY_LEFTBRACKET";
+ keyMap[SurgSim::Device::KeyCode::KEY_BACKSLASH] = "KEY_BACKSLASH";
+ keyMap[SurgSim::Device::KeyCode::KEY_RIGHTBRACKET]= "KEY_RIGHTBRACKET";
+ keyMap[SurgSim::Device::KeyCode::KEY_CARET] = "KEY_CARET";
+ keyMap[SurgSim::Device::KeyCode::KEY_UNDERSCORE] = "KEY_UNDERSCORE";
+ keyMap[SurgSim::Device::KeyCode::KEY_BACKQUOTE] = "KEY_BACKQUOTE";
+ keyMap[SurgSim::Device::KeyCode::KEY_BACKSPACE] = "KEY_BACKSPACE";
+ keyMap[SurgSim::Device::KeyCode::KEY_TAB] = "KEY_TAB";
+ keyMap[SurgSim::Device::KeyCode::KEY_LINEFEED] = "KEY_LINEFEED";
+ keyMap[SurgSim::Device::KeyCode::KEY_CLEAR] = "KEY_CLEAR";
+ keyMap[SurgSim::Device::KeyCode::KEY_RETURN] = "KEY_RETURN";
+ keyMap[SurgSim::Device::KeyCode::KEY_PAUSE] = "KEY_PAUSE";
+ keyMap[SurgSim::Device::KeyCode::KEY_SCROLL_LOCK] = "KEY_SCROLL_LOCK";
+ keyMap[SurgSim::Device::KeyCode::KEY_SYS_REQ] = "KEY_SYS_REQ";
+ keyMap[SurgSim::Device::KeyCode::KEY_ESCAPE] = "KEY_ESCAPE";
+ keyMap[SurgSim::Device::KeyCode::KEY_DELETE] = "KEY_DELETE";
+ keyMap[SurgSim::Device::KeyCode::KEY_HOME] = "KEY_HOME";
+ keyMap[SurgSim::Device::KeyCode::KEY_LEFT] = "KEY_LEFT";
+ keyMap[SurgSim::Device::KeyCode::KEY_UP] = "KEY_UP";
+ keyMap[SurgSim::Device::KeyCode::KEY_RIGHT] = "KEY_RIGHT";
+ keyMap[SurgSim::Device::KeyCode::KEY_DOWN] = "KEY_DOWN";
+ keyMap[SurgSim::Device::KeyCode::KEY_PRIOR] = "KEY_PRIOR";
+ keyMap[SurgSim::Device::KeyCode::KEY_PAGE_UP] = "KEY_PAGE_UP";
+ keyMap[SurgSim::Device::KeyCode::KEY_NEXT] = "KEY_NEXT";
+ keyMap[SurgSim::Device::KeyCode::KEY_PAGE_DOWN] = "KEY_PAGE_DOWN";
+ keyMap[SurgSim::Device::KeyCode::KEY_END] = "KEY_END";
+ keyMap[SurgSim::Device::KeyCode::KEY_BEGIN] = "KEY_BEGIN";
+ keyMap[SurgSim::Device::KeyCode::KEY_SELECT] = "KEY_SELECT";
+ keyMap[SurgSim::Device::KeyCode::KEY_PRINT] = "KEY_PRINT";
+ keyMap[SurgSim::Device::KeyCode::KEY_EXECUTE] = "KEY_EXECUTE";
+ keyMap[SurgSim::Device::KeyCode::KEY_INSERT] = "KEY_INSERT";
+ keyMap[SurgSim::Device::KeyCode::KEY_UNDO] = "KEY_UNDO";
+ keyMap[SurgSim::Device::KeyCode::KEY_REDO] = "KEY_REDO";
+ keyMap[SurgSim::Device::KeyCode::KEY_MENU] = "KEY_MENU";
+ keyMap[SurgSim::Device::KeyCode::KEY_FIND] = "KEY_FIND";
+ keyMap[SurgSim::Device::KeyCode::KEY_CANCEL] = "KEY_CANCEL";
+ keyMap[SurgSim::Device::KeyCode::KEY_HELP] = "KEY_HELP";
+ keyMap[SurgSim::Device::KeyCode::KEY_BREAK] = "KEY_BREAK";
+ keyMap[SurgSim::Device::KeyCode::KEY_MODE_SWITCH] = "KEY_MODE_SWITCH";
+ keyMap[SurgSim::Device::KeyCode::KEY_SCRIPT_SWITCH] = "KEY_SCRIPT_SWITCH";
+ keyMap[SurgSim::Device::KeyCode::KEY_NUM_LOCK] = "KEY_NUM_LOCK";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_SPACE] = "KEY_KP_SPACE";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_TAB] = "KEY_KP_TAB";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_ENTER] = "KEY_KP_ENTER";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_F1] = "KEY_KP_F1";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_F2] = "KEY_KP_F2";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_F3] = "KEY_KP_F3";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_F4] = "KEY_KP_F4";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_HOME] = "KEY_KP_HOME";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_LEFT] = "KEY_KP_LEFT";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_UP] = "KEY_KP_UP";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_RIGHT] = "KEY_KP_RIGHT";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_DOWN] = "KEY_KP_DOWN";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_PRIOR] = "KEY_KP_PRIOR";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_PAGE_UP] = "KEY_KP_PAGE_UP";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_NEXT] = "KEY_KP_NEXT";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_PAGE_DOWN] = "KEY_KP_PAGE_DOWN";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_END] = "KEY_KP_END";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_BEGIN] = "KEY_KP_BEGIN";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_INSERT] = "KEY_KP_INSERT";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_DELETE] = "KEY_KP_DELETE";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_EQUAL] = "KEY_KP_EQUAL";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_MULTIPLY] = "KEY_KP_MULTIPLY";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_ADD] = "KEY_KP_ADD";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_SEPARATOR]= "KEY_KP_SEPARATOR";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_SUBTRACT] = "KEY_KP_SUBTRACT";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_DECIMAL] = "KEY_KP_DECIMAL";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_DIVIDE] = "KEY_KP_DIVIDE";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_0] = "KEY_KP_0";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_1] = "KEY_KP_1";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_2] = "KEY_KP_2";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_3] = "KEY_KP_3";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_4] = "KEY_KP_4";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_5] = "KEY_KP_5";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_6] = "KEY_KP_6";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_7] = "KEY_KP_7";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_8] = "KEY_KP_8";
+ keyMap[SurgSim::Device::KeyCode::KEY_KP_9] = "KEY_KP_9";
+ keyMap[SurgSim::Device::KeyCode::KEY_F1] = "KEY_F1";
+ keyMap[SurgSim::Device::KeyCode::KEY_F2] = "KEY_F2";
+ keyMap[SurgSim::Device::KeyCode::KEY_F3] = "KEY_F3";
+ keyMap[SurgSim::Device::KeyCode::KEY_F4] = "KEY_F4";
+ keyMap[SurgSim::Device::KeyCode::KEY_F5] = "KEY_F5";
+ keyMap[SurgSim::Device::KeyCode::KEY_F6] = "KEY_F6";
+ keyMap[SurgSim::Device::KeyCode::KEY_F7] = "KEY_F7";
+ keyMap[SurgSim::Device::KeyCode::KEY_F8] = "KEY_F8";
+ keyMap[SurgSim::Device::KeyCode::KEY_F9] = "KEY_F9";
+ keyMap[SurgSim::Device::KeyCode::KEY_F10] = "KEY_F10";
+ keyMap[SurgSim::Device::KeyCode::KEY_F11] = "KEY_F11";
+ keyMap[SurgSim::Device::KeyCode::KEY_F12] = "KEY_F12";
+ keyMap[SurgSim::Device::KeyCode::KEY_F13] = "KEY_F13";
+ keyMap[SurgSim::Device::KeyCode::KEY_F14] = "KEY_F14";
+ keyMap[SurgSim::Device::KeyCode::KEY_F15] = "KEY_F15";
+ keyMap[SurgSim::Device::KeyCode::KEY_F16] = "KEY_F16";
+ keyMap[SurgSim::Device::KeyCode::KEY_F17] = "KEY_F17";
+ keyMap[SurgSim::Device::KeyCode::KEY_F18] = "KEY_F18";
+ keyMap[SurgSim::Device::KeyCode::KEY_F19] = "KEY_F19";
+ keyMap[SurgSim::Device::KeyCode::KEY_F20] = "KEY_F20";
+ keyMap[SurgSim::Device::KeyCode::KEY_F21] = "KEY_F21";
+ keyMap[SurgSim::Device::KeyCode::KEY_F22] = "KEY_F22";
+ keyMap[SurgSim::Device::KeyCode::KEY_F23] = "KEY_F23";
+ keyMap[SurgSim::Device::KeyCode::KEY_F24] = "KEY_F24";
+ keyMap[SurgSim::Device::KeyCode::KEY_F25] = "KEY_F25";
+ keyMap[SurgSim::Device::KeyCode::KEY_F26] = "KEY_F26";
+ keyMap[SurgSim::Device::KeyCode::KEY_F27] = "KEY_F27";
+ keyMap[SurgSim::Device::KeyCode::KEY_F28] = "KEY_F28";
+ keyMap[SurgSim::Device::KeyCode::KEY_F29] = "KEY_F29";
+ keyMap[SurgSim::Device::KeyCode::KEY_F30] = "KEY_F30";
+ keyMap[SurgSim::Device::KeyCode::KEY_F31] = "KEY_F31";
+ keyMap[SurgSim::Device::KeyCode::KEY_F32] = "KEY_F32";
+ keyMap[SurgSim::Device::KeyCode::KEY_F33] = "KEY_F33";
+ keyMap[SurgSim::Device::KeyCode::KEY_F34] = "KEY_F34";
+ keyMap[SurgSim::Device::KeyCode::KEY_F35] = "KEY_F35";
+ keyMap[SurgSim::Device::KeyCode::KEY_SHIFT_L] = "KEY_SHIFT_L";
+ keyMap[SurgSim::Device::KeyCode::KEY_SHIFT_R] = "KEY_SHIFT_R";
+ keyMap[SurgSim::Device::KeyCode::KEY_CONTROL_L] = "KEY_CONTROL_L";
+ keyMap[SurgSim::Device::KeyCode::KEY_CONTROL_R] = "KEY_CONTROL_R";
+ keyMap[SurgSim::Device::KeyCode::KEY_CAPS_LOCK] = "KEY_CAPS_LOCK";
+ keyMap[SurgSim::Device::KeyCode::KEY_SHIFT_LOCK] = "KEY_SHIFT_LOCK";
+ keyMap[SurgSim::Device::KeyCode::KEY_META_L] = "KEY_META_L";
+ keyMap[SurgSim::Device::KeyCode::KEY_META_R] = "KEY_META_R";
+ keyMap[SurgSim::Device::KeyCode::KEY_ALT_L] = "KEY_ALT_L";
+ keyMap[SurgSim::Device::KeyCode::KEY_ALT_R] = "KEY_ALT_R";
+ keyMap[SurgSim::Device::KeyCode::KEY_SUPER_L] = "KEY_SUPER_L";
+ keyMap[SurgSim::Device::KeyCode::KEY_SUPER_R] = "KEY_SUPER_R";
+ keyMap[SurgSim::Device::KeyCode::KEY_HYPER_L] = "KEY_HYPER_L";
+ keyMap[SurgSim::Device::KeyCode::KEY_HYPER_R] = "KEY_HYPER_R";
+ };
+
+ void createModifierMap()
+ {
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_LEFT_SHIFT] = "KEY_SHIFT_L";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_RIGHT_SHIFT] = "KEY_SHIFT_R";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_LEFT_CTRL] = "KEY_CONTROL_L";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_RIGHT_CTRL] = "KEY_CONTROL_R";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_LEFT_ALT] = "KEY_ALT_L";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_RIGHT_ALT] = "KEY_ALT_R";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_LEFT_META] = "KEY_META_L";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_RIGHT_META] = "KEY_META_R";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_LEFT_SUPER] = "KEY_SUPER_L";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_RIGHT_SUPER] = "KEY_SUPER_R";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_LEFT_HYPER] = "KEY_HYPER_L";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_RIGHT_HYPER] = "KEY_HYPER_R";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_NUM_LOCK] = "KEY_NUM_LOCK";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_CAPS_LOCK] = "KEY_CAPS_LOCK";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_CTRL] = "KEY_CTRL";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_SHIFT] = "KEY_SHIFT";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_ALT] = "KEY_ALT";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_META] = "KEY_META";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_SUPER] = "KEY_SUPER";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_HYPER] = "KEY_HYPER";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_CAPS_SHIFT_L] = "KEY_CAPS_SHIFT_L";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_CAPS_SHIFT_R] = "KEY_CAPS_SHIFT_R";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_CAPS_CONTROL_L] = "KEY_CAPS_CONTROL_L";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_CAPS_CONTROL_R] = "KEY_CAPS_CONTROL_R";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_CAPS_ALT_L] = "KEY_CAPS_ALT_L";
+ modifierMap[SurgSim::Device::ModKeyMask::MODKEY_CAPS_ALT_R] = "KEY_CAPS_ALT_R";
+ }
+};
+
+int main(int argc, char* argv[])
+{
+ auto toolDevice = std::make_shared<SurgSim::Device::KeyboardDevice>("Keyboard");
+ toolDevice->initialize();
+
+ osg::ref_ptr<osgGA::GUIEventHandler> keyboardHandler = toolDevice->getKeyboardHandler();
+ auto consumer = std::make_shared<TestListener>();
+ toolDevice->addInputConsumer(consumer);
+
+ osg::ref_ptr<osgText::Text> text = new osgText::Text;
+ text->setText("Press any key in this window \n\nto verify keyboard driver \n\nworks correctly.");
+ text->setPosition(osg::Vec3(0.0f, 300.0f, 0.0f));
+
+ osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+ geode->addDrawable(text);
+
+ osg::ref_ptr<osg::Camera> camera = new osg::Camera;
+ camera->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
+ camera->setProjectionMatrixAsOrtho2D(0, 600 ,0, 400);
+ camera->getOrCreateStateSet()->setMode(GL_LIGHTING,osg::StateAttribute::OFF);
+ camera->addChild(geode);
+
+ osg::ref_ptr<osg::Group> group = new osg::Group;
+ group->addChild(camera);
+
+ osg::ref_ptr<osgViewer::Viewer> viewer = new osgViewer::Viewer;
+ viewer->setUpViewInWindow(400, 400, 640, 480);
+ viewer->addEventHandler(keyboardHandler);
+ viewer->setSceneData(group);
+
+ viewer->run();
+ return 0;
+}
diff --git a/SurgSim/Devices/LabJack/CMakeLists.txt b/SurgSim/Devices/LabJack/CMakeLists.txt
new file mode 100644
index 0000000..a69e006
--- /dev/null
+++ b/SurgSim/Devices/LabJack/CMakeLists.txt
@@ -0,0 +1,85 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+find_package(LabJack REQUIRED)
+
+link_directories(${Boost_LIBRARY_DIRS})
+
+set(LIBS
+ ${Boost_LIBRARIES}
+ SurgSimFramework
+ SurgSimInput
+ ${LABJACK_LIBRARY}
+)
+
+include_directories(
+ ${CMAKE_CURRENT_SOURCE_DIR}
+ ${LABJACK_INCLUDE_DIR}
+)
+
+set(LABJACK_DEVICE_SOURCES
+ LabJackDevice.cpp
+ LabJackThread.cpp
+)
+
+set(LABJACK_DEVICE_HEADERS
+ LabJackDevice.h
+ LabJackScaffold.h
+ LabJackThread.h
+)
+
+set(LABJACK_DEVICE_LINUX_SOURCES
+ linux/LabJackChecksums.cpp
+ linux/LabJackScaffold.cpp
+ linux/LabJackTypeConverters.cpp
+)
+
+set(LABJACK_DEVICE_LINUX_HEADERS
+ linux/LabJackChecksums.h
+ linux/LabJackConstants.h
+ linux/LabJackTypeConverters.h
+)
+
+set(LABJACK_DEVICE_WIN_SOURCES
+ win32/LabJackScaffold.cpp
+)
+
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Windows")
+ list(APPEND LABJACK_DEVICE_SOURCES ${LABJACK_DEVICE_WIN_SOURCES})
+else()
+ list(APPEND LABJACK_DEVICE_HEADERS ${LABJACK_DEVICE_LINUX_HEADERS})
+ list(APPEND LABJACK_DEVICE_SOURCES ${LABJACK_DEVICE_LINUX_SOURCES})
+endif()
+
+# TODO(advornik): the installation should NOT copy all the headers...
+surgsim_add_library(
+ LabJackDevice
+ "${LABJACK_DEVICE_SOURCES}" "${LABJACK_DEVICE_HEADERS}"
+ SurgSim/Devices/LabJack
+)
+
+target_link_libraries(LabJackDevice ${LIBS})
+
+if(BUILD_TESTING)
+ # The unit tests will be built whenever the library is built.
+ add_subdirectory(UnitTests)
+
+ if(GLUT_FOUND)
+ add_subdirectory(VisualTest)
+ endif(GLUT_FOUND)
+endif()
+
+
+set_target_properties(LabJackDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/LabJack/LabJack.dox b/SurgSim/Devices/LabJack/LabJack.dox
new file mode 100644
index 0000000..93996e5
--- /dev/null
+++ b/SurgSim/Devices/LabJack/LabJack.dox
@@ -0,0 +1,20 @@
+/*!
+
+\page LabJack LabJack: A Measurement and Automation Device
+
+LabJack Corporation produces measurement and automation devices (e.g., DAQs). Currently OpenSurgSim supports only some devices (dependent on operating system, see below), and only some functionality on any device.
+
+Supported functionality: digital input, digital output, and timers (e.g., PWM, frequency output, and quadrature input).
+
+Supported models:
+- Linux (and Mac OS X): The LabJack U3 and U6 (including Pro version) are supported with limited functionality.
+- Windows: The LabJack U3, U6 (including Pro version), and UE9 are supported with limited functionality.
+
+Dependencies:
+- Linux (and Mac OS X): The exodriver (aka labjackusb) driver must be installed https://github.com/labjack/exodriver
+ - Follow the instructions in the INSTALL file; installation of libusb-1.0 library and development files is required.
+ - (Probably optional) An environment variable named LABJACK_SDK may be set to the directory containing <tt>(include/)LabJackUD.h</tt>
+- Windows: The LabJackUD driver must be installed from the company's website http://labjack.com/
+ - An environment variable named LABJACK_SDK must be set to the directory containing <tt>(include\\)LabJackUD.h</tt>
+
+*/
\ No newline at end of file
diff --git a/SurgSim/Devices/LabJack/LabJackDevice.cpp b/SurgSim/Devices/LabJack/LabJackDevice.cpp
new file mode 100644
index 0000000..ea9360d
--- /dev/null
+++ b/SurgSim/Devices/LabJack/LabJackDevice.cpp
@@ -0,0 +1,306 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/LabJack/LabJackDevice.h"
+
+#include "SurgSim/Devices/LabJack/LabJackScaffold.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+LabJackDevice::LabJackDevice(const std::string& uniqueName) :
+ SurgSim::Input::CommonDevice(uniqueName, LabJackScaffold::buildDeviceInputData()),
+ m_model(LabJack::MODEL_SEARCH),
+ m_connection(LabJack::CONNECTION_SEARCH),
+ m_address(""),
+ m_timerBase(LabJack::TIMERBASE_DEFAULT),
+ m_timerClockDivisor(1),
+ m_timerCounterPinOffset(0),
+ m_threadRate(1000.0),
+ m_analogInputResolution(0),
+ m_analogInputSettling(0)
+{
+}
+
+LabJackDevice::~LabJackDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+bool LabJackDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized()) << "LabJackDevice already initialized.";
+
+ std::shared_ptr<LabJackScaffold> scaffold = LabJackScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold) << "LabJackDevice failed to get a LabJackScaffold.";
+
+ if (getDigitalOutputs().size() > 0)
+ {
+ SURGSIM_LOG_IF(!hasOutputProducer(), scaffold->getLogger(), WARNING) << "LabJackDevice named " << getName() <<
+ " has digital output channels but no output producer to provide the output data. Call setOutputProducer.";
+ }
+
+ if (getAnalogOutputs().size() > 0)
+ {
+ SURGSIM_LOG_IF(!hasOutputProducer(), scaffold->getLogger(), WARNING) << "LabJackDevice named " << getName() <<
+ " has analog output channels but no output producer to provide the output data. Call setOutputProducer.";
+ }
+
+ bool found = false;
+ // registerDevice will set this object's type and/or connection, if they are currently set to SEARCH.
+ if (scaffold->registerDevice(this))
+ {
+ m_scaffold = std::move(scaffold);
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": Initialized.";
+ found = true;
+ }
+ return found;
+}
+
+bool LabJackDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized()) << "LabJackDevice has not been initialized before finalize.";
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ const bool ok = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return ok;
+}
+
+bool LabJackDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+void LabJackDevice::setModel(LabJack::Model model)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "LabJackDevice's model cannot be set after it is initialized.";
+ m_model = model;
+}
+
+LabJack::Model LabJackDevice::getModel() const
+{
+ return m_model;
+}
+
+void LabJackDevice::setConnection(LabJack::Connection connection)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "LabJackDevice's connection cannot be set after it is initialized.";
+ m_connection = connection;
+}
+
+LabJack::Connection LabJackDevice::getConnection() const
+{
+ return m_connection;
+}
+
+void LabJackDevice::setAddress(std::string address)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "LabJackDevice's address cannot be set after it is initialized.";
+ m_address = address;
+}
+
+const std::string& LabJackDevice::getAddress() const
+{
+ return m_address;
+}
+
+void LabJackDevice::enableDigitalInput(int channel)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Digital input cannot be enabled for a LabJackDevice after it is initialized.";
+ m_digitalInputChannels.insert(channel);
+}
+
+void LabJackDevice::setDigitalInputs(const std::unordered_set<int>& digitalInputChannels)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Digital inputs cannot be enabled for a LabJackDevice after it is initialized.";
+ m_digitalInputChannels = digitalInputChannels;
+}
+
+const std::unordered_set<int>& LabJackDevice::getDigitalInputs() const
+{
+ return m_digitalInputChannels;
+}
+
+void LabJackDevice::enableDigitalOutput(int channel)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Digital output cannot be enabled for a LabJackDevice after it is initialized.";
+ m_digitalOutputChannels.insert(channel);
+}
+
+void LabJackDevice::setDigitalOutputs(const std::unordered_set<int>& digitalOutputChannels)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "Digital outputs cannot be enabled for a LabJackDevice after it is initialized.";
+ m_digitalOutputChannels = digitalOutputChannels;
+}
+
+const std::unordered_set<int>& LabJackDevice::getDigitalOutputs() const
+{
+ return m_digitalOutputChannels;
+}
+
+void LabJackDevice::setTimerBase(LabJack::TimerBase base)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "LabJackDevice's timer base cannot be set after it is initialized.";
+ m_timerBase = base;
+}
+
+LabJack::TimerBase LabJackDevice::getTimerBase() const
+{
+ return m_timerBase;
+}
+
+void LabJackDevice::setTimerClockDivisor(int divisor)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "LabJackDevice's timer clock divisor cannot be set after it is initialized.";
+ m_timerClockDivisor = divisor;
+}
+
+int LabJackDevice::getTimerClockDivisor() const
+{
+ return m_timerClockDivisor;
+}
+
+void LabJackDevice::setTimerCounterPinOffset(int offset)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "LabJackDevice's timer/counter pin offset cannot be set after it is initialized.";
+ m_timerCounterPinOffset = offset;
+}
+
+int LabJackDevice::getTimerCounterPinOffset() const
+{
+ return m_timerCounterPinOffset;
+}
+
+void LabJackDevice::enableTimer(int index, LabJack::TimerMode mode)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Timers cannot be enabled for a LabJackDevice after it is initialized.";
+ LabJack::TimerSettings timerModeAndOptionalInitialValue = {mode,
+ SurgSim::DataStructures::OptionalValue<int>()};
+ m_timers[index] = std::move(timerModeAndOptionalInitialValue);
+}
+
+void LabJackDevice::enableTimer(int index, LabJack::TimerMode mode, int initialValue)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Timers cannot be enabled for a LabJackDevice after it is initialized.";
+ LabJack::TimerSettings timerModeAndOptionalInitialValue = {mode,
+ SurgSim::DataStructures::OptionalValue<int>(initialValue)};
+ m_timers[index] = std::move(timerModeAndOptionalInitialValue);
+}
+
+void LabJackDevice::setTimers(const std::unordered_map<int, LabJack::TimerSettings>& timers)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Timers cannot be enabled for a LabJackDevice after it is initialized.";
+ m_timers = timers;
+}
+
+const std::unordered_map<int, LabJack::TimerSettings>& LabJackDevice::getTimers() const
+{
+ return m_timers;
+}
+
+void LabJackDevice::setMaximumUpdateRate(double rate)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "LabJackDevice's maximum update rate cannot be set after it is initialized.";
+ m_threadRate = rate;
+}
+
+double LabJackDevice::getMaximumUpdateRate() const
+{
+ return m_threadRate;
+}
+
+void LabJackDevice::enableAnalogInput(int positiveChannel, LabJack::Range range, int negativeChannel)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "Analog inputs cannot be enabled for a LabJackDevice after it is initialized.";
+ LabJack::AnalogInputSettings rangeAndOptionalNegativeChannel = {range,
+ SurgSim::DataStructures::OptionalValue<int>(negativeChannel)};
+ m_analogInputs[positiveChannel] = std::move(rangeAndOptionalNegativeChannel);
+}
+
+void LabJackDevice::enableAnalogInput(int channel, LabJack::Range range)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "Analog inputs cannot be enabled for a LabJackDevice after it is initialized.";
+ LabJack::AnalogInputSettings rangeAndOptionalNegativeChannel = {range,
+ SurgSim::DataStructures::OptionalValue<int>()};
+ m_analogInputs[channel] = std::move(rangeAndOptionalNegativeChannel);
+}
+
+void LabJackDevice::setAnalogInputs(const std::unordered_map<int,
+ LabJack::AnalogInputSettings>& analogInputs)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "Analog inputs cannot be enabled for a LabJackDevice after it is initialized.";
+ m_analogInputs = analogInputs;
+}
+
+const std::unordered_map<int, LabJack::AnalogInputSettings>& LabJackDevice::getAnalogInputs() const
+{
+ return m_analogInputs;
+}
+
+void LabJackDevice::enableAnalogOutput(int channel)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Analog outputs cannot be enabled for a LabJackDevice after it is initialized.";
+ m_analogOutputChannels.insert(channel);
+}
+
+void LabJackDevice::setAnalogOutputs(const std::unordered_set<int>& analogOutputChannels)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Analog outputs cannot be enabled for a LabJackDevice after it is initialized.";
+ m_analogOutputChannels = analogOutputChannels;
+}
+
+const std::unordered_set<int>& LabJackDevice::getAnalogOutputs() const
+{
+ return m_analogOutputChannels;
+}
+
+void LabJackDevice::setAnalogInputResolution(int resolution)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "Analog input resolution cannot be set for a LabJackDevice after it is initialized.";
+ m_analogInputResolution = resolution;
+}
+
+int LabJackDevice::getAnalogInputResolution() const
+{
+ return m_analogInputResolution;
+}
+
+void LabJackDevice::setAnalogInputSettling(int settling)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "Analog input settling time cannot be set for a LabJackDevice after it is initialized.";
+ m_analogInputSettling = settling;
+}
+
+int LabJackDevice::getAnalogInputSettling() const
+{
+ return m_analogInputSettling;
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/LabJack/LabJackDevice.h b/SurgSim/Devices/LabJack/LabJackDevice.h
new file mode 100644
index 0000000..c9bdbeb
--- /dev/null
+++ b/SurgSim/Devices/LabJack/LabJackDevice.h
@@ -0,0 +1,523 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_LABJACK_LABJACKDEVICE_H
+#define SURGSIM_DEVICES_LABJACK_LABJACKDEVICE_H
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <unordered_set>
+
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+class LabJackScaffold;
+
+namespace LabJack
+{
+///@{
+/// The timer or channel number that corresponds with the descriptive name used by the LabJack.
+/// Use these for the arguments to the enable* functions.
+enum TIMER
+{
+ TIMER0,
+ TIMER1,
+ TIMER2,
+ TIMER3,
+ TIMER4,
+ TIMER5
+};
+
+enum FIO_LINE
+{
+ FIO0,
+ FIO1,
+ FIO2,
+ FIO3,
+ FIO4,
+ FIO5,
+ FIO6,
+ FIO7
+};
+
+enum EIO_LINE
+{
+ EIO0 = 8,
+ EIO1,
+ EIO2,
+ EIO3,
+ EIO4,
+ EIO5,
+ EIO6,
+ EIO7
+};
+
+enum CIO_LINE
+{
+ CIO0 = 16,
+ CIO1,
+ CIO2,
+ CIO3
+};
+
+enum MIO_LINE
+{
+ MIO0 = 20,
+ MIO1,
+ MIO2
+};
+
+enum AIN
+{
+ AIN0,
+ AIN1,
+ AIN2,
+ AIN3,
+ AIN4,
+ AIN5,
+ AIN6,
+ AIN7,
+ AIN8,
+ AIN9,
+ AIN10,
+ AIN11,
+ AIN12,
+ AIN13,
+ AIN14,
+ AIN15
+};
+
+enum DAC
+{
+ DAC0,
+ DAC1
+};
+///@}
+
+/// The models of LabJack devices. Numbers come from LabJackUD.h.
+enum Model
+{
+ MODEL_SEARCH = -1,
+ MODEL_UE9 = 9,
+ MODEL_U3 = 3,
+ MODEL_U6 = 6
+};
+
+/// The connection (i.e., communication media) for LabJacks. Numbers come from LabJackUD.h.
+enum Connection
+{
+ CONNECTION_SEARCH = -1,
+ CONNECTION_USB = 1,
+ CONNECTION_ETHERNET = 2,
+ CONNECTION_ETHERNET_MB = 3,
+ CONNECTION_ETHERNET_DATA_ONLY = 4
+};
+
+/// The timer base frequencies for LabJacks. A given value can correspond to different clock frequencies for different
+/// LabJack models. The same clock frequency corresponds to different values depending on whether the
+/// high- or low-level driver is used. See section 2.10 - Timers/Counters in the respective model's User's Guide.
+enum TimerBase
+{
+ TIMERBASE_DEFAULT = -1,
+ TIMERBASE_0 = 0,
+ TIMERBASE_1 = 1,
+ TIMERBASE_2 = 2,
+ TIMERBASE_3 = 3,
+ TIMERBASE_4 = 4,
+ TIMERBASE_5 = 5,
+ TIMERBASE_6 = 6,
+ TIMERBASE_20 = 20,
+ TIMERBASE_21 = 21,
+ TIMERBASE_22 = 22,
+ TIMERBASE_23 = 23,
+ TIMERBASE_24 = 24,
+ TIMERBASE_25 = 25,
+ TIMERBASE_26 = 26
+};
+
+/// The timer modes. Numbers come from LabJackUD.h. Note that edge-counting modes require processing time: see the
+/// LabJack manual for restrictions on number of edges counted per second over all timers
+/// (e.g., 30,000/second for U3 or U6).
+enum TimerMode
+{
+ TIMERMODE_PWM_16BIT = 0, // 16 bit PWM
+ TIMERMODE_PWM_8BIT = 1, // 8 bit PWM
+ TIMERMODE_RISING_EDGES_32BIT = 2, // 32-bit rising to rising edge measurement
+ TIMERMODE_FALLING_EDGES_32BIT = 3, // 32-bit falling to falling edge measurement
+ TIMERMODE_DUTY_CYCLE = 4, // duty cycle measurement
+ TIMERMODE_FIRMWARE_COUNTER = 5, // firmware based rising edge counter
+ TIMERMODE_FIRMWARE_COUNTER_DEBOUNCED = 6, // firmware counter with debounce
+ TIMERMODE_FREQUENCY_OUTPUT = 7, // frequency output
+ TIMERMODE_QUADRATURE = 8, // Quadrature
+ TIMERMODE_TIMER_STOP = 9, // stops another timer after n pulses
+ TIMERMODE_SYSTEM_TIMER_LOWER_32BITS = 10, // read lower 32-bits of system timer
+ TIMERMODE_SYSTEM_TIMER_UPPR_32BITS = 11, // read upper 32-bits of system timer
+ TIMERMODE_RISING_EDGES_16BIT = 12, // 16-bit rising to rising edge measurement
+ TIMERMODE_FALLING_EDGES_16BIT = 13, // 16-bit falling to falling edge measurement
+ TIMERMODE_LINE_TO_LINE = 14 // Line to Line measurement
+};
+
+/// A struct holding the data to be associated with a Timer.
+struct TimerSettings
+{
+ /// Equality comparison.
+ /// \param other The object with which to compare.
+ /// \return true if equivalent.
+ bool operator==(const TimerSettings& other) const
+ {
+ return (mode == other.mode) && (initialValue == other.initialValue);
+ }
+
+ /// The mode.
+ TimerMode mode;
+
+ /// The initial value.
+ SurgSim::DataStructures::OptionalValue<int> initialValue;
+};
+
+/// The analog input ranges. Equivalent to gain. Ignored for Linux scaffold, which auto-ranges.
+enum Range
+{
+ RANGE_20 = 1, // -20V to +20V, LJ_rgBIP20V
+ RANGE_10 = 2, // -10V to +10V, LJ_rgBIP10V
+ RANGE_5 = 3, // -5V to +5V, LJ_rgBIP5V
+ RANGE_4 = 4, // -4V to +4V, LJ_rgBIP4V
+ RANGE_2_POINT_5 = 5, // -2.5V to +2.5V, LJ_rgBIP2P5V
+ RANGE_2 = 6, // -2V to +2V, LJ_rgBIP2V
+ RANGE_1_POINT_25 = 7, // -1.25V to +1.25V, LJ_rgBIP1P25V
+ RANGE_1 = 8, // -1V to +1V, LJ_rgBIP1V
+ RANGE_0_POINT_625 = 9, // -0.625V to +0.625V, LJ_rgBIPP625V
+ RANGE_0_POINT_1 = 10, // -0.1V to +0.1V, LJ_rgBIPP1V
+ RANGE_0_POINT_01 = 11 // -0.01V to +0.01V, LJ_rgBIPP01V
+};
+
+/// A struct holding the data to be associated with the positive channel for an analog input.
+struct AnalogInputSettings
+{
+ /// Equality comparison.
+ /// \param other The object with which to compare.
+ /// \return true if equivalent.
+ bool operator==(const AnalogInputSettings& other) const
+ {
+ return (negativeChannel == other.negativeChannel) && (range == other.range);
+ }
+
+ /// The range.
+ Range range;
+
+ /// The negative channel.
+ SurgSim::DataStructures::OptionalValue<int> negativeChannel;
+};
+};
+
+/// A class implementing the communication with a LabJack data acquisition (DAQ) device. Should work for the U3, U6,
+/// and U9 models on Windows and the U3 and U6 on Linux. See the manual(s) for your LabJack device(s) to understand
+/// the input and output data, the configuration parameters, timing limitations, etc. The various parameters and
+/// inputs are almost always passed through unchanged to the device driver. Timers, digital input/output, and
+/// analog input/output are supported. Currently not supported are counters, using the same channel as
+/// the positive channel for multiple analog inputs, and reconfiguring the device after initialization.
+/// \warning The LabJack device is configurable to such a degree that neither this class nor LabJackScaffold are able
+/// to do significant error-checking. If the output DataGroup and the calls (e.g., addTimer) to this class
+/// are not in agreement, the requests to the LabJack device driver will not be correct.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | scalar | "analogInput0" | %Analog input with AIN0 as the positive channel |
+/// | scalar | "analogInput" | %Analog input with AIN1 as the positive channel |
+/// | ... | ... | ... |
+/// | scalar | "analogInput16" | %Analog input with AIN16 as the positive channel |
+/// | boolean| "digitalInput0" | %Digital input, line #0, true for high input, false for low |
+/// | boolean| "digitalInput1" | %Digital input, line #1, true for high input, false for low |
+/// | ... | ... | ... |
+/// | boolean| "digitalInput23" | %Digital input, line #23, true for high input, false for low |
+/// | scalar | "timerInput0" | %The input from timer #0 if that timer provides input values |
+/// | scalar | "timerInput1" | %The input from timer #1 if that timer provides input values |
+/// | ... | ... | ... |
+/// | scalar | "timerInput6" | %The input from timer #6 if that timer provides input values |
+///
+///
+/// \par Application output used by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | scalar | "analogOutput0" | %Analog output, DAC0 |
+/// | scalar | "analogOutput1" | %Analog output, DAC1 |
+/// | boolean| "digitalOutput0" | %Digital output, line #0, true for high output, false for low |
+/// | boolean| "digitalOutput1" | %Digital output, line #1, true for high output, false for low |
+/// | ... | ... | ... |
+/// | boolean| "digitalOutput23" | %Digital output, line #23, true for high output, false for low |
+/// | scalar | "timerOutput0" | %The output for timer #0 if that timer accepts output values |
+/// | scalar | "timerOutput1" | %The output for timer #1 if that timer accepts output values |
+/// | ... | ... | ... |
+/// | scalar | "timerOutput6" | %The output for timer #6 if that timer accepts output values |
+///
+/// \sa SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface, LabJackScaffold
+class LabJackDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ explicit LabJackDevice(const std::string& uniqueName);
+
+ /// Destructor.
+ virtual ~LabJackDevice();
+
+ /// Fully initialize the device.
+ /// When the manager object creates the device, the internal state of the device usually isn't fully
+ /// initialized yet. This method performs any needed initialization.
+ /// \return True on success.
+ /// \exception Asserts if already initialized, or if unable to get a scaffold.
+ virtual bool initialize() override;
+
+ /// Check whether this device is initialized.
+ bool isInitialized() const;
+
+ /// Set the model, e.g., U6.
+ /// \param model The model.
+ /// \exception Asserts if already initialized.
+ void setModel(LabJack::Model model);
+
+ /// \return The model, e.g., U6.
+ LabJack::Model getModel() const;
+
+ /// Set the connection type of the LabJack, e.g., USB.
+ /// \param connection The communication medium.
+ /// \exception Asserts if already initialized.
+ void setConnection(LabJack::Connection connection);
+
+ /// \return The connection type of the LabJack, e.g., USB.
+ LabJack::Connection getConnection() const;
+
+ /// Set the address of the LabJack, e.g., "1" or "192.168.7.23". If the address is zero-length, attempt to open the
+ /// first-found device of the specified type on the specified connection.
+ /// \param address The address for the device, or a zero-length string.
+ /// \exception Asserts if already initialized.
+ void setAddress(std::string address);
+
+ /// \return The address of the LabJack, e.g., "1" or "192.168.7.23".
+ const std::string& getAddress() const;
+
+ /// Enable digital input line.
+ /// \param channel The channel number.
+ /// \exception Asserts if already initialized.
+ void enableDigitalInput(int channel);
+
+ /// Set which digital input lines are enabled.
+ /// \param digitalInputChannels The line numbers for the digital inputs.
+ /// \exception Asserts if already initialized.
+ void setDigitalInputs(const std::unordered_set<int>& digitalInputChannels);
+
+ /// \return The enabled digital input lines.
+ const std::unordered_set<int>& getDigitalInputs() const;
+
+ /// Enable digital output line.
+ /// \param channel The channel number.
+ /// \exception Asserts if already initialized.
+ void enableDigitalOutput(int channel);
+
+ /// Set which digital output lines are enabled.
+ /// \param digitalOutputChannels The line numbers for the digital outputs.
+ /// \exception Asserts if already initialized.
+ void setDigitalOutputs(const std::unordered_set<int>& digitalOutputChannels);
+
+ /// \return The enabled digital output lines.
+ const std::unordered_set<int>& getDigitalOutputs() const;
+
+ /// Set the timer base rate. Timer base rates that end in "_DIV" are divided by the divisor to get the actual timer
+ /// frequency. See section 2.10 - Timers/Counters in the respective LabJack model's User's Guide.
+ /// \param base The timer base rate.
+ /// \exception Asserts if already initialized.
+ void setTimerBase(LabJack::TimerBase base);
+
+ /// \return The timer base rate.
+ LabJack::TimerBase getTimerBase() const;
+
+ /// If the Timer type ends in "_DIV", then the actual timer frequency is divided by the divisor.
+ /// \param divisor The amount by which to divide the frequency. Values from 1-255 are used directly, while 0 means
+ /// divide by 256. Values above 255 are not supported and cause an error.
+ /// \exception Asserts if already initialized.
+ void setTimerClockDivisor(int divisor);
+
+ /// \return The timer clock divisor.
+ int getTimerClockDivisor() const;
+
+ /// The timers and counters are always on consecutive pins, but the start pin can be varied within limits.
+ /// \param offset The channel number of the first timer/counter.
+ /// \exception Asserts if already initialized.
+ void setTimerCounterPinOffset(int offset);
+
+ /// \return The channel number of the first timer/counter.
+ int getTimerCounterPinOffset() const;
+
+ /// Enable timer.
+ /// Since quadrature requires two lines, to measure a single quadrature encoder this function
+ /// must be called twice on consecutive timerNumbers. All output timers use the same clock (see setTimerBase and
+ /// setTimerClockDivisor).
+ /// \param index The index of the timer (not the line number, see setTimerCounterPinOffset).
+ /// \param mode The type of timer.
+ /// \exception Asserts if already initialized.
+ void enableTimer(int index, LabJack::TimerMode mode);
+
+ /// Enable timer with an initial value.
+ /// Since quadrature requires two lines, to measure a single quadrature encoder this function
+ /// must be called twice on consecutive timerNumbers. For example, to enable z-phase support for a quadrature
+ /// timer, with the Z (aka index) signal on digital channel FIO4 (channel 4), set initialValue for both timer
+ /// channels to ((1 << 15) | 4). All output timers use the same clock (see setTimerBase and setTimerClockDivisor).
+ /// \param index The index of the timer (not the line number, see setTimerCounterPinOffset).
+ /// \param mode The type of timer.
+ /// \param initialValue The initial value.
+ /// \exception Asserts if already initialized.
+ void enableTimer(int index, LabJack::TimerMode mode, int initialValue);
+
+ /// Set which timers are enabled.
+ /// \sa enableTimer
+ /// \param timers The map from timer index (not line number) to mode and optional initial value.
+ /// \exception Asserts if already initialized.
+ void setTimers(const std::unordered_map<int, LabJack::TimerSettings>& timers);
+
+ /// \return The enabled timers.
+ const std::unordered_map<int, LabJack::TimerSettings>& getTimers() const;
+
+ /// Set the maximum update rate for the LabJackThread. Since the device driver blocks thread execution
+ /// while acquiring new data, update rates have a definite upper-bound that is dependent on the requested
+ /// inputs (at least). See the LabJack User's Guide for details.
+ void setMaximumUpdateRate(double rate);
+
+ /// \return The maximum update rate for the LabJackThread.
+ double getMaximumUpdateRate() const;
+
+ /// Enable differential analog input.
+ /// \param positiveChannel The positive channel.
+ /// \param range The voltage range.
+ /// \param negativeChannel The negative channel.
+ /// \exception Asserts if already initialized.
+ /// \note On Linux, does not correctly handle negative channels 31 or 32 for U3 model.
+ void enableAnalogInput(int positiveChannel, LabJack::Range range, int negativeChannel);
+
+ /// Enable single-ended analog input.
+ /// \param channel The channel.
+ /// \param range The voltage range.
+ /// \exception Asserts if already initialized.
+ void enableAnalogInput(int channel, LabJack::Range range);
+
+ /// Set which analog inputs are enabled.
+ /// \sa enableAnalogInput
+ /// \param analogInputs The map from the line number of the positive channel to the range and
+ /// (for differential readings only) the line number of the negative channel.
+ /// \exception Asserts if already initialized.
+ void setAnalogInputs(const std::unordered_map<int, LabJack::AnalogInputSettings>& analogInputs);
+
+ /// \return The enabled analog inputs.
+ const std::unordered_map<int, LabJack::AnalogInputSettings>& getAnalogInputs() const;
+
+ /// Enable analog output.
+ /// \param channel The channel.
+ /// \exception Asserts if already initialized.
+ void enableAnalogOutput(int channel);
+
+ /// Set which analog outputs are enabled.
+ /// \sa enableAnalogOutput
+ /// \param analogOutputChannels The line numbers for the analog outputs.
+ /// \exception Asserts if already initialized.
+ void setAnalogOutputs(const std::unordered_set<int>& analogOutputChannels);
+
+ /// \return The enabled analog output channels.
+ const std::unordered_set<int>& getAnalogOutputs() const;
+
+ /// Set the resolution for all the analog inputs. The resolution parameter is a model-dependent code. Refer to the
+ /// User's Guide for the specific model to determine behavior for different codes. For example, for the U6 see
+ /// http://labjack.com/support/u6/users-guide/4.3.3 and http://labjack.com/support/u6/users-guide/appendix-b
+ /// \param resolution The resolution code.
+ /// \exception Asserts if already initialized.
+ void setAnalogInputResolution(int resolution);
+
+ /// \return The resolution code for all the analog inputs.
+ int getAnalogInputResolution() const;
+
+ /// Set the settling time for all the analog inputs. The settling parameter is a model-dependent code. Refer to the
+ /// User's Guide for the specific model to determine behavior for different codes. For example, for the U6 see
+ /// http://labjack.com/support/u6/users-guide/2.6
+ /// \param settling The settling time code.
+ /// \exception Asserts if already initialized.
+ void setAnalogInputSettling(int settling);
+
+ /// \return The settling time code for all the analog inputs.
+ int getAnalogInputSettling() const;
+
+private:
+ /// Finalize (de-initialize) the device.
+ /// \return True if device was successfully un-registered.
+ /// \exception Asserts if not initialized.
+ virtual bool finalize() override;
+
+ friend class LabJackScaffold;
+
+ /// The single scaffold object that handles communications with all instances of LabJackDevice.
+ std::shared_ptr<LabJackScaffold> m_scaffold;
+
+ /// The model, e.g., U6.
+ LabJack::Model m_model;
+
+ /// The type of communication connection, e.g., USB.
+ LabJack::Connection m_connection;
+
+ /// The address, or a zero-length string to indicate the first-found device of this type on this connection.
+ std::string m_address;
+
+ /// The line numbers for the digital inputs.
+ std::unordered_set<int> m_digitalInputChannels;
+
+ /// The analog inputs. The key is the positive channel.
+ std::unordered_map<int, LabJack::AnalogInputSettings> m_analogInputs;
+
+ /// The line numbers for the digital outputs.
+ std::unordered_set<int> m_digitalOutputChannels;
+
+ /// The line numbers for the analog outputs.
+ std::unordered_set<int> m_analogOutputChannels;
+
+ /// The timer base, which is the frequency of all the output timers unless it ends in "_DIV",
+ /// in which case the frequency is the base divided by the divisor. See section 2.10 - Timers/Counters in the
+ /// respective LabJack model's User's Guide.
+ LabJack::TimerBase m_timerBase;
+
+ /// The timer clock's divisor, see m_timerBase.
+ int m_timerClockDivisor;
+
+ /// The number of the lowest FIO pin that is a timer or counter.
+ int m_timerCounterPinOffset;
+
+ /// A map from the timers' line numbers to their mode and optional initial value.
+ std::unordered_map<int, LabJack::TimerSettings> m_timers;
+
+ /// The maximum update rate for the LabJackThread.
+ double m_threadRate;
+
+ /// The resolution for all the analog inputs.
+ int m_analogInputResolution;
+
+ /// The settling time for all the analog inputs.
+ int m_analogInputSettling;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_LABJACK_LABJACKDEVICE_H
diff --git a/SurgSim/Devices/LabJack/LabJackScaffold.h b/SurgSim/Devices/LabJack/LabJackScaffold.h
new file mode 100644
index 0000000..4aeff8e
--- /dev/null
+++ b/SurgSim/Devices/LabJack/LabJackScaffold.h
@@ -0,0 +1,150 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_LABJACK_LABJACKSCAFFOLD_H
+#define SURGSIM_DEVICES_LABJACK_LABJACKSCAFFOLD_H
+
+#include <memory>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class DataGroup;
+};
+
+namespace Framework
+{
+class Logger;
+}
+
+namespace Device
+{
+
+class LabJackDevice;
+class LabJackThread;
+
+/// A class that implements the behavior of LabJackDevice objects.
+/// \sa SurgSim::Device::LabJackDevice
+class LabJackScaffold
+{
+public:
+ /// Internal per-device information. This is public because it is passed to the LabJackThread and back.
+ struct DeviceData;
+
+ /// Constructor.
+ LabJackScaffold();
+
+ /// Destructor.
+ ~LabJackScaffold();
+
+ /// Gets the logger used by this object and the devices it manages.
+ /// \return The logger.
+ std::shared_ptr<SurgSim::Framework::Logger> getLogger() const;
+
+ /// Gets or creates the scaffold shared by all LabJackDevice instances.
+ /// The scaffold is managed using a SingleInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<LabJackScaffold> getOrCreateSharedInstance();
+
+ /// Does one-time configuration of the LabJack for timers, counters, and analog inputs.
+ /// Must be called by the LabJackThread because the LabJack separates all commands by the calling thread.
+ /// \param device The internal device data.
+ /// \return False if any errors.
+ bool configureDevice(DeviceData* device);
+
+private:
+ /// Internal shared state data type.
+ struct StateData;
+
+ /// Wrapper for the LabJack device handle.
+ class Handle;
+
+ friend class LabJackDevice;
+ friend class LabJackThread;
+ friend struct StateData;
+
+ /// Registers the specified device object.
+ /// If successful, the device object will become connected to an unused hardware device.
+ /// \param device The device object to be used, which should have a unique name.
+ /// \return True if the initialization succeeds, false if it fails.
+ bool registerDevice(LabJackDevice* device);
+
+ /// Unregisters the specified device object.
+ /// The corresponding hardware device will become unused, and can be re-registered later.
+ /// \param device The device object.
+ /// \return true on success, false on failure.
+ bool unregisterDevice(const LabJackDevice* device);
+
+ /// Executes the operations for a single input frame for a single device.
+ /// Should only be called from the context of the input loop thread.
+ /// \param info The internal device data.
+ /// \return True to keep the LabJackThread running.
+ bool runInputFrame(DeviceData* info);
+
+ /// Updates the device information for a single device.
+ /// \param info The internal device data.
+ /// \return true on success.
+ bool updateDevice(DeviceData* info);
+
+ /// Destroys the input loop thread.
+ /// \param data The internal device data.
+ /// \return true on success.
+ bool destroyPerDeviceThread(DeviceData* data);
+
+ /// One-time configuration of the clock and timers.
+ /// \param deviceData The internal device data.
+ /// \return False if any errors.
+ bool configureClockAndTimers(DeviceData* deviceData);
+
+ /// One-time configuration of the number of timers.
+ /// \param deviceData The internal device data.
+ /// \return False if any errors.
+ bool configureNumberOfTimers(DeviceData* deviceData);
+
+ /// One-time configuration of the clock.
+ /// \param deviceData The internal device data.
+ /// \return False if any errors.
+ bool configureClock(DeviceData* deviceData);
+
+ /// One-time configuration of the timers.
+ /// \param deviceData The internal device data.
+ /// \return False if any errors.
+ bool configureTimers(DeviceData* deviceData);
+
+ /// One-time configuration of the digital inputs and outputs.
+ /// \param deviceData The internal device data.
+ /// \return False if any errors.
+ bool configureDigital(DeviceData* deviceData);
+
+ /// One-time configuration of the analog inputs.
+ /// \param deviceData The internal device data.
+ /// \return False if any errors.
+ bool configureAnalog(DeviceData* deviceData);
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static SurgSim::DataStructures::DataGroup buildDeviceInputData();
+
+ /// Logger used by the scaffold and all devices.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+
+ /// Internal scaffold state.
+ std::unique_ptr<StateData> m_state;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_LABJACK_LABJACKSCAFFOLD_H
diff --git a/SurgSim/Devices/LabJack/LabJackThread.cpp b/SurgSim/Devices/LabJack/LabJackThread.cpp
new file mode 100644
index 0000000..8532672
--- /dev/null
+++ b/SurgSim/Devices/LabJack/LabJackThread.cpp
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/LabJack/LabJackThread.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+LabJackThread::LabJackThread(LabJackScaffold* scaffold, LabJackScaffold::DeviceData* deviceData) :
+ BasicThread("LabJack thread"),
+ m_scaffold(scaffold),
+ m_deviceData(deviceData)
+{
+}
+
+LabJackThread::~LabJackThread()
+{
+}
+
+bool LabJackThread::doInitialize()
+{
+ return m_scaffold->configureDevice(m_deviceData);
+}
+
+bool LabJackThread::doStartUp()
+{
+ return true;
+}
+
+bool LabJackThread::doUpdate(double dt)
+{
+ return m_scaffold->runInputFrame(m_deviceData);
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/LabJack/LabJackThread.h b/SurgSim/Devices/LabJack/LabJackThread.h
new file mode 100644
index 0000000..d997bc1
--- /dev/null
+++ b/SurgSim/Devices/LabJack/LabJackThread.h
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_LABJACK_LABJACKTHREAD_H
+#define SURGSIM_DEVICES_LABJACK_LABJACKTHREAD_H
+
+#include <memory>
+
+#include "SurgSim/Framework/BasicThread.h"
+#include "SurgSim/Devices/LabJack/LabJackScaffold.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+/// A class implementing the thread context for communicating with LabJack devices.
+/// \sa SurgSim::Device::LabJackScaffold
+class LabJackThread : public SurgSim::Framework::BasicThread
+{
+public:
+ explicit LabJackThread(LabJackScaffold* scaffold, LabJackScaffold::DeviceData* deviceData);
+
+ virtual ~LabJackThread();
+
+protected:
+ virtual bool doInitialize() override;
+ virtual bool doStartUp() override;
+ virtual bool doUpdate(double dt) override;
+
+private:
+ LabJackScaffold* m_scaffold;
+ LabJackScaffold::DeviceData* m_deviceData;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_LABJACK_LABJACKTHREAD_H
diff --git a/SurgSim/Devices/LabJack/UnitTests/CMakeLists.txt b/SurgSim/Devices/LabJack/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..dd82a95
--- /dev/null
+++ b/SurgSim/Devices/LabJack/UnitTests/CMakeLists.txt
@@ -0,0 +1,45 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ LabJackDeviceTest.cpp
+ LabJackScaffoldTest.cpp
+)
+
+set(UNIT_TEST_LINUX_SOURCES
+ LabJackChecksumsTest.cpp
+ LabJackTypeConvertersTest.cpp
+)
+
+if(NOT ${CMAKE_SYSTEM_NAME} STREQUAL "Windows")
+ list(APPEND UNIT_TEST_SOURCES ${UNIT_TEST_LINUX_SOURCES})
+endif()
+
+set(LIBS
+ LabJackDevice
+ SurgSimInput
+ SurgSimFramework
+ SurgSimDataStructures
+ ${Boost_LIBRARIES}
+)
+
+surgsim_add_unit_tests(LabJackDeviceTest)
+
+set_target_properties(LabJackDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/LabJack/UnitTests/LabJackChecksumsTest.cpp b/SurgSim/Devices/LabJack/UnitTests/LabJackChecksumsTest.cpp
new file mode 100644
index 0000000..3c532f0
--- /dev/null
+++ b/SurgSim/Devices/LabJack/UnitTests/LabJackChecksumsTest.cpp
@@ -0,0 +1,95 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 LabJack specific checksum functions.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Devices/LabJack/linux/LabJackConstants.h"
+#include "SurgSim/Devices/LabJack/linux/LabJackChecksums.h"
+
+TEST(LabJackChecksumsTest, NormalChecksum)
+{
+ // Sum less than 256
+ std::array<unsigned char, SurgSim::Device::LabJack::MAXIMUM_BUFFER> bytes;
+ unsigned char fillValue = 2;
+ bytes.fill(fillValue);
+ int count = 10;
+ int expectedValue = fillValue * (count - 1);
+ EXPECT_EQ(expectedValue, SurgSim::Device::LabJack::normalChecksum8(bytes, count));
+ SurgSim::Device::LabJack::normalChecksum(&bytes, count);
+ EXPECT_EQ(expectedValue, bytes[0]);
+
+ // Sum greater than 256, quotient + remainder < 256
+ fillValue = 100;
+ bytes.fill(fillValue);
+ count = 4;
+ int sum = fillValue * (count - 1); // 300
+ int quotient = sum / 256; // 1
+ int remainder = sum % 256; // 44
+ expectedValue = quotient + remainder;
+ EXPECT_EQ(expectedValue, SurgSim::Device::LabJack::normalChecksum8(bytes, count));
+ SurgSim::Device::LabJack::normalChecksum(&bytes, count);
+ EXPECT_EQ(expectedValue, bytes[0]);
+
+ // Sum greater than 256, quotient + remainder > 256
+ fillValue = 255;
+ bytes.fill(fillValue);
+ bytes[4] = 2;
+ count = 5;
+ // sum = 767, quotient = 2, remainder = 255, quotient + remainder = 257
+ // second_quotient = 1, second_remainder = 1, second_quotient + second_remainder = 2
+ expectedValue = 2;
+ EXPECT_EQ(expectedValue, SurgSim::Device::LabJack::normalChecksum8(bytes, count));
+ SurgSim::Device::LabJack::normalChecksum(&bytes, count);
+ EXPECT_EQ(expectedValue, bytes[0]);
+}
+
+
+TEST(LabJackChecksumsTest, ExtendedChecksum)
+{
+ // Sums less than 256
+ std::array<unsigned char, SurgSim::Device::LabJack::MAXIMUM_BUFFER> bytes;
+ unsigned char fillValue = 2;
+ bytes.fill(fillValue);
+ int count = 10;
+ int expectedValue16 = (count - 6) * fillValue; // 4 * 2 = 8
+ EXPECT_EQ(expectedValue16, SurgSim::Device::LabJack::extendedChecksum16(bytes, count));
+ int expectedValue8 = (6 - 1) * fillValue; // 5 * 2 = 10
+ EXPECT_EQ(expectedValue8, SurgSim::Device::LabJack::extendedChecksum8(bytes));
+
+ SurgSim::Device::LabJack::extendedChecksum(&bytes, count);
+ EXPECT_EQ(expectedValue16, bytes[4]);
+ EXPECT_EQ(0, bytes[5]);
+ // extendedChecksum alters the buffer before setting bytes[0] to the return value of extendedChecksum8.
+ EXPECT_EQ(expectedValue8 - 2 * fillValue + expectedValue16, bytes[0]);
+
+ // Sum greater than 256, quotient + remainder < 256
+ fillValue = 100;
+ bytes.fill(fillValue);
+ count = 20;
+ expectedValue16 = (count - 6) * fillValue; // 14 * 100 = 1400
+ EXPECT_EQ(expectedValue16, SurgSim::Device::LabJack::extendedChecksum16(bytes, count));
+ expectedValue8 = 245; // sum = 5 * 100 = 500, quotient = 1, remainder = 244, quotient + remainder = 245
+ EXPECT_EQ(expectedValue8, SurgSim::Device::LabJack::extendedChecksum8(bytes));
+
+ SurgSim::Device::LabJack::extendedChecksum(&bytes, count);
+ EXPECT_EQ(120, bytes[4]);
+ EXPECT_EQ(5, bytes[5]);
+ // extendedChecksum alters the buffer before setting bytes[0] to the return value of extendedChecksum8.
+ // sum = 100 + 100 + 100 + 120 + 5 = 425, quotient = 1, remainder = 169, quotient + remainder = 170
+ EXPECT_EQ(170, bytes[0]);
+}
diff --git a/SurgSim/Devices/LabJack/UnitTests/LabJackDeviceTest.cpp b/SurgSim/Devices/LabJack/UnitTests/LabJackDeviceTest.cpp
new file mode 100644
index 0000000..feac585
--- /dev/null
+++ b/SurgSim/Devices/LabJack/UnitTests/LabJackDeviceTest.cpp
@@ -0,0 +1,359 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 LabJackDevice class.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/LabJack/LabJackDevice.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Device::LabJackDevice;
+using SurgSim::Device::LabJackScaffold;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Input::InputConsumerInterface;
+using SurgSim::Input::OutputProducerInterface;
+using SurgSim::Testing::MockInputOutput;
+
+namespace
+{
+void testCreateDeviceSeveralTimes(bool doSleep)
+{
+ for (int i = 0; i < 6; ++i)
+ {
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+ if (doSleep)
+ {
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(100));
+ }
+ // the device will be destroyed here
+ }
+}
+};
+
+TEST(LabJackDeviceTest, CreateUninitializedDevice)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+}
+
+TEST(LabJackDeviceTest, CreateAndInitializeDevice)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+TEST(LabJackDeviceTest, Name)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_EQ("TestLabJack", device->getName());
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+ EXPECT_EQ("TestLabJack", device->getName());
+}
+
+TEST(LabJackDeviceTest, CreateDeviceSeveralTimes)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ testCreateDeviceSeveralTimes(true);
+}
+
+TEST(LabJackDeviceTest, CreateSeveralDevices)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<LabJackDevice> device1 = std::make_shared<LabJackDevice>("LabJack1");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+
+ // We can't check what happens with the scaffolds, since those are no longer a part of the device's API...
+
+ std::shared_ptr<LabJackDevice> device2 = std::make_shared<LabJackDevice>("LabJack2");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ if (!device2->initialize())
+ {
+ std::cerr << "[Warning: second LabJack controller did not come up; is it plugged in?]" << std::endl;
+ }
+}
+
+TEST(LabJackDeviceTest, CreateDevicesWithSameName)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<LabJackDevice> device1 = std::make_shared<LabJackDevice>("LabJack");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+
+ std::shared_ptr<LabJackDevice> device2 = std::make_shared<LabJackDevice>("LabJack");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate name.";
+}
+
+TEST(LabJackDeviceTest, InputConsumer)
+{
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer->m_numTimesInitializedInput);
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+ EXPECT_EQ(0, consumer->m_numTimesInitializedInput);
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device->addInputConsumer(consumer));
+
+ // Adding the same input consumer again should fail.
+ EXPECT_FALSE(device->addInputConsumer(consumer));
+
+ // Sleep to see how many times the consumer is invoked.
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeInputConsumer(consumer));
+
+ // Removing the same input consumer again should fail.
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+
+ // Check the number of invocations.
+ EXPECT_EQ(1, consumer->m_numTimesInitializedInput);
+ // The update rate is not compared against a larger lower bound because the LabJack's response time is highly
+ // dependent on the model, the connection (e.g., whether a high-speed USB2 hub is between the device and the
+ // USB host), the number of USB frames required, and the calculations necessary for inputs/outputs.
+ // See section 3.1 - Command/Response in the LabJack User's Guide.
+ EXPECT_GE(consumer->m_numTimesReceivedInput, 1);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 1.1 * device->getMaximumUpdateRate());
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasEntry(SurgSim::DataStructures::Names::DIGITAL_INPUT_PREFIX +
+ std::to_string(0))); // The LabJackDevice provides entries for digital input lines 0 - 23.
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.scalars().hasEntry(SurgSim::DataStructures::Names::TIMER_INPUT_PREFIX +
+ std::to_string(0))); // The LabJackDevice provides entries for timer input lines 0 - 6.
+}
+
+TEST(LabJackDeviceTest, OutputProducer)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+
+ std::shared_ptr<MockInputOutput> producer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_TRUE(device->setOutputProducer(producer));
+
+ // Sleep to see how many times the producer is invoked.
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeOutputProducer(producer));
+
+ // Removing the same input producer again should fail.
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+
+ // Check the number of invocations.
+ // The update rate is not compared against a larger lower bound because the LabJack's response time is highly
+ // dependent on the model, the connection (e.g., whether a high-speed USB2 hub is between the device and the
+ // USB host), the number of USB frames required, and the calculations necessary for inputs/outputs.
+ // See section 3.1 - Command/Response in the LabJack User's Guide.
+ EXPECT_GE(producer->m_numTimesRequestedOutput, 1);
+ EXPECT_LE(producer->m_numTimesRequestedOutput, 1.1 * device->getMaximumUpdateRate());
+}
+
+TEST(LabJackDeviceTest, GettersAndSetters)
+{
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+
+ const SurgSim::Device::LabJack::Model model = SurgSim::Device::LabJack::MODEL_U6;
+ EXPECT_NO_THROW(device->setModel(model));
+ EXPECT_EQ(model, device->getModel());
+
+ const SurgSim::Device::LabJack::Connection connection = SurgSim::Device::LabJack::CONNECTION_USB;
+ EXPECT_NO_THROW(device->setConnection(connection));
+ EXPECT_EQ(connection, device->getConnection());
+
+ const std::string address = "14";
+ EXPECT_NO_THROW(device->setAddress(address));
+ EXPECT_EQ(address, device->getAddress());
+
+ std::unordered_set<int> digitalInputChannels;
+ digitalInputChannels.insert(2);
+ digitalInputChannels.insert(11);
+ EXPECT_NO_THROW(device->enableDigitalInput(SurgSim::Device::LabJack::FIO2));
+ EXPECT_NO_THROW(device->enableDigitalInput(SurgSim::Device::LabJack::EIO3));
+ EXPECT_EQ(digitalInputChannels, device->getDigitalInputs());
+
+ digitalInputChannels.insert(14);
+ EXPECT_NO_THROW(device->setDigitalInputs(digitalInputChannels));
+ EXPECT_EQ(digitalInputChannels, device->getDigitalInputs());
+
+ std::unordered_set<int> digitalOutputChannels;
+ digitalOutputChannels.insert(3);
+ digitalOutputChannels.insert(17);
+ EXPECT_NO_THROW(device->enableDigitalOutput(SurgSim::Device::LabJack::FIO3));
+ EXPECT_NO_THROW(device->enableDigitalOutput(SurgSim::Device::LabJack::CIO1));
+ EXPECT_EQ(digitalOutputChannels, device->getDigitalOutputs());
+
+ digitalOutputChannels.insert(5);
+ EXPECT_NO_THROW(device->setDigitalOutputs(digitalOutputChannels));
+ EXPECT_EQ(digitalOutputChannels, device->getDigitalOutputs());
+
+ const SurgSim::Device::LabJack::TimerBase timerBase = SurgSim::Device::LabJack::TIMERBASE_DEFAULT;
+ EXPECT_NO_THROW(device->setTimerBase(timerBase));
+ EXPECT_EQ(timerBase, device->getTimerBase());
+
+ const int timerDivisor = 7;
+ EXPECT_NO_THROW(device->setTimerClockDivisor(timerDivisor));
+ EXPECT_EQ(timerDivisor, device->getTimerClockDivisor());
+
+ const int pinOffset = 3;
+ EXPECT_NO_THROW(device->setTimerCounterPinOffset(pinOffset));
+ EXPECT_EQ(pinOffset, device->getTimerCounterPinOffset());
+
+ std::unordered_map<int, SurgSim::Device::LabJack::TimerSettings> timers;
+ SurgSim::Device::LabJack::TimerSettings quadrature =
+ {SurgSim::Device::LabJack::TIMERMODE_QUADRATURE, SurgSim::DataStructures::OptionalValue<int>()};
+ timers[0] = quadrature;
+ SurgSim::Device::LabJack::TimerSettings frequencyOutput =
+ {SurgSim::Device::LabJack::TIMERMODE_FREQUENCY_OUTPUT, SurgSim::DataStructures::OptionalValue<int>(234)};
+ timers[3] = frequencyOutput;
+ EXPECT_NO_THROW(device->enableTimer(SurgSim::Device::LabJack::TIMER0,
+ SurgSim::Device::LabJack::TIMERMODE_QUADRATURE));
+ EXPECT_NO_THROW(device->enableTimer(SurgSim::Device::LabJack::TIMER3,
+ SurgSim::Device::LabJack::TIMERMODE_FREQUENCY_OUTPUT, 234));
+ EXPECT_EQ(timers, device->getTimers());
+
+ SurgSim::Device::LabJack::TimerSettings dutyCycle =
+ {SurgSim::Device::LabJack::TIMERMODE_DUTY_CYCLE, SurgSim::DataStructures::OptionalValue<int>()};
+ timers[4] = dutyCycle;
+ EXPECT_NO_THROW(device->setTimers(timers));
+ EXPECT_EQ(timers, device->getTimers());
+
+ const double rate = 300.0;
+ EXPECT_NO_THROW(device->setMaximumUpdateRate(rate));
+ EXPECT_NEAR(rate, device->getMaximumUpdateRate(), 1e-9);
+
+ std::unordered_map<int, SurgSim::Device::LabJack::AnalogInputSettings> analogInputs;
+ const SurgSim::Device::LabJack::AnalogInputSettings differentialRangeAndChannel =
+ {SurgSim::Device::LabJack::Range::RANGE_10,
+ SurgSim::DataStructures::OptionalValue<int>(1)};
+ analogInputs[2] = differentialRangeAndChannel;
+ const SurgSim::Device::LabJack::AnalogInputSettings singleEndedRange =
+ {SurgSim::Device::LabJack::Range::RANGE_0_POINT_1,
+ SurgSim::DataStructures::OptionalValue<int>()};
+ analogInputs[5] = singleEndedRange;
+ EXPECT_NO_THROW(device->enableAnalogInput(SurgSim::Device::LabJack::AIN2,
+ SurgSim::Device::LabJack::Range::RANGE_10, 1));
+ EXPECT_NO_THROW(device->enableAnalogInput(SurgSim::Device::LabJack::AIN5,
+ SurgSim::Device::LabJack::Range::RANGE_0_POINT_1));
+ EXPECT_EQ(analogInputs, device->getAnalogInputs());
+
+ const SurgSim::Device::LabJack::AnalogInputSettings anotherRange =
+ {SurgSim::Device::LabJack::Range::RANGE_0_POINT_01,
+ SurgSim::DataStructures::OptionalValue<int>()};
+ analogInputs[6] = anotherRange;
+ EXPECT_NO_THROW(device->setAnalogInputs(analogInputs));
+ EXPECT_EQ(analogInputs, device->getAnalogInputs());
+
+ const int resolution = 3;
+ EXPECT_NO_THROW(device->setAnalogInputResolution(resolution));
+ EXPECT_EQ(resolution, device->getAnalogInputResolution());
+
+ const int settling = 2;
+ EXPECT_NO_THROW(device->setAnalogInputSettling(settling));
+ EXPECT_EQ(settling, device->getAnalogInputSettling());
+
+ std::unordered_set<int> analogOutputChannels;
+ analogOutputChannels.insert(1);
+ EXPECT_NO_THROW(device->enableAnalogOutput(SurgSim::Device::LabJack::DAC1));
+ EXPECT_EQ(analogOutputChannels, device->getAnalogOutputs());
+
+ analogOutputChannels.insert(0);
+ EXPECT_NO_THROW(device->setAnalogOutputs(analogOutputChannels));
+ EXPECT_EQ(analogOutputChannels, device->getAnalogOutputs());
+}
+
+
+
+TEST(LabJackDeviceTest, NoSettingAfterInitialization)
+{
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+
+ EXPECT_THROW(device->setModel(SurgSim::Device::LabJack::MODEL_U6), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setConnection(SurgSim::Device::LabJack::CONNECTION_USB), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setAddress("14"), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->enableDigitalInput(SurgSim::Device::LabJack::FIO2), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setDigitalInputs(std::unordered_set<int>()), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->enableDigitalOutput(SurgSim::Device::LabJack::FIO3), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setDigitalOutputs(std::unordered_set<int>()), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setTimerBase(SurgSim::Device::LabJack::TIMERBASE_DEFAULT),
+ SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setTimerClockDivisor(7), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setTimerCounterPinOffset(3), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->enableTimer(SurgSim::Device::LabJack::TIMER0, SurgSim::Device::LabJack::TIMERMODE_QUADRATURE),
+ SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setTimers(std::unordered_map<int,
+ SurgSim::Device::LabJack::TimerSettings>()),
+ SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setMaximumUpdateRate(300.0), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->enableAnalogInput(SurgSim::Device::LabJack::AIN2,
+ SurgSim::Device::LabJack::Range::RANGE_10, 1),
+ SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setAnalogInputs(std::unordered_map<int,
+ SurgSim::Device::LabJack::AnalogInputSettings>()),
+ SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setAnalogInputResolution(3), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setAnalogInputSettling(2), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->enableAnalogOutput(SurgSim::Device::LabJack::DAC0), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(device->setAnalogOutputs(std::unordered_set<int>()), SurgSim::Framework::AssertionFailure);
+}
\ No newline at end of file
diff --git a/SurgSim/Devices/LabJack/UnitTests/LabJackScaffoldTest.cpp b/SurgSim/Devices/LabJack/UnitTests/LabJackScaffoldTest.cpp
new file mode 100644
index 0000000..31ac6f5
--- /dev/null
+++ b/SurgSim/Devices/LabJack/UnitTests/LabJackScaffoldTest.cpp
@@ -0,0 +1,185 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 LabJackScaffold class and its device interactions.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/LabJack/LabJackDevice.h"
+#include "SurgSim/Devices/LabJack/LabJackScaffold.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+
+using SurgSim::Device::LabJackDevice;
+using SurgSim::Device::LabJackScaffold;
+
+TEST(LabJackScaffoldTest, CreateAndDestroyScaffold)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<LabJackScaffold> scaffold = LabJackScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ std::weak_ptr<LabJackScaffold> scaffold1 = scaffold;
+ {
+ std::shared_ptr<LabJackScaffold> stillHaveScaffold = scaffold1.lock();
+ EXPECT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ EXPECT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+ {
+ std::shared_ptr<LabJackScaffold> sameScaffold = LabJackScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, sameScaffold) << "Unable to get scaffold from class";
+ EXPECT_EQ(scaffold, sameScaffold) << "Scaffold mismatch!";
+ }
+
+ scaffold.reset();
+ {
+ std::shared_ptr<LabJackScaffold> dontHaveScaffold = scaffold1.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ }
+
+ scaffold = LabJackScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created the 2nd time!";
+ std::weak_ptr<LabJackScaffold> scaffold2 = scaffold;
+ {
+ std::shared_ptr<LabJackScaffold> stillHaveScaffold = scaffold2.lock();
+ ASSERT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ ASSERT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+}
+
+TEST(LabJackScaffoldTest, ScaffoldLifeCycle)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::weak_ptr<LabJackScaffold> lastScaffold;
+ {
+ std::shared_ptr<LabJackScaffold> scaffold = LabJackScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ lastScaffold = scaffold;
+ }
+ {
+ std::shared_ptr<LabJackScaffold> dontHaveScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ lastScaffold.reset();
+ }
+
+ {
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_NE(nullptr, device) << "Creation failed. Is a LabJack device plugged in?";
+ // note: the device is NOT initialized!
+ {
+ std::shared_ptr<LabJackScaffold> scaffold = LabJackScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<LabJackScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The device has not been initialized, so it should NOT be hanging on to the device!
+ {
+ std::shared_ptr<LabJackScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+ // the ("empty") device is about to get destroyed
+ }
+
+ {
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+ {
+ std::shared_ptr<LabJackScaffold> scaffold = LabJackScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<LabJackScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The same scaffold is supposed to still be around because of the device
+ {
+ std::shared_ptr<LabJackScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+
+ std::shared_ptr<LabJackScaffold> scaffold = LabJackScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ EXPECT_EQ(sameScaffold, scaffold);
+ }
+ // the device and the scaffold are about to get destroyed
+ }
+
+ {
+ std::shared_ptr<LabJackScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+
+ {
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Didn't this work a moment ago?";
+ std::shared_ptr<LabJackScaffold> scaffold = LabJackScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+
+ std::shared_ptr<LabJackScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+}
+
+
+TEST(LabJackScaffoldTest, CreateDeviceSeveralTimes)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::weak_ptr<LabJackScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ EXPECT_EQ(nullptr, lastScaffold.lock());
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+ std::shared_ptr<LabJackScaffold> scaffold = LabJackScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold;
+ // the device and the scaffold will be destroyed here
+ }
+}
+
+
+TEST(LabJackScaffoldTest, CreateDeviceSeveralTimesWithScaffoldRef)
+{
+ //LabJackScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<LabJackScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ std::shared_ptr<LabJackDevice> device = std::make_shared<LabJackDevice>("TestLabJack");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a LabJack device plugged in?";
+ std::shared_ptr<LabJackScaffold> scaffold = LabJackScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ if (! lastScaffold)
+ {
+ lastScaffold = scaffold;
+ }
+ EXPECT_EQ(lastScaffold, scaffold);
+ // the device will be destroyed here, but the scaffold stays around because we have a shared_ptr to it.
+ }
+}
diff --git a/SurgSim/Devices/LabJack/UnitTests/LabJackTypeConvertersTest.cpp b/SurgSim/Devices/LabJack/UnitTests/LabJackTypeConvertersTest.cpp
new file mode 100644
index 0000000..f6b4bdd
--- /dev/null
+++ b/SurgSim/Devices/LabJack/UnitTests/LabJackTypeConvertersTest.cpp
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 LabJack specific type converters.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Devices/LabJack/linux/LabJackConstants.h"
+#include "SurgSim/Devices/LabJack/linux/LabJackTypeConverters.h"
+
+namespace
+{
+ const double EPSILON = 1e-9;
+}
+
+TEST(LabJackTypeConvertersTest, DoubleFromChars)
+{
+ std::array<unsigned char, SurgSim::Device::LabJack::MAXIMUM_BUFFER> bytes;
+ for (int i = 0; i < 24; ++i)
+ {
+ bytes[i] = 0;
+ }
+
+ EXPECT_NEAR(0.0, SurgSim::Device::LabJack::doubleFromChars(bytes, 0), EPSILON);
+
+ bytes[12] = 2;
+ EXPECT_NEAR(2.0, SurgSim::Device::LabJack::doubleFromChars(bytes, 8), EPSILON);
+
+ bytes[20] = 255;
+ bytes[21] = 255;
+ bytes[22] = 255;
+ bytes[23] = 255;
+ EXPECT_NEAR(-1.0, SurgSim::Device::LabJack::doubleFromChars(bytes, 16), EPSILON);
+}
+
+TEST(LabJackTypeConvertersTest, Uint32FromChars)
+{
+ std::array<unsigned char, SurgSim::Device::LabJack::MAXIMUM_BUFFER> bytes;
+ for (int i = 0; i < 8; ++i)
+ {
+ bytes[i] = 0;
+ }
+ bytes[4] = 1;
+
+ EXPECT_ANY_THROW(SurgSim::Device::LabJack::uint32FromChars(bytes, 0, 5));
+ EXPECT_EQ(0, SurgSim::Device::LabJack::uint32FromChars(bytes, 0, 4));
+ EXPECT_EQ(1 << 24, SurgSim::Device::LabJack::uint32FromChars(bytes, 1, 4));
+ EXPECT_EQ(1 << 16, SurgSim::Device::LabJack::uint32FromChars(bytes, 2, 3));
+ EXPECT_EQ(1, SurgSim::Device::LabJack::uint32FromChars(bytes, 4, 4));
+}
+
+TEST(LabJackTypeConvertersTest, Uint16FromChars)
+{
+ std::array<unsigned char, SurgSim::Device::LabJack::MAXIMUM_BUFFER> bytes;
+ for (int i = 0; i < 4; ++i)
+ {
+ bytes[i] = 0;
+ }
+ bytes[2] = 1;
+
+ EXPECT_ANY_THROW(SurgSim::Device::LabJack::uint16FromChars(bytes, 0, 3));
+ EXPECT_EQ(0, SurgSim::Device::LabJack::uint16FromChars(bytes, 0, 2));
+ EXPECT_EQ(1 << 8, SurgSim::Device::LabJack::uint16FromChars(bytes, 1, 2));
+ EXPECT_EQ(1, SurgSim::Device::LabJack::uint16FromChars(bytes, 2, 1));
+ EXPECT_EQ(1, SurgSim::Device::LabJack::uint16FromChars(bytes, 2, 2));
+}
\ No newline at end of file
diff --git a/SurgSim/Devices/LabJack/VisualTest/CMakeLists.txt b/SurgSim/Devices/LabJack/VisualTest/CMakeLists.txt
new file mode 100644
index 0000000..f8758b1
--- /dev/null
+++ b/SurgSim/Devices/LabJack/VisualTest/CMakeLists.txt
@@ -0,0 +1,50 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+)
+
+set(EXAMPLE_SOURCES
+ main.cpp
+)
+
+set(EXAMPLE_HEADERS
+)
+
+add_executable(LabJackVisualTest
+ ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
+surgsim_show_ide_folders(
+ "${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
+
+set(LIBS
+ LabJackDevice
+ IdentityPoseDevice
+ VisualTestCommon
+ SurgSimInput
+ SurgSimFramework
+ SurgSimDataStructures
+ ${Boost_LIBRARIES}
+ ${OPENGL_LIBRARIES}
+)
+
+target_link_libraries(LabJackVisualTest ${LIBS})
+
+set_target_properties(LabJackVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/LabJack/VisualTest/main.cpp b/SurgSim/Devices/LabJack/VisualTest/main.cpp
new file mode 100644
index 0000000..916390b
--- /dev/null
+++ b/SurgSim/Devices/LabJack/VisualTest/main.cpp
@@ -0,0 +1,312 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+#include "SurgSim/Devices/LabJack/LabJackDevice.h"
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Device::LabJackDevice;
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+class LabJackToPoseFilter : public SurgSim::Input::CommonDevice,
+ public SurgSim::Input::InputConsumerInterface, public SurgSim::Input::OutputProducerInterface
+{
+
+public:
+ LabJackToPoseFilter(const std::string& name, int firstTimerForQuadrature, int plusX, int minusX,
+ double translationPerUpdate, int positiveAnalogDifferential, int analogSingleEnded, int xOut, int loopbackOut) :
+ SurgSim::Input::CommonDevice(name),
+ m_pose(RigidTransform3d::Identity()),
+ m_translationPerUpdate(translationPerUpdate),
+ m_lineForPlusX(plusX),
+ m_lineForMinusX(minusX),
+ m_firstTimerForQuadrature(firstTimerForQuadrature),
+ m_analogInputDifferentialPositive(positiveAnalogDifferential),
+ m_analogInputSingleEnded(analogSingleEnded),
+ m_cachedOutputIndices(false),
+ m_digitalInputPlusXIndex(-1),
+ m_digitalInputMinusXIndex(-1),
+ m_timerInputIndex(-1),
+ m_analogInputDifferentialIndex(-1),
+ m_analogInputSingleEndedIndex(-1),
+ m_analogOutputIndex(-1),
+ m_digitalOutputIndex(-1)
+ {
+ DataGroupBuilder inputBuilder;
+ inputBuilder.addPose(SurgSim::DataStructures::Names::POSE);
+ getInputData() = inputBuilder.createData();
+ m_poseIndex = getInputData().poses().getIndex(SurgSim::DataStructures::Names::POSE);
+
+ DataGroupBuilder outputBuilder;
+ const std::string outputName =
+ SurgSim::DataStructures::Names::ANALOG_OUTPUT_PREFIX + std::to_string(xOut);
+ outputBuilder.addScalar(outputName);
+ const std::string digitalOutputName =
+ SurgSim::DataStructures::Names::DIGITAL_OUTPUT_PREFIX + std::to_string(loopbackOut);
+ outputBuilder.addBoolean(digitalOutputName);
+ m_outputData = outputBuilder.createData();
+ m_analogOutputIndex = m_outputData.scalars().getIndex(outputName);
+ m_digitalOutputIndex = m_outputData.booleans().getIndex(digitalOutputName);
+ }
+
+ virtual ~LabJackToPoseFilter()
+ {
+ finalize();
+ }
+
+ bool initialize()
+ {
+ return true;
+ }
+
+ bool finalize()
+ {
+ return true;
+ }
+
+ void initializeInput(const std::string& device, const DataGroup& inputData)
+ {
+ m_digitalInputPlusXIndex = inputData.booleans().getIndex(SurgSim::DataStructures::Names::DIGITAL_INPUT_PREFIX +
+ std::to_string(m_lineForPlusX));
+ m_digitalInputMinusXIndex = inputData.booleans().getIndex(SurgSim::DataStructures::Names::DIGITAL_INPUT_PREFIX +
+ std::to_string(m_lineForMinusX));
+ m_timerInputIndex = inputData.scalars().getIndex(SurgSim::DataStructures::Names::TIMER_INPUT_PREFIX +
+ std::to_string(m_firstTimerForQuadrature));
+ m_analogInputDifferentialIndex =
+ inputData.scalars().getIndex(SurgSim::DataStructures::Names::ANALOG_INPUT_PREFIX +
+ std::to_string(m_analogInputDifferentialPositive));
+ m_analogInputSingleEndedIndex =
+ inputData.scalars().getIndex(SurgSim::DataStructures::Names::ANALOG_INPUT_PREFIX +
+ std::to_string(m_analogInputSingleEnded));
+
+ inputFilter(inputData, &getInputData());
+ }
+
+ void handleInput(const std::string& device, const DataGroup& inputData)
+ {
+ m_lastInputData = inputData;
+ inputFilter(inputData, &getInputData());
+ pushInput();
+ }
+
+ bool requestOutput(const std::string& device, DataGroup* outputData)
+ {
+ bool state = pullOutput();
+ if (state)
+ {
+ outputFilter(m_outputData, outputData);
+ }
+ return state;
+ }
+
+ void inputFilter(const DataGroup& dataToFilter, DataGroup* result)
+ {
+ // Turn LabJack inputs into a pose so it can control the sphere.
+ if (m_digitalInputPlusXIndex >= 0)
+ {
+ bool value;
+ if (dataToFilter.booleans().get(m_digitalInputPlusXIndex, &value))
+ {
+ // If the device passed us this line's input, and the input is high...
+ if (value)
+ {
+ m_pose.translation() += Vector3d::UnitX() * m_translationPerUpdate;
+ }
+ }
+ }
+
+ if (m_digitalInputMinusXIndex >= 0)
+ {
+ bool value;
+ if (dataToFilter.booleans().get(m_digitalInputMinusXIndex, &value))
+ {
+ // If the device passed us this line's input, and the input is high...
+ if (value)
+ {
+ m_pose.translation() -= Vector3d::UnitX() * m_translationPerUpdate;
+ }
+ }
+ }
+
+ if (m_timerInputIndex >= 0)
+ {
+ double value;
+ // For quadrature inputs, we only need the input value of the first of the timers.
+ if (dataToFilter.scalars().get(m_timerInputIndex, &value))
+ {
+ m_pose.translation()[1] = value * m_translationPerUpdate;
+ }
+ }
+
+ const double rotationScaling = 0.0001 * 180.0 / M_PI;
+ if (m_analogInputDifferentialIndex >= 0)
+ {
+ double value;
+ if (dataToFilter.scalars().get(m_analogInputDifferentialIndex, &value))
+ {
+ m_pose.rotate(SurgSim::Math::makeRotationQuaternion(value * rotationScaling, Vector3d::UnitX().eval()));
+ }
+ }
+
+ if (m_analogInputSingleEndedIndex >= 0)
+ {
+ double value;
+ if (dataToFilter.scalars().get(m_analogInputSingleEndedIndex, &value))
+ {
+ m_pose.rotate(SurgSim::Math::makeRotationQuaternion(value * rotationScaling, Vector3d::UnitY().eval()));
+ }
+ }
+
+ result->poses().set(m_poseIndex, m_pose);
+ }
+
+ void outputFilter(const DataGroup& dataToFilter, DataGroup* result)
+ {
+ *result = dataToFilter;
+ const double xScaling = 100.0;
+ const double x = std::min(5.0, std::abs(m_pose.translation().x() * xScaling));
+ result->scalars().set(m_analogOutputIndex, x);
+
+ bool value;
+ if (m_lastInputData.booleans().get(m_digitalInputMinusXIndex, &value))
+ {
+ result->booleans().set(m_digitalOutputIndex, value);
+ }
+ else
+ {
+ result->booleans().reset(m_digitalOutputIndex);
+ }
+ }
+
+private:
+ DataGroup m_outputData;
+ DataGroup m_lastInputData;
+
+ RigidTransform3d m_pose;
+ double m_translationPerUpdate;
+
+ int m_lineForPlusX;
+ int m_lineForMinusX;
+ int m_firstTimerForQuadrature;
+ int m_analogInputDifferentialPositive;
+ int m_analogInputDifferentialNegative;
+ int m_analogInputSingleEnded;
+
+ bool m_cachedOutputIndices;
+
+ int m_poseIndex;
+ int m_digitalInputPlusXIndex;
+ int m_digitalInputMinusXIndex;
+ int m_timerInputIndex;
+ int m_analogInputDifferentialIndex;
+ int m_analogInputSingleEndedIndex;
+ int m_analogOutputIndex;
+ int m_digitalOutputIndex;
+};
+
+int main(int argc, char** argv)
+{
+ std::shared_ptr<LabJackDevice> toolDevice = std::make_shared<LabJackDevice>("LabJackDevice");
+ toolDevice->setAddress(""); // Get the first-found of the specified type and connection.
+
+ const int plusX = SurgSim::Device::LabJack::FIO0;
+ toolDevice->enableDigitalInput(plusX);
+ const int minusX = SurgSim::Device::LabJack::FIO1;
+ toolDevice->enableDigitalInput(minusX);
+
+ const int loopbackOut = SurgSim::Device::LabJack::FIO2;
+ toolDevice->enableDigitalOutput(loopbackOut);
+
+ const int offset = 4;
+ toolDevice->setTimerCounterPinOffset(offset); // the U3 requires the offset to be 4+.
+
+ const int firstTimerForQuadrature = SurgSim::Device::LabJack::TIMER0;
+ toolDevice->enableTimer(firstTimerForQuadrature, SurgSim::Device::LabJack::TIMERMODE_QUADRATURE);
+ toolDevice->enableTimer(firstTimerForQuadrature + 1, SurgSim::Device::LabJack::TIMERMODE_QUADRATURE);
+
+ const int singleEndedAnalog = SurgSim::Device::LabJack::AIN1;
+ toolDevice->enableAnalogInput(singleEndedAnalog, SurgSim::Device::LabJack::Range::RANGE_10);
+
+ const int positiveAnalogDifferential = SurgSim::Device::LabJack::AIN2;
+ const int negativeAnalogDifferential = SurgSim::Device::LabJack::AIN3;
+ toolDevice->enableAnalogInput(positiveAnalogDifferential, SurgSim::Device::LabJack::Range::RANGE_10,
+ negativeAnalogDifferential);
+
+ const int xOut = SurgSim::Device::LabJack::DAC1;
+ toolDevice->enableAnalogOutput(xOut);
+
+ const double translationPerUpdate = 0.001; // Millimeter per update.
+ auto filter = std::make_shared<LabJackToPoseFilter>("LabJack to Pose filter", firstTimerForQuadrature,
+ plusX, minusX, translationPerUpdate, positiveAnalogDifferential, singleEndedAnalog,
+ xOut, loopbackOut);
+ toolDevice->setOutputProducer(filter);
+ toolDevice->addInputConsumer(filter);
+
+ if (toolDevice->initialize())
+ {
+ filter->initialize();
+
+ // The square is controlled by a second device. For a simple test, we're using an IdentityPoseDevice--
+ // a pretend device that doesn't actually move.
+ std::shared_ptr<DeviceInterface> squareDevice = std::make_shared<IdentityPoseDevice>("IdentityPoseDevice");
+
+ std::string text = "Set FIO" + std::to_string(plusX);
+ text += " low to move the sphere tool in positive x-direction. ";
+ text += "Set FIO" + std::to_string(minusX);
+ text += " low to move in negative x, and that input will be output to FIO";
+ text += std::to_string(loopbackOut) + " (a loopback). ";
+
+ text += "DAC" + std::to_string(xOut);
+ text += " will provide a voltage proportional to the absolute value of the x-position, up to 5v. ";
+
+ text += "Spin a quadrature encoder attached to FIO" + std::to_string(firstTimerForQuadrature + offset);
+ text += " and FIO" + std::to_string(firstTimerForQuadrature + offset + 1);
+ text += " to move the sphere +/- y-direction. ";
+
+ text += "Provide a differential analog input to AIN" + std::to_string(positiveAnalogDifferential);
+ text += " and AIN" + std::to_string(negativeAnalogDifferential) + " to spin about the red axis. ";
+
+ text += "Provide a single-ended analog input to AIN" + std::to_string(singleEndedAnalog);
+ text += " to spin about the green axis.";
+
+ runToolSquareTest(filter, squareDevice,
+ //2345678901234567890123456789012345678901234567890123456789012345678901234567890
+ text.c_str());
+ }
+ else
+ {
+ std::cout << std::endl << "Error initializing tool." << std::endl;
+ }
+
+ std::cout << std::endl << "Exiting." << std::endl;
+ // Cleanup and shutdown will happen automatically as objects go out of scope.
+
+ return 0;
+}
diff --git a/SurgSim/Devices/LabJack/linux/LabJackChecksums.cpp b/SurgSim/Devices/LabJack/linux/LabJackChecksums.cpp
new file mode 100644
index 0000000..db470b4
--- /dev/null
+++ b/SurgSim/Devices/LabJack/linux/LabJackChecksums.cpp
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/LabJack/linux/LabJackChecksums.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+namespace LabJack
+{
+
+unsigned char normalChecksum8(const std::array<unsigned char, MAXIMUM_BUFFER>& bytes, int count)
+{
+ uint16_t accumulator = 0;
+
+ //Sums bytes 1 to n-1 unsigned to a 2 byte value. Sums quotient and
+ //remainder of 256 division. Again, sums quotient and remainder of
+ //256 division.
+ for (int i = 1; i < count; ++i)
+ {
+ accumulator += static_cast<uint16_t>(bytes.at(i));
+ }
+
+ uint16_t quotient = accumulator / 256;
+ accumulator = (accumulator - 256 * quotient) + quotient;
+ quotient = accumulator / 256;
+
+ return static_cast<unsigned char>((accumulator - 256 * quotient) + quotient);
+}
+
+uint16_t extendedChecksum16(const std::array<unsigned char, MAXIMUM_BUFFER>& bytes, int count)
+{
+ uint16_t accumulator = 0;
+
+ //Sums bytes 6 to n-1 to an unsigned 2 byte value
+ for (int i = 6; i < count; ++i)
+ {
+ accumulator += static_cast<uint16_t>(bytes.at(i));
+ }
+
+ return accumulator;
+}
+
+unsigned char extendedChecksum8(const std::array<unsigned char, MAXIMUM_BUFFER>& bytes)
+{
+ return normalChecksum8(bytes, 6);
+}
+
+void normalChecksum(std::array<unsigned char, MAXIMUM_BUFFER>* bytes, int count)
+{
+ (*bytes)[0] = normalChecksum8(*bytes, count);
+}
+
+void extendedChecksum(std::array<unsigned char, MAXIMUM_BUFFER>* bytes, int count)
+{
+ uint16_t accumulator = extendedChecksum16(*bytes, count);
+ (*bytes)[4] = static_cast<unsigned char>(accumulator & 0xff);
+ (*bytes)[5] = static_cast<unsigned char>((accumulator / 256) & 0xff);
+ (*bytes)[0] = extendedChecksum8(*bytes);
+}
+
+}; // namespace LabJack
+}; // namespace Device
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/LabJack/linux/LabJackChecksums.h b/SurgSim/Devices/LabJack/linux/LabJackChecksums.h
new file mode 100644
index 0000000..bff2b81
--- /dev/null
+++ b/SurgSim/Devices/LabJack/linux/LabJackChecksums.h
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_LABJACK_LINUX_LABJACKCHECKSUMS_H
+#define SURGSIM_DEVICES_LABJACK_LINUX_LABJACKCHECKSUMS_H
+
+#include <array>
+
+#include "SurgSim/Devices/LabJack/linux/LabJackConstants.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+/// A collection of checksum functions specifically tailored for the labjackusb driver. These functions are based off
+/// the description in the LabJack User's Guide, and the examples provided with labjackusb.
+namespace LabJack
+{
+/// Calculates an 8-bit 1's complement unsigned checksum specifically for normal command communication with the
+/// low-level LabJack driver.
+/// \param bytes The buffer of bytes.
+/// \param count The number of bytes to check.
+/// \return The checksum byte.
+unsigned char normalChecksum8(const std::array<unsigned char, MAXIMUM_BUFFER>& bytes, int count);
+
+/// Calculates a 16-bit 1's complement unsigned checksum specifically for extended command communication with the
+/// low-level LabJack driver.
+/// \param bytes The buffer of bytes.
+/// \param count The number of bytes to check.
+/// \return The checksum byte.
+uint16_t extendedChecksum16(const std::array<unsigned char, MAXIMUM_BUFFER>& bytes, int count);
+
+/// Calculates an 8-bit 1's complement unsigned checksum specifically for extended command communication with the
+/// low-level LabJack driver.
+/// \param bytes The buffer of bytes.
+/// \return The checksum byte.
+unsigned char extendedChecksum8(const std::array<unsigned char, MAXIMUM_BUFFER>& bytes);
+
+/// Performs the 8-bit 1's complement unsigned checksum required for normal command communication with the
+/// low-level LabJack driver, and stores the result in the buffer. This function is called prior to writing data
+/// to the device for a "normal" command, so that the device can do a checksum.
+/// \param [in,out] bytes The buffer of bytes.
+/// \param count The number of bytes to check.
+void normalChecksum(std::array<unsigned char, MAXIMUM_BUFFER>* bytes, int count);
+
+/// Performs the 1's complement unsigned checksums required for extended command communication with the
+/// low-level LabJack driver, and stores the results in the buffer. This function is called prior to writing data
+/// to the device for an "extended" command, so that the device can do a checksum.
+/// \param [in,out] bytes The buffer of bytes.
+/// \param count The number of bytes to check.
+void extendedChecksum(std::array<unsigned char, MAXIMUM_BUFFER>* bytes, int count);
+
+}; // namespace LabJack
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_LABJACK_LINUX_LABJACKCHECKSUMS_H
\ No newline at end of file
diff --git a/SurgSim/Devices/LabJack/linux/LabJackConstants.h b/SurgSim/Devices/LabJack/linux/LabJackConstants.h
new file mode 100644
index 0000000..cd37f77
--- /dev/null
+++ b/SurgSim/Devices/LabJack/linux/LabJackConstants.h
@@ -0,0 +1,35 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_LABJACK_LINUX_LABJACKCONSTANTS_H
+#define SURGSIM_DEVICES_LABJACK_LINUX_LABJACKCONSTANTS_H
+
+#include <array>
+
+namespace SurgSim
+{
+namespace Device
+{
+namespace LabJack
+{
+
+/// The maximum size of a read or write buffer for labjackusb driver.
+static const int MAXIMUM_BUFFER = 64;
+
+}; // namespace LabJack
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_LABJACK_LINUX_LABJACKCONSTANTS_H
\ No newline at end of file
diff --git a/SurgSim/Devices/LabJack/linux/LabJackScaffold.cpp b/SurgSim/Devices/LabJack/linux/LabJackScaffold.cpp
new file mode 100644
index 0000000..2b28e3c
--- /dev/null
+++ b/SurgSim/Devices/LabJack/linux/LabJackScaffold.cpp
@@ -0,0 +1,1514 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/LabJack/LabJackScaffold.h"
+
+#include <algorithm>
+#include <array>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+#include <errno.h>
+#include <labjackusb.h> // the low-level LabJack library (aka exodriver)
+#include <list>
+#include <memory>
+#include <stdint.h> // fixed-width integer types are used for low-level data communication
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/LabJack/LabJackDevice.h"
+#include "SurgSim/Devices/LabJack/LabJackThread.h"
+#include "SurgSim/Devices/LabJack/linux/LabJackChecksums.h"
+#include "SurgSim/Devices/LabJack/linux/LabJackConstants.h"
+#include "SurgSim/Devices/LabJack/linux/LabJackTypeConverters.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+namespace
+{
+/// Distinguish the name of the hardware library's handle from our wrapper with a prefix.
+typedef HANDLE LJ_HANDLE;
+
+/// The low-level LabJack library returns a NULL handle when failing to open a new device.
+static const LJ_HANDLE LABJACK_INVALID_HANDLE = NULL;
+
+/// A struct containing the default settings that depend on the model of LabJack.
+struct LabJackDefaults
+{
+ LabJackDefaults()
+ {
+ timerBase[LabJack::MODEL_U3] = LabJack::TIMERBASE_2;
+ timerBase[LabJack::MODEL_U6] = LabJack::TIMERBASE_2;
+ timerBase[LabJack::MODEL_UE9] = LabJack::TIMERBASE_1;
+ }
+
+ /// The default timer base rate.
+ std::unordered_map<LabJack::Model, LabJack::TimerBase, std::hash<int>> timerBase;
+};
+
+
+/// A struct containing various parameters that depend on the model of LabJack.
+struct LabJackParameters
+{
+ LabJackParameters()
+ {
+ configBlocks[LabJack::MODEL_U3] = 16; // hardware version 1.30 & 1.21
+ configBlocks[LabJack::MODEL_U6] = 10;
+ configBlocks[LabJack::MODEL_UE9] = 4;
+
+ calibrationCommand[LabJack::MODEL_U3] = 0x2D;
+ calibrationCommand[LabJack::MODEL_U6] = 0x2D;
+ calibrationCommand[LabJack::MODEL_UE9] = 0x2A;
+
+ calibrationThirdByte[LabJack::MODEL_U3] = 0x11;
+ calibrationThirdByte[LabJack::MODEL_U6] = 0x11;
+ calibrationThirdByte[LabJack::MODEL_UE9] = 0x41;
+
+ calibrationReadBytes[LabJack::MODEL_U3] = 40;
+ calibrationReadBytes[LabJack::MODEL_U6] = 40;
+ calibrationReadBytes[LabJack::MODEL_UE9] = 136;
+ }
+
+ /// The number of config memory blocks.
+ std::unordered_map<LabJack::Model, int, std::hash<int>> configBlocks;
+ /// The extended command for reading the calibration data.
+ std::unordered_map<LabJack::Model, BYTE, std::hash<int>> calibrationCommand;
+ /// The expected read third byte for the read calibration command.
+ std::unordered_map<LabJack::Model, BYTE, std::hash<int>> calibrationThirdByte;
+ /// The number of bytes read per calibration read.
+ std::unordered_map<LabJack::Model, int, std::hash<int>> calibrationReadBytes;
+};
+
+/// Convert the LabJack::Range to the gain code expected by the low level driver.
+/// \param range The LabJack::Range code.
+/// \return The gain code.
+int getGain(LabJack::Range range)
+{
+ int gain;
+ switch (range)
+ {
+ case LabJack::RANGE_10:
+ gain = 0;
+ break;
+ case LabJack::RANGE_1:
+ gain = 1;
+ break;
+ case LabJack::RANGE_0_POINT_1:
+ gain = 2;
+ break;
+ case LabJack::RANGE_0_POINT_01:
+ gain = 3;
+ break;
+ default:
+ gain = 15;
+ }
+ return gain;
+}
+
+/// Read from the LabJack and do most of the checksum tests. This function does not test readBytes[2] because its
+/// expected value varies.
+/// \param rawHandle The handle.
+/// \param readBytes The array of bytes.
+/// \param readBytesSize The number of bytes that should be read.
+/// \param name The name of the device being read.
+/// \param sendBytes The bytes that were sent.
+/// \param text The text explaining what the read is for, used in error log messages.
+/// \param logger The logger for error messages.
+/// \return true if no errors.
+bool readAndCheck(const LJ_HANDLE rawHandle, std::array<BYTE, LabJack::MAXIMUM_BUFFER>* readBytes,
+ int readBytesSize, const std::string& name,
+ const std::array<BYTE, LabJack::MAXIMUM_BUFFER>& sendBytes, const std::string& text,
+ std::shared_ptr<SurgSim::Framework::Logger> logger)
+{
+ bool result = true;
+
+ const int read = LJUSB_Read(rawHandle, &((*readBytes)[0]), readBytesSize);
+ const uint16_t checksumTotal = LabJack::extendedChecksum16(*readBytes, readBytesSize);
+ if (read < readBytesSize)
+ {
+ SURGSIM_LOG_SEVERE(logger) << "Failed to read response of " << text << " a device named '" << name << "'. "
+ << readBytesSize << " bytes were expected, but only " << read << " were received." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ else if ((((checksumTotal / 256 ) & 0xff) != (*readBytes)[5]) ||
+ ((checksumTotal & 0xff) != (*readBytes)[4]) ||
+ (LabJack::extendedChecksum8(*readBytes) != (*readBytes)[0]))
+ {
+ SURGSIM_LOG_SEVERE(logger) << "Failed to read response of " << text << " a device named '" << name <<
+ "'. The checksums are bad." << std::endl << " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ else if (((*readBytes)[1] != sendBytes[1]) || ((*readBytes)[3] != sendBytes[3]))
+ {
+ SURGSIM_LOG_SEVERE(logger) << "Failed to read response of " << text << " a device named '" << name <<
+ "'. The command bytes are wrong. Expected bytes 1 & 3: " << static_cast<int>(sendBytes[1]) << ","
+ << static_cast<int>(sendBytes[3]) << ". Received bytes 1 & 3: " << static_cast<int>((*readBytes)[1]) <<
+ ", " << static_cast<int>((*readBytes)[3]) << "." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ else if ((*readBytes)[6] != 0)
+ {
+ SURGSIM_LOG_SEVERE(logger) << "Failed to read response of " << text << " a device named '" << name <<
+ "'. The device library returned an error code: " << static_cast<int>((*readBytes)[6]) << ", for frame: " <<
+ static_cast<int>((*readBytes)[7]) << std::endl << " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+
+ return result;
+}
+
+};
+
+class LabJackScaffold::Handle
+{
+public:
+ /// Constructor that attempts to open a device.
+ /// \param model The model of LabJack device to open (see strings in LabJackUD.h).
+ /// \param connection How to connect to the device (e.g., USB) (see strings in LabJackUD.h).
+ /// \param address Either the ID or serial number (if USB), or the IP address.
+ Handle(SurgSim::Device::LabJack::Model model, SurgSim::Device::LabJack::Connection connection,
+ const std::string& address) :
+ m_deviceHandle(LABJACK_INVALID_HANDLE),
+ m_address(address),
+ m_model(model),
+ m_connection(connection),
+ m_scaffold(LabJackScaffold::getOrCreateSharedInstance())
+ {
+ create();
+ }
+
+ /// Destructor.
+ ~Handle()
+ {
+ SURGSIM_ASSERT(!isValid()) << "Expected destroy() to be called before Handle object destruction.";
+ }
+
+ /// \return Whether or not the wrapped handle is valid.
+ bool isValid() const
+ {
+ return (m_deviceHandle != LABJACK_INVALID_HANDLE);
+ }
+
+ /// Helper function called by the constructor to open the LabJack device for communications.
+ void create()
+ {
+ SURGSIM_ASSERT(!isValid()) <<
+ "Expected LabJackScaffold::Handle::create() to be called on an uninitialized object.";
+
+ bool result = true;
+
+ if (m_model == LabJack::MODEL_UE9)
+ {
+ SURGSIM_LOG_SEVERE(m_scaffold->getLogger()) << "Failed to open a device. " <<
+ "The UE9 model LabJack is not supported for the low-level driver used on Linux & Mac. " <<
+ "The commands for the UE9 have a different structure, which is not currently implemented." <<
+ std::endl <<
+ " Model: '" << m_model << "'. Connection: '" << m_connection << "'. Address: '" <<
+ m_address << "'." << std::endl;
+ result = false;
+ }
+
+ if (m_connection != LabJack::CONNECTION_USB)
+ {
+ SURGSIM_LOG_SEVERE(m_scaffold->getLogger()) << "Failed to open a device. " <<
+ "The LabJackDevice connection must be set to USB for the low-level driver used on Linux & Mac." <<
+ std::endl <<
+ " Model: '" << m_model << "'. Connection: '" << m_connection << "'. Address: '" <<
+ m_address << "'." << std::endl;
+ result = false;
+ }
+
+ unsigned int deviceNumber = 1; // If no address is specified, grab the first device found of this model.
+ if (m_address.length() > 0)
+ {
+ try
+ {
+ deviceNumber = std::stoi(m_address);
+ }
+ catch (int e)
+ {
+ SURGSIM_LOG_SEVERE(m_scaffold->getLogger()) << "Failed to open a device. " <<
+ "The LabJackDevice address should be a string representation of an unsigned integer " <<
+ "corresponding to the device number (or the empty string to get the first device), " <<
+ "but the conversion from string to integer failed." << std::endl <<
+ " Model: '" << m_model << "'. Connection: '" << m_connection << "'. Address: '" <<
+ m_address << "'." << std::endl;
+ result = false;
+ }
+ }
+
+ if (result)
+ {
+ const unsigned int dwReserved = 0; // Not used, set to 0.
+ m_deviceHandle = LJUSB_OpenDevice(deviceNumber, dwReserved, m_model);
+ if (m_deviceHandle == LABJACK_INVALID_HANDLE)
+ {
+ SURGSIM_LOG_SEVERE(m_scaffold->getLogger()) << "Failed to open a device." <<
+ std::endl <<
+ " Model: '" << m_model << "'. Connection: '" << m_connection << "'. Address: '" <<
+ m_address << "'." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ }
+ }
+
+ bool destroy()
+ {
+ bool result = false;
+ if (isValid())
+ {
+ LJUSB_CloseDevice(m_deviceHandle);
+ m_deviceHandle = LABJACK_INVALID_HANDLE;
+ result = true;
+ }
+ return result;
+ }
+
+ /// \return The LabJack SDK's handle wrapped by this Handle.
+ LJ_HANDLE get() const
+ {
+ return m_deviceHandle;
+ }
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ Handle(const Handle&) /*= delete*/;
+ Handle& operator=(const Handle&) /*= delete*/;
+
+ /// The exodriver device handle (or LABJACK_INVALID_HANDLE if not valid).
+ LJ_HANDLE m_deviceHandle;
+ /// The address used to open the device. Can be the empty string if the first-found device was opened.
+ std::string m_address;
+ /// The model of the device.
+ SurgSim::Device::LabJack::Model m_model;
+ /// The connection to the device.
+ SurgSim::Device::LabJack::Connection m_connection;
+ /// The scaffold.
+ std::shared_ptr<LabJackScaffold> m_scaffold;
+};
+
+/// The per-device data.
+struct LabJackScaffold::DeviceData
+{
+public:
+ /// Initialize the data, creating a thread.
+ DeviceData(LabJackDevice* device, std::unique_ptr<Handle>&& handle) :
+ deviceObject(device),
+ thread(),
+ deviceHandle(std::move(handle)),
+ digitalInputChannels(device->getDigitalInputs()),
+ digitalOutputChannels(device->getDigitalOutputs()),
+ timerInputChannels(getTimerInputChannels(device->getTimers())),
+ timerOutputChannels(getTimerOutputChannels(device->getTimers())),
+ analogInputs(device->getAnalogInputs()),
+ analogOutputChannels(device->getAnalogOutputs()),
+ cachedOutputIndices(false)
+ {
+ }
+
+
+ ~DeviceData()
+ {
+ }
+
+ /// The corresponding device object.
+ LabJackDevice* const deviceObject;
+ /// Processing thread.
+ std::unique_ptr<LabJackThread> thread;
+ /// Device handle to read from.
+ std::unique_ptr<Handle> deviceHandle;
+ /// The channels read for digital inputs.
+ const std::unordered_set<int> digitalInputChannels;
+ /// The channels set for digital outputs.
+ const std::unordered_set<int> digitalOutputChannels;
+ /// The timer channels that provide inputs.
+ const std::unordered_set<int> timerInputChannels;
+ /// The timer channels set for timer outputs (e.g., PWM outputs).
+ const std::unordered_set<int> timerOutputChannels;
+ /// The analog inputs.
+ const std::unordered_map<int, LabJack::AnalogInputSettings> analogInputs;
+ /// The channels set for analog outputs.
+ const std::unordered_set<int> analogOutputChannels;
+ /// The DataGroup indices for the digital outputs.
+ std::unordered_map<int, int> digitalOutputIndices;
+ /// The DataGroup indices for the digital inputs.
+ std::unordered_map<int, int> digitalInputIndices;
+ /// The DataGroup indices for the timer outputs.
+ std::unordered_map<int, int> timerOutputIndices;
+ /// The DataGroup indices for the timer inputs.
+ std::unordered_map<int, int> timerInputIndices;
+ /// The DataGroup indices for the analog outputs.
+ std::unordered_map<int, int> analogOutputIndices;
+ /// The DataGroup indices for the analog inputs.
+ std::unordered_map<int, int> analogInputIndices;
+ /// True if the output indices have been cached.
+ bool cachedOutputIndices;
+ // Calibration constants. The meaning of each entry is specific to the model (i.e., LabJack::Model).
+ double calibration[40];
+
+private:
+ /// Given all the timers, return just the ones that provide inputs.
+ /// \param timers The timers.
+ /// \return The timers that provide inputs.
+ const std::unordered_set<int> getTimerInputChannels(const std::unordered_map<int,
+ LabJack::TimerSettings>& timers) const
+ {
+ std::unordered_set<int> timersWithInputs;
+ for (auto timer = timers.cbegin(); timer != timers.cend(); ++timer)
+ {
+ if ((timer->second.mode != LabJack::TIMERMODE_PWM_16BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_PWM_8BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_FREQUENCY_OUTPUT))
+ {
+ timersWithInputs.insert(timer->first);
+ }
+ }
+ return timersWithInputs;
+ }
+
+ /// Given all the timers, return just the ones that take outputs.
+ /// \param timers The timers.
+ /// \return The timers that take outputs.
+ const std::unordered_set<int> getTimerOutputChannels(const std::unordered_map<int,
+ LabJack::TimerSettings>& timers) const
+ {
+ std::unordered_set<int> timersWithOutputs;
+ for (auto timer = timers.cbegin(); timer != timers.cend(); ++timer)
+ {
+ if ((timer->second.mode != LabJack::TIMERMODE_PWM_16BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_PWM_8BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_RISING_EDGES_32BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_FALLING_EDGES_32BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_DUTY_CYCLE) &&
+ (timer->second.mode != LabJack::TIMERMODE_FIRMWARE_COUNTER) &&
+ (timer->second.mode != LabJack::TIMERMODE_FIRMWARE_COUNTER_DEBOUNCED) &&
+ (timer->second.mode != LabJack::TIMERMODE_FREQUENCY_OUTPUT) &&
+ (timer->second.mode != LabJack::TIMERMODE_QUADRATURE) &&
+ (timer->second.mode != LabJack::TIMERMODE_RISING_EDGES_16BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_FALLING_EDGES_16BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_LINE_TO_LINE))
+ {
+ timersWithOutputs.insert(timer->first);
+ }
+ }
+ return timersWithOutputs;
+ }
+
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ DeviceData(const DeviceData&) /*= delete*/;
+ DeviceData& operator=(const DeviceData&) /*= delete*/;
+};
+
+/// The per-scaffold data (in comparison to DeviceData the per-device data).
+/// Note that there is only a single instance of LabJackScaffold and so only a single instance of this struct.
+struct LabJackScaffold::StateData
+{
+public:
+ /// Initialize the state.
+ StateData()
+ {
+ }
+
+ /// The list of known devices.
+ std::list<std::unique_ptr<LabJackScaffold::DeviceData>> activeDeviceList;
+
+ /// The mutex that protects the list of known devices.
+ boost::mutex mutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ StateData(const StateData&) /*= delete*/;
+ StateData& operator=(const StateData&) /*= delete*/;
+};
+
+LabJackScaffold::LabJackScaffold() :
+ m_state(new StateData)
+{
+ m_logger = SurgSim::Framework::Logger::getLogger("LabJack device");
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created. labjackusb driver version: " <<
+ LJUSB_GetLibraryVersion() << ".";
+}
+
+LabJackScaffold::~LabJackScaffold()
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (!m_state->activeDeviceList.empty())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Destroying scaffold while devices are active!?!";
+ for (auto it = m_state->activeDeviceList.begin(); it != m_state->activeDeviceList.end(); ++it)
+ {
+ if ((*it)->thread)
+ {
+ destroyPerDeviceThread(it->get());
+ }
+ }
+ m_state->activeDeviceList.clear();
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold destroyed.";
+}
+
+bool LabJackScaffold::registerDevice(LabJackDevice* device)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ bool result = true;
+
+ // Make sure the object is unique.
+ auto sameObject = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ SURGSIM_ASSERT(sameObject == m_state->activeDeviceList.end()) << "LabJack: Tried to register a device named '" <<
+ device->getName() << "', which is already present!";
+
+ // Make sure the name is unique.
+ const std::string& deviceName = device->getName();
+ auto const sameName = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [&deviceName](const std::unique_ptr<DeviceData>& info) { return info->deviceObject->getName() == deviceName; });
+ if (sameName != m_state->activeDeviceList.end())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Tried to register a device when the same name, '" <<
+ device->getName() << "', is already present!";
+ result = false;
+ }
+
+ // Make sure the combination of connection and address is unique, unless the address is zero-length, in which
+ // case the first-found device of this model on this connection will be opened.
+ const std::string& address = device->getAddress();
+
+ if (result && (address.length() > 0))
+ {
+ const SurgSim::Device::LabJack::Model model = device->getModel();
+ const SurgSim::Device::LabJack::Connection connection = device->getConnection();
+
+ auto const sameInitialization = std::find_if(m_state->activeDeviceList.cbegin(),
+ m_state->activeDeviceList.cend(),
+ [&address, connection, model](const std::unique_ptr<DeviceData>& info)
+ { return (info->deviceObject->getAddress() == address) &&
+ (info->deviceObject->getConnection() == connection) &&
+ (info->deviceObject->getModel() == model); });
+
+ if (sameInitialization != m_state->activeDeviceList.cend())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Tried to register a device named '" << device->getName() <<
+ "', but a device with the same model (" << model << "), connection (" << connection <<
+ "), and address ('" << address << "') is already present!";
+ result = false;
+ }
+ }
+
+ if (result)
+ {
+ // Create a handle, opening communications.
+ // If the device's model is SEARCH, iterate over the options.
+ std::vector<LabJack::Model> modelsToSearch;
+ if (device->getModel() == LabJack::MODEL_SEARCH)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << ": searching for models U3 and U6.";
+ modelsToSearch.push_back(LabJack::MODEL_U6);
+ modelsToSearch.push_back(LabJack::MODEL_U3);
+ }
+ else
+ {
+ modelsToSearch.push_back(device->getModel());
+ }
+
+ // If the device's connection is search, set it to USB since that is all we currently support for the
+ // low-level driver.
+ if (device->getConnection() == LabJack::CONNECTION_SEARCH)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << ": searching for connection. " <<
+ "The low-level driver currently only supports USB as the medium.";
+ device->setConnection(LabJack::CONNECTION_USB);
+ }
+
+ std::unique_ptr<Handle> handle;
+ for (auto model = modelsToSearch.cbegin(); model != modelsToSearch.cend(); ++model)
+ {
+ device->setModel(*model);
+
+ handle = std::unique_ptr<Handle>(new Handle(*model, device->getConnection(), address));
+ result = handle->isValid();
+ if (result)
+ {
+ auto const sameHandle = std::find_if(m_state->activeDeviceList.cbegin(),
+ m_state->activeDeviceList.cend(),
+ [&handle](const std::unique_ptr<DeviceData>& info)
+ { return (info->deviceHandle->get() == handle->get()); });
+
+ if (sameHandle != m_state->activeDeviceList.cend())
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Tried to register a device named '" << device->getName() <<
+ "', but a device with the same handle (" << handle->get() << ") is already present! " <<
+ "This can happen if multiple LabJack devices are used without setting their addresses.";
+ handle->destroy(); // The handle was initialized and will be destructed.
+ result = false;
+ }
+ else
+ {
+ break;
+ }
+ }
+ }
+
+ if (result)
+ {
+ std::unique_ptr<DeviceData> info(new DeviceData(device, std::move(handle)));
+
+ // Cache the NamedData indices for the input DataGroup.
+ const SurgSim::DataStructures::DataGroup& inputData = device->getInputData();
+
+ for (auto input = info->digitalInputChannels.cbegin();
+ input != info->digitalInputChannels.cend();
+ ++input)
+ {
+ info->digitalInputIndices[*input] =
+ inputData.booleans().getIndex(SurgSim::DataStructures::Names::DIGITAL_INPUT_PREFIX +
+ std::to_string(*input));
+ SURGSIM_ASSERT(info->digitalInputIndices[*input] >= 0) << "LabJackScaffold::DeviceData " <<
+ "failed to get a valid NamedData index for the digital input for line " << *input <<
+ ". Make sure that is a valid line number.";
+ }
+
+ for (auto timer = info->timerInputChannels.cbegin();
+ timer != info->timerInputChannels.cend();
+ ++timer)
+ {
+ info->timerInputIndices[*timer] =
+ inputData.scalars().getIndex(SurgSim::DataStructures::Names::TIMER_INPUT_PREFIX +
+ std::to_string(*timer));
+ SURGSIM_ASSERT(info->timerInputIndices[*timer] >= 0) << "LabJackScaffold::DeviceData " <<
+ "failed to get a valid NamedData index for the timer for channel " << *timer <<
+ ". Make sure that is a valid timer number.";
+ }
+
+ for (auto input = info->analogInputs.cbegin();
+ input != info->analogInputs.cend(); ++input)
+ {
+ std::string name = SurgSim::DataStructures::Names::ANALOG_INPUT_PREFIX + std::to_string(input->first);
+ info->analogInputIndices[input->first] = inputData.scalars().getIndex(name);
+ SURGSIM_ASSERT(info->analogInputIndices[input->first] >= 0) <<
+ "LabJackScaffold::DeviceData failed to get a valid NamedData index for the " <<
+ "analog input for channel " << input->first << ". Make sure that is a valid line number. " <<
+ "Expected an entry named " << name << ".";
+ }
+
+ std::unique_ptr<LabJackThread> thread(new LabJackThread(this, info.get()));
+ thread->setRate(device->getMaximumUpdateRate());
+ thread->start();
+
+ info.get()->thread = std::move(thread);
+ m_state->activeDeviceList.emplace_back(std::move(info));
+ }
+ }
+
+ return result;
+}
+
+bool LabJackScaffold::unregisterDevice(const LabJackDevice* const device)
+{
+ bool found = false;
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ if ((*matching)->thread)
+ {
+ destroyPerDeviceThread(matching->get());
+ matching->get()->deviceHandle->destroy();
+ }
+ m_state->activeDeviceList.erase(matching);
+ // the iterator is now invalid but that's OK
+ found = true;
+ }
+ }
+
+ if (!found)
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Attempted to release a non-registered device named '" <<
+ device->getName() << ".";
+ }
+ return found;
+}
+
+bool LabJackScaffold::runInputFrame(LabJackScaffold::DeviceData* info)
+{
+ if (info->deviceObject->pullOutput() && !info->cachedOutputIndices)
+ {
+ const SurgSim::DataStructures::DataGroup& initialOutputData = info->deviceObject->getOutputData();
+
+ const std::unordered_set<int>& digitalOutputChannels = info->digitalOutputChannels;
+ for (auto output = digitalOutputChannels.cbegin(); output != digitalOutputChannels.cend(); ++output)
+ {
+ info->digitalOutputIndices[*output] =
+ initialOutputData.booleans().getIndex(SurgSim::DataStructures::Names::DIGITAL_OUTPUT_PREFIX +
+ std::to_string(*output));
+ }
+
+ const std::unordered_set<int>& timerOutputChannels = info->timerOutputChannels;
+ for (auto timer = timerOutputChannels.cbegin(); timer != timerOutputChannels.cend(); ++timer)
+ {
+ info->timerOutputIndices[*timer] =
+ initialOutputData.scalars().getIndex(SurgSim::DataStructures::Names::TIMER_OUTPUT_PREFIX +
+ std::to_string(*timer));
+ }
+
+ const std::unordered_set<int>& analogOutputChannels = info->analogOutputChannels;
+ for (auto output = analogOutputChannels.cbegin(); output != analogOutputChannels.cend(); ++output)
+ {
+ info->analogOutputIndices[*output] =
+ initialOutputData.scalars().getIndex(SurgSim::DataStructures::Names::ANALOG_OUTPUT_PREFIX +
+ std::to_string(*output));
+ }
+
+ info->cachedOutputIndices = true;
+ }
+
+ if (!info->deviceObject->hasOutputProducer() || info->cachedOutputIndices)
+ {
+ if (!updateDevice(info))
+ {
+ return false;
+ }
+ info->deviceObject->pushInput();
+ }
+ return true;
+}
+
+bool LabJackScaffold::updateDevice(LabJackScaffold::DeviceData* info)
+{
+ const LJ_HANDLE rawHandle = info->deviceHandle->get();
+ const SurgSim::DataStructures::DataGroup& outputData = info->deviceObject->getOutputData();
+ const LabJackDevice* device = info->deviceObject;
+ bool result = true;
+
+ // Use one Feedback command for all the inputs and outputs.
+ // According to the Users Guide, the Feedback command has a max size of 64 bytes. Currently only a single command is
+ // used, and we try to add all the input/output information. To be safe the std::array::at function is used for any
+ // access to a non-constant index, and will throw an out_of_range exception if the index is too large.
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> sendBytes;
+ sendBytes[1] = 0xF8; //Command byte, Feedback
+ sendBytes[3] = 0x00; //Extended command number
+
+ sendBytes[6] = 0; //Echo
+
+ int sendBytesSize = 7; // the first IOType goes here
+ int readBytesSize = 9; // Reading after a Feedback command provides 9+ bytes.
+
+ // Read digital inputs
+ const std::unordered_set<int>& digitalInputChannels = info->digitalInputChannels;
+ for (auto input = digitalInputChannels.cbegin(); input != digitalInputChannels.cend(); ++input)
+ {
+ sendBytes.at(sendBytesSize++) = 10; //IOType, BitStateRead
+ sendBytes.at(sendBytesSize++) = *input; //Bits 0-4: IO Number
+ ++readBytesSize;
+ }
+
+ // Write digital outputs
+ const std::unordered_set<int>& digitalOutputChannels = info->digitalOutputChannels;
+ for (auto output = digitalOutputChannels.cbegin(); output != digitalOutputChannels.cend(); ++output)
+ {
+ if (info->digitalOutputIndices.count(*output) > 0)
+ {
+ const int index = info->digitalOutputIndices[*output];
+ SURGSIM_ASSERT(index >= 0) << "LabJackScaffold: A LabJackDevice was configured with line " << *output <<
+ " set to digital output, but the scaffold does not know the correct index into the NamedData. " <<
+ " Make sure there is an entry in the booleans with the correct string key.";
+
+ bool value;
+ if (outputData.booleans().get(index, &value))
+ {
+ const BYTE valueAsByte = (value ? 1 : 0);
+ sendBytes.at(sendBytesSize++) = 11; //IOType, BitStateWrite
+ sendBytes.at(sendBytesSize++) = (*output) | ((valueAsByte & 0xF) << 7); //Bits 0-4: IO Number,
+ //Bit 7: State
+ }
+ }
+ }
+
+ // Write to timers (e.g., resetting firmware counters).
+ const std::unordered_set<int>& timerOutputChannels = info->timerOutputChannels;
+ for (auto timer = timerOutputChannels.cbegin(); timer != timerOutputChannels.cend(); ++timer)
+ {
+ if (info->timerOutputIndices.count(*timer) > 0)
+ {
+ const int index = info->timerOutputIndices[*timer];
+ // We do not ensure that all the timers can be output to, because the user might not need to reset them.
+ if (index >= 0)
+ {
+ double value;
+ if (outputData.scalars().get(index, &value))
+ {
+ const int valueAsInt = value + 0.5; // value is non-negative. The conversion will truncate.
+ sendBytes.at(sendBytesSize++) = 42 + 2 * (*timer); //IOType, Timer#
+ sendBytes.at(sendBytesSize++) = 1; //Bit 0: UpdateReset
+ sendBytes.at(sendBytesSize++) = valueAsInt & 255; //LSB
+ sendBytes.at(sendBytesSize++) = (valueAsInt & 65280) / 256; //MSB
+ readBytesSize += 4;
+ }
+ }
+ }
+ }
+
+ // Read from timers.
+ const std::unordered_set<int>& timerInputChannels = info->timerInputChannels;
+ for (auto timer = timerInputChannels.cbegin(); timer != timerInputChannels.cend(); ++timer)
+ {
+ // If we write to a timer, the response to that write will have the timer value so we don't need to request it.
+ if (timerOutputChannels.count(*timer) == 0)
+ {
+ sendBytes.at(sendBytesSize++) = 42 + 2 * (*timer); //IOType, Timer#
+ sendBytes.at(sendBytesSize++) = 0; //Bit 0: UpdateReset
+ sendBytes.at(sendBytesSize++) = 0; //LSB
+ sendBytes.at(sendBytesSize++) = 0; //MSB
+ readBytesSize += 4;
+ }
+ }
+
+ // Request the values of analog inputs.
+ auto const& analogInputs = info->analogInputs;
+ for (auto input = analogInputs.cbegin(); input != analogInputs.cend(); ++input)
+ {
+ if (device->getModel() == LabJack::MODEL_U3)
+ {
+ const BYTE longSettling = (device->getAnalogInputSettling() > 0 ? (1 << 6) : 0);
+ const BYTE quickSample = (device->getAnalogInputResolution() > 0 ? (1 << 7) : 0);
+ // Third byte is the negative channel for differential input, or 31 for single-ended input.
+ const BYTE negativeChannel = input->second.negativeChannel.hasValue() ?
+ input->second.negativeChannel.getValue() : 31;
+ sendBytes.at(sendBytesSize++) = 1;
+ sendBytes.at(sendBytesSize++) = input->first | longSettling | quickSample;
+ sendBytes.at(sendBytesSize++) = negativeChannel;
+ readBytesSize += 2;
+ }
+ else
+ {
+ // The U6 can only do consecutive channels for a differential read, so the actual channel is not sent.
+ const BYTE differential = input->second.negativeChannel.hasValue() ? 1 << 7 : 0;
+ sendBytes.at(sendBytesSize++) = 3;
+ sendBytes.at(sendBytesSize++) = input->first;
+ sendBytes.at(sendBytesSize++) = device->getAnalogInputResolution() | (getGain(input->second.range) << 4);
+ sendBytes.at(sendBytesSize++) = device->getAnalogInputSettling() | differential;
+ readBytesSize += 5;
+ }
+ }
+
+ // Set analog outputs.
+ const std::unordered_set<int>& analogOutputs = info->analogOutputChannels;
+ for (auto output = analogOutputs.cbegin(); output != analogOutputs.cend(); ++output)
+ {
+ if (info->analogOutputIndices.count(*output) > 0)
+ {
+ const int index = info->analogOutputIndices[*output];
+ if (index >= 0)
+ {
+ double value;
+ if (outputData.scalars().get(index, &value))
+ {
+ const BYTE dacConfigCode = *output + 38;
+ sendBytes.at(sendBytesSize++) = dacConfigCode;
+
+ int bytes;
+ if (device->getModel() == LabJack::MODEL_U3)
+ {
+ const int calibrationIndex = *output * 2 + 4;
+ bytes = value * info->calibration[calibrationIndex] * 256 +
+ info->calibration[calibrationIndex + 1] * 256;
+ }
+ else
+ {
+ const int calibrationIndex = *output * 2 + 16;
+ bytes = value * info->calibration[calibrationIndex] + info->calibration[calibrationIndex + 1];
+ }
+ bytes = std::min(bytes, 65535);
+ bytes = std::max(bytes, 0);
+ sendBytes.at(sendBytesSize++) = bytes & 255;
+ sendBytes.at(sendBytesSize++) = bytes / 256;
+ }
+ }
+ }
+ }
+
+ // Write the Feedback command.
+ // Must send an even number of bytes.
+ if (sendBytesSize % 2 == 1)
+ {
+ sendBytes.at(sendBytesSize++) = 0;
+ }
+
+ // Must read an even number of bytes
+ if (readBytesSize % 2 == 1)
+ {
+ ++readBytesSize;
+ }
+
+ // According to the Users Guide, the Feedback command has a max Response/Read size of 64 bytes.
+ // Most IOTypes Read no more bytes than they Write, including the digital input/output and timers currently
+ // supported, but some IOTypes do Read more than they Write. Thus, this assertion should never be triggered, and
+ // to make sure the access indices into the buffer are being correctly calculated, the std::array::at function is
+ // used for any access to a non-constant index, and will throw an out_of_range exception if the index is too large.
+ if (readBytesSize > LabJack::MAXIMUM_BUFFER)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to update device named '" << device->getName() <<
+ "'. The low-level LabJack communication function 'Feedback' has a max Response size of " <<
+ std::to_string(LabJack::MAXIMUM_BUFFER) << " bytes. " <<
+ "The current configuration and output DataGroup would cause a size of " << readBytesSize << " bytes." <<
+ std::endl;
+ result = false;
+ }
+
+ if (result)
+ {
+ const BYTE sendCommandBytes = 6;
+ const BYTE dataWords = (sendBytesSize - sendCommandBytes) / 2;
+ sendBytes[2] = dataWords;
+ LabJack::extendedChecksum(&sendBytes, sendBytesSize);
+
+ const int sent = LJUSB_Write(rawHandle, &(sendBytes[0]), sendBytesSize);
+
+ if (sent < sendBytesSize)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "Failed to write feedback command to update a device named '" <<
+ device->getName() << "'. " << sendBytesSize << " bytes should have been sent, but only " <<
+ sent << " were actually sent." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ }
+
+ const BYTE readCommandBytes = 6;
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> readBytes; // Feedback commands respond with a max of 64 bytes
+
+ // Read the response for the feedback command.
+ if (result)
+ {
+ const std::string errorText = "feedback command to update";
+ result = readAndCheck(rawHandle, &readBytes, readBytesSize, device->getName(), sendBytes, errorText, m_logger);
+
+ const BYTE dataWords = (readBytesSize - readCommandBytes) / 2;
+ if (readBytes[2] != dataWords)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "Failed to read response of " << errorText << " a device named '" << device->getName() <<
+ "'. The number of words in the response is wrong. Expected: " << static_cast<int>(dataWords) <<
+ ". Received: " << static_cast<int>(readBytes[2]) << "." << std::endl << " labjackusb error code: " <<
+ errno << "." << std::endl;
+ result = false;
+ }
+ }
+
+ SurgSim::DataStructures::DataGroup& inputData = info->deviceObject->getInputData();
+ if (result)
+ {
+ const int errorBytes = 2;
+ const int echoBytes = 1;
+ int currentByte = readCommandBytes + errorBytes + echoBytes;
+
+ // Digital inputs.
+ for (auto input = digitalInputChannels.cbegin(); input != digitalInputChannels.cend(); ++input)
+ {
+ const bool value = (readBytes.at(currentByte++) > 0 ? true : false);
+ inputData.booleans().set(info->digitalInputIndices[*input], value);
+ }
+
+ // Read from the timers in the same order as above.
+ for (auto timer = timerOutputChannels.cbegin(); timer != timerOutputChannels.cend(); ++timer)
+ {
+ const int count = 4;
+ const uint32_t value = LabJack::uint32FromChars(readBytes, currentByte, count);
+ inputData.scalars().set(info->timerInputIndices[*timer], value);
+ currentByte += 4;
+ }
+
+ for (auto timer = timerInputChannels.cbegin(); timer != timerInputChannels.cend(); ++timer)
+ {
+ if (timerOutputChannels.count(*timer) == 0)
+ {
+ const int count = 4;
+ const uint32_t value = LabJack::uint32FromChars(readBytes, currentByte, count);
+ currentByte += 4;
+ // Interpret the value as signed.
+ inputData.scalars().set(info->timerInputIndices[*timer], value);
+ }
+ }
+
+ for (auto input = analogInputs.cbegin(); input != analogInputs.cend(); ++input)
+ {
+ if (device->getModel() == LabJack::MODEL_U3)
+ {
+ const int count = 2;
+ const uint16_t value = LabJack::uint16FromChars(readBytes, currentByte, count);
+ currentByte += 2;
+ const double volts = info->calibration[2] * static_cast<double>(value) + info->calibration[3];
+ inputData.scalars().set(info->analogInputIndices[input->first], volts);
+ }
+ else
+ {
+ const int count = 3;
+ const uint32_t value = LabJack::uint32FromChars(readBytes, currentByte, count);
+ double bits = value;
+ bits /= 256.0;
+
+ const int gain = readBytes.at(currentByte + 3) / 16;
+ int index = gain * 2 + 8;
+
+ const int resolution = readBytes.at(currentByte + 3) & 15;
+ if (resolution > 8)
+ {
+ index += 24;
+ }
+
+ currentByte += 5;
+
+ double volts;
+ const double center = info->calibration[index + 1];
+ if (bits < center)
+ {
+ volts = (center - bits) * info->calibration[index];
+ }
+ else
+ {
+ volts = (bits - center) * info->calibration[index - 8];
+ }
+ inputData.scalars().set(info->analogInputIndices[input->first], volts);
+ }
+ }
+ }
+ else
+ {
+ // Failure communicating with the LabJack
+ inputData.resetAll();
+ }
+
+ return true; // Returning false ends the thread.
+}
+
+bool LabJackScaffold::destroyPerDeviceThread(DeviceData* data)
+{
+ SURGSIM_ASSERT(data->thread) << "LabJack: destroying a per-device thread, but none exists for this DeviceData";
+
+ std::unique_ptr<LabJackThread> thread = std::move(data->thread);
+ thread->stop();
+ thread.reset();
+
+ return true;
+}
+
+SurgSim::DataStructures::DataGroup LabJackScaffold::buildDeviceInputData()
+{
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ // We don't know which input lines we need until after the configuration, but LabJackDevice must be constructed with
+ // a valid DataGroup, so we add every possible line.
+ const int maxDigitalInputs = 23; // The UE9 can have 23 digital inputs.
+ for (int i = 0; i < maxDigitalInputs; ++i)
+ {
+ builder.addBoolean(SurgSim::DataStructures::Names::DIGITAL_INPUT_PREFIX + std::to_string(i));
+ }
+
+ const int maxTimerInputs = 6; // The UE9 can have 6 timers.
+ for (int i = 0; i < maxTimerInputs; ++i)
+ {
+ builder.addScalar(SurgSim::DataStructures::Names::TIMER_INPUT_PREFIX + std::to_string(i));
+ }
+
+ const int maxAnalogInputs = 16; // The U3 can have 16 analog inputs.
+ for (int i = 0; i < maxAnalogInputs; ++i)
+ {
+ builder.addScalar(SurgSim::DataStructures::Names::ANALOG_INPUT_PREFIX + std::to_string(i));
+ }
+ return builder.createData();
+}
+
+std::shared_ptr<LabJackScaffold> LabJackScaffold::getOrCreateSharedInstance()
+{
+ static SurgSim::Framework::SharedInstance<LabJackScaffold> sharedInstance;
+ return sharedInstance.get();
+}
+
+bool LabJackScaffold::configureDevice(DeviceData* deviceData)
+{
+ return configureClockAndTimers(deviceData) && configureDigital(deviceData) && configureAnalog(deviceData);
+}
+
+bool LabJackScaffold::configureClockAndTimers(DeviceData* deviceData)
+{
+ bool result = configureNumberOfTimers(deviceData);
+
+ if (result && (deviceData->deviceObject->getTimers().size() > 0))
+ {
+ result = configureClock(deviceData) && configureTimers(deviceData);
+ }
+ return result;
+}
+
+bool LabJackScaffold::configureNumberOfTimers(DeviceData* deviceData)
+{
+ LabJackDevice* device = deviceData->deviceObject;
+ LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
+
+ // One-time configuration of counters and timers.
+ const std::unordered_map<int, LabJack::TimerSettings>& timers = device->getTimers();
+
+ for (auto timer : timers)
+ {
+ SURGSIM_LOG_IF(timer.first >= static_cast<int>(timers.size()), m_logger, SEVERE) <<
+ "Error configuring enabled timers for a device named '" << device->getName() <<
+ "', with number of timers: " << timers.size() << "." << std::endl <<
+ " Timers must be enabled consecutively, starting with #0." << std::endl <<
+ " With the currently enabled number of timers, the highest allowable timer is #" <<
+ timers.size() - 1 << ", but one of the enabled timers is #" << timer.first << "." << std::endl;
+ }
+
+ const BYTE numberOfTimers = timers.size();
+ const BYTE counterEnable = 0; // Counters are not currently supported.
+ const BYTE pinOffset = device->getTimerCounterPinOffset();
+
+ // First, configure the number of timers and counters
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> sendBytes;
+ sendBytes[1] = 0xF8; //Command byte, ConfigIO
+ sendBytes[2] = 0x05; //Number of data words
+ sendBytes[3] = 0x0B; //Extended command number
+
+ sendBytes[6] = 1; //Writemask : Setting writemask for timerCounterConfig (bit 0)
+
+ sendBytes[7] = numberOfTimers; //NumberTimersEnabled
+ sendBytes[8] = counterEnable; //CounterEnable: Bit 0 is Counter 0, Bit 1 is Counter 1
+ sendBytes[9] = pinOffset; //TimerCounterPinOffset
+
+ int sendBytesSize = 16; //ConfigIO command sends 16 bytes
+ for (int i = 10; i < sendBytesSize; ++i)
+ {
+ sendBytes[i] = 0; //Reserved
+ }
+ LabJack::extendedChecksum(&sendBytes, 16);
+
+ int sent = LJUSB_Write(rawHandle, &(sendBytes[0]), sendBytesSize);
+
+ bool result = true;
+
+ if (sent < sendBytesSize)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "Failed to write timer/counter configuration information for a device named '" << device->getName() <<
+ "'. " << sendBytesSize << " bytes should have been sent, but only " << sent <<
+ " were actually sent." << std::endl << " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+
+ if (result)
+ {
+ const int readBytesSize = 16; // timerCounterConfig replies with 16 bytes.
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> readBytes;
+
+ const std::string errorText = "timer/counter configuration for";
+ result = readAndCheck(rawHandle, &readBytes, readBytesSize, device->getName(), sendBytes, errorText, m_logger);
+
+ if (readBytes[2] != sendBytes[2])
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to read response of " << errorText << " a device named '" <<
+ device->getName() << "'. The command bytes are wrong. Expected: " << static_cast<int>(sendBytes[1]) <<
+ ", " << static_cast<int>(sendBytes[2]) << ", " << static_cast<int>(sendBytes[3]) << ". Received: " <<
+ static_cast<int>(readBytes[1]) << ", " << static_cast<int>(readBytes[2]) << ", " <<
+ static_cast<int>(readBytes[3]) << "." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ }
+ return result;
+}
+
+bool LabJackScaffold::configureClock(DeviceData* deviceData)
+{
+ LabJackDevice* device = deviceData->deviceObject;
+ LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
+
+ LabJack::TimerBase base = device->getTimerBase();
+ if (base == LabJack::TimerBase::TIMERBASE_DEFAULT)
+ {
+ LabJackDefaults defaults;
+ base = defaults.timerBase[device->getModel()];
+ }
+
+ // Second, set the clock base and divisor
+
+ // The low-level settings for the timer base are 20 less than the high-level values for the U3 and U6.
+ // The low- and high-level settings are the same for the UE9.
+ const int timerBaseOffset = 20;
+ BYTE timerBase = device->getTimerBase();
+ // If the TimerBase is above 20, it was set for a high-level value, convert to the equivalent value for
+ // the low-level driver.
+ if (timerBase > timerBaseOffset)
+ {
+ timerBase -= timerBaseOffset;
+ }
+ const BYTE divisor = device->getTimerClockDivisor();
+
+ // Write the clock configuration.
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> sendBytes;
+ sendBytes[1] = 0xF8; //Command byte, ConfigTimerClock
+ sendBytes[2] = 0x02; //Number of data words
+ sendBytes[3] = 0x0A; //Extended command number
+
+ sendBytes[6] = 0; //Reserved
+ sendBytes[7] = 0; //Reserved
+
+ //TimerClockConfig : Configuring the clock (bit 7) and setting the TimerClockBase (bits 0-2)
+ const BYTE configureClockCode = 1 << 7;
+ sendBytes[8] = timerBase | configureClockCode;
+ sendBytes[9] = divisor; //TimerClockDivisor
+
+ int sendBytesSize = 10; // ConfigTimerClock sends 10 bytes
+ LabJack::extendedChecksum(&sendBytes, sendBytesSize);
+
+ int sent = LJUSB_Write(rawHandle, &(sendBytes[0]), sendBytesSize);
+
+ bool result = true;
+
+ if (sent < sendBytesSize)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "Failed to write clock configuration information for a device named '" <<
+ device->getName() << "'. " << sendBytesSize << " bytes should have been sent, but only " <<
+ sent << " were actually sent." << std::endl << " labjackusb error code: " << errno << "." <<
+ std::endl;
+ result = false;
+ }
+
+ if (result)
+ {
+ // Read the response for the clock configuration.
+ const int readBytesSize = 10; // ConfigTimerClock replies with 10 bytes.
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> readBytes;
+
+ const std::string errorText = "clock configuration for";
+ result = readAndCheck(rawHandle, &readBytes, readBytesSize, device->getName(), sendBytes, errorText, m_logger);
+
+ if (readBytes[2] != sendBytes[2])
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to read response of " << errorText << " a device named '" <<
+ device->getName() << "'. The command bytes are wrong. Expected: " << static_cast<int>(sendBytes[1]) <<
+ ", " << static_cast<int>(sendBytes[2]) << ", " << static_cast<int>(sendBytes[3]) << ". Received: " <<
+ static_cast<int>(readBytes[1]) << ", " << static_cast<int>(readBytes[2]) << ", " <<
+ static_cast<int>(readBytes[3]) << "." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ }
+ return result;
+}
+
+bool LabJackScaffold::configureTimers(DeviceData* deviceData)
+{
+ LabJackDevice* device = deviceData->deviceObject;
+ LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
+
+ bool result = true;
+
+ // Configure each timer's mode via a Feedback command.
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> sendBytes;
+ sendBytes[1] = 0xF8; //Command byte, Feedback
+ sendBytes[3] = 0x00; //Extended command number
+
+ sendBytes[6] = 0; //Echo
+ int sendBytesSize = 7; // the first IOType goes here
+
+ int readBytesSize = 9; // Reading after a Feedback command provides 9+ bytes.
+ const std::unordered_map<int, LabJack::TimerSettings> timers = device->getTimers();
+ for (auto timer = timers.cbegin(); timer != timers.cend(); ++timer)
+ {
+ const int minimumTimer = 0;
+ const int maximumTimer = 3;
+ if ((timer->first < minimumTimer) || (timer->first > maximumTimer))
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to configure a timer for a device named '" <<
+ device->getName() << "'. Timer number (the key in the argument to LabJackDevice::setTimers) " <<
+ timer->first << " is outside of the valid range " << minimumTimer << "-" << maximumTimer <<
+ "." << std::endl;
+ result = false;
+ }
+
+ if (result)
+ {
+ const BYTE timerConfigCode = timer->first * 2 + 43; // The timer configs are 43, 45, 47, and 49.
+ sendBytes.at(sendBytesSize++) = timerConfigCode; //IOType, Timer#Config (e.g., Timer0Config)
+ sendBytes.at(sendBytesSize++) = timer->second.mode; //TimerMode
+
+ const int initialValue = timer->second.initialValue.hasValue() ? timer->second.initialValue.getValue() : 0;
+ sendBytes.at(sendBytesSize++) = static_cast<BYTE>(initialValue & 0xFF); //Value LSB
+ sendBytes.at(sendBytesSize++) = static_cast<BYTE>((initialValue >> 8) & 0xFF); //Value MSB
+ }
+ }
+
+ if (result)
+ {
+ // Write the timer Feedback command.
+ // Must send an even number of bytes.
+ if (sendBytesSize % 2 == 1)
+ {
+ sendBytes.at(sendBytesSize++) = 0;
+ }
+ const BYTE sendCommandBytes = 6;
+ const BYTE dataWords = (sendBytesSize - sendCommandBytes) / 2;
+ sendBytes[2] = dataWords;
+ LabJack::extendedChecksum(&sendBytes, sendBytesSize);
+
+ int sent = LJUSB_Write(rawHandle, &(sendBytes[0]), sendBytesSize);
+
+ if (sent < sendBytesSize)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "Failed to write timer mode feedback command for a device named '" <<
+ device->getName() << "'. " << sendBytesSize << " bytes should have been sent, but only " <<
+ sent << " were actually sent." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ }
+
+ if (result)
+ {
+ // Read the response for the timer feedback command.
+ // Must read an even number of bytes
+ if (readBytesSize % 2 == 1)
+ {
+ ++readBytesSize;
+ }
+
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> readBytes;
+ const std::string errorText = "timer mode feedback command for";
+ result = readAndCheck(rawHandle, &readBytes, readBytesSize, device->getName(), sendBytes, errorText, m_logger);
+
+ const BYTE readCommandBytes = 6;
+ const BYTE dataWords = (readBytesSize - readCommandBytes) / 2;
+ if (readBytes[2] != dataWords)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to read response of " << errorText << " a device named '" <<
+ device->getName() << "'. The number of words in the response is wrong. Expected: " <<
+ static_cast<int>(dataWords) << ". Received: " << static_cast<int>(readBytes[2]) << "." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ }
+ return result;
+}
+
+bool LabJackScaffold::configureAnalog(DeviceData* deviceData)
+{
+ LabJackDevice* device = deviceData->deviceObject;
+ LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
+
+ bool result = true;
+
+ const std::unordered_set<int>& analogOutputs = deviceData->analogOutputChannels;
+ for (auto output = analogOutputs.cbegin(); output != analogOutputs.cend(); ++output)
+ {
+ const int minimumDac = 0;
+ const int maximumDac = 1;
+ if ((*output < minimumDac) || (*output > maximumDac))
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to configure an analog output (DAC) for a device named '" <<
+ device->getName() << "'. DAC number " << *output << " is outside of the valid range " <<
+ minimumDac << "-" << maximumDac << "." << std::endl;
+ result = false;
+ }
+ }
+
+ if (device->getModel() == LabJack::MODEL_U6)
+ {
+ auto const& analogInputs = deviceData->analogInputs;
+ for (auto input = analogInputs.cbegin(); input != analogInputs.cend(); ++input)
+ {
+ if ((input->second.negativeChannel.hasValue()) &&
+ (input->second.negativeChannel.getValue() != input->first + 1))
+ {
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "Failed to configure a differential analog input for a device named '" <<
+ device->getName() << "'. For a model U6(-PRO), with the low-level driver, "<<
+ "the negative channel number must be one greater than the positive channel number, " <<
+ "but positive channel " << input->first << " is paired with negative channel " <<
+ input->second.negativeChannel.getValue() << ".";
+ result = false;
+ }
+ }
+ }
+
+ LabJackParameters parameters;
+ const int blocks = parameters.configBlocks[device->getModel()];
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> sendBytes;
+ sendBytes[1] = 0xF8; //command byte
+ sendBytes[2] = 0x01; //number of data words
+ sendBytes[3] = parameters.calibrationCommand[device->getModel()];
+ sendBytes[6] = 0;
+ const int sendBytesSize = 8;
+ const int readBytesSize = parameters.calibrationReadBytes[device->getModel()];
+
+ for (int i = 0; i < blocks; ++i)
+ {
+ if (result)
+ {
+ sendBytes[7] = i;
+ LabJack::extendedChecksum(&sendBytes, sendBytesSize);
+
+ const int sent = LJUSB_Write(rawHandle, &(sendBytes[0]), sendBytesSize);
+ if (sent < sendBytesSize)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "Failed to write a feedback command to read the calibration data for a device named '" <<
+ device->getName() << "'. " << sendBytesSize << " bytes should have been sent, but only " <<
+ sent << " were actually sent." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ }
+
+ if (result)
+ {
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> readBytes;
+ const std::string errorText = "feedback command to read the calibration data for";
+ result = readAndCheck(rawHandle, &readBytes, readBytesSize, device->getName(), sendBytes, errorText,
+ m_logger);
+
+ if (readBytes[2] != parameters.calibrationThirdByte[device->getModel()])
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to read response of " << errorText << " a device named '" <<
+ device->getName() << "'. The command bytes are wrong. Expected byte 2: " <<
+ static_cast<int>(parameters.calibrationThirdByte[device->getModel()]) << ". Received: " <<
+ static_cast<int>(readBytes[2]) << "." << std::endl << " labjackusb error code: " << errno << "." <<
+ std::endl;
+ result = false;
+ }
+
+ if (result)
+ {
+ const int offset = i * 4;
+ deviceData->calibration[offset] = LabJack::doubleFromChars(readBytes, 8);
+ deviceData->calibration[offset + 1] = LabJack::doubleFromChars(readBytes, 16);
+ deviceData->calibration[offset + 2] = LabJack::doubleFromChars(readBytes, 24);
+ deviceData->calibration[offset + 3] = LabJack::doubleFromChars(readBytes, 32);
+ }
+ }
+ }
+
+ return result;
+}
+
+bool LabJackScaffold::configureDigital(DeviceData* deviceData)
+{
+ LabJackDevice* device = deviceData->deviceObject;
+ LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
+
+ bool result = true;
+
+ const std::unordered_set<int>& digitalInputChannels = deviceData->digitalInputChannels;
+ const std::unordered_set<int>& digitalOutputChannels = deviceData->digitalOutputChannels;
+ if ((digitalInputChannels.size() > 0) || (digitalOutputChannels.size() > 0))
+ {
+ // Configure each digital line's direction via a Feedback command. The output lines will automatically be
+ // forced high when we write their states in updateDevice, but we must explicitly set the direction on
+ // input lines so we just do both here.
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> sendBytes;
+ sendBytes[1] = 0xF8; //Command byte, Feedback
+ sendBytes[3] = 0x00; //Extended command number
+
+ sendBytes[6] = 0; //Echo
+ int sendBytesSize = 7; // the first IOType goes here
+
+ int readBytesSize = 9; // Reading after a Feedback command provides 9+ bytes.
+ for (auto input = digitalInputChannels.cbegin(); input != digitalInputChannels.cend(); ++input)
+ {
+ sendBytes.at(sendBytesSize++) = 13; //IOType, BitDirWrite
+ sendBytes.at(sendBytesSize++) = *input; //Bits 0-4: IO Number, Bit 7: Direction (1=output, 0=input)
+ }
+ for (auto output = digitalOutputChannels.cbegin(); output != digitalOutputChannels.cend(); ++output)
+ {
+ const BYTE direction = 1 << 7;
+ sendBytes.at(sendBytesSize++) = 13; //IOType, BitDirWrite
+ sendBytes.at(sendBytesSize++) = *output | direction; //Bits 0-4: IO Number,
+ // Bit 7: Direction (1=output, 0=input)
+ }
+
+ // Write the digital input/output Feedback command.
+ // Must send an even number of bytes.
+ if (sendBytesSize % 2 == 1)
+ {
+ sendBytes.at(sendBytesSize++) = 0;
+ }
+ const BYTE sendCommandBytes = 6;
+ const BYTE dataWords = (sendBytesSize - sendCommandBytes) / 2;
+ sendBytes[2] = dataWords;
+ LabJack::extendedChecksum(&sendBytes, sendBytesSize);
+
+ int sent = LJUSB_Write(rawHandle, &(sendBytes[0]), sendBytesSize);
+
+ if (sent < sendBytesSize)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "Failed to write digital line direction feedback command for a device named '" <<
+ device->getName() << "'. " << sendBytesSize << " bytes should have been sent, but only " <<
+ sent << " were actually sent." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+
+ if (result)
+ {
+ // Read the response for the digital input/output feedback command.
+ // Must read an even number of bytes
+ if (readBytesSize % 2 == 1)
+ {
+ ++readBytesSize;
+ }
+
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> readBytes;
+ const std::string errorText = "digital line direction configuration for";
+ result = readAndCheck(rawHandle, &readBytes, readBytesSize, device->getName(), sendBytes, errorText,
+ m_logger);
+
+ const BYTE readCommandBytes = 6;
+ const BYTE dataWords = (readBytesSize - readCommandBytes) / 2;
+ if (readBytes[2] != dataWords)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to read response of " << errorText << " a device named '" <<
+ device->getName() << "'. The number of words in the response is wrong. Expected: " <<
+ static_cast<int>(dataWords) << ". Received: " << static_cast<int>(readBytes[2]) << "." <<
+ std::endl << " labjackusb error code: " << errno << "." << std::endl;
+ result = false;
+ }
+ }
+ }
+ return result;
+}
+
+std::shared_ptr<SurgSim::Framework::Logger> LabJackScaffold::getLogger() const
+{
+ return m_logger;
+}
+
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.cpp b/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.cpp
new file mode 100644
index 0000000..cab7bb4
--- /dev/null
+++ b/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.cpp
@@ -0,0 +1,71 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/LabJack/linux/LabJackTypeConverters.h"
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+namespace LabJack
+{
+
+double doubleFromChars(const std::array<unsigned char, MAXIMUM_BUFFER>& bytes, int startIndex)
+{
+ uint32_t decimal = static_cast<uint32_t>(bytes.at(startIndex)) |
+ (static_cast<uint32_t>(bytes.at(startIndex + 1)) << 8) |
+ (static_cast<uint32_t>(bytes.at(startIndex + 2)) << 16) |
+ (static_cast<uint32_t>(bytes.at(startIndex + 3)) << 24);
+
+ uint32_t whole = static_cast<uint32_t>(bytes[startIndex + 4]) |
+ (static_cast<uint32_t>(bytes.at(startIndex + 5)) << 8) |
+ (static_cast<uint32_t>(bytes.at(startIndex + 6)) << 16) |
+ (static_cast<uint32_t>(bytes.at(startIndex + 7)) << 24);
+
+ return static_cast<double>(static_cast<int>(whole)) + static_cast<double>(decimal)/4294967296.0;
+}
+
+uint32_t uint32FromChars(const std::array<unsigned char, LabJack::MAXIMUM_BUFFER> &bytes, int startIndex, int count)
+{
+ SURGSIM_ASSERT(count <= 4) << __FUNCTION__ << " got a count of " << count << "; that is too large.";
+
+ uint32_t value = 0;
+ for (int i = count - 1; i > 0; --i)
+ {
+ value += bytes.at(startIndex + i);
+ value = value << 8;
+ }
+ value += bytes.at(startIndex);
+ return value;
+}
+
+uint16_t uint16FromChars(const std::array<unsigned char, LabJack::MAXIMUM_BUFFER> &bytes, int startIndex, int count)
+{
+ SURGSIM_ASSERT(count <= 2) << __FUNCTION__ << " got a count of " << count << "; that is too large.";
+
+ uint16_t value = 0;
+ if (count > 1)
+ {
+ value += bytes.at(startIndex + 1);
+ value = value << 8;
+ }
+ value += bytes.at(startIndex);
+ return value;
+}
+
+}; // namespace LabJack
+}; // namespace Device
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.h b/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.h
new file mode 100644
index 0000000..567ca08
--- /dev/null
+++ b/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.h
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_LABJACK_LINUX_LABJACKTYPECONVERTERS_H
+#define SURGSIM_DEVICES_LABJACK_LINUX_LABJACKTYPECONVERTERS_H
+
+#include <array>
+
+#include "SurgSim/Devices/LabJack/linux/LabJackConstants.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+namespace LabJack
+{
+
+/// Converts an eight byte array to a floating point double value. This function is necessary for
+/// communication with the low-level driver for the LabJack, which passes all data via unsigned char. The expected
+/// format is: the first four bytes store the truncated absolute value of the decimal portion times 4294967296,
+/// the last four bytes store the signed whole-number portion, and the bytes are stored in little endian order.
+/// \param bytes The array.
+/// \param startIndex The index of the first element.
+/// \return The double.
+double doubleFromChars(const std::array<unsigned char, MAXIMUM_BUFFER>& bytes, int startIndex);
+
+/// Converts an array of bytes to a uint32_t, with the least significant byte at startIndex, and the most significant
+/// byte at startIndex + byteCount - 1.
+/// \param bytes The array.
+/// \param startIndex The index in the array of the first byte to use.
+/// \param count The number of bytes to convert.
+/// \return A uint32_t.
+/// \exception Asserts if byteCount is greater than 4, or it attempts to access beyond the end of the byte array.
+uint32_t uint32FromChars(const std::array<unsigned char, LabJack::MAXIMUM_BUFFER> &bytes, int startIndex, int count);
+
+/// Converts an array of bytes to a uint16_t, with the least significant byte at startIndex.
+/// \param bytes The array.
+/// \param startIndex The index in the array of the first byte to use.
+/// \param count The number of bytes to convert.
+/// \return A uint16_t.
+/// \exception Asserts if byteCount is greater than 2, or it attempts to access beyond the end of the byte array.
+uint16_t uint16FromChars(const std::array<unsigned char, LabJack::MAXIMUM_BUFFER> &bytes, int startIndex, int count);
+
+}; // namespace LabJack
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_LABJACK_LINUX_LABJACKTYPECONVERTERS_H
\ No newline at end of file
diff --git a/SurgSim/Devices/LabJack/win32/LabJackScaffold.cpp b/SurgSim/Devices/LabJack/win32/LabJackScaffold.cpp
new file mode 100644
index 0000000..6ed2908
--- /dev/null
+++ b/SurgSim/Devices/LabJack/win32/LabJackScaffold.cpp
@@ -0,0 +1,1031 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/LabJack/LabJackScaffold.h"
+
+#include <algorithm>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+#include <iostream>
+#include <LabJackUD.h> // the high-level LabJack library.
+#include <list>
+#include <memory>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/LabJack/LabJackDevice.h"
+#include "SurgSim/Devices/LabJack/LabJackThread.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+namespace
+{
+/// The LabJackUD returns a handle of 0 when failing to open a new device.
+static const LJ_HANDLE LABJACK_INVALID_HANDLE = 0;
+
+/// Returns true if there are no LabJackUD errors.
+/// \param code The error code.
+/// \return True if no LabJackUD errors.
+bool isOk(LJ_ERROR code)
+{
+ return (code == LJE_NOERROR);
+}
+
+/// Outputs a string containing the LabJackUD error code and text equivalent.
+/// \param errorCode The error code used by LabJackUD.
+/// \return A string containing the message.
+std::string formatErrorMessage(LJ_ERROR code)
+{
+ char error[256]; // According to LabJackUD.h, the buffer must store at least 256 elements.
+ ErrorToString(code, error);
+ return std::string("LabJackUD returned error code: ") + std::to_string(code) + ", and string: " + error;
+}
+
+/// A struct containing the default settings that depend on the model of LabJack.
+struct LabJackDefaults
+{
+ LabJackDefaults()
+ {
+ timerBase[LabJack::MODEL_U3] = LabJack::TIMERBASE_22;
+ timerBase[LabJack::MODEL_U6] = LabJack::TIMERBASE_22;
+ timerBase[LabJack::MODEL_UE9] = LabJack::TIMERBASE_1;
+ }
+
+ /// The default timer base rate.
+ std::unordered_map<LabJack::Model, LabJack::TimerBase> timerBase;
+};
+};
+
+class LabJackScaffold::Handle
+{
+public:
+ /// Constructor that attempts to open a device.
+ /// \param model The model of LabJack device to open (see strings in LabJackUD.h).
+ /// \param connection How to connect to the device (e.g., USB) (see strings in LabJackUD.h).
+ /// \param address Either the ID or serial number (if USB), or the IP address.
+ Handle(LabJack::Model model, LabJack::Connection connection,
+ const std::string& address) :
+ m_address(address),
+ m_model(model),
+ m_connection(connection),
+ m_deviceHandle(LABJACK_INVALID_HANDLE),
+ m_scaffold(LabJackScaffold::getOrCreateSharedInstance())
+ {
+ create();
+ }
+
+ /// Destructor.
+ ~Handle()
+ {
+ SURGSIM_ASSERT(!isValid()) << "Expected destroy() to be called before Handle object destruction.";
+ }
+
+ /// \return Whether or not the wrapped handle is valid.
+ bool isValid() const
+ {
+ return (m_deviceHandle != LABJACK_INVALID_HANDLE);
+ }
+
+ /// Helper function called by the constructor to open the LabJack device for communications.
+ void create()
+ {
+ SURGSIM_ASSERT(!isValid()) <<
+ "Expected LabJackScaffold::Handle::create() to be called on an uninitialized object.";
+
+ int firstFound = 0;
+ if (m_address.length() == 0)
+ {
+ firstFound = 1; // If no address is specified, grab the first device found of this model and connection.
+ }
+
+ const LJ_ERROR error = OpenLabJack(m_model, m_connection, m_address.c_str(), firstFound, &m_deviceHandle);
+ SURGSIM_LOG_IF(!isOk(error), m_scaffold->getLogger(), SEVERE) <<
+ "Failed to initialize a device. Model: " << m_model << ". Connection: " << m_connection << ". Address: '" <<
+ m_address << "'." << std::endl << formatErrorMessage(error);
+ }
+
+ bool destroy()
+ {
+ bool result = false;
+ if (isValid())
+ {
+ // Reset the pin configuration.
+ const LJ_ERROR error = ePut(m_deviceHandle, LJ_ioPIN_CONFIGURATION_RESET, 0, 0, 0);
+ result = isOk(error);
+ SURGSIM_LOG_IF(!result, m_scaffold->getLogger(), SEVERE) <<
+ "Failed to reset a device's pin configuration. Model: " << m_model << ". Connection: " <<
+ m_connection << ". Address: '" << m_address << "'." << std::endl << formatErrorMessage(error);
+ if (result)
+ {
+ m_deviceHandle = LABJACK_INVALID_HANDLE;
+ }
+ }
+ return result;
+ }
+
+ /// \return The LabJackUD's handle wrapped by this Handle.
+ LJ_HANDLE get() const
+ {
+ return m_deviceHandle;
+ }
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ Handle(const Handle&) /*= delete*/;
+ Handle& operator=(const Handle&) /*= delete*/;
+
+ /// The HDAL device handle (or LABJACK_INVALID_HANDLE if not valid).
+ LJ_HANDLE m_deviceHandle;
+ /// The address used to open the device. Can be the empty string if the first-found device was opened.
+ std::string m_address;
+ /// The device model.
+ LabJack::Model m_model;
+ /// The connection to the device.
+ LabJack::Connection m_connection;
+ /// The scaffold.
+ std::shared_ptr<LabJackScaffold> m_scaffold;
+};
+
+/// The per-device data.
+struct LabJackScaffold::DeviceData
+{
+public:
+ /// Initialize the data, creating a thread.
+ DeviceData(LabJackDevice* device, std::unique_ptr<Handle>&& handle) :
+ deviceObject(device),
+ deviceHandle(std::move(handle)),
+ thread(),
+ digitalInputChannels(device->getDigitalInputs()),
+ digitalOutputChannels(device->getDigitalOutputs()),
+ timerInputChannels(getTimerInputChannels(device->getTimers())),
+ timerOutputChannels(getTimerOutputChannels(device->getTimers())),
+ analogInputs(device->getAnalogInputs()),
+ analogOutputChannels(device->getAnalogOutputs()),
+ cachedOutputIndices(false)
+ {
+ }
+
+
+ ~DeviceData()
+ {
+ }
+
+ /// The corresponding device object.
+ LabJackDevice* const deviceObject;
+ /// Processing thread.
+ std::unique_ptr<LabJackThread> thread;
+ /// Device handle to read from.
+ std::unique_ptr<Handle> deviceHandle;
+ /// The channels read for digital inputs.
+ const std::unordered_set<int> digitalInputChannels;
+ /// The channels set for digital outputs.
+ const std::unordered_set<int> digitalOutputChannels;
+ /// The timer channels that provide inputs.
+ const std::unordered_set<int> timerInputChannels;
+ /// The timer channels set for timer outputs (e.g., PWM outputs).
+ const std::unordered_set<int> timerOutputChannels;
+ /// The analog inputs.
+ const std::unordered_map<int, LabJack::AnalogInputSettings> analogInputs;
+ /// The channels set for analog outputs.
+ const std::unordered_set<int> analogOutputChannels;
+ /// The DataGroup indices for the digital outputs.
+ std::unordered_map<int, int> digitalOutputIndices;
+ /// The DataGroup indices for the digital inputs.
+ std::unordered_map<int, int> digitalInputIndices;
+ /// The DataGroup indices for the timer outputs.
+ std::unordered_map<int, int> timerOutputIndices;
+ /// The DataGroup indices for the timer inputs.
+ std::unordered_map<int, int> timerInputIndices;
+ /// The DataGroup indices for the analog outputs.
+ std::unordered_map<int, int> analogOutputIndices;
+ /// The DataGroup indices for the analog inputs.
+ std::unordered_map<int, int> analogInputIndices;
+ /// True if the output indices have been cached.
+ bool cachedOutputIndices;
+
+private:
+ /// Given all the timers, return just the ones that provide inputs.
+ /// \param timers The timers.
+ /// \return The timers that provide inputs.
+ const std::unordered_set<int> getTimerInputChannels(const std::unordered_map<int,
+ LabJack::TimerSettings>& timers) const
+ {
+ std::unordered_set<int> timersWithInputs;
+ for (auto timer = timers.cbegin(); timer != timers.cend(); ++timer)
+ {
+ if ((timer->second.mode != LabJack::TIMERMODE_PWM_16BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_PWM_8BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_FREQUENCY_OUTPUT))
+ {
+ timersWithInputs.insert(timer->first);
+ }
+ }
+ return timersWithInputs;
+ }
+
+ /// Given all the timers, return just the ones that take outputs.
+ /// \param timers The timers.
+ /// \return The timers that take outputs.
+ const std::unordered_set<int> getTimerOutputChannels(const std::unordered_map<int,
+ LabJack::TimerSettings>& timers) const
+ {
+ std::unordered_set<int> timersWithOutputs;
+ for (auto timer = timers.cbegin(); timer != timers.cend(); ++timer)
+ {
+ if ((timer->second.mode != LabJack::TIMERMODE_PWM_16BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_PWM_8BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_RISING_EDGES_32BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_FALLING_EDGES_32BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_DUTY_CYCLE) &&
+ (timer->second.mode != LabJack::TIMERMODE_FIRMWARE_COUNTER) &&
+ (timer->second.mode != LabJack::TIMERMODE_FIRMWARE_COUNTER_DEBOUNCED) &&
+ (timer->second.mode != LabJack::TIMERMODE_FREQUENCY_OUTPUT) &&
+ (timer->second.mode != LabJack::TIMERMODE_QUADRATURE) &&
+ (timer->second.mode != LabJack::TIMERMODE_RISING_EDGES_16BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_FALLING_EDGES_16BIT) &&
+ (timer->second.mode != LabJack::TIMERMODE_LINE_TO_LINE))
+ {
+ timersWithOutputs.insert(timer->first);
+ }
+ }
+ return timersWithOutputs;
+ }
+
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ DeviceData(const DeviceData&) /*= delete*/;
+ DeviceData& operator=(const DeviceData&) /*= delete*/;
+};
+
+/// The per-scaffold data (in comparison to DeviceData the per-device data).
+/// Note that there is only a single instance of LabJackScaffold and so only a single instance of this struct.
+struct LabJackScaffold::StateData
+{
+public:
+ /// Initialize the state.
+ StateData()
+ {
+ }
+
+ /// The list of known devices.
+ std::list<std::unique_ptr<LabJackScaffold::DeviceData>> activeDeviceList;
+
+ /// The mutex that protects the list of known devices.
+ boost::mutex mutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ StateData(const StateData&) /*= delete*/;
+ StateData& operator=(const StateData&) /*= delete*/;
+};
+
+LabJackScaffold::LabJackScaffold() :
+ m_state(new StateData)
+{
+ m_logger = SurgSim::Framework::Logger::getLogger("LabJack device");
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created. LabJackUD driver version: " <<
+ GetDriverVersion() << ".";
+}
+
+LabJackScaffold::~LabJackScaffold()
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (!m_state->activeDeviceList.empty())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Destroying scaffold while devices are active!?!";
+ for (auto it = m_state->activeDeviceList.begin(); it != m_state->activeDeviceList.end(); ++it)
+ {
+ if ((*it)->thread)
+ {
+ destroyPerDeviceThread(it->get());
+ }
+ }
+ m_state->activeDeviceList.clear();
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold destroyed.";
+}
+
+bool LabJackScaffold::registerDevice(LabJackDevice* device)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ bool result = true;
+
+ // Make sure the object is unique.
+ auto sameObject = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ SURGSIM_ASSERT(sameObject == m_state->activeDeviceList.end()) << "LabJack: Tried to register a device named '" <<
+ device->getName() << "', which is already present!";
+
+ // Make sure the name is unique.
+ const std::string& deviceName = device->getName();
+ auto const sameName = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [&deviceName](const std::unique_ptr<DeviceData>& info) { return info->deviceObject->getName() == deviceName; });
+ if (sameName != m_state->activeDeviceList.end())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Tried to register a device when the same name, '" <<
+ device->getName() << "', is already present!";
+ result = false;
+ }
+
+ // Make sure the combination of connection and address is unique, unless the address is zero-length, in which
+ // case the first-found device of this model on this connection will be opened.
+ const std::string& address = device->getAddress();
+ if (result && (address.length() > 0))
+ {
+ const LabJack::Model model = device->getModel();
+ const LabJack::Connection connection = device->getConnection();
+
+ auto const sameInitialization = std::find_if(m_state->activeDeviceList.cbegin(),
+ m_state->activeDeviceList.cend(),
+ [&address, connection, model](const std::unique_ptr<DeviceData>& info)
+ { return (info->deviceObject->getAddress() == address) &&
+ (info->deviceObject->getConnection() == connection) &&
+ (info->deviceObject->getModel() == model); });
+
+ if (sameInitialization != m_state->activeDeviceList.cend())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Tried to register a device named '" << device->getName() <<
+ "', but a device with the same model (" << model << "), connection (" << connection <<
+ "), and address ('" << address << "') is already present!";
+ result = false;
+ }
+ }
+
+ if (result)
+ {
+ // Create a handle, opening communications.
+ // If the device's model or connection are SEARCH, iterate over the options.
+ std::vector<LabJack::Model> modelsToSearch;
+ if (device->getModel() == LabJack::MODEL_SEARCH)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << ": searching for models U3, U6, and UE9.";
+ modelsToSearch.push_back(LabJack::MODEL_U6);
+ modelsToSearch.push_back(LabJack::MODEL_U3);
+ modelsToSearch.push_back(LabJack::MODEL_UE9);
+ }
+ else
+ {
+ modelsToSearch.push_back(device->getModel());
+ }
+
+ std::vector<LabJack::Connection> connectionsToSearch;
+ if (device->getConnection() == LabJack::CONNECTION_SEARCH)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() <<
+ ": searching for connections USB and Ethernet.";
+ connectionsToSearch.push_back(LabJack::CONNECTION_USB);
+ connectionsToSearch.push_back(LabJack::CONNECTION_ETHERNET);
+ }
+ else
+ {
+ connectionsToSearch.push_back(device->getConnection());
+ }
+
+ std::unique_ptr<Handle> handle;
+ for (auto model = modelsToSearch.cbegin(); model != modelsToSearch.cend(); ++model)
+ {
+ for (auto connection = connectionsToSearch.cbegin(); connection != connectionsToSearch.cend(); ++connection)
+ {
+ device->setModel(*model);
+ device->setConnection(*connection);
+
+ handle = std::unique_ptr<Handle>(new Handle(*model, *connection, address));
+ result = handle->isValid();
+ if (result)
+ {
+ auto const sameHandle = std::find_if(m_state->activeDeviceList.cbegin(),
+ m_state->activeDeviceList.cend(),
+ [&handle](const std::unique_ptr<DeviceData>& info)
+ { return (info->deviceHandle->get() == handle->get()); });
+
+ if (sameHandle != m_state->activeDeviceList.cend())
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Tried to register a device named '" << device->getName() <<
+ "', but a device with the same handle (" << handle->get() << ") is already present! " <<
+ "This can happen if multiple LabJack devices are used without setting their addresses.";
+ handle->destroy(); // The handle was initialized and will be destructed.
+ result = false;
+ }
+ else
+ {
+ break;
+ }
+ }
+ }
+ if (result)
+ {
+ break;
+ }
+ }
+
+ if (result)
+ {
+ std::unique_ptr<DeviceData> info(new DeviceData(device, std::move(handle)));
+
+ // Cache the NamedData indices for the input DataGroup.
+ const SurgSim::DataStructures::DataGroup& inputData = device->getInputData();
+
+ for (auto input = info->digitalInputChannels.cbegin();
+ input != info->digitalInputChannels.cend();
+ ++input)
+ {
+ const std::string name = SurgSim::DataStructures::Names::DIGITAL_INPUT_PREFIX + std::to_string(*input);
+ info->digitalInputIndices[*input] = inputData.booleans().getIndex(name);
+ SURGSIM_ASSERT(info->digitalInputIndices[*input] >= 0) << "LabJackScaffold::DeviceData " <<
+ "failed to get a valid NamedData index for the digital input for line " << *input <<
+ ". Make sure that is a valid line number. Expected an entry named " << name << ".";
+ }
+
+ for (auto timer = info->timerInputChannels.cbegin();
+ timer != info->timerInputChannels.cend();
+ ++timer)
+ {
+ const std::string name = SurgSim::DataStructures::Names::TIMER_INPUT_PREFIX + std::to_string(*timer);
+ info->timerInputIndices[*timer] = inputData.scalars().getIndex(name);
+ SURGSIM_ASSERT(info->timerInputIndices[*timer] >= 0) << "LabJackScaffold::DeviceData " <<
+ "failed to get a valid NamedData index for the timer for channel " << *timer <<
+ ". Make sure that is a valid timer number. Expected an entry named " << name << ".";
+ }
+
+ for (auto input = info->analogInputs.cbegin(); input != info->analogInputs.cend(); ++input)
+ {
+ std::string name = SurgSim::DataStructures::Names::ANALOG_INPUT_PREFIX + std::to_string(input->first);
+ info->analogInputIndices[input->first] = inputData.scalars().getIndex(name);
+ SURGSIM_ASSERT(info->analogInputIndices[input->first] >= 0) <<
+ "LabJackScaffold::DeviceData failed to get a valid NamedData index for the " <<
+ "analog input for channel " << input->first << ". Make sure that is a valid line number. " <<
+ "Expected an entry named " << name << ".";
+ }
+
+ std::unique_ptr<LabJackThread> thread(new LabJackThread(this, info.get()));
+ thread->setRate(device->getMaximumUpdateRate());
+ thread->start();
+
+ info.get()->thread = std::move(thread);
+ m_state->activeDeviceList.emplace_back(std::move(info));
+ }
+ }
+
+ return result;
+}
+
+bool LabJackScaffold::unregisterDevice(const LabJackDevice* const device)
+{
+ bool found = false;
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ if ((*matching)->thread)
+ {
+ destroyPerDeviceThread(matching->get());
+ matching->get()->deviceHandle->destroy();
+ }
+ m_state->activeDeviceList.erase(matching);
+ // the iterator is now invalid but that's OK
+ found = true;
+ }
+ }
+
+ if (!found)
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Attempted to release a non-registered device named '" <<
+ device->getName() << ".";
+ }
+ return found;
+}
+
+bool LabJackScaffold::runInputFrame(LabJackScaffold::DeviceData* info)
+{
+ if (info->deviceObject->pullOutput() && !info->cachedOutputIndices)
+ {
+ const SurgSim::DataStructures::DataGroup& initialOutputData = info->deviceObject->getOutputData();
+
+ const std::unordered_set<int>& digitalOutputChannels = info->digitalOutputChannels;
+ for (auto output = digitalOutputChannels.cbegin(); output != digitalOutputChannels.cend(); ++output)
+ {
+ info->digitalOutputIndices[*output] =
+ initialOutputData.booleans().getIndex(SurgSim::DataStructures::Names::DIGITAL_OUTPUT_PREFIX +
+ std::to_string(*output));
+ }
+
+ const std::unordered_set<int>& timerOutputChannels = info->timerOutputChannels;
+ for (auto timer = timerOutputChannels.cbegin(); timer != timerOutputChannels.cend(); ++timer)
+ {
+ info->timerOutputIndices[*timer] =
+ initialOutputData.scalars().getIndex(SurgSim::DataStructures::Names::TIMER_OUTPUT_PREFIX +
+ std::to_string(*timer));
+ }
+
+ const std::unordered_set<int>& analogOutputChannels = info->analogOutputChannels;
+ for (auto output = analogOutputChannels.cbegin(); output != analogOutputChannels.cend(); ++output)
+ {
+ info->analogOutputIndices[*output] =
+ initialOutputData.scalars().getIndex(SurgSim::DataStructures::Names::ANALOG_OUTPUT_PREFIX +
+ std::to_string(*output));
+ }
+
+ info->cachedOutputIndices = true;
+ }
+
+ if (!info->deviceObject->hasOutputProducer() || info->cachedOutputIndices)
+ {
+ if (!updateDevice(info))
+ {
+ return false;
+ }
+ info->deviceObject->pushInput();
+ }
+ return true;
+}
+
+bool LabJackScaffold::updateDevice(LabJackScaffold::DeviceData* info)
+{
+ const LJ_HANDLE rawHandle = info->deviceHandle->get();
+
+ // First we AddRequests. This clears the old data.
+ const SurgSim::DataStructures::DataGroup& outputData = info->deviceObject->getOutputData();
+
+ // Request the values of digital inputs.
+ const std::unordered_set<int>& digitalInputChannels = info->digitalInputChannels;
+ for (auto input = digitalInputChannels.cbegin(); input != digitalInputChannels.cend(); ++input)
+ {
+ const LJ_ERROR error = AddRequest(rawHandle, LJ_ioGET_DIGITAL_BIT, *input, 0, 0, 0);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, WARNING) <<
+ "Failed to request digital input for a device named '" << info->deviceObject->getName() <<
+ "', line number " << *input << "." << std::endl << formatErrorMessage(error);
+ }
+
+ // Request to set digital outputs.
+ const std::unordered_set<int>& digitalOutputChannels = info->digitalOutputChannels;
+ for (auto output = digitalOutputChannels.cbegin(); output != digitalOutputChannels.cend(); ++output)
+ {
+ if (info->digitalOutputIndices.count(*output) > 0)
+ {
+ const int index = info->digitalOutputIndices[*output];
+ SURGSIM_ASSERT(index >= 0) << "LabJackScaffold: A LabJackDevice was configured with line " << *output <<
+ " set to digital output, but the scaffold does not know the correct index into the NamedData. " <<
+ " Make sure there is an entry in the booleans with the correct string key.";
+
+ bool value;
+ if (outputData.booleans().get(index, &value))
+ {
+ const double valueToSend = (value ? 1.0 : 0.0);
+ const LJ_ERROR error = AddRequest(rawHandle, LJ_ioPUT_DIGITAL_BIT, *output, valueToSend, 0, 0);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, WARNING) <<
+ "Failed to set digital output for a device named '" << info->deviceObject->getName() <<
+ "', line number " << *output << ", value " << valueToSend << "." <<
+ std::endl << formatErrorMessage(error);
+ }
+ }
+ }
+
+ // Request to set to timers (e.g., resetting firmware counters).
+ const std::unordered_set<int>& timerOutputChannels = info->timerOutputChannels;
+ for (auto timer = timerOutputChannels.cbegin(); timer != timerOutputChannels.cend(); ++timer)
+ {
+ if (info->timerOutputIndices.count(*timer) > 0)
+ {
+ const int index = info->timerOutputIndices[*timer];
+ // We do not ensure that all the timers can be output to, because the user might not need to reset them.
+ if (index >= 0)
+ {
+ double value;
+ if (outputData.scalars().get(index, &value))
+ {
+ const LJ_ERROR error = AddRequest(rawHandle, LJ_ioPUT_TIMER_VALUE, *timer, value, 0, 0);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, WARNING) <<
+ "Failed to set timer value for a device named '" << info->deviceObject->getName() <<
+ "', channel number " << *timer << ", value " << value << "." <<
+ std::endl << formatErrorMessage(error);
+ }
+ }
+ }
+ }
+
+ // Request inputs from timers.
+ const std::unordered_set<int>& timerInputChannels = info->timerInputChannels;
+ for (auto timer = timerInputChannels.cbegin(); timer != timerInputChannels.cend(); ++timer)
+ {
+ const LJ_ERROR error = AddRequest(rawHandle, LJ_ioGET_TIMER, *timer, 0, 0, 0);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, WARNING) <<
+ "Failed to request timer input for a device named '" << info->deviceObject->getName() <<
+ "', channel number " << *timer << "." << std::endl << formatErrorMessage(error);
+ }
+
+ // Request the values of analog inputs.
+ auto const& analogInputs = info->analogInputs;
+ for (auto input = analogInputs.cbegin(); input != analogInputs.cend(); ++input)
+ {
+ if (input->second.negativeChannel.hasValue())
+ {
+ const LJ_ERROR error = AddRequest(rawHandle, LJ_ioGET_AIN_DIFF, input->first, 0,
+ input->second.negativeChannel.getValue(), 0);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, WARNING) <<
+ "Failed to request differential analog input for a device named '" << info->deviceObject->getName() <<
+ "', positive channel " << input->first << ", negative channel " <<
+ input->second.negativeChannel.getValue() << "." << std::endl << formatErrorMessage(error);
+ }
+ else
+ {
+ const LJ_ERROR error = AddRequest(rawHandle, LJ_ioGET_AIN, input->first, 0, 0, 0);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, WARNING) <<
+ "Failed to request single-ended analog input for a device named '" << info->deviceObject->getName() <<
+ "', channel " << input->first << "." << std::endl << formatErrorMessage(error);
+ }
+ }
+
+ // Request to set analog outputs.
+ const std::unordered_set<int>& analogOutputChannels = info->analogOutputChannels;
+ for (auto output = analogOutputChannels.cbegin(); output != analogOutputChannels.cend(); ++output)
+ {
+ if (info->analogOutputIndices.count(*output) > 0)
+ {
+ const int index = info->analogOutputIndices[*output];
+ SURGSIM_ASSERT(index >= 0) << "LabJackScaffold: A LabJackDevice was configured with line " << *output <<
+ " set to analog output, but the scaffold does not know the correct index into the NamedData. " <<
+ " Make sure there is an entry in the scalars with the correct string key.";
+
+ double value;
+ if (outputData.scalars().get(index, &value))
+ {
+ const LJ_ERROR error = AddRequest(rawHandle, LJ_ioPUT_DAC, *output, value, 0, 0);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, WARNING) <<
+ "Failed to set analog output for a device named '" << info->deviceObject->getName() <<
+ "', line number " << *output << ", value " << value << "." <<
+ std::endl << formatErrorMessage(error);
+ }
+ }
+ }
+
+ // GoOne, telling this specific LabJack to perform the requests.
+ const LJ_ERROR error = GoOne(rawHandle);
+
+ // Finally we get the results.
+ SurgSim::DataStructures::DataGroup& inputData = info->deviceObject->getInputData();
+ if (isOk(error))
+ {
+ // Digital inputs.
+ for (auto input = digitalInputChannels.cbegin(); input != digitalInputChannels.cend(); ++input)
+ {
+ double value;
+ const LJ_ERROR error = GetResult(rawHandle, LJ_ioGET_DIGITAL_BIT, *input, &value);
+ if (isOk(error))
+ {
+ const bool valueToSet = value > 0.5 ? true : false;
+ inputData.booleans().set(info->digitalInputIndices[*input], valueToSet);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Failed to get digital input for a device named '" <<
+ info->deviceObject->getName() << "', line number " << *input << "." << std::endl <<
+ formatErrorMessage(error);
+ inputData.booleans().reset(info->digitalInputIndices[*input]);
+ }
+ }
+
+ // Timer inputs.
+ for (auto timer = timerInputChannels.cbegin(); timer != timerInputChannels.cend(); ++timer)
+ {
+ double value;
+ const LJ_ERROR error = GetResult(rawHandle, LJ_ioGET_TIMER, *timer, &value);
+ if (isOk(error))
+ {
+ inputData.scalars().set(info->timerInputIndices[*timer], value);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Failed to get timer input for a device named '" <<
+ info->deviceObject->getName() << "', channel number " << *timer << "." << std::endl <<
+ formatErrorMessage(error);
+ inputData.scalars().reset(info->timerInputIndices[*timer]);
+ }
+ }
+
+ // Analog inputs.
+ for (auto input = analogInputs.cbegin(); input != analogInputs.cend(); ++input)
+ {
+ double value;
+ if (input->second.negativeChannel.hasValue())
+ {
+ const LJ_ERROR error = GetResult(rawHandle, LJ_ioGET_AIN_DIFF, input->first, &value);
+ if (isOk(error))
+ {
+ inputData.scalars().set(info->analogInputIndices[input->first], value);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Failed to get differential analog input for a device named '" <<
+ info->deviceObject->getName() << "', positive channel " << input->first <<
+ ", negative channel " << input->second.negativeChannel.getValue() << "." << std::endl
+ << formatErrorMessage(error);
+ inputData.scalars().reset(info->analogInputIndices[input->first]);
+ }
+ }
+ else
+ {
+ const LJ_ERROR error = GetResult(rawHandle, LJ_ioGET_AIN, input->first, &value);
+ if (isOk(error))
+ {
+ inputData.scalars().set(info->analogInputIndices[input->first], value);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Failed to get single-ended analog input for a device named '" <<
+ info->deviceObject->getName() << "', channel " << input->first << "." << std::endl <<
+ formatErrorMessage(error);
+ inputData.scalars().reset(info->analogInputIndices[input->first]);
+ }
+ }
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Failed to submit requests for a device named '" <<
+ info->deviceObject->getName() << "." << std::endl << formatErrorMessage(error);
+ inputData.resetAll();
+ }
+
+ return true;
+}
+
+bool LabJackScaffold::destroyPerDeviceThread(DeviceData* data)
+{
+ SURGSIM_ASSERT(data->thread) << "LabJack: destroying a per-device thread, but none exists for this DeviceData";
+
+ std::unique_ptr<LabJackThread> thread = std::move(data->thread);
+ thread->stop();
+ thread.reset();
+
+ return true;
+}
+
+SurgSim::DataStructures::DataGroup LabJackScaffold::buildDeviceInputData()
+{
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ // We don't know which input lines we need until after the configuration, but LabJackDevice must be constructed with
+ // a valid DataGroup, so we add every possible line.
+ const int maxDigitalInputs = 23; // The UE9 can have 23 digital inputs.
+ for (int i = 0; i < maxDigitalInputs; ++i)
+ {
+ builder.addBoolean(SurgSim::DataStructures::Names::DIGITAL_INPUT_PREFIX + std::to_string(i));
+ }
+
+ const int maxTimerInputs = 6; // The UE9 can have 6 timers.
+ for (int i = 0; i < maxTimerInputs; ++i)
+ {
+ builder.addScalar(SurgSim::DataStructures::Names::TIMER_INPUT_PREFIX + std::to_string(i));
+ }
+
+ const int maxAnalogInputs = 16; // The U3 can have 16 analog inputs.
+ for (int i = 0; i < maxAnalogInputs; ++i)
+ {
+ builder.addScalar(SurgSim::DataStructures::Names::ANALOG_INPUT_PREFIX + std::to_string(i));
+ }
+ return builder.createData();
+}
+
+std::shared_ptr<LabJackScaffold> LabJackScaffold::getOrCreateSharedInstance()
+{
+ static SurgSim::Framework::SharedInstance<LabJackScaffold> sharedInstance;
+ return sharedInstance.get();
+}
+
+bool LabJackScaffold::configureDevice(DeviceData* deviceData)
+{
+ LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
+
+ // Reset the configuration.
+ LJ_ERROR error = ePut(rawHandle, LJ_ioPIN_CONFIGURATION_RESET, 0, 0, 0);
+ bool result = isOk(error);
+ SURGSIM_LOG_IF(!result, m_logger, SEVERE) <<
+ "Failed to reset configuration for a device named '" << deviceData->deviceObject->getName() << "." <<
+ std::endl << formatErrorMessage(error);
+
+ return result && configureClockAndTimers(deviceData) && configureDigital(deviceData) && configureAnalog(deviceData);
+}
+
+bool LabJackScaffold::configureClockAndTimers(DeviceData* deviceData)
+{
+ bool result = configureNumberOfTimers(deviceData);
+
+ if (result && (deviceData->deviceObject->getTimers().size() > 0))
+ {
+ result = configureClock(deviceData) && configureTimers(deviceData);
+ }
+ return result;
+}
+
+bool LabJackScaffold::configureNumberOfTimers(DeviceData* deviceData)
+{
+ LabJackDevice* device = deviceData->deviceObject;
+ LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
+
+ const std::unordered_map<int, LabJack::TimerSettings>& timers = device->getTimers();
+
+ for (auto timer : timers)
+ {
+ SURGSIM_LOG_IF(timer.first >= static_cast<int>(timers.size()), m_logger, SEVERE) <<
+ "Error configuring enabled timers for a device named '" << device->getName() <<
+ "', with number of timers: " << timers.size() << "." << std::endl <<
+ " Timers must be enabled consecutively, starting with #0." << std::endl <<
+ " With the currently enabled number of timers, the highest allowable timer is #" <<
+ timers.size() - 1 << ", but one of the enabled timers is #" << timer.first << "." << std::endl;
+ }
+
+ LJ_ERROR error =
+ ePut(rawHandle, LJ_ioPUT_CONFIG, LJ_chNUMBER_TIMERS_ENABLED, static_cast<double>(timers.size()), 0);
+ bool result = isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to configure number of enabled timers for a device named '" << device->getName() <<
+ "', with number of timers " << timers.size() << "." << std::endl << formatErrorMessage(error);
+
+ // Counters are not yet supported so they are explicitly disabled.
+ const int numberOfChannels = 2; // The LabJack U3, U6, and UE9 models each have two counters.
+ for (int channel = 0; channel < numberOfChannels; ++channel)
+ {
+ const int enable = 0; // Disable both counters.
+ const LJ_ERROR error = ePut(rawHandle, LJ_ioPUT_COUNTER_ENABLE, channel, enable, 0);
+ result = result && isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to enable/disable counter for a device named '" << device->getName() << "." <<
+ std::endl << formatErrorMessage(error);
+ }
+
+ error = ePut(rawHandle, LJ_ioPUT_CONFIG, LJ_chTIMER_COUNTER_PIN_OFFSET, device->getTimerCounterPinOffset(), 0);
+ result = result && isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to configure timer/counter pin offset for a device named '" << device->getName() <<
+ "', with offset " << device->getTimerCounterPinOffset() << "." << std::endl << formatErrorMessage(error);
+
+ return result;
+}
+
+bool LabJackScaffold::configureClock(DeviceData* deviceData)
+{
+ LabJackDevice* device = deviceData->deviceObject;
+ LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
+
+ LabJack::TimerBase base = device->getTimerBase();
+ if (base == LabJack::TIMERBASE_DEFAULT)
+ {
+ LabJackDefaults defaults;
+ base = defaults.timerBase[device->getModel()];
+ }
+ LJ_ERROR error = ePut(rawHandle, LJ_ioPUT_CONFIG, LJ_chTIMER_CLOCK_BASE, base, 0);
+ bool result = isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to configure the timer base rate for a device named '" << device->getName() <<
+ "', with timer base " << device->getTimerBase() << "." << std::endl << formatErrorMessage(error);
+
+ error = ePut(rawHandle, LJ_ioPUT_CONFIG, LJ_chTIMER_CLOCK_DIVISOR, device->getTimerClockDivisor(), 0);
+ result = result && isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to configure the timer/clock divisor for a device named '" << device->getName() <<
+ "', with divisor " << device->getTimerClockDivisor() << "." << std::endl << formatErrorMessage(error);
+
+ return result;
+}
+
+bool LabJackScaffold::configureTimers(DeviceData* deviceData)
+{
+ LabJackDevice* device = deviceData->deviceObject;
+ LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
+
+ bool result = true;
+
+ const std::unordered_map<int, LabJack::TimerSettings>& timers = device->getTimers();
+ for (auto timer = timers.cbegin(); timer != timers.cend(); ++timer)
+ {
+ LJ_ERROR error = AddRequest(rawHandle, LJ_ioPUT_TIMER_MODE, timer->first, timer->second.mode, 0, 0);
+ result = result && isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to configure a timer for a device named '" << device->getName() <<
+ "', timer number " << timer->first << ", with mode code " << timer->second.mode << "." <<
+ std::endl << formatErrorMessage(error);
+ if (result && (timer->second.initialValue.hasValue()))
+ {
+ error = AddRequest(rawHandle, LJ_ioPUT_TIMER_VALUE, timer->first, timer->second.initialValue.getValue(), 0,
+ 0);
+ result = result && isOk(error);
+ SURGSIM_LOG_IF(!result, m_logger, SEVERE) <<
+ "Failed to set the initial value for a timer for a device named '" << device->getName() <<
+ "', timer number " << timer->first << ", with mode code " << timer->second.mode <<
+ ", and value " << timer->second.initialValue.getValue() << "." << std::endl <<
+ formatErrorMessage(error);
+ }
+
+ if (result)
+ {
+ error = GoOne(rawHandle);
+ result = result && isOk(error);
+
+ double value;
+ error = GetResult(rawHandle, LJ_ioPUT_TIMER_MODE, timer->first, &value);
+ result = result && isOk(error);
+
+ if (result && timer->second.initialValue.hasValue())
+ {
+ error = GetResult(rawHandle, LJ_ioPUT_TIMER_VALUE, timer->first, &value);
+ result = result && isOk(error);
+ }
+
+ SURGSIM_LOG_IF(!result, m_logger, SEVERE) <<
+ "Failed to configure timer for a device named '" << device->getName() <<
+ "', timer number " << timer->first << ", with mode code " << timer->second.mode <<
+ "." << std::endl << formatErrorMessage(error);
+ }
+ }
+
+ return result;
+}
+
+bool LabJackScaffold::configureAnalog(DeviceData* deviceData)
+{
+ LabJackDevice* device = deviceData->deviceObject;
+ LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
+
+ bool result = true;
+
+ const std::unordered_set<int>& analogOutputs = deviceData->analogOutputChannels;
+ for (auto output = analogOutputs.cbegin(); output != analogOutputs.cend(); ++output)
+ {
+ LJ_ERROR error = ePut(rawHandle, LJ_ioPUT_DAC_ENABLE, *output, 1, 0);
+ result = result && isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to enable analog output for a device named '" << device->getName() <<
+ "', channel " << *output << "." << std::endl << formatErrorMessage(error);
+ }
+
+ auto const& analogInputs = deviceData->analogInputs;
+ if (analogInputs.size() > 0)
+ {
+ LJ_ERROR error = ePut(rawHandle, LJ_ioPUT_CONFIG, LJ_chAIN_RESOLUTION, device->getAnalogInputResolution(), 0);
+ result = result && isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to configure analog input resolution for a device named '" << device->getName() <<
+ "', with resolution code " << device->getAnalogInputResolution() << "." << std::endl <<
+ formatErrorMessage(error);
+
+ error = ePut(rawHandle, LJ_ioPUT_CONFIG, LJ_chAIN_SETTLING_TIME, device->getAnalogInputSettling(), 0);
+ result = result && isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to configure analog input settling time for a device named '" << device->getName() <<
+ "', with settling time code " << device->getAnalogInputSettling() << "." << std::endl <<
+ formatErrorMessage(error);
+
+ for (auto input = analogInputs.cbegin(); input != analogInputs.cend(); ++input)
+ {
+ error = ePut(rawHandle, LJ_ioPUT_ANALOG_ENABLE_BIT, input->first, 1, 0);
+ result = result && isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to enable analog input for a device named '" << device->getName() <<
+ "', channel " << input->first << "." << std::endl << formatErrorMessage(error);
+
+ error = ePut(rawHandle, LJ_ioPUT_AIN_RANGE, input->first, input->second.range, 0);
+ result = result && isOk(error);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to set the range for an analog input for a device named '" << device->getName() <<
+ "', channel " << input->first << ", with range code " << input->second.range << "." <<
+ std::endl << formatErrorMessage(error);
+ }
+ }
+
+ return result;
+}
+
+bool LabJackScaffold::configureDigital(DeviceData* deviceData)
+{
+ return true;
+}
+
+std::shared_ptr<SurgSim::Framework::Logger> LabJackScaffold::getLogger() const
+{
+ return m_logger;
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Mouse/CMakeLists.txt b/SurgSim/Devices/Mouse/CMakeLists.txt
new file mode 100644
index 0000000..8a0bcf3
--- /dev/null
+++ b/SurgSim/Devices/Mouse/CMakeLists.txt
@@ -0,0 +1,54 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${OPENSCENEGRAPH_INCLUDE_DIR}"
+)
+
+set(Mouse_DEVICE_SOURCES
+ MouseDevice.cpp
+ MouseScaffold.cpp
+ OsgMouseHandler.cpp
+)
+
+set(Mouse_DEVICE_HEADERS
+ MouseDevice.h
+ MouseScaffold.h
+ OsgMouseHandler.h
+)
+
+surgsim_add_library(
+ MouseDevice
+ "${Mouse_DEVICE_SOURCES}"
+ "${Mouse_DEVICE_HEADERS}"
+ SurgSim/Devices/Mouse
+)
+
+set(LIBS
+ SurgSimInput
+)
+
+target_link_libraries(MouseDevice ${LIBS}
+)
+
+if(BUILD_TESTING)
+ # The unit tests will be built whenever the library is built.
+ add_subdirectory(UnitTests)
+ add_subdirectory(VisualTests)
+endif()
+
+# Put MouseDevice into folder "Devices"
+set_target_properties(MouseDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Mouse/MouseDevice.cpp b/SurgSim/Devices/Mouse/MouseDevice.cpp
new file mode 100644
index 0000000..340c7b0
--- /dev/null
+++ b/SurgSim/Devices/Mouse/MouseDevice.cpp
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Mouse/MouseDevice.h"
+#include "SurgSim/Devices/Mouse/MouseScaffold.h"
+#include "SurgSim/Devices/Mouse/OsgMouseHandler.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+MouseDevice::MouseDevice(const std::string& deviceName) :
+ SurgSim::Input::CommonDevice(deviceName, MouseScaffold::buildDeviceInputData())
+{
+}
+
+MouseDevice::~MouseDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+bool MouseDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized());
+
+ m_scaffold = MouseScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(m_scaffold);
+
+ m_scaffold->registerDevice(this);
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
+
+ return true;
+}
+
+bool MouseDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized());
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ m_scaffold.reset();
+ return true;
+}
+
+bool MouseDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+OsgMouseHandler* MouseDevice::getMouseHandler() const
+{
+ return m_scaffold->getMouseHandler();
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Mouse/MouseDevice.h b/SurgSim/Devices/Mouse/MouseDevice.h
new file mode 100644
index 0000000..96666c9
--- /dev/null
+++ b/SurgSim/Devices/Mouse/MouseDevice.h
@@ -0,0 +1,86 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MOUSE_MOUSEDEVICE_H
+#define SURGSIM_DEVICES_MOUSE_MOUSEDEVICE_H
+
+#include "SurgSim/Input/CommonDevice.h"
+
+#include <memory>
+#include <string>
+
+namespace SurgSim
+{
+namespace Device
+{
+
+class MouseScaffold;
+class OsgMouseHandler;
+
+/// A class implementing the communication with a mouse
+///
+/// \par Application input provided from the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | bool | "button1" | %State of mouse left button |
+/// | bool | "button2" | %State of mouse middle button |
+/// | bool | "button3" | %State of mouse right button |
+/// | scalar | "x" | %X-coordinate of mouse |
+/// | scalar | "y" | %Y-coordinate of mouse |
+/// | int | "scrollDeltaX" | %Indicates vertical movement direction of mouse wheel |
+/// | int | "scrollDeltaY" | %Indicates horizontal movement direction of mouse wheel |
+///
+/// \par Application output used by the device:
+/// NONE
+///
+/// \note Mouse wheel movement will be indicated by +1/-1 (scroll up/down, right/left) followed by a 0.
+///
+/// \sa SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class MouseDevice : public SurgSim::Input::CommonDevice
+{
+ friend class MouseScaffold;
+ friend class MouseDeviceTest;
+
+public:
+ /// Constructor
+ /// \param deviceName Name for mouse device
+ explicit MouseDevice(const std::string& deviceName);
+ /// Destructor
+ virtual ~MouseDevice();
+
+ /// Initialize corresponding MouseScaffold.
+ /// \return True if MouseScaffold is initialized successfully; Otherwise, false.
+ virtual bool initialize() override;
+ /// "De"-initialize corresponding MouseScaffold.
+ /// \return True if MouseScaffold is 'de'-initialized successfully; Otherwise, false.
+ virtual bool finalize() override;
+
+ /// Check if the scaffold of this device is initialized.
+ /// \return True if this the scaffold of this device is initialized; Otherwise, false.
+ bool isInitialized() const;
+
+ /// Get mouse handler
+ /// \return The mouse handler associated with this device
+ OsgMouseHandler* getMouseHandler() const;
+
+private:
+ /// Communication with hardware is handled by scaffold.
+ std::shared_ptr<MouseScaffold> m_scaffold;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif //SURGSIM_DEVICES_MOUSE_MOUSEDEVICE_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Mouse/MouseScaffold.cpp b/SurgSim/Devices/Mouse/MouseScaffold.cpp
new file mode 100644
index 0000000..01b3795
--- /dev/null
+++ b/SurgSim/Devices/Mouse/MouseScaffold.cpp
@@ -0,0 +1,153 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Mouse/MouseScaffold.h"
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/Mouse/MouseDevice.h"
+#include "SurgSim/Devices/Mouse/OsgMouseHandler.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+
+/// Struct to hold a MouseDevice object, a OsgMouseHandler, and a mutex for data passing.
+struct MouseScaffold::DeviceData
+{
+ /// Constructor
+ /// \param device Device to be managed by this scaffold
+ explicit DeviceData(MouseDevice* device) : deviceObject(device)
+ {
+ mouseHandler = new OsgMouseHandler();
+ }
+
+ /// Device object managed by this scaffold.
+ MouseDevice* const deviceObject;
+ /// Mouse Handler to communicate with underneath API.
+ osg::ref_ptr<OsgMouseHandler> mouseHandler;
+ /// The mutex that protects the externally modifiable parameters.
+ boost::mutex mutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ DeviceData(const DeviceData&) /*= delete*/;
+ DeviceData& operator=(const DeviceData&) /*= delete*/;
+};
+
+MouseScaffold::MouseScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) : m_logger(logger)
+{
+ if (nullptr == m_logger)
+ {
+ m_logger = SurgSim::Framework::Logger::getLogger("Mouse device");
+ m_logger->setThreshold(m_defaultLogLevel);
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Mouse: Shared scaffold created.";
+}
+
+MouseScaffold::~MouseScaffold()
+{
+ unregisterDevice();
+}
+
+bool MouseScaffold::registerDevice(MouseDevice* device)
+{
+ m_device.reset(new DeviceData(device));
+ if (nullptr == m_device)
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "MouseScaffold::registerDevice(): failed to create a DeviceData";
+ return false;
+ }
+ return true;
+}
+
+bool MouseScaffold::unregisterDevice()
+{
+ m_device.reset();
+ if (nullptr == m_device)
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "Mouse: Shared scaffold unregistered.";
+ return true;
+ }
+ return false;
+}
+
+bool MouseScaffold::updateDevice(int buttonMask, float x, float y, int scrollDeltaX, int scrollDeltaY)
+{
+ boost::lock_guard<boost::mutex> lock(m_device->mutex);
+ SurgSim::DataStructures::DataGroup& inputData = m_device->deviceObject->getInputData();
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_1,
+ (buttonMask & osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON) != 0);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_2,
+ (buttonMask & osgGA::GUIEventAdapter::MIDDLE_MOUSE_BUTTON) != 0);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_3,
+ (buttonMask & osgGA::GUIEventAdapter::RIGHT_MOUSE_BUTTON) != 0);
+ inputData.scalars().set("x", static_cast<double>(x));
+ inputData.scalars().set("y", static_cast<double>(y));
+ inputData.integers().set("scrollDeltaX", scrollDeltaX);
+ inputData.integers().set("scrollDeltaY", scrollDeltaY);
+
+ m_device->deviceObject->pushInput();
+ return true;
+}
+
+OsgMouseHandler* MouseScaffold::getMouseHandler() const
+{
+ return m_device->mouseHandler.get();
+}
+
+
+/// Builds the data layout for the application input (i.e. device output).
+SurgSim::DataStructures::DataGroup MouseScaffold::buildDeviceInputData()
+{
+ DataGroupBuilder builder;
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_1); // Indicates mouse left button
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_2); // Indicates mouse middle button (i.e. wheel)
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_3); // Indicates mouse right button
+ builder.addScalar("x"); // Indicates mouse's X-coordinate in the current window, left bottom = (0, 0)
+ builder.addScalar("y"); // Indicates mouse's Y-coordinate in the current window, left bottom = (0, 0)
+ builder.addInteger("scrollDeltaX"); // Indicates mouse wheel vertical movement
+ builder.addInteger("scrollDeltaY"); // Indicates mouse wheel horizontal movement
+
+ return builder.createData();
+}
+
+std::shared_ptr<MouseScaffold> MouseScaffold::getOrCreateSharedInstance()
+{
+ static SurgSim::Framework::SharedInstance<MouseScaffold> sharedInstance;
+ return sharedInstance.get();
+}
+
+void MouseScaffold::setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel)
+{
+ m_defaultLogLevel = logLevel;
+}
+
+std::shared_ptr<SurgSim::Framework::Logger> MouseScaffold::getLogger() const
+{
+ return m_logger;
+}
+
+
+SurgSim::Framework::LogLevel MouseScaffold::m_defaultLogLevel = SurgSim::Framework::LOG_LEVEL_INFO;
+
+}; // namespace Device
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/Mouse/MouseScaffold.h b/SurgSim/Devices/Mouse/MouseScaffold.h
new file mode 100644
index 0000000..a6115ee
--- /dev/null
+++ b/SurgSim/Devices/Mouse/MouseScaffold.h
@@ -0,0 +1,108 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MOUSE_MOUSESCAFFOLD_H
+#define SURGSIM_DEVICES_MOUSE_MOUSESCAFFOLD_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Logger.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+class DataGroup;
+}
+
+namespace Device
+{
+class MouseDevice;
+class OsgMouseHandler;
+
+/// A class that implements the behavior of MouseDevice objects.
+/// \sa SurgSim::Device::MouseDevice
+class MouseScaffold
+{
+ friend class MouseDevice;
+ friend class MouseDeviceTest;
+ friend class OsgMouseHandler;
+
+public:
+ /// Constructor.
+ /// \param logger (optional) The logger to be used by the scaffold object and the devices it manages.
+ /// If unspecified or empty, a console logger will be created and used.
+ explicit MouseScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger = nullptr);
+ /// Destructor
+ ~MouseScaffold();
+
+ /// Gets the logger used by this object and the devices it manages.
+ /// \return The logger.
+ std::shared_ptr<SurgSim::Framework::Logger> getLogger() const;
+
+ /// Gets or creates the scaffold shared by all MouseDevice instances.
+ /// The scaffold is managed using a SingleInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<MouseScaffold> getOrCreateSharedInstance();
+
+ /// Sets the default log level.
+ /// Has no effect unless called before a scaffold is created (i.e. before the first device).
+ /// \param logLevel The log level.
+ static void setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel);
+
+private:
+ /// Internal per-device information.
+ struct DeviceData;
+
+ /// Registers the specified device object.
+ /// If successful, the device object will become connected to an hardware device.
+ /// \param device The device object to be used, which should have a unique name.
+ /// \return True if the initialization succeeds, false if it fails.
+ bool registerDevice(MouseDevice* device);
+ /// Unregisters the specified device object.
+ /// The corresponding controller will become unused, and can be re-registered later.
+ /// \return True on success, false on failure.
+ bool unregisterDevice();
+
+ /// Updates the device information for a single device.
+ /// \param buttons Buttons being pressed.
+ /// \param x X-Coordinate for the device.
+ /// \param y Y-Coordinate for the device.
+ /// \param scrollDeltaX Horizontal indicator for mouse wheel.
+ /// \param scrollDeltaY Vertical indicator for mouse wheel.
+ /// \return True on success.
+ bool updateDevice(int buttons, float x, float y, int scrollDeltaX, int scrollDeltaY);
+
+ /// Get mouse handler
+ /// \return The mouse handler associated with this device
+ OsgMouseHandler* getMouseHandler() const;
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static SurgSim::DataStructures::DataGroup buildDeviceInputData();
+
+
+ /// Logger used by the scaffold and all devices.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+ /// The default logging level.
+ static SurgSim::Framework::LogLevel m_defaultLogLevel;
+ /// The mouse device managed by this scaffold
+ std::unique_ptr<DeviceData> m_device;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MOUSE_MOUSESCAFFOLD_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Mouse/OsgMouseHandler.cpp b/SurgSim/Devices/Mouse/OsgMouseHandler.cpp
new file mode 100644
index 0000000..862c040
--- /dev/null
+++ b/SurgSim/Devices/Mouse/OsgMouseHandler.cpp
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Mouse/OsgMouseHandler.h"
+#include "SurgSim/Devices/Mouse/MouseScaffold.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+OsgMouseHandler::OsgMouseHandler() : m_mouseScaffold(MouseScaffold::getOrCreateSharedInstance()),
+ m_lastX(0.0), m_lastY(0.0), m_lastButtonMask(0), m_lastScrollX(0), m_lastScrollY(0)
+{
+}
+
+bool OsgMouseHandler::handle(const osgGA::GUIEventAdapter& eventHandler, osgGA::GUIActionAdapter&)
+{
+ int scrollX = 0, scrollY = 0;
+ if (eventHandler.getEventType() == osgGA::GUIEventAdapter::EventType::SCROLL)
+ {
+ switch(eventHandler.getScrollingMotion())
+ {
+ case(osgGA::GUIEventAdapter::SCROLL_UP) :
+ {
+ scrollY = 1;
+ break;
+ }
+ case(osgGA::GUIEventAdapter::SCROLL_DOWN) :
+ {
+ scrollY = -1;
+ break;
+ }
+ case(osgGA::GUIEventAdapter::SCROLL_LEFT) :
+ {
+ scrollX = -1;
+ break;
+ }
+ case(osgGA::GUIEventAdapter::SCROLL_RIGHT) :
+ {
+ scrollX = 1;
+ break;
+ }
+ default:
+ break;
+ }
+ }
+
+ // Any mouse state change will cause an update
+ if( eventHandler.getX() != m_lastX || eventHandler.getY() != m_lastY ||
+ eventHandler.getButtonMask() != m_lastButtonMask ||
+ m_lastScrollX != scrollX || m_lastScrollY != scrollY)
+ {
+ m_lastX = eventHandler.getX();
+ m_lastY = eventHandler.getY();
+ m_lastButtonMask = eventHandler.getButtonMask();
+ m_lastScrollX = scrollX;
+ m_lastScrollY = scrollY;
+
+ m_mouseScaffold.lock()->updateDevice(m_lastButtonMask, m_lastX, m_lastY, m_lastScrollX, m_lastScrollY);
+ }
+
+ return true;
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Mouse/OsgMouseHandler.h b/SurgSim/Devices/Mouse/OsgMouseHandler.h
new file mode 100644
index 0000000..6f3333e
--- /dev/null
+++ b/SurgSim/Devices/Mouse/OsgMouseHandler.h
@@ -0,0 +1,59 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MOUSE_OSGMOUSEHANDLER_H
+#define SURGSIM_DEVICES_MOUSE_OSGMOUSEHANDLER_H
+
+#include <memory>
+
+#include <osgGA/GUIEventHandler>
+
+namespace SurgSim
+{
+namespace Device
+{
+
+class MouseScaffold;
+
+class OsgMouseHandler : public osgGA::GUIEventHandler
+{
+public:
+ /// Constructor
+ OsgMouseHandler();
+
+ /// Method to handle GUI event
+ /// \param eventHandler A osgGA::GUIEventAdapter
+ /// \param actionAdapter A osgGA::GUIActionAdapter (required by this virtual method)
+ /// \return True if the event has been handled by this method; Otherwise, false.
+ virtual bool handle(const osgGA::GUIEventAdapter& eventHandler, osgGA::GUIActionAdapter& actionAdapter) override;
+
+private:
+ /// A back pointer to the scaffold which owns this handle
+ std::weak_ptr<MouseScaffold> m_mouseScaffold;
+
+ /// lastX is the X-coordinate of mouse's last location
+ /// lastY is the Y-coordinate of mouse's last location
+ float m_lastX, m_lastY;
+
+ /// Last button mask
+ int m_lastButtonMask;
+ /// Last direction of mouse wheel's horizontal (X) and vertical (Y) movement.
+ int m_lastScrollX, m_lastScrollY;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MOUSE_OSGMOUSEHANDLER_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Mouse/UnitTests/CMakeLists.txt b/SurgSim/Devices/Mouse/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..04670f6
--- /dev/null
+++ b/SurgSim/Devices/Mouse/UnitTests/CMakeLists.txt
@@ -0,0 +1,35 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ MouseDeviceTest.cpp
+ MouseScaffoldTest.cpp
+)
+
+set(LIBS MouseDevice
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ ${OPENSCENEGRAPH_LIBRARIES}
+)
+
+surgsim_add_unit_tests(MouseDeviceTest)
+
+set_target_properties(MouseDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Mouse/UnitTests/MouseDeviceTest.cpp b/SurgSim/Devices/Mouse/UnitTests/MouseDeviceTest.cpp
new file mode 100644
index 0000000..1f1f138
--- /dev/null
+++ b/SurgSim/Devices/Mouse/UnitTests/MouseDeviceTest.cpp
@@ -0,0 +1,88 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MouseDevice class.
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Mouse/MouseDevice.h"
+#include "SurgSim/Devices/Mouse/MouseScaffold.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+using SurgSim::Device::MouseDevice;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Testing::MockInputOutput;
+
+class MouseDeviceTest : public ::testing::Test
+{
+public:
+ static void update(std::shared_ptr<MouseDevice> device)
+ {
+ device->m_scaffold->updateDevice(0, 0, 0, 0, 0);
+ }
+};
+
+
+TEST_F(MouseDeviceTest, CreateInitializeAndDestroyDevice)
+{
+ std::shared_ptr<MouseDevice> device = std::make_shared<MouseDevice>("TestMouse");
+
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Mouse device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+
+ ASSERT_TRUE(device->finalize()) << "Device finalization failed";
+ EXPECT_FALSE(device->isInitialized());
+
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Mouse device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+
+ ASSERT_TRUE(device->finalize()) << "Device finalization failed";
+ EXPECT_FALSE(device->isInitialized());
+}
+
+TEST_F(MouseDeviceTest, InputConsumer)
+{
+ std::shared_ptr<MouseDevice> device = std::make_shared<MouseDevice>("TestMouse");
+ device->initialize();
+
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_TRUE(device->addInputConsumer(consumer));
+
+ MouseDeviceTest::update(device);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_1));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_2));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_3));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.scalars().hasData("x"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.scalars().hasData("y"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.integers().hasData("scrollDeltaX"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.integers().hasData("scrollDeltaY"));
+
+ device->finalize();
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Mouse/UnitTests/MouseScaffoldTest.cpp b/SurgSim/Devices/Mouse/UnitTests/MouseScaffoldTest.cpp
new file mode 100644
index 0000000..dac638b
--- /dev/null
+++ b/SurgSim/Devices/Mouse/UnitTests/MouseScaffoldTest.cpp
@@ -0,0 +1,55 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MouseScaffold class and its device interactions.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Devices/Mouse/MouseScaffold.h"
+
+using SurgSim::Device::MouseScaffold;
+
+TEST(MouseScaffoldTest, CreateAndDestroyScaffold)
+{
+ std::shared_ptr<MouseScaffold> scaffold = MouseScaffold::getOrCreateSharedInstance();
+ ASSERT_TRUE(nullptr != scaffold) << "The scaffold was not created!";
+ std::weak_ptr<MouseScaffold> scaffold1 = scaffold;
+ {
+ std::shared_ptr<MouseScaffold> stillHaveScaffold = scaffold1.lock();
+ EXPECT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ EXPECT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+ {
+ std::shared_ptr<MouseScaffold> sameScaffold = MouseScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, sameScaffold) << "Unable to get scaffold from class";
+ EXPECT_EQ(scaffold, sameScaffold) << "Scaffold mismatch!";
+ }
+
+ scaffold.reset();
+ {
+ std::shared_ptr<MouseScaffold> dontHaveScaffold = scaffold1.lock();
+ EXPECT_TRUE(nullptr == dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ }
+
+ scaffold = MouseScaffold::getOrCreateSharedInstance();
+ ASSERT_TRUE(nullptr != scaffold) << "The scaffold was not created the 2nd time!";
+ std::weak_ptr<MouseScaffold> scaffold2 = scaffold;
+ {
+ std::shared_ptr<MouseScaffold> stillHaveScaffold = scaffold2.lock();
+ ASSERT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ ASSERT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+}
\ No newline at end of file
diff --git a/SurgSim/Devices/Mouse/VisualTests/CMakeLists.txt b/SurgSim/Devices/Mouse/VisualTests/CMakeLists.txt
new file mode 100644
index 0000000..54ed59a
--- /dev/null
+++ b/SurgSim/Devices/Mouse/VisualTests/CMakeLists.txt
@@ -0,0 +1,35 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+set(UNIT_TEST_SOURCES
+ MouseVisualTests.cpp
+)
+
+set(UNIT_TEST_HEADERS
+)
+
+add_executable(MouseVisualTest
+ ${UNIT_TEST_SOURCES} ${UNIT_TEST_HEADERS})
+
+set(LIBS MouseDevice
+ SurgSimDataStructures
+ SurgSimInput
+ ${OPENSCENEGRAPH_LIBRARIES}
+)
+
+target_link_libraries(MouseVisualTest ${LIBS})
+
+# Put MouseVisualTest into folder "Devices"
+set_target_properties(MouseVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Mouse/VisualTests/MouseVisualTests.cpp b/SurgSim/Devices/Mouse/VisualTests/MouseVisualTests.cpp
new file mode 100644
index 0000000..a749f92
--- /dev/null
+++ b/SurgSim/Devices/Mouse/VisualTests/MouseVisualTests.cpp
@@ -0,0 +1,112 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <iostream>
+#include <map>
+#include <memory>
+#include <string>
+
+#include <osg/Camera>
+#include <osg/Geode>
+#include <osgText/Text>
+#include <osgViewer/Viewer>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Mouse/MouseDevice.h"
+#include "SurgSim/Devices/Mouse/OsgMouseHandler.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+
+
+
+using SurgSim::DataStructures::DataGroup;
+
+struct TestListener : public SurgSim::Input::InputConsumerInterface
+{
+ virtual void initializeInput(const std::string& device, const DataGroup& inputData) override
+ {
+ }
+
+ virtual void handleInput(const std::string& device, const DataGroup& inputData) override
+ {
+ bool button1, button2, button3;
+ double x, y;
+ int scrollDeltaX, scrollDeltaY;
+
+ if (inputData.booleans().get(SurgSim::DataStructures::Names::BUTTON_1, &button1))
+ {
+ std::cerr << "button1 = " << button1 << std::endl;
+ }
+ if (inputData.booleans().get(SurgSim::DataStructures::Names::BUTTON_2, &button2))
+ {
+ std::cerr << "button2 = " << button2 << std::endl;
+ }
+ if (inputData.booleans().get(SurgSim::DataStructures::Names::BUTTON_3, &button3))
+ {
+ std::cerr << "button3 = " << button3 << std::endl;
+ }
+ if (inputData.scalars().get("x", &x))
+ {
+ std::cerr << "x = " << x << std::endl;
+ }
+ if (inputData.scalars().get("y", &y))
+ {
+ std::cerr << "y = " << y << std::endl;
+ }
+ if (inputData.integers().get("scrollDeltaX", &scrollDeltaX))
+ {
+ std::cerr << "scrollDeltaX = " << scrollDeltaX << std::endl;
+ }
+ if (inputData.integers().get("scrollDeltaY", &scrollDeltaY))
+ {
+ std::cerr << "scrollDeltaY = " << scrollDeltaY << std::endl;
+ }
+
+ std::cerr << std::endl;
+ }
+};
+
+int main(int argc, char* argv[])
+{
+ auto toolDevice = std::make_shared<SurgSim::Device::MouseDevice>("Mouse");
+ toolDevice->initialize();
+
+ osg::ref_ptr<osgGA::GUIEventHandler> mouseHandler = toolDevice->getMouseHandler();
+ auto consumer = std::make_shared<TestListener>();
+ toolDevice->addInputConsumer(consumer);
+
+ osg::ref_ptr<osgText::Text> text = new osgText::Text;
+ text->setText("Move/click/drag mouse in\n\nthis window to verify that\n\nmouse driver works correctly.");
+ text->setPosition(osg::Vec3(0.0f, 300.0f, 0.0f));
+
+ osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+ geode->addDrawable(text);
+
+ osg::ref_ptr<osg::Camera> camera = new osg::Camera;
+ camera->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
+ camera->setProjectionMatrixAsOrtho2D(0, 600, 0, 400);
+ camera->getOrCreateStateSet()->setMode(GL_LIGHTING,osg::StateAttribute::OFF);
+ camera->addChild(geode);
+
+ osg::ref_ptr<osg::Group> group = new osg::Group;
+ group->addChild(camera);
+
+ osg::ref_ptr<osgViewer::Viewer> viewer = new osgViewer::Viewer;
+ viewer->setUpViewInWindow(400, 400, 640, 480);
+ viewer->addEventHandler(mouseHandler);
+ viewer->setSceneData(group);
+
+ viewer->run();
+ return 0;
+}
diff --git a/SurgSim/Devices/MultiAxis/BitSetBuffer.h b/SurgSim/Devices/MultiAxis/BitSetBuffer.h
new file mode 100644
index 0000000..2fadf81
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/BitSetBuffer.h
@@ -0,0 +1,133 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_BITSETBUFFER_H
+#define SURGSIM_DEVICES_MULTIAXIS_BITSETBUFFER_H
+
+#include <string>
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+/// A bit set corresponding to a contiguous memory buffer.
+///
+/// A std::bitset {\em almost} does everything we need, but we need to also access the storage as bytes.
+///
+/// The method names are generally stolen straight from std::bitset.
+///
+/// \tparam N The number of bits in the bit set.
+template <size_t N>
+class BitSetBuffer
+{
+public:
+ /// Create a bit buffer with all bits set to zero.
+ BitSetBuffer()
+ {
+ reset();
+ }
+
+ /// Create a bit buffer by copying another buffer.
+ BitSetBuffer(const BitSetBuffer& other) :
+ m_bytes(other.m_bytes)
+ {
+ }
+
+ /// Copy bit buffer contents from another buffer.
+ BitSetBuffer& operator=(const BitSetBuffer& other)
+ {
+ m_bytes = other.m_bytes;
+ return *this;
+ }
+
+ /// Set all bits in the buffer to on.
+ void set()
+ {
+ m_bytes.fill(~static_cast<value_type>(0));
+ }
+
+ /// Set the specified bit in the buffer to on.
+ /// \param pos The index of the bit to turn on.
+ void set(size_t pos)
+ {
+ SURGSIM_ASSERT(pos < NUM_BITS);
+ m_bytes[pos / ELEMENT_BITS] |= (1U << (pos % ELEMENT_BITS));
+ }
+
+ /// Reset all bits in the buffer to off.
+ void reset()
+ {
+ m_bytes.fill(0);
+ }
+
+ /// Reset the specified bit in the buffer to off.
+ /// \param pos The index of the bit to turn off.
+ void reset(size_t pos)
+ {
+ SURGSIM_ASSERT(pos < NUM_BITS);
+ m_bytes[pos / ELEMENT_BITS] &= ~(1U << (pos % ELEMENT_BITS));
+ }
+
+ /// Get the specified bit in the buffer.
+ /// \param pos The index of the bit to test.
+ bool test(size_t pos) const
+ {
+ SURGSIM_ASSERT(pos < NUM_BITS);
+ return ((m_bytes[pos / ELEMENT_BITS] & (1U << (pos % ELEMENT_BITS))) != 0);
+ }
+
+ /// Get a pointer to the buffer's storage.
+ void* getPointer()
+ {
+ return &(m_bytes[0]);
+ }
+
+ /// Get a pointer to the buffer's storage.
+ const void* getPointer() const
+ {
+ return &(m_bytes[0]);
+ }
+
+ /// Get the number of bits in the bit set.
+ static size_t size()
+ {
+ return NUM_BITS;
+ }
+
+ /// Get the number of bytes in the bit set.
+ static size_t sizeBytes()
+ {
+ return NUM_BYTES;
+ }
+
+private:
+ typedef unsigned char value_type;
+ static const size_t ELEMENT_BYTES = sizeof(value_type);
+ static const size_t ELEMENT_BITS = ELEMENT_BYTES * 8;
+ static_assert(ELEMENT_BITS == 8, "An unsigned char is not 8 bits?!");
+
+ static const size_t NUM_BITS = N;
+ static const size_t NUM_BYTES = (NUM_BITS + ELEMENT_BITS - 1) / ELEMENT_BITS;
+
+ std::array<value_type, NUM_BYTES> m_bytes;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_BITSETBUFFER_H
diff --git a/SurgSim/Devices/MultiAxis/CMakeLists.txt b/SurgSim/Devices/MultiAxis/CMakeLists.txt
new file mode 100644
index 0000000..d0832e0
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/CMakeLists.txt
@@ -0,0 +1,123 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+else(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ find_package(WDK REQUIRED)
+endif(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+
+include_directories("${CMAKE_CURRENT_SOURCE_DIR}")
+
+if(WDK_FOUND)
+ if(WDK_CAN_BUILD_DIRECTLY)
+ #message(STATUS "WDK does not need additional include paths; see the try_compile in FindWDK.cmake")
+ else(WDK_CAN_BUILD_DIRECTLY)
+ #message(STATUS "WDK was found, and needs the include directory '${WDK_INCLUDE_DIR}'.")
+ include_directories(
+ # This is a hack needed to get the Visual Studio includes into the
+ # include path ahead of the WDK ones, which is needed for the WDK
+ # 7.1.0 not to interfere with includes provided by Visual Studio.
+ # CMake will prefix this entry by the current path, but Visual
+ # Studio will split the path at the semicolon; the net effect is to
+ # add $(IncludePath) to the Visual Studio include directories.
+ # In cmake 2.8.11+, this hack generates a Policy CMP0021 warning due to relative path.
+ ".;$(IncludePath)"
+ # Now it's safe to add ${WDK_INCLUDE_DIR}.
+ "${WDK_INCLUDE_DIR}"
+ )
+ endif(WDK_CAN_BUILD_DIRECTLY)
+endif(WDK_FOUND)
+
+set(MULTIAXIS_DEVICE_SOURCES
+ MultiAxisDevice.cpp
+ RawMultiAxisDevice.cpp
+ RawMultiAxisScaffold.cpp
+ RawMultiAxisThread.cpp
+ SystemInputDeviceHandle.cpp
+)
+
+set(MULTIAXIS_DEVICE_HEADERS
+ CreateInputDeviceHandle.h
+ GetSystemError.h
+ MultiAxisDevice.h
+ RawMultiAxisDevice.h
+ RawMultiAxisScaffold.h
+ RawMultiAxisThread.h
+ SystemInputDeviceHandle.h
+)
+
+set(MULTIAXIS_DEVICE_LINUX_SOURCES
+ linux/CreateInputDeviceHandle.cpp
+ linux/FileDescriptor.cpp
+ linux/GetSystemError.cpp
+ linux/InputDeviceHandle.cpp
+)
+
+set(MULTIAXIS_DEVICE_LINUX_HEADERS
+ BitSetBuffer.h
+ linux/FileDescriptor.h
+ linux/InputDeviceHandle.h
+)
+
+set(MULTIAXIS_DEVICE_WDK_HID_SOURCES
+ win32/CreateInputDeviceHandle.cpp
+ win32/FileHandle.cpp
+ win32/GetSystemError.cpp
+ win32/WdkHidDeviceHandle.cpp
+)
+
+set(MULTIAXIS_DEVICE_WDK_HID_HEADERS
+ win32/FileHandle.h
+ win32/WdkHidDeviceHandle.h
+)
+
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ list(APPEND MULTIAXIS_DEVICE_SOURCES ${MULTIAXIS_DEVICE_LINUX_SOURCES})
+ list(APPEND MULTIAXIS_DEVICE_HEADERS ${MULTIAXIS_DEVICE_LINUX_HEADERS})
+elseif(WDK_FOUND)
+ list(APPEND MULTIAXIS_DEVICE_SOURCES ${MULTIAXIS_DEVICE_WDK_HID_SOURCES})
+ list(APPEND MULTIAXIS_DEVICE_HEADERS ${MULTIAXIS_DEVICE_WDK_HID_HEADERS})
+endif()
+
+# TODO(advornik): the installation should NOT copy all the headers...
+surgsim_add_library(
+ MultiAxisDevice
+ "${MULTIAXIS_DEVICE_SOURCES}" "${MULTIAXIS_DEVICE_HEADERS}"
+ SurgSim/Devices/MultiAxis
+)
+
+set(LIBS
+ SurgSimDeviceFilters
+ SurgSimInput
+ SurgSimFramework
+)
+
+if(WDK_FOUND)
+ list(APPEND LIBS ${WDK_LIBRARIES})
+endif()
+
+target_link_libraries(MultiAxisDevice ${LIBS}
+)
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+
+ if(GLUT_FOUND)
+ add_subdirectory(VisualTest)
+ endif(GLUT_FOUND)
+endif()
+
+# Put MultiAxisDevice into folder "Devices"
+set_target_properties(MultiAxisDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/MultiAxis/CreateInputDeviceHandle.h b/SurgSim/Devices/MultiAxis/CreateInputDeviceHandle.h
new file mode 100644
index 0000000..bb78b0a
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/CreateInputDeviceHandle.h
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_CREATEINPUTDEVICEHANDLE_H
+#define SURGSIM_DEVICES_MULTIAXIS_CREATEINPUTDEVICEHANDLE_H
+
+#include <string>
+#include <memory>
+#include <vector>
+
+namespace SurgSim
+{
+namespace Framework
+{
+class Logger;
+}; // namespace Framework
+
+namespace Device
+{
+class SystemInputDeviceHandle;
+
+
+/// Opens the given path and creates an access wrapper for the device.
+/// \param path Full pathname for the device.
+/// \param logger The logger to be used by the device.
+/// \return The created device object, or an empty unique_ptr on failure.
+std::unique_ptr<SystemInputDeviceHandle> createInputDeviceHandle(const std::string& path,
+ std::shared_ptr<SurgSim::Framework::Logger> logger);
+
+/// Enumerates input devices.
+/// \param logger The logger to be used during enumeration.
+/// \return A list of device paths.
+std::vector<std::string> enumerateInputDevicePaths(SurgSim::Framework::Logger* logger);
+
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_CREATEINPUTDEVICEHANDLE_H
diff --git a/SurgSim/Devices/MultiAxis/GetSystemError.h b/SurgSim/Devices/MultiAxis/GetSystemError.h
new file mode 100644
index 0000000..22fc030
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/GetSystemError.h
@@ -0,0 +1,43 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_GETSYSTEMERROR_H
+#define SURGSIM_DEVICES_MULTIAXIS_GETSYSTEMERROR_H
+
+#include <stdint.h>
+#include <string>
+
+namespace SurgSim
+{
+namespace Device
+{
+namespace Internal
+{
+/// Gets the most recent error code from the operating system.
+/// The error code will correspond to the status of the most recent failed (or, in some cases, successful) call
+/// to the operating system APIs from this thread.
+/// \return The OS-dependent system error code.
+int64_t getSystemErrorCode();
+
+/// Gets the system error text corresponding to the specified error code, or the most recent error text.
+/// \param errorCode (Optional) The error code. If omitted, the most recent OS error code will be used.
+/// \return The error text corresponding to the error code.
+std::string getSystemErrorText(int64_t errorCode = getSystemErrorCode());
+
+}; // namespace Internal
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_GETSYSTEMERROR_H
diff --git a/SurgSim/Devices/MultiAxis/MultiAxis.dox b/SurgSim/Devices/MultiAxis/MultiAxis.dox
new file mode 100644
index 0000000..2cc2477
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/MultiAxis.dox
@@ -0,0 +1,23 @@
+/*!
+
+\page MultiAxis MultiAxis: 3D Mouse Device (e.g, 3Dconnexion SpaceNavigator)
+
+3D mice (aka 3D motion controllers, 3D navigation devices, or 6 DOF devices) are input devices similar to typical computer mice, but enabling control of position and orientation. They are commonly used to operate CAD, 3D modeling, and animation applications.
+
+Supported devices:
+- 3Dconnexion
+ - The SpaceNavigator is tested, other models are expected to work.
+
+Dependencies:
+ - Linux: No requirements.
+ - If experiencing difficulties:
+ -# check the udev rules
+ - By default, regular users on most Linux systems DO NOT have the permissions necessary to access the multi-axis controller devices on their system. Fortunately, the permissions that are set on those devices are usually relatively easy to configure using udev rules.
+ - To allow all users direct access to various 3DConnexion devices, you can simply copy the rules file from the udev-rules directory into the system directory <tt>/etc/udev/rules.d</tt>. (You will need administrative privileges to do so.) For example, on many systems you can run: <tt>sudo cp SurgSim/Devices/MultiAxis/udev-rules/90-3dconnexion.rules /etc/udev/rules.d</tt>
+ - For other devices, or to set up the privileges differently, you will need to set up your own rules file.
+ -# try the drivers provided by 3Dconnexion http://www.3dconnexion.com/service/drivers.html
+ - Windows: Drivers from 3Dconnexion http://www.3dconnexion.com/service/drivers.html
+ - Visual Studio 2012+: No further requirements.
+ - Other build tools: May require Windows Driver Kit (WDK), available from http://www.microsoft.com/en-us/default.aspx
+
+*/
\ No newline at end of file
diff --git a/SurgSim/Devices/MultiAxis/MultiAxisDevice.cpp b/SurgSim/Devices/MultiAxis/MultiAxisDevice.cpp
new file mode 100644
index 0000000..9be17d0
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/MultiAxisDevice.cpp
@@ -0,0 +1,153 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/MultiAxisDevice.h"
+
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h"
+#include "SurgSim/Devices/DeviceFilters/PoseIntegrator.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+MultiAxisDevice::MultiAxisDevice(const std::string& uniqueName) :
+ m_name(uniqueName),
+ m_rawDevice(new RawMultiAxisDevice(uniqueName + "_RawBase"))
+{
+ m_filter = std::make_shared<PoseIntegrator>(uniqueName + "_Integrator");
+ m_filter->setNameForCallback(uniqueName); // the filter should make callbacks as the entire device
+
+ m_rawDevice->setPositionScale(defaultPositionScale());
+ m_rawDevice->setOrientationScale(defaultOrientationScale());
+ m_rawDevice->setAxisDominance(true);
+}
+
+
+MultiAxisDevice::~MultiAxisDevice()
+{
+}
+
+
+std::string MultiAxisDevice::getName() const
+{
+ return m_name;
+}
+
+
+bool MultiAxisDevice::initialize()
+{
+ m_rawDevice->addInputConsumer(m_filter);
+ m_rawDevice->setOutputProducer(m_filter);
+
+ // Elements need to be initialized, in proper order.
+ return m_filter->initialize() && m_rawDevice->initialize();
+}
+
+
+bool MultiAxisDevice::finalize()
+{
+ // All elements need to be finalized (beware of &&), in proper order.
+ bool deviceOk = m_rawDevice->finalize();
+ bool filterOk = m_filter->finalize();
+ return deviceOk && filterOk;
+}
+
+
+bool MultiAxisDevice::isInitialized() const
+{
+ return m_rawDevice->isInitialized();
+}
+
+bool MultiAxisDevice::addInputConsumer(std::shared_ptr<SurgSim::Input::InputConsumerInterface> inputConsumer)
+{
+ return m_filter->addInputConsumer(inputConsumer);
+}
+
+bool MultiAxisDevice::removeInputConsumer(std::shared_ptr<SurgSim::Input::InputConsumerInterface> inputConsumer)
+{
+ return m_filter->removeInputConsumer(inputConsumer);
+}
+
+bool MultiAxisDevice::setOutputProducer(std::shared_ptr<SurgSim::Input::OutputProducerInterface> outputProducer)
+{
+ return m_filter->setOutputProducer(outputProducer);
+}
+
+bool MultiAxisDevice::removeOutputProducer(std::shared_ptr<SurgSim::Input::OutputProducerInterface> outputProducer)
+{
+ return m_filter->removeOutputProducer(outputProducer);
+}
+
+bool MultiAxisDevice::hasOutputProducer()
+{
+ return m_filter->hasOutputProducer();
+}
+
+void MultiAxisDevice::setPositionScale(double scale)
+{
+ m_rawDevice->setPositionScale(scale);
+}
+
+
+double MultiAxisDevice::getPositionScale() const
+{
+ return m_rawDevice->getPositionScale();
+}
+
+
+void MultiAxisDevice::setOrientationScale(double scale)
+{
+ m_rawDevice->setOrientationScale(scale);
+}
+
+
+double MultiAxisDevice::getOrientationScale() const
+{
+ return m_rawDevice->getOrientationScale();
+}
+
+void MultiAxisDevice::setAxisDominance(bool onOff)
+{
+ m_rawDevice->setAxisDominance(onOff);
+}
+
+
+bool MultiAxisDevice::isUsingAxisDominance() const
+{
+ return m_rawDevice->isUsingAxisDominance();
+}
+
+double MultiAxisDevice::defaultPositionScale()
+{
+ return 0.00001; // The default position scale, in meters per tick.
+}
+
+double MultiAxisDevice::defaultOrientationScale()
+{
+ return 0.0001; // The default rotation scale, in radians per tick.
+}
+
+void MultiAxisDevice::setReset(const std::string& name)
+{
+ m_filter->setReset(name);
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/MultiAxisDevice.h b/SurgSim/Devices/MultiAxis/MultiAxisDevice.h
new file mode 100644
index 0000000..a95ff52
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/MultiAxisDevice.h
@@ -0,0 +1,164 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_MULTIAXISDEVICE_H
+#define SURGSIM_DEVICES_MULTIAXIS_MULTIAXISDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+class RawMultiAxisDevice;
+class PoseIntegrator;
+
+
+/// A class implementing the communication with a multi-axis controller input device, for example a 3DConnexion
+/// SpaceNavigator.
+///
+/// This object will integrate the output of the physical device, treating it as a differential device. In other
+/// words, holding the controller moves the pose and releasing the controller lets the pose hold steady in its
+/// new state.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Absolute device pose (units are ill-defined, but nominally meters). |
+/// | bool | "button1" | True if button 1 exists and is pressed. |
+/// | bool | "button2" | True if button 2 exists and is pressed. |
+/// | bool | "button3" | True if button 3 exists and is pressed. |
+/// | bool | "button4" | True if button 4 exists and is pressed. |
+///
+/// \par Application output used by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | bool | "led1" | If the device has at least one LED light, controls the first one. |
+///
+/// \sa RawMultiAxisDevice, SurgSim::Input::DeviceInterface
+class MultiAxisDevice : public SurgSim::Input::DeviceInterface
+{
+public:
+ /// Constructor.
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ explicit MultiAxisDevice(const std::string& uniqueName);
+
+ /// Destructor.
+ virtual ~MultiAxisDevice();
+
+ /// Get the device name.
+ /// \return The device name.
+ virtual std::string getName() const override;
+
+ /// Fully initialize the device.
+ /// When the manager object creates the device, the internal state of the device usually isn't fully
+ /// initialized yet. This method performs any needed initialization.
+ /// \return True on success.
+ virtual bool initialize() override;
+
+ /// Finalize (de-initialize) the device.
+ /// \return True on success.
+ virtual bool finalize() override;
+
+ /// Check whether this device is initialized.
+ /// \return True if initialized.
+ bool isInitialized() const;
+
+ /// Connect this device to an InputConsumerInterface, which will receive the data that comes from this device.
+ /// \param inputConsumer The InputConsumerInterface to connect with.
+ /// \return True if successful.
+ virtual bool addInputConsumer(std::shared_ptr<SurgSim::Input::InputConsumerInterface> inputConsumer) override;
+
+ /// Disconnect this device from an InputConsumerInterface, which will no longer receive data from this device.
+ /// \param inputConsumer The InputConsumerInterface to disconnect from.
+ /// \return True if successful.
+ virtual bool removeInputConsumer(std::shared_ptr<SurgSim::Input::InputConsumerInterface> inputConsumer) override;
+
+ /// Connect this device to an OutputProducerInterface, which will send data to this device.
+ /// \param outputProducer The OutputProducerInterface to connect with.
+ /// \return True if successful.
+ virtual bool setOutputProducer(std::shared_ptr<SurgSim::Input::OutputProducerInterface> outputProducer) override;
+
+ /// Disconnect this device from an OutputProducerInterface, which will no longer send data to this device.
+ /// \param outputProducer The OutputProducerInterface to disconnect from.
+ /// \return True if successful.
+ virtual bool removeOutputProducer(std::shared_ptr<SurgSim::Input::OutputProducerInterface> outputProducer) override;
+
+ /// Getter for whether or not this device is connected with an OutputProducerInterface.
+ /// \return True if an OutputProducerInterface is connected.
+ virtual bool hasOutputProducer() override;
+
+ /// Sets the position scale for this device.
+ /// The position scale controls how much the pose changes for a given device translation.
+ /// The default value for a raw device tries to correspond to the actual physical motion of the device.
+ void setPositionScale(double scale);
+
+ /// Gets the position scale for this device.
+ /// \return The position scale.
+ double getPositionScale() const;
+
+ /// Sets the orientation scale for this device.
+ /// The orientation scale controls how much the pose changes for a given device rotation.
+ /// \param scale The new scale.
+ /// \note The default value for a raw device tries to correspond to the actual physical motion of the device.
+ void setOrientationScale(double scale);
+
+ /// Gets the orientation scale for this device.
+ /// \return The orientation scale.
+ double getOrientationScale() const;
+
+ /// Turns on or off the axis dominance setting for this device.
+ /// When axis dominance is on, only one (the largest) of the 6 pure axis directions is allowed to be active.
+ /// In other words, the device will be translating in X, or in Y, or in Z, or rotating around X, or around Y,
+ /// or around Z; but only one of those at a time.
+ /// \param onOff Whether or not to use only the dominant axis.
+ void setAxisDominance(bool onOff);
+
+ /// Gets the axis dominance setting for this device.
+ /// \return True if using axis dominance.
+ bool isUsingAxisDominance() const;
+
+ /// Sets the string name of the boolean entry that will reset the pose to its initial value.
+ /// \param name The name of the NamedData<bool> entry, e.g., SurgSim::DataStructures::Names::BUTTON_1.
+ /// \sa PoseIntegrator::setReset
+ void setReset(const std::string& name);
+
+private:
+ /// Get the default position scale from device ticks to meters.
+ /// \return The default position scale, in meters per tick.
+ static double defaultPositionScale();
+
+ /// Get the default rotation from device ticks to radians.
+ /// \return The default rotation scale, in radians per tick.
+ static double defaultOrientationScale();
+
+ /// The device name.
+ std::string m_name;
+
+ /// The raw underlying device.
+ std::shared_ptr<RawMultiAxisDevice> m_rawDevice;
+
+ /// The pose integration filter.
+ std::shared_ptr<PoseIntegrator> m_filter;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_MULTIAXISDEVICE_H
diff --git a/SurgSim/Devices/MultiAxis/README.linux b/SurgSim/Devices/MultiAxis/README.linux
new file mode 100644
index 0000000..5f55886
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/README.linux
@@ -0,0 +1,14 @@
+By default, regular users on most Linux systems DO NOT have the
+permissions necessary to access the multi-axis controller devices on
+their system. Fortunately, the permissions that are set on those
+devices are usually relatively easy to configure using udev rules.
+
+To allow all users direct access to various 3DConnexion devices, you
+can simply copy the rules file from the udev-rules directory into the
+system directory /etc/udev/rules.d. (You will need administrative
+privileges to do so.) For example, on many systems you can run:
+
+ sudo cp udev-rules/90-3dconnexion.rules /etc/udev/rules.d
+
+For other devices, or to set up the privileges differently, you will
+need to set up your own rules file.
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.cpp b/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.cpp
new file mode 100644
index 0000000..6eda566
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.cpp
@@ -0,0 +1,132 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h"
+
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+RawMultiAxisDevice::RawMultiAxisDevice(const std::string& uniqueName) :
+ SurgSim::Input::CommonDevice(uniqueName, RawMultiAxisScaffold::buildDeviceInputData()),
+ m_positionScale(defaultPositionScale()),
+ m_orientationScale(defaultOrientationScale()),
+ m_useAxisDominance(false)
+{
+}
+
+
+RawMultiAxisDevice::~RawMultiAxisDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+
+bool RawMultiAxisDevice::initialize()
+{
+ SURGSIM_ASSERT(! isInitialized());
+ std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold);
+
+ if (! scaffold->registerDevice(this))
+ {
+ return false;
+ }
+
+ m_scaffold = std::move(scaffold);
+ m_scaffold->setPositionScale(this, m_positionScale);
+ m_scaffold->setOrientationScale(this, m_orientationScale);
+ m_scaffold->setAxisDominance(this, m_useAxisDominance);
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
+ return true;
+}
+
+
+bool RawMultiAxisDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized());
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ bool ok = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return ok;
+}
+
+
+bool RawMultiAxisDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+
+void RawMultiAxisDevice::setPositionScale(double scale)
+{
+ m_positionScale = scale;
+ if (m_scaffold)
+ {
+ m_scaffold->setPositionScale(this, m_positionScale);
+ }
+}
+
+
+double RawMultiAxisDevice::getPositionScale() const
+{
+ return m_positionScale;
+}
+
+
+void RawMultiAxisDevice::setOrientationScale(double scale)
+{
+ m_orientationScale = scale;
+ if (m_scaffold)
+ {
+ m_scaffold->setOrientationScale(this, m_orientationScale);
+ }
+}
+
+
+double RawMultiAxisDevice::getOrientationScale() const
+{
+ return m_orientationScale;
+}
+
+
+void RawMultiAxisDevice::setAxisDominance(bool onOff)
+{
+ m_useAxisDominance = onOff;
+ if (m_scaffold)
+ {
+ m_scaffold->setAxisDominance(this, m_useAxisDominance);
+ }
+}
+
+
+bool RawMultiAxisDevice::isUsingAxisDominance() const
+{
+ return m_useAxisDominance;
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h b/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h
new file mode 100644
index 0000000..e1c57ee
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h
@@ -0,0 +1,127 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISDEVICE_H
+#define SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Devices/MultiAxis/MultiAxisDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+class RawMultiAxisScaffold;
+
+
+/// A class implementing the communication with a multi-axis controller input device, for example a 3DConnexion
+/// SpaceNavigator.
+///
+/// This object will only generate raw output reported by the controller, which indicates the
+/// movement of the controller from its rest state. Normally, that result will need to be integrated to allow the
+/// controller to be treated as a differential device, where holding the controller moves the pose and releasing
+/// the controller lets the pose hold steady in its new state. The MultiAxisDevice class provides that.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Absolute device pose (units are ill-defined, but nominally meters). |
+/// | bool | "button1" | True if button 1 exists and is pressed. |
+/// | bool | "button2" | True if button 2 exists and is pressed. |
+/// | bool | "button3" | True if button 3 exists and is pressed. |
+/// | bool | "button4" | True if button 4 exists and is pressed. |
+///
+/// \par Application output used by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | bool | "led1" | If the device has at least one LED light, controls the first one. |
+///
+/// \sa MultiAxisDevice, SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class RawMultiAxisDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ ///
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ explicit RawMultiAxisDevice(const std::string& uniqueName);
+
+ /// Destructor.
+ virtual ~RawMultiAxisDevice();
+
+ virtual bool initialize() override;
+
+ virtual bool finalize() override;
+
+ /// Check whether this device is initialized.
+ bool isInitialized() const;
+
+ /// Sets the position scale for this device.
+ /// The position scale controls how much the pose changes for a given device translation.
+ /// The default value for a raw device tries to correspond to the actual physical motion of the device.
+ void setPositionScale(double scale);
+ /// Gets the position scale for this device.
+ double getPositionScale() const;
+
+ /// Sets the orientation scale for this device.
+ /// The orientation scale controls how much the pose changes for a given device rotation.
+ /// The default value for a raw device tries to correspond to the actual physical motion of the device.
+ void setOrientationScale(double scale);
+ /// Gets the orientation scale for this device.
+ double getOrientationScale() const;
+
+ /// Turns on or off the axis dominance setting for this device.
+ /// When axis dominance is on, only one (the largest) of the 6 pure axis directions is allowed to be active.
+ /// In other words, the device will be translating in X, or in Y, or in Z, or rotating around X, or around Y,
+ /// or around Z; but only one of those at a time.
+ void setAxisDominance(bool onOff);
+ /// Gets the axis dominance setting for this device.
+ bool isUsingAxisDominance() const;
+
+private:
+ // Returns the default position scale, in meters per tick.
+ static double defaultPositionScale()
+ {
+ // the position scale from Paul N's measurements of the SpaceNavigator; 1/16"/350 ticks
+ return 0.0000045;
+ }
+
+ // Returns the default rotation scale, in radians per tick.
+ static double defaultOrientationScale()
+ {
+ // the rotation scale from Paul N's measurements of the SpaceNavigator
+ return 0.0003;
+ }
+
+
+ friend SurgSim::Device::MultiAxisDevice::MultiAxisDevice(const std::string& uniqueName);
+ friend class RawMultiAxisScaffold;
+
+ std::shared_ptr<RawMultiAxisScaffold> m_scaffold;
+
+ /// Scale factor for the position axes; stored locally before the device is initialized.
+ double m_positionScale;
+ /// Scale factor for the orientation axes; stored locally before the device is initialized.
+ double m_orientationScale;
+ /// Controls whether dominance will be enabled; stored locally before the device is initialized.
+ bool m_useAxisDominance;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISDEVICE_H
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.cpp b/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.cpp
new file mode 100644
index 0000000..62465f4
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.cpp
@@ -0,0 +1,622 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h"
+
+#include <vector>
+#include <list>
+#include <array>
+#include <memory>
+#include <algorithm>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h"
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisThread.h"
+#include "SurgSim/Devices/MultiAxis/GetSystemError.h"
+#include "SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h"
+#include "SurgSim/Devices/MultiAxis/CreateInputDeviceHandle.h"
+#include "SurgSim/Devices/MultiAxis/BitSetBuffer.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector3f;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::RigidTransform3d;
+
+using SurgSim::Device::Internal::getSystemErrorCode;
+using SurgSim::Device::Internal::getSystemErrorText;
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+struct RawMultiAxisScaffold::DeviceData
+{
+public:
+ /// Initialize the data.
+ DeviceData(const std::string& path, RawMultiAxisDevice* device, std::unique_ptr<SystemInputDeviceHandle>&& handle) :
+ devicePath(path),
+ deviceObject(device),
+ thread(),
+ deviceHandle(std::move(handle)),
+ axisStates(initialAxisStates()),
+ buttonStates(initialButtonStates()),
+ coordinateSystemRotation(defaultCoordinateSystemRotation()),
+ positionScale(device->getPositionScale()),
+ orientationScale(device->getOrientationScale()),
+ useAxisDominance(device->isUsingAxisDominance())
+ {
+ }
+
+ // Initialize by moving the data from another object.
+ // Needed because Visual Studio 2010 doesn't support multi-argument emplace_back() for STL containers.
+ DeviceData(DeviceData&& other) :
+ devicePath(std::move(other.devicePath)),
+ deviceObject(std::move(other.deviceObject)),
+ thread(std::move(other.thread)),
+ deviceHandle(std::move(other.deviceHandle)),
+ axisStates(std::move(other.axisStates)),
+ buttonStates(std::move(other.buttonStates)),
+ coordinateSystemRotation(std::move(other.coordinateSystemRotation)),
+ positionScale(std::move(other.positionScale)),
+ orientationScale(std::move(other.orientationScale)),
+ useAxisDominance(std::move(other.useAxisDominance))
+ {
+ }
+
+ ~DeviceData()
+ {
+ }
+
+ // Returns the default coordinate system rotation matrix.
+ static SurgSim::Math::Matrix33d defaultCoordinateSystemRotation()
+ {
+ SurgSim::Math::Matrix33d coordinateSystemRotation;
+ // Make +Y point up (3DConnexion devices use +Z up)
+ coordinateSystemRotation <<
+ 1, 0, 0,
+ 0, 0, -1,
+ 0, 1, 0;
+ return coordinateSystemRotation;
+ }
+
+ static SystemInputDeviceHandle::AxisStates initialAxisStates()
+ {
+ SystemInputDeviceHandle::AxisStates states;
+ states.fill(0);
+ return states;
+ }
+
+ static SystemInputDeviceHandle::ButtonStates initialButtonStates()
+ {
+ SystemInputDeviceHandle::ButtonStates states;
+ states.fill(false);
+ return states;
+ }
+
+ /// The system device path corresponding to this device.
+ const std::string devicePath;
+ /// The corresponding device object.
+ RawMultiAxisDevice* const deviceObject;
+ /// Processing thread.
+ std::unique_ptr<RawMultiAxisThread> thread;
+ /// Device handle to read from.
+ std::unique_ptr<SystemInputDeviceHandle> deviceHandle;
+ /// Persistent axis states.
+ SystemInputDeviceHandle::AxisStates axisStates;
+ /// Persistent button states.
+ SystemInputDeviceHandle::ButtonStates buttonStates;
+ /// The rotation of the coordinate system (used to reorient, e.g. point +Y up)
+ SurgSim::Math::Matrix33d coordinateSystemRotation;
+ /// Scale factor for the position axes.
+ double positionScale;
+ /// Scale factor for the orientation axes.
+ double orientationScale;
+ /// Controls whether dominance will be enabled.
+ bool useAxisDominance;
+ /// The mutex that protects the externally modifiable parameters.
+ boost::mutex parametersMutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ DeviceData(const DeviceData&) /*= delete*/;
+ DeviceData& operator=(const DeviceData&) /*= delete*/;
+};
+
+struct RawMultiAxisScaffold::StateData
+{
+public:
+ /// Initialize the state.
+ StateData() : isApiInitialized(false)
+ {
+ }
+
+ /// True if the API has been initialized (and not finalized).
+ bool isApiInitialized;
+
+ /// The list of known devices.
+ std::list<std::unique_ptr<RawMultiAxisScaffold::DeviceData>> activeDeviceList;
+
+ /// The mutex that protects the list of known devices.
+ boost::mutex mutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ StateData(const StateData&) /*= delete*/;
+ StateData& operator=(const StateData&) /*= delete*/;
+};
+
+
+RawMultiAxisScaffold::RawMultiAxisScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
+ m_logger(logger), m_state(new StateData)
+{
+ if (! m_logger)
+ {
+ m_logger = SurgSim::Framework::Logger::getLogger("RawMultiAxis device");
+ m_logger->setThreshold(m_defaultLogLevel);
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "RawMultiAxis: Shared scaffold created.";
+}
+
+
+RawMultiAxisScaffold::~RawMultiAxisScaffold()
+{
+ // The following block controls the duration of the mutex being locked.
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (! m_state->activeDeviceList.empty())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "RawMultiAxis: Destroying scaffold while devices are active!?!";
+ for (auto it = m_state->activeDeviceList.begin(); it != m_state->activeDeviceList.end(); ++it)
+ {
+ if ((*it)->thread)
+ {
+ destroyPerDeviceThread(it->get());
+ }
+ }
+ m_state->activeDeviceList.clear();
+ }
+
+ if (m_state->isApiInitialized)
+ {
+ finalizeSdk();
+ }
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "RawMultiAxis: Shared scaffold destroyed.";
+}
+
+
+bool RawMultiAxisScaffold::registerDevice(RawMultiAxisDevice* device)
+{
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (! m_state->isApiInitialized)
+ {
+ if (! initializeSdk())
+ {
+ return false;
+ }
+ }
+ }
+
+ int numUsedDevicesSeen = 0;
+ bool deviceFound = findUnusedDeviceAndRegister(device, &numUsedDevicesSeen);
+ if (! deviceFound)
+ {
+ if (numUsedDevicesSeen > 0)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "RawMultiAxis: Failed to find any unused multi-axis controllers," <<
+ " out of " << numUsedDevicesSeen << " present!";
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "RawMultiAxis: Failed to find any multi-axis controllers." <<
+ " Is one plugged in? Are the permissions correct?";
+ }
+ return false;
+ }
+
+ return true;
+}
+
+
+bool RawMultiAxisScaffold::unregisterDevice(const RawMultiAxisDevice* const device)
+{
+ bool found = false;
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ if ((*matching)->thread)
+ {
+ destroyPerDeviceThread(matching->get());
+ }
+ m_state->activeDeviceList.erase(matching);
+ // the iterator is now invalid but that's OK
+ found = true;
+ }
+ }
+
+ if (! found)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "RawMultiAxis: Attempted to release a non-registered device.";
+ }
+ return found;
+}
+
+void RawMultiAxisScaffold::setPositionScale(const RawMultiAxisDevice* device, double scale)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
+ (*matching)->positionScale = scale;
+ }
+}
+
+void RawMultiAxisScaffold::setOrientationScale(const RawMultiAxisDevice* device, double scale)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
+ (*matching)->orientationScale = scale;
+ }
+}
+
+void RawMultiAxisScaffold::setAxisDominance(const RawMultiAxisDevice* device, bool onOff)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
+ (*matching)->useAxisDominance = onOff;
+ }
+}
+
+bool RawMultiAxisScaffold::runInputFrame(RawMultiAxisScaffold::DeviceData* info)
+{
+ info->deviceObject->pullOutput();
+ if (! updateDevice(info))
+ {
+ return false;
+ }
+ info->deviceObject->pushInput();
+ return true;
+}
+
+bool RawMultiAxisScaffold::runAfterLastFrame(RawMultiAxisScaffold::DeviceData* info)
+{
+ info->deviceHandle->prepareForShutdown();
+ return true;
+}
+
+static int findDominantAxis(const std::array<int, 6>& axes)
+{
+ int biggestAxis = 0;
+ int biggestValue = abs(axes[biggestAxis]);
+ for (int i = 1; i < 6; ++i)
+ {
+ int value = abs(axes[i]);
+ if (value > biggestValue)
+ {
+ biggestValue = value;
+ biggestAxis = i;
+ }
+ }
+ // Note that if a tie occurs, the 1st entry with the biggest absolute value wins !
+ return biggestAxis;
+}
+
+bool RawMultiAxisScaffold::updateDevice(RawMultiAxisScaffold::DeviceData* info)
+{
+ const SurgSim::DataStructures::DataGroup& outputData = info->deviceObject->getOutputData();
+
+ boost::lock_guard<boost::mutex> lock(info->parametersMutex);
+
+ bool ledState = false;
+ if (outputData.booleans().get("led1", &ledState))
+ {
+ static bool firstTimeWarning = true;
+ if (firstTimeWarning)
+ {
+ firstTimeWarning = false;
+ SURGSIM_LOG_CRITICAL(m_logger) << "RawMultiAxis: controlling LEDs is not supported yet!";
+ // TODO(advornik): We should implement LED control. But that probably doesn't mix well with blocking
+ // reads here, and would either need to be done in another thread, or we'd need to poll here.
+ }
+ }
+
+ bool didUpdate = false;
+ if (! info->deviceHandle->updateStates(&(info->axisStates), &(info->buttonStates), &didUpdate))
+ {
+ // If updateStates returns false, the device is no longer usable, so we exit and stop its update loop.
+ return false;
+ }
+
+ if (didUpdate)
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "RawMultiAxis: STATE " <<
+ std::setw(3) << info->axisStates[0] << " " << std::setw(3) << info->axisStates[1] << " " <<
+ std::setw(3) << info->axisStates[2] << " / " << std::setw(3) << info->axisStates[3] << " " <<
+ std::setw(3) << info->axisStates[4] << " " << std::setw(3) << info->axisStates[5] << " :" <<
+ (info->buttonStates[0] ? " X" : " _") << (info->buttonStates[1] ? " X" : " _") <<
+ (info->buttonStates[2] ? " X" : " _") << (info->buttonStates[3] ? " X" : " _");
+ }
+
+ // Deal with dominance and scaling.
+ // It would be neat to put dominance into a filter, outside of the raw multi-axis device... but it has to
+ // happen before filtering, and putting dominance AND filtering (and integrator) into components is too much.
+ Vector3d position;
+ Vector3d rotation;
+ if (! info->useAxisDominance)
+ {
+ position = Vector3d(info->axisStates[0], info->axisStates[1], info->axisStates[2]) * info->positionScale;
+ rotation = Vector3d(info->axisStates[3], info->axisStates[4], info->axisStates[5]) * info->orientationScale;
+ }
+ else
+ {
+ position.setZero();
+ rotation.setZero();
+ int index = findDominantAxis(info->axisStates);
+ if (index >= 0 && index < 3)
+ {
+ position[index] = info->axisStates[index] * info->positionScale;
+ }
+ else if (index >= 3 && index < 6)
+ {
+ rotation[index-3] = info->axisStates[index] * info->orientationScale;
+ }
+ }
+
+ // Fix up the coordinate system (3DConnexion devices use +Z up coordinates, we want +Y up).
+ position = info->coordinateSystemRotation * position;
+ rotation = info->coordinateSystemRotation * rotation;
+
+ // Convert to a pose.
+ Matrix33d orientation;
+ double angle = rotation.norm();
+ if (angle < 1e-9)
+ {
+ orientation.setIdentity();
+ }
+ else
+ {
+ orientation = SurgSim::Math::makeRotationMatrix(angle, Vector3d(rotation / angle));
+ }
+
+ RigidTransform3d pose;
+ pose.makeAffine();
+ pose.linear() = orientation;
+ pose.translation() = position;
+
+ // TODO(bert): this code should cache the access indices.
+ SurgSim::DataStructures::DataGroup& inputData = info->deviceObject->getInputData();
+ inputData.poses().set(SurgSim::DataStructures::Names::POSE, pose);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_1, info->buttonStates[0]);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_2, info->buttonStates[1]);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_3, info->buttonStates[2]);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_4, info->buttonStates[3]);
+
+ return true;
+}
+
+bool RawMultiAxisScaffold::initializeSdk()
+{
+ SURGSIM_ASSERT(! m_state->isApiInitialized);
+
+ // nothing to do!
+
+ m_state->isApiInitialized = true;
+ return true;
+}
+
+bool RawMultiAxisScaffold::finalizeSdk()
+{
+ SURGSIM_ASSERT(m_state->isApiInitialized);
+
+ // nothing to do!
+
+ m_state->isApiInitialized = false;
+ return true;
+}
+
+std::unique_ptr<SystemInputDeviceHandle> RawMultiAxisScaffold::openDevice(const std::string& path)
+{
+ std::unique_ptr<SystemInputDeviceHandle> handle = createInputDeviceHandle(path, m_logger);
+ if (! handle)
+ {
+ int64_t error = getSystemErrorCode();
+ SURGSIM_LOG_INFO(m_logger) << "RawMultiAxis: Could not open device " << path << ": error " << error <<
+ ", " << getSystemErrorText(error);
+ }
+ return std::move(handle);
+}
+
+bool RawMultiAxisScaffold::findUnusedDeviceAndRegister(RawMultiAxisDevice* device, int* numUsedDevicesSeen)
+{
+ *numUsedDevicesSeen = 0;
+
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ // Make sure the object is unique.
+ auto sameObject = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ SURGSIM_ASSERT(sameObject == m_state->activeDeviceList.end()) << "RawMultiAxis: Tried to register a device" <<
+ " which is already present!";
+
+ // Make sure the name is unique.
+ const std::string deviceName = device->getName();
+ auto sameName = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [&deviceName](const std::unique_ptr<DeviceData>& info) { return info->deviceObject->getName() == deviceName; });
+ if (sameName != m_state->activeDeviceList.end())
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "RawMultiAxis: Tried to register a device when the same name is" <<
+ " already present!";
+ return false;
+ }
+
+ const std::vector<std::string> devicePaths = enumerateInputDevicePaths(m_logger.get());
+
+ for (auto it = devicePaths.cbegin(); it != devicePaths.cend(); ++it)
+ {
+ const std::string& devicePath = *it;
+
+ // Check if this is the device we wanted.
+
+ std::unique_ptr<SystemInputDeviceHandle> handle = openDevice(devicePath);
+ if (! handle)
+ {
+ // message was already printed
+ continue;
+ }
+
+ const std::string reportedName = handle->getDeviceName();
+
+ int vendorId, productId;
+ if (handle->getDeviceIds(&vendorId, &productId))
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "RawMultiAxis: Examining device " << devicePath << " (" <<
+ std::hex << std::setfill('0') << std::setw(4) << vendorId << ":" << std::setw(4) << productId << " " <<
+ reportedName << ")";
+ }
+ else
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "RawMultiAxis: Examining device " << devicePath << " (????:???? " <<
+ reportedName << ")";
+ }
+
+ if (! handle->hasTranslationAndRotationAxes())
+ {
+ continue;
+ }
+
+ handle.reset();
+
+ if (registerIfUnused(devicePath, device, numUsedDevicesSeen))
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool RawMultiAxisScaffold::registerIfUnused(const std::string& path, RawMultiAxisDevice* device,
+ int* numUsedDevicesSeen)
+{
+ // Check existing devices.
+ auto sameIndices = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [path](const std::unique_ptr<DeviceData>& info) { return (info->devicePath == path); });
+ if (sameIndices != m_state->activeDeviceList.end())
+ {
+ // We found an existing device for this controller.
+ ++(*numUsedDevicesSeen);
+ return false;
+ }
+
+ // The controller is not yet in use.
+
+ std::unique_ptr<SystemInputDeviceHandle> handle = openDevice(path);
+ if (! handle)
+ {
+ return false;
+ }
+
+ // Construct the object, start its thread, then move it to the list.
+ // The thread needs a device entry pointer, but the unique_ptr indirection means we have one to provide even
+ // before we've put an entry in the active device array.
+ std::unique_ptr<DeviceData> info(new DeviceData(path, device, std::move(handle)));
+
+ createPerDeviceThread(info.get());
+ SURGSIM_ASSERT(info->thread);
+ m_state->activeDeviceList.emplace_back(std::move(info));
+
+ return true;
+}
+
+bool RawMultiAxisScaffold::createPerDeviceThread(DeviceData* data)
+{
+ SURGSIM_ASSERT(! data->thread);
+
+ std::unique_ptr<RawMultiAxisThread> thread(new RawMultiAxisThread(this, data));
+ thread->start();
+ data->thread = std::move(thread);
+
+ return true;
+}
+
+bool RawMultiAxisScaffold::destroyPerDeviceThread(DeviceData* data)
+{
+ SURGSIM_ASSERT(data->thread);
+
+ std::unique_ptr<RawMultiAxisThread> thread = std::move(data->thread);
+ thread->stop();
+ thread.reset();
+
+ return true;
+}
+
+SurgSim::DataStructures::DataGroup RawMultiAxisScaffold::buildDeviceInputData()
+{
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_1);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_2);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_3);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_4);
+ return builder.createData();
+}
+
+std::shared_ptr<RawMultiAxisScaffold> RawMultiAxisScaffold::getOrCreateSharedInstance()
+{
+ static SurgSim::Framework::SharedInstance<RawMultiAxisScaffold> sharedInstance;
+ return sharedInstance.get();
+}
+
+void RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel)
+{
+ m_defaultLogLevel = logLevel;
+}
+
+SurgSim::Framework::LogLevel RawMultiAxisScaffold::m_defaultLogLevel = SurgSim::Framework::LOG_LEVEL_INFO;
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h b/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h
new file mode 100644
index 0000000..5f8c64a
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h
@@ -0,0 +1,168 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISSCAFFOLD_H
+#define SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISSCAFFOLD_H
+
+#include <memory>
+#include <vector>
+
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+class RawMultiAxisDevice;
+class RawMultiAxisThread;
+class SystemInputDeviceHandle;
+
+/// A class that implements the behavior of RawMultiAxisDevice objects.
+///
+/// \sa SurgSim::Device::RawMultiAxisDevice
+class RawMultiAxisScaffold
+{
+public:
+ /// Constructor.
+ /// \param logger (optional) The logger to be used for the scaffold object and the devices it manages.
+ /// If unspecified or empty, a console logger will be created and used.
+ explicit RawMultiAxisScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger = nullptr);
+
+ /// Destructor.
+ ~RawMultiAxisScaffold();
+
+ /// Gets the logger used by this object and the devices it manages.
+ /// \return The logger.
+ std::shared_ptr<SurgSim::Framework::Logger> getLogger() const
+ {
+ return m_logger;
+ }
+
+ /// Gets or creates the scaffold shared by all RawMultiAxisDevice instances.
+ /// The scaffold is managed using a SingleInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<RawMultiAxisScaffold> getOrCreateSharedInstance();
+
+ /// Sets the default log level.
+ /// Has no effect unless called before a scaffold is created (i.e. before the first device).
+ /// \param logLevel The log level.
+ static void setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel);
+
+private:
+ /// Internal shared state data type.
+ struct StateData;
+ /// Interal per-device information.
+ struct DeviceData;
+
+ friend class RawMultiAxisDevice;
+ friend class RawMultiAxisThread;
+ friend struct StateData;
+
+ /// Registers the specified device object.
+ /// If successful, the device object will become connected to an unused hardware device.
+ ///
+ /// \param device The device object to be used, which should have a unique name.
+ /// \return True if the initialization succeeds, false if it fails.
+ bool registerDevice(RawMultiAxisDevice* device);
+
+ /// Unregisters the specified device object.
+ /// The corresponding hardware device will become unused, and can be re-registered later.
+ ///
+ /// \param device The device object.
+ /// \return true on success, false on failure.
+ bool unregisterDevice(const RawMultiAxisDevice* device);
+
+ /// Sets the position scale for this device.
+ void setPositionScale(const RawMultiAxisDevice* device, double scale);
+
+ /// Sets the orientation scale for this device.
+ void setOrientationScale(const RawMultiAxisDevice* device, double scale);
+
+ /// Turns on or off the axis dominance setting for this device.
+ void setAxisDominance(const RawMultiAxisDevice* device, bool onOff);
+
+ /// Executes the operations for a single input frame for a single device.
+ /// Should only be called from the context of the input loop thread.
+ /// \param info The internal device data.
+ /// \return true on success.
+ bool runInputFrame(DeviceData* info);
+
+ /// Executes the operations after the last input frame, as the device input loop thread is shutting down.
+ /// Should only be called from the context of the input loop thread.
+ /// \param info The internal device data.
+ /// \return true on success.
+ bool runAfterLastFrame(DeviceData* info);
+
+ /// Updates the device information for a single device.
+ /// \return true on success.
+ bool updateDevice(DeviceData* info);
+
+ /// Initializes the RawMultiAxis SDK.
+ /// \return true on success.
+ bool initializeSdk();
+
+ /// Finalizes (de-initializes) the RawMultiAxis SDK.
+ /// \return true on success.
+ bool finalizeSdk();
+
+ /// Scans hardware that is present in the system, and if an unused device is found, register an object for it.
+ ///
+ /// \param device The device object to register if an unused device is found.
+ /// \param [out] numUsedDevicesSeen The number of devices that were found during the scan but were already
+ /// in use. Can be used if the scan fails to determine the error message that should be displayed.
+ /// \return true on success.
+ bool findUnusedDeviceAndRegister(RawMultiAxisDevice* device, int* numUsedDevicesSeen);
+
+ /// Register a device object given a device path, if the same path is not already in use.
+ ///
+ /// \param path A unique system path that can be used to communicate with the device.
+ /// \param device The device object to register if the index pair is in fact unused.
+ /// \param [in,out] numUsedDevicesSeen The number of devices that were found during the scan but were
+ /// already in use; incremented when an unused device is seen.
+ /// \return true on success.
+ bool registerIfUnused(const std::string& path, RawMultiAxisDevice* device, int* numUsedDevicesSeen);
+
+ /// Creates the input loop thread.
+ /// \return true on success.
+ bool createPerDeviceThread(DeviceData* data);
+
+ /// Destroys the input loop thread.
+ /// \return true on success.
+ bool destroyPerDeviceThread(DeviceData* data);
+
+ /// Opens the specified device.
+ /// \return The system-specific wrapper for the device.
+ std::unique_ptr<SystemInputDeviceHandle> openDevice(const std::string& path);
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static SurgSim::DataStructures::DataGroup buildDeviceInputData();
+
+
+
+ /// Logger used by the scaffold and all devices.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+ /// Internal scaffold state.
+ std::unique_ptr<StateData> m_state;
+
+ /// The default logging level.
+ static SurgSim::Framework::LogLevel m_defaultLogLevel;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISSCAFFOLD_H
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisThread.cpp b/SurgSim/Devices/MultiAxis/RawMultiAxisThread.cpp
new file mode 100644
index 0000000..5e352c7
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisThread.cpp
@@ -0,0 +1,56 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisThread.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+RawMultiAxisThread::RawMultiAxisThread(RawMultiAxisScaffold* scaffold, RawMultiAxisScaffold::DeviceData* deviceData) :
+ BasicThread("RawMultiAxis thread"),
+ m_scaffold(scaffold),
+ m_deviceData(deviceData)
+{
+ setRate(100.0);
+}
+
+RawMultiAxisThread::~RawMultiAxisThread()
+{
+}
+
+bool RawMultiAxisThread::doInitialize()
+{
+ return true;
+}
+
+bool RawMultiAxisThread::doStartUp()
+{
+ return true;
+}
+
+bool RawMultiAxisThread::doUpdate(double dt)
+{
+ return m_scaffold->runInputFrame(m_deviceData);
+}
+
+void RawMultiAxisThread::doBeforeStop()
+{
+ m_scaffold->runAfterLastFrame(m_deviceData);
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisThread.h b/SurgSim/Devices/MultiAxis/RawMultiAxisThread.h
new file mode 100644
index 0000000..5d498fb
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisThread.h
@@ -0,0 +1,54 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISTHREAD_H
+#define SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISTHREAD_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Framework/BasicThread.h"
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h"
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+/// A class implementing the thread context for sampling RawMultiAxis devices.
+/// \sa SurgSim::Device::RawMultiAxisScaffold
+class RawMultiAxisThread : public SurgSim::Framework::BasicThread
+{
+public:
+ explicit RawMultiAxisThread(RawMultiAxisScaffold* scaffold, RawMultiAxisScaffold::DeviceData* deviceData);
+
+ virtual ~RawMultiAxisThread();
+
+protected:
+ virtual bool doInitialize() override;
+ virtual bool doStartUp() override;
+ virtual bool doUpdate(double dt) override;
+ virtual void doBeforeStop() override;
+
+private:
+ RawMultiAxisScaffold* m_scaffold;
+ RawMultiAxisScaffold::DeviceData* m_deviceData;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISTHREAD_H
diff --git a/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.cpp b/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.cpp
new file mode 100644
index 0000000..25b6828
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.cpp
@@ -0,0 +1,38 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+SystemInputDeviceHandle::SystemInputDeviceHandle()
+{
+}
+
+SystemInputDeviceHandle::~SystemInputDeviceHandle()
+{
+}
+
+void SystemInputDeviceHandle::prepareForShutdown()
+{
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h b/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h
new file mode 100644
index 0000000..516711a
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h
@@ -0,0 +1,88 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_SYSTEMINPUTDEVICEHANDLE_H
+#define SURGSIM_DEVICES_MULTIAXIS_SYSTEMINPUTDEVICEHANDLE_H
+
+#include <string>
+#include <memory>
+#include <array>
+#include <vector>
+
+namespace SurgSim
+{
+namespace Device
+{
+
+/// A wrapper for system-dependent access to an input/HID device.
+class SystemInputDeviceHandle
+{
+public:
+ /// The maximum number of axes supported by any device object.
+ static const size_t MAX_NUM_AXES = 6;
+ /// The maximum number of buttons supported by any device object.
+ static const size_t MAX_NUM_BUTTONS = 4;
+
+ /// Type used to store axis states.
+ typedef std::array<int, MAX_NUM_AXES> AxisStates;
+ /// Type used to store button states.
+ typedef std::array<bool, MAX_NUM_BUTTONS> ButtonStates;
+
+
+ /// Destructor.
+ virtual ~SystemInputDeviceHandle();
+
+ /// Gets the name returned by the operating system for this device.
+ /// \return The reported name, or "???" if no name information could be found.
+ virtual std::string getDeviceName() const = 0;
+
+ /// Gets the device identifiers.
+ /// \param [out] vendorId The USB or PCI vendor identifier.
+ /// \param [out] productId The USB or PCI product identifier.
+ /// \return true if it succeeds.
+ virtual bool getDeviceIds(int* vendorId, int* productId) const = 0;
+
+ /// Queries if this device has 3 translation and 3 rotation axes.
+ /// \return true if the desired axes are present.
+ virtual bool hasTranslationAndRotationAxes() const = 0;
+
+ /// Updates the axis and states from the device input, if any.
+ /// \param [in,out] axisStates The states for each axis of the device.
+ /// \param [in,out] buttonStates The states for each device button.
+ /// \param [out] updated True if any states were actually updated. (Note that even if this value is true, the
+ /// states may not have changed value; one or more states could have been updated to the same value.)
+ /// \return true if the operation was successful; false if the device is no longer in a usable state.
+ virtual bool updateStates(AxisStates* axisStates, ButtonStates* buttonStates, bool* updated) = 0;
+
+ /// Prepares the handle for sampling thread shutdown.
+ /// Should be called from the same thread that was calling updateStates, after the calls to updateStates
+ /// have been stopped, but before object destruction.
+ virtual void prepareForShutdown();
+
+protected:
+ /// Default constructor.
+ /// Cannot be called directly; see open and enumerate.
+ SystemInputDeviceHandle();
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ SystemInputDeviceHandle(const SystemInputDeviceHandle& other) /*= delete*/;
+ SystemInputDeviceHandle& operator=(const SystemInputDeviceHandle& other) /*= delete*/;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_SYSTEMINPUTDEVICEHANDLE_H
diff --git a/SurgSim/Devices/MultiAxis/UnitTests/CMakeLists.txt b/SurgSim/Devices/MultiAxis/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..0882483
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/UnitTests/CMakeLists.txt
@@ -0,0 +1,37 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ MultiAxisDeviceTest.cpp
+ RawMultiAxisDeviceTest.cpp
+ RawMultiAxisScaffoldTest.cpp
+)
+
+set(LIBS
+ MultiAxisDevice
+)
+
+if(WDK_FOUND)
+ list(APPEND LIBS ${WDK_LIBRARIES})
+endif()
+
+surgsim_add_unit_tests(MultiAxisDeviceTest)
+
+set_target_properties(MultiAxisDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/MultiAxis/UnitTests/MultiAxisDeviceTest.cpp b/SurgSim/Devices/MultiAxis/UnitTests/MultiAxisDeviceTest.cpp
new file mode 100644
index 0000000..c8220a0
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/UnitTests/MultiAxisDeviceTest.cpp
@@ -0,0 +1,204 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MultiAxisDevice class.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/MultiAxis/MultiAxisDevice.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Device::MultiAxisDevice;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Testing::MockInputOutput;
+
+
+TEST(MultiAxisDeviceTest, CreateUninitializedDevice)
+{
+ std::shared_ptr<MultiAxisDevice> device = std::make_shared<MultiAxisDevice>("TestMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+}
+
+TEST(MultiAxisDeviceTest, CreateAndInitializeDevice)
+{
+ std::shared_ptr<MultiAxisDevice> device = std::make_shared<MultiAxisDevice>("TestMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a MultiAxis device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+TEST(MultiAxisDeviceTest, Name)
+{
+ std::shared_ptr<MultiAxisDevice> device = std::make_shared<MultiAxisDevice>("TestMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_EQ("TestMultiAxis", device->getName());
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a MultiAxis device plugged in?";
+ EXPECT_EQ("TestMultiAxis", device->getName());
+}
+
+static void testCreateDeviceSeveralTimes(bool doSleep)
+{
+ for (int i = 0; i < 6; ++i)
+ {
+ std::shared_ptr<MultiAxisDevice> device = std::make_shared<MultiAxisDevice>("TestMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a MultiAxis device plugged in?";
+ if (doSleep)
+ {
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(100));
+ }
+ // the device will be destroyed here
+ }
+}
+
+TEST(MultiAxisDeviceTest, CreateDeviceSeveralTimes)
+{
+ testCreateDeviceSeveralTimes(true);
+}
+
+TEST(MultiAxisDeviceTest, CreateSeveralDevices)
+{
+ std::shared_ptr<MultiAxisDevice> device1 = std::make_shared<MultiAxisDevice>("MultiAxis1");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a MultiAxis device plugged in?";
+
+ // We can't check what happens with the scaffolds, since those are no longer a part of the device's API...
+
+ std::shared_ptr<MultiAxisDevice> device2 = std::make_shared<MultiAxisDevice>("MultiAxis2");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ if (! device2->initialize())
+ {
+ std::cerr << "[Warning: second MultiAxis controller did not come up; is it plugged in?]" << std::endl;
+ }
+}
+
+TEST(MultiAxisDeviceTest, CreateDevicesWithSameName)
+{
+ std::shared_ptr<MultiAxisDevice> device1 = std::make_shared<MultiAxisDevice>("MultiAxis");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a MultiAxis device plugged in?";
+
+ std::shared_ptr<MultiAxisDevice> device2 = std::make_shared<MultiAxisDevice>("MultiAxis");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate name.";
+}
+
+// Create a string representation from an int.
+// C++11 adds std::to_string() to do this for various types, but VS2010 only half-supports that.
+template <typename T>
+inline std::string makeString(T value)
+{
+ std::ostringstream out;
+ out << value;
+ return out.str();
+}
+
+TEST(MultiAxisDeviceTest, CreateAllDevices)
+{
+ std::vector<std::shared_ptr<MultiAxisDevice>> devices;
+
+ for (int i = 1; ; ++i)
+ {
+ std::string name = "MultiAxis" + makeString(i);
+ std::shared_ptr<MultiAxisDevice> device = std::make_shared<MultiAxisDevice>(name);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ if (! device->initialize())
+ {
+ break;
+ }
+ devices.emplace_back(std::move(device));
+ }
+
+ std::cout << devices.size() << " devices initialized." << std::endl;
+ ASSERT_GT(devices.size(), 0U) << "Initialization failed. Is a MultiAxis device plugged in?";
+}
+
+TEST(MultiAxisDeviceTest, InputConsumer)
+{
+ std::shared_ptr<MultiAxisDevice> device = std::make_shared<MultiAxisDevice>("TestMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a MultiAxis device plugged in?";
+
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer->m_numTimesInitializedInput);
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+ EXPECT_EQ(0, consumer->m_numTimesInitializedInput);
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device->addInputConsumer(consumer));
+
+ // Adding the same input consumer again should fail.
+ EXPECT_FALSE(device->addInputConsumer(consumer));
+
+ // Sleep for a second, to see how many times the consumer is invoked.
+ // (A MultiAxis device updates internally at 60Hz, but our code currently runs at 100Hz to reduce latency.)
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeInputConsumer(consumer));
+
+ // Removing the same input consumer again should fail.
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+
+ // Check the number of invocations.
+ EXPECT_EQ(1, consumer->m_numTimesInitializedInput);
+ EXPECT_GE(consumer->m_numTimesReceivedInput, 90);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 110);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData(SurgSim::DataStructures::Names::POSE));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_1));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_2));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_3));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_4));
+}
+
+TEST(MultiAxisDeviceTest, OutputProducer)
+{
+ std::shared_ptr<MultiAxisDevice> device = std::make_shared<MultiAxisDevice>("TestMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a MultiAxis device plugged in?";
+
+ std::shared_ptr<MockInputOutput> producer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_TRUE(device->setOutputProducer(producer));
+
+ // Sleep for a second, to see how many times the producer is invoked.
+ // (A MultiAxis device is does not request any output.)
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeOutputProducer(producer));
+
+ // Removing the same input producer again should fail.
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+
+ // Check the number of invocations.
+ EXPECT_GE(producer->m_numTimesRequestedOutput, 90);
+ EXPECT_LE(producer->m_numTimesRequestedOutput, 110);
+}
diff --git a/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisDeviceTest.cpp b/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisDeviceTest.cpp
new file mode 100644
index 0000000..c7f7559
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisDeviceTest.cpp
@@ -0,0 +1,214 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 RawMultiAxisDevice class.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h"
+//#include "SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h" // only needed if calling setDefaultLogLevel()
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Device::RawMultiAxisDevice;
+using SurgSim::Device::RawMultiAxisScaffold;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Testing::MockInputOutput;
+
+TEST(RawMultiAxisDeviceTest, CreateUninitializedDevice)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+}
+
+TEST(RawMultiAxisDeviceTest, CreateAndInitializeDevice)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+TEST(RawMultiAxisDeviceTest, Name)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_EQ("TestRawMultiAxis", device->getName());
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+ EXPECT_EQ("TestRawMultiAxis", device->getName());
+}
+
+static void testCreateDeviceSeveralTimes(bool doSleep)
+{
+ for (int i = 0; i < 6; ++i)
+ {
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+ if (doSleep)
+ {
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(100));
+ }
+ // the device will be destroyed here
+ }
+}
+
+TEST(RawMultiAxisDeviceTest, CreateDeviceSeveralTimes)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ testCreateDeviceSeveralTimes(true);
+}
+
+TEST(RawMultiAxisDeviceTest, CreateSeveralDevices)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<RawMultiAxisDevice> device1 = std::make_shared<RawMultiAxisDevice>("RawMultiAxis1");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+
+ // We can't check what happens with the scaffolds, since those are no longer a part of the device's API...
+
+ std::shared_ptr<RawMultiAxisDevice> device2 = std::make_shared<RawMultiAxisDevice>("RawMultiAxis2");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ if (! device2->initialize())
+ {
+ std::cerr << "[Warning: second RawMultiAxis controller did not come up; is it plugged in?]" << std::endl;
+ }
+}
+
+TEST(RawMultiAxisDeviceTest, CreateDevicesWithSameName)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<RawMultiAxisDevice> device1 = std::make_shared<RawMultiAxisDevice>("RawMultiAxis");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+
+ std::shared_ptr<RawMultiAxisDevice> device2 = std::make_shared<RawMultiAxisDevice>("RawMultiAxis");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate name.";
+}
+
+// Create a string representation from an int.
+// C++11 adds std::to_string() to do this for various types, but VS2010 only half-supports that.
+template <typename T>
+inline std::string makeString(T value)
+{
+ std::ostringstream out;
+ out << value;
+ return out.str();
+}
+
+TEST(RawMultiAxisDeviceTest, CreateAllDevices)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::vector<std::shared_ptr<RawMultiAxisDevice>> devices;
+
+ for (int i = 1; ; ++i)
+ {
+ std::string name = "RawMultiAxis" + makeString(i);
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>(name);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ if (! device->initialize())
+ {
+ break;
+ }
+ devices.emplace_back(std::move(device));
+ }
+
+ std::cout << devices.size() << " devices initialized." << std::endl;
+ ASSERT_GT(devices.size(), 0U) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+}
+
+TEST(RawMultiAxisDeviceTest, InputConsumer)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer->m_numTimesInitializedInput);
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+ EXPECT_EQ(0, consumer->m_numTimesInitializedInput);
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device->addInputConsumer(consumer));
+
+ // Adding the same input consumer again should fail.
+ EXPECT_FALSE(device->addInputConsumer(consumer));
+
+ // Sleep for a second, to see how many times the consumer is invoked.
+ // (A RawMultiAxis device updates internally at 60Hz, but our code currently runs at 100Hz to reduce latency.)
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeInputConsumer(consumer));
+
+ // Removing the same input consumer again should fail.
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+
+ // Check the number of invocations.
+ EXPECT_EQ(1, consumer->m_numTimesInitializedInput);
+ EXPECT_GE(consumer->m_numTimesReceivedInput, 90);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 110);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData(SurgSim::DataStructures::Names::POSE));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_1));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_2));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_3));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_4));
+}
+
+TEST(RawMultiAxisDeviceTest, OutputProducer)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+
+ std::shared_ptr<MockInputOutput> producer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_TRUE(device->setOutputProducer(producer));
+
+ // Sleep for a second, to see how many times the producer is invoked.
+ // (A RawMultiAxis device is does not request any output.)
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeOutputProducer(producer));
+
+ // Removing the same input producer again should fail.
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+
+ // Check the number of invocations.
+ EXPECT_GE(producer->m_numTimesRequestedOutput, 90);
+ EXPECT_LE(producer->m_numTimesRequestedOutput, 110);
+}
diff --git a/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisScaffoldTest.cpp b/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisScaffoldTest.cpp
new file mode 100644
index 0000000..51d8eea
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisScaffoldTest.cpp
@@ -0,0 +1,185 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 RawMultiAxisScaffold class and its device interactions.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h"
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+
+using SurgSim::Device::RawMultiAxisDevice;
+using SurgSim::Device::RawMultiAxisScaffold;
+
+TEST(RawMultiAxisScaffoldTest, CreateAndDestroyScaffold)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ std::weak_ptr<RawMultiAxisScaffold> scaffold1 = scaffold;
+ {
+ std::shared_ptr<RawMultiAxisScaffold> stillHaveScaffold = scaffold1.lock();
+ EXPECT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ EXPECT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+ {
+ std::shared_ptr<RawMultiAxisScaffold> sameScaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, sameScaffold) << "Unable to get scaffold from class";
+ EXPECT_EQ(scaffold, sameScaffold) << "Scaffold mismatch!";
+ }
+
+ scaffold.reset();
+ {
+ std::shared_ptr<RawMultiAxisScaffold> dontHaveScaffold = scaffold1.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ }
+
+ scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created the 2nd time!";
+ std::weak_ptr<RawMultiAxisScaffold> scaffold2 = scaffold;
+ {
+ std::shared_ptr<RawMultiAxisScaffold> stillHaveScaffold = scaffold2.lock();
+ ASSERT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ ASSERT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+}
+
+TEST(RawMultiAxisScaffoldTest, ScaffoldLifeCycle)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::weak_ptr<RawMultiAxisScaffold> lastScaffold;
+ {
+ std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ lastScaffold = scaffold;
+ }
+ {
+ std::shared_ptr<RawMultiAxisScaffold> dontHaveScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ lastScaffold.reset();
+ }
+
+ {
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_NE(nullptr, device) << "Creation failed. Is a RawMultiAxis device plugged in?";
+ // note: the device is NOT initialized!
+ {
+ std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<RawMultiAxisScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The device has not been initialized, so it should NOT be hanging on to the device!
+ {
+ std::shared_ptr<RawMultiAxisScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+ // the ("empty") device is about to get destroyed
+ }
+
+ {
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+ {
+ std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<RawMultiAxisScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The same scaffold is supposed to still be around because of the device
+ {
+ std::shared_ptr<RawMultiAxisScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+
+ std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ EXPECT_EQ(sameScaffold, scaffold);
+ }
+ // the device and the scaffold are about to get destroyed
+ }
+
+ {
+ std::shared_ptr<RawMultiAxisScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+
+ {
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Didn't this work a moment ago?";
+ std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+
+ std::shared_ptr<RawMultiAxisScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+}
+
+
+TEST(RawMultiAxisScaffoldTest, CreateDeviceSeveralTimes)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::weak_ptr<RawMultiAxisScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ EXPECT_EQ(nullptr, lastScaffold.lock());
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+ std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold;
+ // the device and the scaffold will be destroyed here
+ }
+}
+
+
+TEST(RawMultiAxisScaffoldTest, CreateDeviceSeveralTimesWithScaffoldRef)
+{
+ //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<RawMultiAxisScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ std::shared_ptr<RawMultiAxisDevice> device = std::make_shared<RawMultiAxisDevice>("TestRawMultiAxis");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a RawMultiAxis device plugged in?";
+ std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ if (! lastScaffold)
+ {
+ lastScaffold = scaffold;
+ }
+ EXPECT_EQ(lastScaffold, scaffold);
+ // the device will be destroyed here, but the scaffold stays around because we have a shared_ptr to it.
+ }
+}
diff --git a/SurgSim/Devices/MultiAxis/VisualTest/CMakeLists.txt b/SurgSim/Devices/MultiAxis/VisualTest/CMakeLists.txt
new file mode 100644
index 0000000..e27bac2
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/VisualTest/CMakeLists.txt
@@ -0,0 +1,68 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+)
+
+set(EXAMPLE_SOURCES
+ main.cpp
+)
+
+set(EXAMPLE_HEADERS
+)
+
+set(RAW_EXAMPLE_SOURCES
+ raw_test_main.cpp
+)
+
+set(RAW_EXAMPLE_HEADERS
+)
+
+set(LIBS
+ MultiAxisDevice
+ IdentityPoseDevice
+ VisualTestCommon
+ SurgSimInput
+ SurgSimFramework
+ SurgSimDataStructures
+ ${Boost_LIBRARIES}
+ ${OPENGL_LIBRARIES}
+)
+
+if(WDK_FOUND)
+ list(APPEND LIBS ${WDK_LIBRARIES})
+endif()
+
+add_executable(MultiAxisVisualTest
+ ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
+surgsim_show_ide_folders(
+ "${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
+target_link_libraries(MultiAxisVisualTest ${LIBS})
+
+add_executable(RawMultiAxisVisualTest
+ ${RAW_EXAMPLE_SOURCES} ${RAW_EXAMPLE_HEADERS})
+surgsim_show_ide_folders(
+ "${RAW_EXAMPLE_SOURCES}" "${RAW_EXAMPLE_HEADERS}")
+target_link_libraries(RawMultiAxisVisualTest ${LIBS})
+
+# Put all projects into folder "Devices"
+set_target_properties(RawMultiAxisVisualTest PROPERTIES FOLDER "Devices")
+set_target_properties(MultiAxisVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/MultiAxis/VisualTest/main.cpp b/SurgSim/Devices/MultiAxis/VisualTest/main.cpp
new file mode 100644
index 0000000..b7b1a44
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/VisualTest/main.cpp
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Devices/MultiAxis/MultiAxisDevice.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+
+#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
+
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Device::MultiAxisDevice;
+using SurgSim::Device::IdentityPoseDevice;
+
+
+int main(int argc, char** argv)
+{
+ std::shared_ptr<MultiAxisDevice> toolDevice = std::make_shared<MultiAxisDevice>("MultiAxisDevice");
+ //toolDevice->setPositionScale(0.0002);
+ //toolDevice->setOrientationScale(0.005);
+ //toolDevice->setAxisDominance(false);
+
+ // The square is controlled by a second device. For a simple test, we're using an IdentityPoseDevice--
+ // a pretend device that doesn't actually move.
+ std::shared_ptr<DeviceInterface> squareDevice = std::make_shared<IdentityPoseDevice>("IdentityPoseDevice");
+
+ runToolSquareTest(toolDevice, squareDevice,
+ //2345678901234567890123456789012345678901234567890123456789012345678901234567890
+ "Move the multi-axis device (e.g. 3DConnexion) to move the sphere tool.");
+
+ std::cout << std::endl << "Exiting." << std::endl;
+ // Cleanup and shutdown will happen automatically as objects go out of scope.
+
+ return 0;
+}
diff --git a/SurgSim/Devices/MultiAxis/VisualTest/raw_test_main.cpp b/SurgSim/Devices/MultiAxis/VisualTest/raw_test_main.cpp
new file mode 100644
index 0000000..a4be13f
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/VisualTest/raw_test_main.cpp
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+
+#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
+
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Device::RawMultiAxisDevice;
+using SurgSim::Device::IdentityPoseDevice;
+
+
+int main(int argc, char** argv)
+{
+ std::shared_ptr<RawMultiAxisDevice> toolDevice = std::make_shared<RawMultiAxisDevice>("RawMultiAxisDevice");
+ toolDevice->setPositionScale(0.0002);
+ toolDevice->setOrientationScale(0.005);
+ toolDevice->setAxisDominance(false);
+
+ // The square is controlled by a second device. For a simple test, we're using an IdentityPoseDevice--
+ // a pretend device that doesn't actually move.
+ std::shared_ptr<DeviceInterface> squareDevice = std::make_shared<IdentityPoseDevice>("IdentityPoseDevice");
+
+ runToolSquareTest(toolDevice, squareDevice,
+ //2345678901234567890123456789012345678901234567890123456789012345678901234567890
+ "Move the multi-axis device (e.g. 3DConnexion) to move the sphere tool.");
+
+ std::cout << std::endl << "Exiting." << std::endl;
+ // Cleanup and shutdown will happen automatically as objects go out of scope.
+
+ return 0;
+}
diff --git a/SurgSim/Devices/MultiAxis/linux/CreateInputDeviceHandle.cpp b/SurgSim/Devices/MultiAxis/linux/CreateInputDeviceHandle.cpp
new file mode 100644
index 0000000..b714e6a
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/linux/CreateInputDeviceHandle.cpp
@@ -0,0 +1,40 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/CreateInputDeviceHandle.h"
+
+#include "SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.h"
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+std::unique_ptr<SystemInputDeviceHandle> createInputDeviceHandle(const std::string& path,
+ std::shared_ptr<SurgSim::Framework::Logger> logger)
+{
+ return InputDeviceHandle::open(path, logger);
+}
+
+std::vector<std::string> enumerateInputDevicePaths(SurgSim::Framework::Logger* logger)
+{
+ return InputDeviceHandle::enumeratePaths(logger);
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/linux/FileDescriptor.cpp b/SurgSim/Devices/MultiAxis/linux/FileDescriptor.cpp
new file mode 100644
index 0000000..2762a97
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/linux/FileDescriptor.cpp
@@ -0,0 +1,166 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/linux/FileDescriptor.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <poll.h>
+
+#include <string>
+
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+FileDescriptor::FileDescriptor() :
+ m_descriptor(INVALID_VALUE),
+ m_canRead(false),
+ m_canWrite(false)
+{
+}
+
+FileDescriptor::FileDescriptor(FileDescriptor&& other) :
+ m_descriptor(other.m_descriptor),
+ m_canRead(other.m_canRead),
+ m_canWrite(other.m_canWrite)
+{
+ other.m_descriptor = INVALID_VALUE; // take ownership
+}
+
+FileDescriptor& FileDescriptor::operator=(FileDescriptor&& other)
+{
+ m_descriptor = other.m_descriptor;
+ m_canRead = other.m_canRead;
+ m_canWrite = other.m_canWrite;
+ other.m_descriptor = INVALID_VALUE; // take ownership
+ return *this;
+}
+
+FileDescriptor::~FileDescriptor()
+{
+ reset();
+}
+
+bool FileDescriptor::isValid() const
+{
+ return (m_descriptor != INVALID_VALUE);
+}
+
+bool FileDescriptor::canRead() const
+{
+ return isValid() && m_canRead;
+}
+
+bool FileDescriptor::canWrite() const
+{
+ return isValid() && m_canWrite;
+}
+
+int FileDescriptor::get() const
+{
+ SURGSIM_ASSERT(m_descriptor != INVALID_VALUE);
+ return m_descriptor;
+}
+
+bool FileDescriptor::openForReadingAndWriting(const std::string& path)
+{
+ reset();
+ m_descriptor = open(path.c_str(), O_RDWR);
+ m_canRead = true;
+ m_canWrite = true;
+ return isValid();
+}
+
+bool FileDescriptor::openForReading(const std::string& path)
+{
+ reset();
+ m_descriptor = open(path.c_str(), O_RDONLY);
+ m_canRead = true;
+ m_canWrite = false;
+ return isValid();
+}
+
+bool FileDescriptor::openForWriting(const std::string& path)
+{
+ reset();
+ m_descriptor = open(path.c_str(), O_WRONLY);
+ m_canRead = false;
+ m_canWrite = true;
+ return isValid();
+}
+
+bool FileDescriptor::openForReadingAndMaybeWriting(const std::string& path)
+{
+ if (! openForReadingAndWriting(path))
+ {
+ if (! openForReading(path))
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
+void FileDescriptor::reset()
+{
+ if (m_descriptor != INVALID_VALUE)
+ {
+ close(m_descriptor);
+ m_descriptor = INVALID_VALUE;
+ }
+}
+
+bool FileDescriptor::hasDataToRead() const
+{
+ if (! canRead())
+ {
+ return false;
+ }
+
+ struct pollfd pollData[1];
+ pollData[0].fd = m_descriptor;
+ pollData[0].events = POLLIN;
+
+ //const int timeoutMsec = 10;
+ const int nonBlockingOnly = 0;
+ int status = poll(pollData, 1, nonBlockingOnly);
+ return (status > 0);
+}
+
+bool FileDescriptor::readBytes(void* dataBuffer, size_t bytesToRead, size_t* bytesActuallyRead)
+{
+ SURGSIM_ASSERT(canRead());
+ ssize_t numBytesRead = read(m_descriptor, dataBuffer, bytesToRead);
+ if (numBytesRead < 0)
+ {
+ *bytesActuallyRead = 0;
+ return false;
+ }
+ else
+ {
+ *bytesActuallyRead = numBytesRead;
+ return true;
+ }
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/linux/FileDescriptor.h b/SurgSim/Devices/MultiAxis/linux/FileDescriptor.h
new file mode 100644
index 0000000..27c7c15
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/linux/FileDescriptor.h
@@ -0,0 +1,113 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_LINUX_FILEDESCRIPTOR_H
+#define SURGSIM_DEVICES_MULTIAXIS_LINUX_FILEDESCRIPTOR_H
+
+#include <string>
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+/// A wrapper for an UNIX-style integer file descriptor.
+/// Allows callers to implement RAII-style resource management.
+class FileDescriptor
+{
+public:
+ /// Default constructor.
+ /// Initializes the file descriptor to an invalid state.
+ FileDescriptor();
+
+ /// Move constructor.
+ /// \param [in,out] other The object to move. The original object will be invalidated.
+ FileDescriptor(FileDescriptor&& other);
+
+ /// Move assignment operator.
+ /// \param [in,out] other The object to move. The original object will be invalidated.
+ /// \return A reference to this object.
+ FileDescriptor& operator=(FileDescriptor&& other);
+
+ /// Destructor.
+ ~FileDescriptor();
+
+ /// Checks if the file descriptor is valid, i.e. has been opened.
+ /// \return true if valid, false if not.
+ bool isValid() const;
+
+ /// Determines if the file descriptor can be read from.
+ /// \return true if the descriptor has been open for reading.
+ bool canRead() const;
+
+ /// Determines if the file descriptor can be written to.
+ /// \return true if the descriptor has been open for writing.
+ bool canWrite() const;
+
+ /// Checks whether this object has data available to be read.
+ /// \return true if there is data currently available.
+ bool hasDataToRead() const;
+
+ /// Reads bytes from the file descriptor.
+ /// \param [out] dataBuffer Buffer to read into. Must have room for at least bytesToRead bytes of data.
+ /// \param bytesToRead The number of bytes to try reading. Actual number of bytes received may be smaller.
+ /// \param [out] bytesActuallyRead The number of bytes that were actually read into the buffer.
+ /// \return true if it succeeds, false if it fails.
+ bool readBytes(void* dataBuffer, size_t bytesToRead, size_t* bytesActuallyRead);
+
+ /// Gets the raw underlying OS file descriptor.
+ /// \return The raw file descriptor.
+ int get() const;
+
+ /// Attempts to open the file descriptor for reading and writing.
+ /// \param path Full pathname of the file.
+ /// \return true if it succeeds, false if it fails.
+ bool openForReadingAndWriting(const std::string& path);
+
+ /// Attempts to open the file descriptor for reading only.
+ /// \param path Full pathname of the file.
+ /// \return true if it succeeds, false if it fails.
+ bool openForReading(const std::string& path);
+
+ /// Attempts to open the file descriptor for writing only.
+ /// \param path Full pathname of the file.
+ /// \return true if it succeeds, false if it fails.
+ bool openForWriting(const std::string& path);
+
+ /// Attempts to open the file descriptor for reading and (if permissions allow it) writing.
+ /// \param path Full pathname of the file.
+ /// \return true if it succeeds, false if it fails.
+ bool openForReadingAndMaybeWriting(const std::string& path);
+
+ /// Resets the file descriptor back to an invalid state.
+ /// If the descriptor was open, it will be closed.
+ void reset();
+
+private:
+ FileDescriptor(const FileDescriptor& other) = delete;
+ FileDescriptor& operator=(const FileDescriptor& other) = delete;
+
+ static const int INVALID_VALUE = -1;
+
+ int m_descriptor;
+ bool m_canRead;
+ bool m_canWrite;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_LINUX_FILEDESCRIPTOR_H
diff --git a/SurgSim/Devices/MultiAxis/linux/GetSystemError.cpp b/SurgSim/Devices/MultiAxis/linux/GetSystemError.cpp
new file mode 100644
index 0000000..b740a2d
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/linux/GetSystemError.cpp
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/GetSystemError.h"
+
+#include <errno.h>
+#include <string.h>
+
+
+namespace SurgSim
+{
+namespace Device
+{
+namespace Internal
+{
+
+int64_t getSystemErrorCode()
+{
+ return errno;
+}
+
+
+// Helps retrieve the output location for the XSI version of strerror_r.
+static inline const char* systemErrorTextHelper(const char* buffer, int returnValue)
+{
+ if (returnValue != 0)
+ {
+ return nullptr;
+ }
+ return buffer;
+}
+
+// Helps retrieve the output location for the GNU version of strerror_r.
+static inline const char* systemErrorTextHelper(const char* buffer, const char* returnValue)
+{
+ return returnValue;
+}
+
+std::string getSystemErrorText(int64_t errorCode)
+{
+ const size_t BUFFER_SIZE = 1024;
+ char errorBuffer[BUFFER_SIZE];
+ errorBuffer[0] = '\0';
+
+ // Unfortunately, on Linux you can't really know if you will get the XSI version or the GNU version of
+ // strerror_r. Fortunately, the arguments are the same (only return type differs), so we can cheat.
+ const char* message = systemErrorTextHelper(errorBuffer, strerror_r(errorCode, errorBuffer, sizeof(errorBuffer)));
+ if (message == nullptr || message[0] == '\0')
+ {
+ snprintf(errorBuffer, sizeof(errorBuffer), "<system error %d>", static_cast<int>(errorCode));
+ message = errorBuffer;
+ }
+ return std::string(message);
+}
+
+}; // namespace Internal
+}; // namespace Device
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.cpp b/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.cpp
new file mode 100644
index 0000000..22277d4
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.cpp
@@ -0,0 +1,342 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.h"
+
+#include <linux/input.h>
+#include <sys/ioctl.h>
+#include <errno.h>
+
+#include "SurgSim/Devices/MultiAxis/GetSystemError.h"
+#include "SurgSim/Devices/MultiAxis/BitSetBuffer.h"
+#include "SurgSim/Devices/MultiAxis/linux/FileDescriptor.h"
+#include "SurgSim/Framework/Log.h"
+
+using SurgSim::Device::Internal::getSystemErrorCode;
+using SurgSim::Device::Internal::getSystemErrorText;
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+struct InputDeviceHandle::State
+{
+public:
+ explicit State(std::shared_ptr<SurgSim::Framework::Logger>&& logger_) :
+ logger(std::move(logger_)),
+ handle()
+ {
+ buttonCodes.fill(-1);
+ }
+
+ /// The logger to use.
+ std::shared_ptr<SurgSim::Framework::Logger> logger;
+ /// The underlying device file descriptor.
+ FileDescriptor handle;
+ /// Event library button code corresponding to each index.
+ std::array<int, SystemInputDeviceHandle::MAX_NUM_BUTTONS> buttonCodes;
+
+private:
+ // Prevent copy construction and copy assignment.
+ State(const State& other) = delete;
+ State& operator=(const State& other) = delete;
+};
+
+InputDeviceHandle::InputDeviceHandle(std::shared_ptr<SurgSim::Framework::Logger>&& logger) :
+ m_state(new InputDeviceHandle::State(std::move(logger)))
+{
+}
+
+InputDeviceHandle::~InputDeviceHandle()
+{
+}
+
+std::vector<std::string> InputDeviceHandle::enumeratePaths(SurgSim::Framework::Logger* logger)
+{
+ std::vector<std::string> results;
+
+ for (int i = 0; i < 100; ++i)
+ {
+ char devicePath[128];
+ snprintf(devicePath, sizeof(devicePath), "/dev/input/event%d", i);
+
+ FileDescriptor handle;
+ if (! handle.openForReadingAndMaybeWriting(devicePath))
+ {
+ int error = errno;
+ if (error != ENOENT)
+ {
+ SURGSIM_LOG_INFO(logger) << "InputDeviceHandle::enumeratePaths: Could not open device " << devicePath <<
+ ": error " << error << ", " << getSystemErrorText(error);
+ }
+ continue;
+ }
+
+ results.push_back(devicePath);
+ }
+
+ return results;
+}
+
+std::unique_ptr<InputDeviceHandle> InputDeviceHandle::open(
+ const std::string& path, std::shared_ptr<SurgSim::Framework::Logger> logger)
+{
+ std::unique_ptr<InputDeviceHandle> object(new InputDeviceHandle(std::move(logger)));
+ if (! object->m_state->handle.openForReadingAndMaybeWriting(path))
+ {
+ object.reset(); // could not open the device handle; destroy the object again
+ }
+ else
+ {
+ const std::vector<int> buttonCodeList = object->getDeviceButtonsAndKeys();
+
+ for (size_t i = 0; (i < object->m_state->buttonCodes.size()) && (i < buttonCodeList.size()); ++i)
+ {
+ object->m_state->buttonCodes[i] = buttonCodeList[i];
+ }
+ for (size_t i = buttonCodeList.size(); i < object->m_state->buttonCodes.size(); ++i)
+ {
+ object->m_state->buttonCodes[i] = -1;
+ }
+ }
+ return object;
+}
+
+std::string InputDeviceHandle::getDeviceName() const
+{
+ char reportedName[1024];
+ if (ioctl(m_state->handle.get(), EVIOCGNAME(sizeof(reportedName)), reportedName) < 0)
+ {
+ int error = errno;
+ SURGSIM_LOG_DEBUG(m_state->logger) << "InputDeviceHandle: ioctl(EVIOCGNAME): error " << error << ", " <<
+ getSystemErrorText(error);
+ snprintf(reportedName, sizeof(reportedName), "???");
+ }
+ else
+ {
+ reportedName[sizeof(reportedName)-1] = '\0';
+ }
+ return std::string(reportedName);
+}
+
+bool InputDeviceHandle::getDeviceIds(int* vendorId, int* productId) const
+{
+ struct input_id reportedId;
+ if (ioctl(m_state->handle.get(), EVIOCGID, &reportedId) < 0)
+ {
+ int error = errno;
+ SURGSIM_LOG_DEBUG(m_state->logger) << "InputDeviceHandle: ioctl(EVIOCGID): error " << error << ", " <<
+ getSystemErrorText(error);
+ *vendorId = *productId = -1;
+ return false;
+ }
+
+ *vendorId = reportedId.vendor;
+ *productId = reportedId.product;
+ return true;
+}
+
+bool InputDeviceHandle::hasAbsoluteTranslationAndRotationAxes() const
+{
+ BitSetBuffer<ABS_CNT> buffer;
+ if (ioctl(m_state->handle.get(), EVIOCGBIT(EV_ABS, buffer.sizeBytes()), buffer.getPointer()) == -1)
+ {
+ int error = errno;
+ SURGSIM_LOG_DEBUG(m_state->logger) << "InputDeviceHandle: ioctl(EVIOCGBIT(EV_ABS)): error " <<
+ error << ", " << getSystemErrorText(error);
+ return false;
+ }
+
+ if (! buffer.test(ABS_X) || ! buffer.test(ABS_Y) || ! buffer.test(ABS_Z) ||
+ ! buffer.test(ABS_RX) || ! buffer.test(ABS_RY) || ! buffer.test(ABS_RZ))
+ {
+ SURGSIM_LOG_DEBUG(m_state->logger) << "InputDeviceHandle: does not have the 6 absolute axes.";
+ return false;
+ }
+
+ int numIgnoredAxes = 0;
+ for (size_t i = 0; i < ABS_CNT; ++i)
+ {
+ if (buffer.test(i))
+ {
+ if ((i != ABS_X) && (i != ABS_Y) && (i != ABS_Z) && (i != ABS_RX) && (i != ABS_RY) && (i != ABS_RZ))
+ {
+ ++numIgnoredAxes;
+ }
+ }
+ }
+
+
+ if (numIgnoredAxes)
+ {
+ SURGSIM_LOG_INFO(m_state->logger) << "InputDeviceHandle: has absolute translation and rotation axes;" <<
+ " ignoring " << numIgnoredAxes << " additional axes.";
+ }
+
+ return true;
+}
+
+bool InputDeviceHandle::hasRelativeTranslationAndRotationAxes() const
+{
+ BitSetBuffer<REL_CNT> buffer;
+ if (ioctl(m_state->handle.get(), EVIOCGBIT(EV_REL, buffer.sizeBytes()), buffer.getPointer()) == -1)
+ {
+ int error = errno;
+ SURGSIM_LOG_DEBUG(m_state->logger) << "InputDeviceHandle: ioctl(EVIOCGBIT(EV_REL)): error " <<
+ error << ", " << getSystemErrorText(error);
+ return false;
+ }
+
+ if (! buffer.test(REL_X) || ! buffer.test(REL_Y) || ! buffer.test(REL_Z) ||
+ ! buffer.test(REL_RX) || ! buffer.test(REL_RY) || ! buffer.test(REL_RZ))
+ {
+ SURGSIM_LOG_DEBUG(m_state->logger) << "InputDeviceHandle: does not have the 6 relative axes.";
+ return false;
+ }
+
+ int numIgnoredAxes = 0;
+ for (size_t i = 0; i < REL_CNT; ++i)
+ {
+ if (buffer.test(i))
+ {
+ if ((i != REL_X) && (i != REL_Y) && (i != REL_Z) && (i != REL_RX) && (i != REL_RY) && (i != REL_RZ))
+ {
+ ++numIgnoredAxes;
+ }
+ }
+ }
+ if (numIgnoredAxes)
+ {
+ SURGSIM_LOG_INFO(m_state->logger) << "InputDeviceHandle: has relative translation and rotation axes;" <<
+ " ignoring " << numIgnoredAxes << " additional axes.";
+ }
+
+ return true;
+}
+
+bool InputDeviceHandle::hasTranslationAndRotationAxes() const
+{
+ return hasAbsoluteTranslationAndRotationAxes() || hasRelativeTranslationAndRotationAxes();
+}
+
+bool InputDeviceHandle::updateStates(AxisStates* axisStates, ButtonStates* buttonStates, bool* updated)
+{
+ *updated = false;
+
+ while (m_state->handle.hasDataToRead())
+ {
+ struct input_event event;
+ size_t numRead;
+ if (! m_state->handle.readBytes(&event, sizeof(event), &numRead))
+ {
+ int64_t error = getSystemErrorCode();
+ if (error == ENODEV)
+ {
+ SURGSIM_LOG_SEVERE(m_state->logger) <<
+ "InputDeviceHandle: read failed; device has been disconnected! (stopping)";
+ return false; // stop updating this device!
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_state->logger) << "InputDeviceHandle: read failed with error " <<
+ error << ", " << getSystemErrorText(error);
+ }
+ }
+ else if (numRead != sizeof(event))
+ {
+ SURGSIM_LOG_WARNING(m_state->logger) << "InputDeviceHandle: reading produced " << numRead <<
+ " bytes (expected " << sizeof(event) << ")";
+ }
+ else
+ {
+ if (event.type == EV_REL)
+ {
+
+ if (event.code >= REL_X && event.code < (REL_X+3)) // Assume that X, Y, Z are consecutive
+ {
+ (*axisStates)[0 + (event.code - REL_X)] = event.value;
+ *updated = true;
+ }
+ else if (event.code >= REL_RX && event.code < (REL_RX+3)) // Assume that RX, RY, RZ are consecutive
+ {
+ (*axisStates)[3 + (event.code - REL_RX)] = event.value;
+ *updated = true;
+ }
+ }
+ else if (event.type == EV_ABS)
+ {
+ if (event.code >= ABS_X && event.code < (ABS_X+3)) // Assume that X, Y, Z are consecutive
+ {
+ (*axisStates)[0 + (event.code - ABS_X)] = event.value;
+ *updated = true;
+ }
+ else if (event.code >= ABS_RX && event.code < (ABS_RX+3)) // Assume that RX, RY, RZ are consecutive
+ {
+ (*axisStates)[3 + (event.code - ABS_RX)] = event.value;
+ *updated = true;
+ }
+ }
+ else if (event.type == EV_KEY)
+ {
+ for (size_t i = 0; i < m_state->buttonCodes.size(); ++i)
+ {
+ if (event.code == m_state->buttonCodes[i])
+ {
+ (*buttonStates)[i] = (event.value != 0);
+ *updated = true;
+ break;
+ }
+ }
+ }
+ }
+ }
+ return true;
+}
+
+std::vector<int> InputDeviceHandle::getDeviceButtonsAndKeys()
+{
+ std::vector<int> result;
+ BitSetBuffer<KEY_CNT> buffer;
+ if (ioctl(m_state->handle.get(), EVIOCGBIT(EV_KEY, buffer.sizeBytes()), buffer.getPointer()) == -1)
+ {
+ int error = errno;
+ SURGSIM_LOG_DEBUG(m_state->logger) << "InputDeviceHandle: ioctl(EVIOCGBIT(EV_KEY)): error " <<
+ error << ", " << getSystemErrorText(error);
+ return result;
+ }
+
+ // Start listing buttons/keys from BTN_0; then go back and cover the earlier ones.
+ for (int i = BTN_0; i < KEY_CNT; ++i)
+ {
+ if (buffer.test(i))
+ {
+ result.push_back(i);
+ }
+ }
+ for (int i = 0; i < BTN_0; ++i)
+ {
+ if (buffer.test(i))
+ {
+ result.push_back(i);
+ }
+ }
+
+ return result;
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.h b/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.h
new file mode 100644
index 0000000..9df464a
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.h
@@ -0,0 +1,93 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_LINUX_INPUTDEVICEHANDLE_H
+#define SURGSIM_DEVICES_MULTIAXIS_LINUX_INPUTDEVICEHANDLE_H
+
+#include <string>
+#include <memory>
+#include <array>
+#include <vector>
+
+#include "SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h"
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+class Logger;
+}; // namespace Framework
+
+namespace Device
+{
+
+/// Access to an input/HID device using the Input API in Linux.
+/// \sa SystemInputDeviceHandle
+class InputDeviceHandle : public SystemInputDeviceHandle
+{
+public:
+ /// Destructor.
+ ~InputDeviceHandle();
+
+ /// Enumerates input devices.
+ /// \param logger The logger to be used during enumeration.
+ /// \return A list of device paths.
+ static std::vector<std::string> enumeratePaths(SurgSim::Framework::Logger* logger);
+
+ /// Opens the given path and creates an access wrapper for the device.
+ /// \param path Full pathname for the device.
+ /// \param logger The logger to be used by the device.
+ /// \return The created device object, or an empty unique_ptr on failure.
+ static std::unique_ptr<InputDeviceHandle> open(const std::string& path,
+ std::shared_ptr<SurgSim::Framework::Logger> logger);
+
+ virtual std::string getDeviceName() const override;
+
+ virtual bool getDeviceIds(int* vendorId, int* productId) const override;
+
+ virtual bool hasTranslationAndRotationAxes() const override;
+
+ virtual bool updateStates(AxisStates* axisStates, ButtonStates* buttonStates, bool* updated) override;
+
+private:
+ /// Constructor.
+ /// Cannot be called directly.
+ /// \sa open
+ explicit InputDeviceHandle(std::shared_ptr<SurgSim::Framework::Logger>&& logger);
+
+ // Prevent copy construction and copy assignment.
+ InputDeviceHandle(const InputDeviceHandle& other) = delete;
+ InputDeviceHandle& operator=(const InputDeviceHandle& other) = delete;
+
+ /// Gets the indices of the available device buttons.
+ /// \return a vector of indices.
+ std::vector<int> getDeviceButtonsAndKeys();
+
+ /// Query if this device has 3 translation and 3 rotation {\em absolute} axes.
+ /// \return true if the desired axes are present.
+ bool hasAbsoluteTranslationAndRotationAxes() const;
+ /// Query if this device has 3 translation and 3 rotation {\em relative} axes.
+ /// \return true if the desired axes are present.
+ bool hasRelativeTranslationAndRotationAxes() const;
+
+ struct State;
+ std::unique_ptr<State> m_state;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_LINUX_INPUTDEVICEHANDLE_H
diff --git a/SurgSim/Devices/MultiAxis/udev-rules/90-3dconnexion.rules b/SurgSim/Devices/MultiAxis/udev-rules/90-3dconnexion.rules
new file mode 100644
index 0000000..ba4ebe0
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/udev-rules/90-3dconnexion.rules
@@ -0,0 +1,17 @@
+# Rules for the Logitech 3DConnexion SpaceNavigator and related devices.
+# By default, any user can read from and write to the devices.
+# The ID mappings taken from
+# http://www.3dconnexion.com/index.php?id=200&faq_red=faq/27
+#
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c603", MODE="0666", GROUP="plugdev", SYMLINK+="input/spacemouse_plus", SYMLINK+="input/3dconnexion"
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c605", MODE="0666", GROUP="plugdev", SYMLINK+="input/cadman", SYMLINK+="input/3dconnexion"
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c606", MODE="0666", GROUP="plugdev", SYMLINK+="input/spacemouse_classic", SYMLINK+="input/3dconnexion"
+#
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c621", MODE="0666", GROUP="plugdev", SYMLINK+="input/spaceball5000", SYMLINK+="input/3dconnexion"
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c623", MODE="0666", GROUP="plugdev", SYMLINK+="input/spacetraveler", SYMLINK+="input/3dconnexion"
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c625", MODE="0666", GROUP="plugdev", SYMLINK+="input/spacepilot", SYMLINK+="input/3dconnexion"
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c626", MODE="0666", GROUP="plugdev", SYMLINK+="input/spacenavigator", SYMLINK+="input/3dconnexion"
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c627", MODE="0666", GROUP="plugdev", SYMLINK+="input/spaceexplorer", SYMLINK+="input/3dconnexion"
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c628", MODE="0666", GROUP="plugdev", SYMLINK+="input/spacenavigator_notebooks", SYMLINK+="input/3dconnexion"
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c629", MODE="0666", GROUP="plugdev", SYMLINK+="input/spacepilot_pro", SYMLINK+="input/3dconnexion"
+KERNEL=="event[0-9]*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c62b", MODE="0666", GROUP="plugdev", SYMLINK+="input/spacemouse_pro", SYMLINK+="input/3dconnexion"
diff --git a/SurgSim/Devices/MultiAxis/win32/CreateInputDeviceHandle.cpp b/SurgSim/Devices/MultiAxis/win32/CreateInputDeviceHandle.cpp
new file mode 100644
index 0000000..a30c2db
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/win32/CreateInputDeviceHandle.cpp
@@ -0,0 +1,40 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/CreateInputDeviceHandle.h"
+
+#include "SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.h"
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+std::unique_ptr<SystemInputDeviceHandle> createInputDeviceHandle(const std::string& path,
+ std::shared_ptr<SurgSim::Framework::Logger> logger)
+{
+ return WdkHidDeviceHandle::open(path, logger);
+}
+
+std::vector<std::string> enumerateInputDevicePaths(SurgSim::Framework::Logger* logger)
+{
+ return WdkHidDeviceHandle::enumeratePaths(logger);
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/win32/FileHandle.cpp b/SurgSim/Devices/MultiAxis/win32/FileHandle.cpp
new file mode 100644
index 0000000..e04734b
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/win32/FileHandle.cpp
@@ -0,0 +1,186 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/win32/FileHandle.h"
+
+#undef _WIN32_WINNT
+#define _WIN32_WINNT 0x0501 // request Windows XP-compatible SDK APIs
+#undef WIN32_LEAN_AND_MEAN
+#define WIN32_LEAN_AND_MEAN // do not automatically include WinSock 1 and some other header files
+#include <windows.h>
+
+#include <string>
+
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+// TODO(advornik): This would be a perfect use for "constexpr", but VS 2012 doesn't support it...
+static const FileHandle::RawHandleType INVALID_VALUE = INVALID_HANDLE_VALUE;
+
+
+FileHandle::FileHandle() :
+ m_handle(INVALID_VALUE),
+ m_canRead(false),
+ m_canWrite(false),
+ m_openFlags(0)
+{
+}
+
+FileHandle::FileHandle(FileHandle&& other) :
+ m_handle(other.m_handle),
+ m_canRead(other.m_canRead),
+ m_canWrite(other.m_canWrite),
+ m_openFlags(other.m_openFlags)
+{
+ other.m_handle = INVALID_VALUE; // take ownership
+}
+
+FileHandle& FileHandle::operator=(FileHandle&& other)
+{
+ m_handle = other.m_handle;
+ m_canRead = other.m_canRead;
+ m_canWrite = other.m_canWrite;
+ m_openFlags = other.m_openFlags;
+ other.m_handle = INVALID_VALUE; // take ownership
+ return *this;
+}
+
+FileHandle::~FileHandle()
+{
+ reset();
+}
+
+bool FileHandle::isValid() const
+{
+ return (m_handle && (m_handle != INVALID_VALUE));
+}
+
+bool FileHandle::canRead() const
+{
+ return isValid() && m_canRead;
+}
+
+bool FileHandle::canWrite() const
+{
+ return isValid() && m_canWrite;
+}
+
+FileHandle::RawHandleType FileHandle::get() const
+{
+ SURGSIM_ASSERT(isValid());
+ return m_handle;
+}
+
+bool FileHandle::openForReadingAndWriting(const std::string& path)
+{
+ reset();
+ m_handle = CreateFile(path.c_str(),
+ GENERIC_READ | GENERIC_WRITE | SYNCHRONIZE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL,
+ OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL | static_cast<DWORD>(m_openFlags), NULL);
+ m_canRead = true;
+ m_canWrite = true;
+ return isValid();
+}
+
+bool FileHandle::openForReading(const std::string& path)
+{
+ reset();
+ m_handle = CreateFile(path.c_str(),
+ GENERIC_READ | SYNCHRONIZE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL,
+ OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL | static_cast<DWORD>(m_openFlags), NULL);
+ m_canRead = true;
+ m_canWrite = false;
+ return isValid();
+}
+
+bool FileHandle::openForWriting(const std::string& path)
+{
+ reset();
+ m_handle = CreateFile(path.c_str(),
+ GENERIC_WRITE | SYNCHRONIZE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL,
+ OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL | static_cast<DWORD>(m_openFlags), NULL);
+ m_canRead = false;
+ m_canWrite = true;
+ return isValid();
+}
+
+bool FileHandle::openForReadingAndMaybeWriting(const std::string& path)
+{
+ if (! openForReadingAndWriting(path))
+ {
+ if (! openForReading(path))
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
+void FileHandle::reset()
+{
+ if (m_handle != INVALID_VALUE)
+ {
+ CloseHandle(m_handle);
+ m_handle = INVALID_VALUE;
+ }
+}
+
+void FileHandle::setFileOpenFlags(uint64_t flags)
+{
+ SURGSIM_ASSERT(! isValid()) << "Flags need to be set before the file is opened!";
+ m_openFlags = flags;
+}
+
+uint64_t FileHandle::getFileOpenFlags() const
+{
+ return m_openFlags;
+}
+
+bool FileHandle::hasDataToRead() const
+{
+ if (! canRead())
+ {
+ return false;
+ }
+
+ //const DWORD timeoutMsec = 10;
+ const DWORD nonBlockingOnly = 0;
+ DWORD status = WaitForSingleObject(m_handle, nonBlockingOnly);
+ return (status == WAIT_OBJECT_0);
+}
+
+bool FileHandle::readBytes(void* dataBuffer, unsigned int bytesToRead, unsigned int* bytesActuallyRead)
+{
+ SURGSIM_ASSERT(canRead());
+ DWORD numBytesRead = 0;
+ if (ReadFile(m_handle, dataBuffer, bytesToRead, &numBytesRead, NULL) != TRUE)
+ {
+ *bytesActuallyRead = 0;
+ return false;
+ }
+ else
+ {
+ *bytesActuallyRead = numBytesRead;
+ return true;
+ }
+}
+
+}; // namespace Device
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/MultiAxis/win32/FileHandle.h b/SurgSim/Devices/MultiAxis/win32/FileHandle.h
new file mode 100644
index 0000000..bd282cc
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/win32/FileHandle.h
@@ -0,0 +1,126 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_WIN32_FILEHANDLE_H
+#define SURGSIM_DEVICES_MULTIAXIS_WIN32_FILEHANDLE_H
+
+#include <stdint.h>
+#include <string>
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+/// A wrapper for an Windows-style HANDLE file descriptor.
+/// Allows callers to implement RAII-style resource management.
+class FileHandle
+{
+public:
+ /// Type of the raw handle used by the operating system.
+ /// Defined this way to avoid including <windows.h> just for the sake of defining HANDLE.
+ typedef void* RawHandleType;
+
+ /// Default constructor.
+ /// Initializes the file handle to an invalid state.
+ FileHandle();
+
+ /// Move constructor.
+ /// \param [in,out] other The object to move. The original object will be invalidated.
+ FileHandle(FileHandle&& other);
+
+ /// Move assignment operator.
+ /// \param [in,out] other The object to move. The original object will be invalidated.
+ /// \return A reference to this object.
+ FileHandle& operator=(FileHandle&& other);
+
+ /// Destructor.
+ ~FileHandle();
+
+ /// Checks if the file handle is valid, i.e. has been opened.
+ /// \return true if valid, false if not.
+ bool isValid() const;
+
+ /// Determines if the file handle can be read from.
+ /// \return true if the handle has been open for reading.
+ bool canRead() const;
+
+ /// Determines if the file handle can be written to.
+ /// \return true if the handle has been open for writing.
+ bool canWrite() const;
+
+ /// Checks whether this object has data available to be read.
+ /// \return true if there is data currently available.
+ bool hasDataToRead() const;
+
+ /// Reads bytes from the file handle.
+ /// \param [out] dataBuffer Buffer to read into. Must have room for at least bytesToRead bytes of data.
+ /// \param bytesToRead The number of bytes to try reading. Actual number of bytes received may be smaller.
+ /// \param [out] bytesActuallyRead The number of bytes that were actually read into the buffer.
+ /// \return true if it succeeds, false if it fails.
+ bool readBytes(void* dataBuffer, unsigned int bytesToRead, unsigned int* bytesActuallyRead);
+
+ /// Gets the raw underlying OS file handle.
+ /// \return The raw file handle.
+ RawHandleType get() const;
+
+ /// Attempts to open the file handle for reading and writing.
+ /// \param path Full pathname of the file.
+ /// \return true if it succeeds, false if it fails.
+ bool openForReadingAndWriting(const std::string& path);
+
+ /// Attempts to open the file handle for reading only.
+ /// \param path Full pathname of the file.
+ /// \return true if it succeeds, false if it fails.
+ bool openForReading(const std::string& path);
+
+ /// Attempts to open the file handle for writing only.
+ /// \param path Full pathname of the file.
+ /// \return true if it succeeds, false if it fails.
+ bool openForWriting(const std::string& path);
+
+ /// Attempts to open the file handle for reading and (if permissions allow it) writing.
+ /// \param path Full pathname of the file.
+ /// \return true if it succeeds, false if it fails.
+ bool openForReadingAndMaybeWriting(const std::string& path);
+
+ /// Sets the flags that will be passed to CreateFile when opening the file.
+ /// \param flags The flags, a combination of zero or more Windows FILE_FLAG_* flags.
+ void setFileOpenFlags(uint64_t flags);
+
+ /// Gets the flags that will be passed to CreateFile when opening the file.
+ /// \return The value passed to setFileOpenFlags (or if never set, a default value).
+ uint64_t getFileOpenFlags() const;
+
+ /// Resets the file handle back to an invalid state.
+ /// If the handle was open, it will be closed.
+ void reset();
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ FileHandle(const FileHandle& other) /*= delete*/;
+ FileHandle& operator=(const FileHandle& other) /*= delete*/;
+
+ RawHandleType m_handle;
+ bool m_canRead;
+ bool m_canWrite;
+ uint64_t m_openFlags;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_WIN32_FILEHANDLE_H
diff --git a/SurgSim/Devices/MultiAxis/win32/GetSystemError.cpp b/SurgSim/Devices/MultiAxis/win32/GetSystemError.cpp
new file mode 100644
index 0000000..18907ef
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/win32/GetSystemError.cpp
@@ -0,0 +1,66 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/GetSystemError.h"
+
+#undef _WIN32_WINNT
+#define _WIN32_WINNT 0x0501 // request Windows XP-compatible SDK APIs
+#undef WIN32_LEAN_AND_MEAN
+#define WIN32_LEAN_AND_MEAN // do not automatically include WinSock 1 and some other header files
+#include <windows.h>
+
+
+namespace SurgSim
+{
+namespace Device
+{
+namespace Internal
+{
+
+int64_t getSystemErrorCode()
+{
+ return GetLastError();
+}
+
+std::string getSystemErrorText(int64_t errorCode)
+{
+ const size_t BUFFER_SIZE = 1024;
+ char errorBuffer[BUFFER_SIZE];
+ if (FormatMessageA(FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, 0, static_cast<DWORD>(errorCode), 0,
+ errorBuffer, BUFFER_SIZE-1, nullptr) <= 0)
+ {
+ _snprintf(errorBuffer, BUFFER_SIZE, "<system error %u>", static_cast<unsigned int>(errorCode));
+ }
+ errorBuffer[BUFFER_SIZE-1] = '\0';
+
+ // Strip terminal whitespace, if any.
+ // Note that this approach only works for fixed-width characters, which is why we use the ASCII API above.
+ const size_t end = strnlen(errorBuffer, BUFFER_SIZE-1);
+ if ((end > 0) && isspace(errorBuffer[end-1]))
+ {
+ size_t lastWhitespace = end - 1;
+ while ((lastWhitespace > 0) && isspace(errorBuffer[lastWhitespace-1]))
+ {
+ --lastWhitespace;
+ }
+ errorBuffer[lastWhitespace] = '\0';
+ }
+
+ return std::string(errorBuffer);
+}
+
+}; // namespace Internal
+}; // namespace Device
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.cpp b/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.cpp
new file mode 100644
index 0000000..2f1618e
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.cpp
@@ -0,0 +1,578 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.h"
+
+#undef _WIN32_WINNT
+#define _WIN32_WINNT 0x0501 // request Windows XP-compatible SDK APIs
+#undef WIN32_LEAN_AND_MEAN
+#define WIN32_LEAN_AND_MEAN // do not automatically include WinSock 1 and some other header files
+#include <windows.h>
+
+#include <setupapi.h>
+extern "C" { // sigh...
+#include <hidsdi.h>
+}
+
+#include <stdint.h>
+
+#include "SurgSim/Devices/MultiAxis/GetSystemError.h"
+#include "SurgSim/Devices/MultiAxis/win32/FileHandle.h"
+#include "SurgSim/Framework/Log.h"
+
+using SurgSim::Device::Internal::getSystemErrorCode;
+using SurgSim::Device::Internal::getSystemErrorText;
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+// Usage page and usage IDs for interface devices; see e.g. http://www.usb.org/developers/devclass_docs/Hut1_12v2.pdf
+enum UsagePageConstants
+{
+ DEV_USAGE_PAGE_GENERIC_DESKTOP = 0x01 // Generic Desktop usage page
+};
+enum UsageConstants
+{
+ // Usages for the DEV_USAGE_PAGE_GENERIC_DESKTOP usage page:
+ DEV_USAGE_ID_MOUSE = 0x02, // Mouse usage ID
+ DEV_USAGE_ID_JOYSTICK = 0x04, // Joystick usage ID
+ DEV_USAGE_ID_GAME_PAD = 0x05, // Game Pad usage ID
+ DEV_USAGE_ID_KEYBOARD = 0x06, // Keyboard usage ID
+ DEV_USAGE_ID_KEYPAD = 0x07, // Keypad usage ID
+ DEV_USAGE_ID_MULTI_AXIS_CONTROLLER = 0x08 // Multi-axis Controller usage ID
+};
+
+
+struct WdkHidDeviceHandle::State
+{
+public:
+ explicit State(std::shared_ptr<SurgSim::Framework::Logger>&& logger_) :
+ logger(std::move(logger_)),
+ handle(),
+ isOverlappedReadPending(false),
+ isDeviceDead(false)
+ {
+ }
+
+ /// The logger to use.
+ std::shared_ptr<SurgSim::Framework::Logger> logger;
+ /// The underlying device file handle.
+ FileHandle handle;
+ /// The OVERLAPPED state structure for overlapped (i.e. asynchronous) reads.
+ OVERLAPPED overlappedReadState;
+ /// The buffer used to store the output of overlapped (i.e. asynchronous) reads.
+ unsigned char overlappedReadBuffer[7*128];
+ /// True if we are waiting for the result of an overlapped (i.e. asynchronous) read.
+ bool isOverlappedReadPending;
+ /// True if the communication with this device has failed without possibility of recovery.
+ bool isDeviceDead;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ State(const State& other) /*= delete*/;
+ State& operator=(const State& other) /*= delete*/;
+};
+
+WdkHidDeviceHandle::WdkHidDeviceHandle(std::shared_ptr<SurgSim::Framework::Logger>&& logger) :
+ m_state(new WdkHidDeviceHandle::State(std::move(logger)))
+{
+}
+
+WdkHidDeviceHandle::~WdkHidDeviceHandle()
+{
+}
+
+std::vector<std::string> WdkHidDeviceHandle::enumeratePaths(SurgSim::Framework::Logger* logger)
+{
+ std::vector<std::string> results;
+
+ // Prepare to iterate over the attached HID devices
+ GUID hidGuid;
+ HidD_GetHidGuid(&hidGuid);
+ HDEVINFO hidDeviceInfo = SetupDiGetClassDevs(&hidGuid, NULL, NULL,
+ DIGCF_DEVICEINTERFACE | DIGCF_PRESENT | DIGCF_PROFILE);
+ if (hidDeviceInfo == INVALID_HANDLE_VALUE)
+ {
+ DWORD error = GetLastError();
+ SURGSIM_LOG_CRITICAL(logger) << "WdkHidDeviceHandle::enumerate: Failed to query HID devices;" <<
+ " SetupDiGetClassDevs() failed with error " << error << ", " << getSystemErrorText(error);
+ return results;
+ }
+
+ // Loop through the device list, looking for the devices we want
+ for (int hidEnumerationIndex = 0; true; ++hidEnumerationIndex)
+ {
+ // Get the next interface in the list.
+ SP_DEVICE_INTERFACE_DATA deviceInterfaceData;
+ deviceInterfaceData.cbSize = sizeof(deviceInterfaceData);
+ if (! SetupDiEnumDeviceInterfaces(hidDeviceInfo, NULL, &hidGuid, hidEnumerationIndex, &deviceInterfaceData))
+ {
+ DWORD error = GetLastError();
+ if (error == ERROR_NO_MORE_ITEMS)
+ {
+ break;
+ }
+ else
+ {
+ SURGSIM_LOG_CRITICAL(logger) << "WdkHidDeviceHandle::enumerate: Failed to query HID devices;" <<
+ " SetupDiEnumDeviceInterfaces() failed with error " << error << ", " << getSystemErrorText(error);
+ return results;
+ }
+ }
+
+ // Find out the required size.
+ DWORD deviceInterfaceDetailSize = 0;
+ if (! SetupDiGetDeviceInterfaceDetail(hidDeviceInfo, &deviceInterfaceData, NULL, 0,
+ &deviceInterfaceDetailSize, NULL))
+ {
+ DWORD error = GetLastError();
+ if (error != ERROR_INSUFFICIENT_BUFFER)
+ {
+ SURGSIM_LOG_INFO(logger) << "WdkHidDeviceHandle::enumerate: Failed to get the device detail size," <<
+ " device will be ignored; error " << error << ", " << getSystemErrorText(error);
+ continue;
+ }
+ }
+
+ // Get the device detail (which actually just means the path).
+ SP_DEVICE_INTERFACE_DETAIL_DATA* deviceInterfaceDetail =
+ static_cast<SP_DEVICE_INTERFACE_DETAIL_DATA*>(malloc(deviceInterfaceDetailSize));
+ deviceInterfaceDetail->cbSize = sizeof(*deviceInterfaceDetail);
+ if (! SetupDiGetDeviceInterfaceDetail(hidDeviceInfo, &deviceInterfaceData,
+ deviceInterfaceDetail, deviceInterfaceDetailSize, NULL, NULL))
+ {
+ DWORD error = GetLastError();
+ SURGSIM_LOG_INFO(logger) << "WdkHidDeviceHandle::enumerate: Failed to get the HID device detail," <<
+ " device will be ignored; error " << error << ", " << getSystemErrorText(error);
+ free(deviceInterfaceDetail);
+ continue;
+ }
+
+ std::string devicePath(deviceInterfaceDetail->DevicePath);
+ free(deviceInterfaceDetail);
+
+ FileHandle handle;
+ if (! handle.openForReadingAndMaybeWriting(devicePath))
+ {
+ DWORD error = GetLastError();
+ SURGSIM_LOG_INFO(logger) << "WdkHidDeviceHandle::enumerate: Could not open device " << devicePath <<
+ ": error " << error << ", " << getSystemErrorText(error);
+ continue;
+ }
+
+ results.push_back(devicePath);
+ }
+
+ return results;
+}
+
+std::unique_ptr<WdkHidDeviceHandle> WdkHidDeviceHandle::open(
+ const std::string& path, std::shared_ptr<SurgSim::Framework::Logger> logger)
+{
+ std::unique_ptr<WdkHidDeviceHandle> object(new WdkHidDeviceHandle(std::move(logger)));
+ object->m_state->handle.setFileOpenFlags(FILE_FLAG_OVERLAPPED); // set up the handle for asynchronous I/O
+ if (! object->m_state->handle.openForReadingAndMaybeWriting(path))
+ {
+ object.reset(); // could not open the device handle; destroy the object again
+ }
+ else
+ {
+ object->m_state->isOverlappedReadPending = false;
+ object->m_state->isDeviceDead = false;
+ }
+ return object;
+}
+
+static std::string convertWideString(const wchar_t* wideString)
+{
+ char buffer[4096];
+ int status = WideCharToMultiByte(CP_UTF8, 0, wideString, -1, buffer, sizeof(buffer), nullptr, nullptr);
+ if (! (status > 0 && status < sizeof(buffer)))
+ {
+ _snprintf(buffer, sizeof(buffer), "???");
+ }
+ return std::string(buffer);
+}
+
+std::string WdkHidDeviceHandle::getDeviceName() const
+{
+ wchar_t manufacturer[1024];
+ if (HidD_GetManufacturerString(m_state->handle.get(), manufacturer, sizeof(manufacturer)) != TRUE)
+ {
+ manufacturer[0] = '\0';
+ }
+
+ wchar_t product[1024];
+ if (HidD_GetProductString(m_state->handle.get(), product, sizeof(product)) != TRUE)
+ {
+ product[0] = '\0';
+ }
+
+ std::string result("");
+ if (manufacturer[0])
+ {
+ result = convertWideString(manufacturer) + " ";
+ }
+ if (product[0])
+ {
+ result += convertWideString(product);
+ }
+ else
+ {
+ result += "???";
+ }
+ return result;
+}
+
+bool WdkHidDeviceHandle::getDeviceIds(int* vendorId, int* productId) const
+{
+ HIDD_ATTRIBUTES attributes;
+ attributes.Size = sizeof(attributes);
+ if (HidD_GetAttributes(m_state->handle.get(), &attributes) != TRUE)
+ {
+ DWORD error = GetLastError();
+ SURGSIM_LOG_INFO(m_state->logger) << "WdkHidDeviceHandle: Could not get attributes/IDs: error " << error <<
+ ", " << getSystemErrorText(error);
+ *vendorId = *productId = -1;
+ return false;
+ }
+
+ *vendorId = attributes.VendorID;
+ *productId = attributes.ProductID;
+ return true;
+}
+
+bool WdkHidDeviceHandle::getCapabilities(HIDP_CAPS* capabilities) const
+{
+ PHIDP_PREPARSED_DATA preParsedData = 0;
+ if (HidD_GetPreparsedData(m_state->handle.get(), &preParsedData) != TRUE)
+ {
+ DWORD error = GetLastError();
+ SURGSIM_LOG_INFO(m_state->logger) << "WdkHidDeviceHandle: Could not get preparsed data: error " << error <<
+ ", " << getSystemErrorText(error);
+ return false;
+ }
+
+ if (HidP_GetCaps(preParsedData, capabilities) != HIDP_STATUS_SUCCESS)
+ {
+ DWORD error = GetLastError();
+ SURGSIM_LOG_INFO(m_state->logger) << "WdkHidDeviceHandle: Could not get capabilities: error " << error <<
+ ", " << getSystemErrorText(error);
+ HidD_FreePreparsedData(preParsedData);
+ return false;
+ }
+
+ HidD_FreePreparsedData(preParsedData); // don't need the pre-parsed data any more
+ return true;
+}
+
+bool WdkHidDeviceHandle::hasTranslationAndRotationAxes() const
+{
+ HIDP_CAPS capabilities;
+ if (! getCapabilities(&capabilities))
+ {
+ // message already shown
+ return false;
+ }
+
+ if ((capabilities.UsagePage != DEV_USAGE_PAGE_GENERIC_DESKTOP) ||
+ (capabilities.Usage != DEV_USAGE_ID_MULTI_AXIS_CONTROLLER))
+ {
+ SURGSIM_LOG_DEBUG(m_state->logger) << "WdkHidDeviceHandle: device is not a multi-axis controller.";
+ return false;
+ }
+
+ int numExtraAxes = static_cast<int>(capabilities.NumberInputValueCaps) - 6;
+ if (numExtraAxes < 0)
+ {
+ SURGSIM_LOG_DEBUG(m_state->logger) << "WdkHidDeviceHandle: device does not have 6 input axes.";
+ return false;
+ }
+ else if (numExtraAxes > 0)
+ {
+ SURGSIM_LOG_INFO(m_state->logger) << "WdkHidDeviceHandle: device has more than 6 axes;" <<
+ " ignoring " << numExtraAxes << " additional axes.";
+ }
+
+ return true;
+}
+
+bool WdkHidDeviceHandle::startAsynchronousRead()
+{
+ SURGSIM_ASSERT(! m_state->isOverlappedReadPending) << "Previous asynchronous read has not been handled!";
+
+ memset(&(m_state->overlappedReadState), 0, sizeof(m_state->overlappedReadState));
+ if (ReadFile(m_state->handle.get(), m_state->overlappedReadBuffer, sizeof(m_state->overlappedReadBuffer),
+ NULL, &(m_state->overlappedReadState)) != TRUE)
+ {
+ DWORD error = GetLastError();
+ if (error == ERROR_IO_PENDING)
+ {
+ m_state->isOverlappedReadPending = true;
+ }
+ else if (error == ERROR_DEVICE_NOT_CONNECTED)
+ {
+ SURGSIM_LOG_SEVERE(m_state->logger) <<
+ "WdkHidDeviceHandle: read failed; device has been disconnected! (stopping)";
+ m_state->isDeviceDead = true;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_state->logger) << "WdkHidDeviceHandle: read failed with error " <<
+ error << ", " << getSystemErrorText(error);
+ }
+ }
+ else
+ {
+ // Read succeeded synchronously. That means that the read has actually completed, but we still need to
+ // retrieve the returned data using GetOverlappedResult, just like for a pending result.
+ m_state->isOverlappedReadPending = true;
+ }
+
+ return m_state->isOverlappedReadPending;
+}
+
+bool WdkHidDeviceHandle::finishAsynchronousRead(size_t* numBytesRead)
+{
+ SURGSIM_ASSERT(m_state->isOverlappedReadPending) << "Asynchronous read has not been started!";
+
+ DWORD numRead = 0;
+ if (GetOverlappedResult(m_state->handle.get(), &(m_state->overlappedReadState), &numRead, FALSE) == FALSE)
+ {
+ DWORD error = GetLastError();
+ if (error == ERROR_IO_INCOMPLETE)
+ {
+ // keep checking for asynchronous I/O completion
+ }
+ else if (error == ERROR_DEVICE_NOT_CONNECTED)
+ {
+ SURGSIM_LOG_SEVERE(m_state->logger) <<
+ "WdkHidDeviceHandle: read failed; device has been disconnected! (stopping)";
+ m_state->isDeviceDead = true;
+ m_state->isOverlappedReadPending = false;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_state->logger) << "WdkHidDeviceHandle: GetOverlappedResult failed with error " <<
+ error << ", " << getSystemErrorText(error);
+ // keep checking for asynchronous I/O completion, I guess
+ }
+ *numBytesRead = 0;
+ return false;
+ }
+
+ // The read has been completed.
+ m_state->isOverlappedReadPending = false;
+ *numBytesRead = numRead;
+ return true;
+}
+
+void WdkHidDeviceHandle::cancelAsynchronousRead()
+{
+ if (CancelIo(m_state->handle.get()) == FALSE)
+ {
+ DWORD error = GetLastError();
+ if (error == ERROR_NOT_FOUND)
+ {
+ // No requests were pending.
+ SURGSIM_LOG_WARNING(m_state->logger) <<
+ "WdkHidDeviceHandle: No asynchronous I/O requests were pending when attempting to cancel.";
+ m_state->isOverlappedReadPending = false;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_state->logger) <<
+ "WdkHidDeviceHandle: Could not cancel pending asynchronous I/O; error " <<
+ error << ", " << getSystemErrorText(error);
+ }
+ }
+ else
+ {
+ DWORD numRead = 0;
+ if (GetOverlappedResult(m_state->handle.get(), &(m_state->overlappedReadState), &numRead, TRUE) == FALSE)
+ {
+ DWORD error = GetLastError();
+ if (error == ERROR_OPERATION_ABORTED)
+ {
+ // It worked.
+ m_state->isOverlappedReadPending = false;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_state->logger) <<
+ "WdkHidDeviceHandle: Final GetOverlappedResult failed with error " <<
+ error << ", " << getSystemErrorText(error);
+ }
+ }
+ else
+ {
+ m_state->isOverlappedReadPending = false;
+ }
+ }
+}
+
+bool WdkHidDeviceHandle::updateStates(AxisStates* axisStates, ButtonStates* buttonStates, bool* updated)
+{
+ *updated = false;
+
+ // Both WaitForSingleObject() and WaitForMultipleObjects() always claim data is available for 3DConnexion device
+ // file handles. So we can't check for data availability that way. Instead, we use overlapped (asynchronous) I/O.
+
+ int numInitialFinished = 0;
+ int numStarted = 0;
+ int numFinished = 0;
+
+ if (m_state->isOverlappedReadPending)
+ {
+ size_t numRead = 0;
+ if (! finishAsynchronousRead(&numRead))
+ {
+ if (m_state->isDeviceDead)
+ {
+ return false; // stop updating this device!
+ }
+ }
+ else
+ {
+ decodeStateUpdates(m_state->overlappedReadBuffer, numRead, axisStates, buttonStates, updated);
+ ++numInitialFinished;
+ }
+ }
+
+ while (! m_state->isOverlappedReadPending)
+ {
+ if (! startAsynchronousRead())
+ {
+ if (m_state->isDeviceDead)
+ {
+ return false; // stop updating this device!
+ }
+ else
+ {
+ break;
+ }
+ }
+ ++numStarted;
+
+ size_t numRead = 0;
+ if (! finishAsynchronousRead(&numRead))
+ {
+ if (m_state->isDeviceDead)
+ {
+ return false; // stop updating this device!
+ }
+ }
+ else
+ {
+ decodeStateUpdates(m_state->overlappedReadBuffer, numRead, axisStates, buttonStates, updated);
+ ++numFinished;
+ }
+ }
+
+ if (numInitialFinished > 0 || numStarted > 0 || numFinished > 0)
+ {
+ SURGSIM_LOG_DEBUG(m_state->logger) << "WdkHidDeviceHandle: started " << numStarted << " reads, finished " <<
+ numInitialFinished << "+" << numFinished << ", delta = " << (numStarted - numInitialFinished - numFinished);
+ }
+
+ return true;
+}
+
+void WdkHidDeviceHandle::prepareForShutdown()
+{
+ // When this code is running, the thread that calls updateStates() should no longer be running.
+ // So we make the assumption that no synchronization is needed.
+
+ // Cancel any pending asynchronous I/O, so it doesn't try reading into memory that is about to be freed.
+ if (m_state->isOverlappedReadPending)
+ {
+ cancelAsynchronousRead();
+ }
+}
+
+static inline int16_t signedShortData(unsigned char byte0, unsigned char byte1)
+{
+ return static_cast<int16_t>(static_cast<uint16_t>(byte0) | (static_cast<uint16_t>(byte1) << 8));
+}
+
+void WdkHidDeviceHandle::decodeStateUpdates(const unsigned char* rawData, size_t rawDataSize,
+ AxisStates* axisStates, ButtonStates* buttonStates, bool* updated)
+{
+ if ((rawDataSize >= 7) && (rawData[0] == 0x01)) // Translation
+ {
+ // We could parse this via HidP_GetData(), but that won't work for buttons (see below)
+ (*axisStates)[0] = signedShortData(rawData[1], rawData[2]);
+ (*axisStates)[1] = signedShortData(rawData[3], rawData[4]);
+ (*axisStates)[2] = signedShortData(rawData[5], rawData[6]);
+ *updated = true;
+
+ if ((rawDataSize >= 14) && (rawData[7] == 0x02)) // translation data may have rotation appended to it
+ {
+ (*axisStates)[3] = signedShortData(rawData[8], rawData[9]);
+ (*axisStates)[4] = signedShortData(rawData[10], rawData[11]);
+ (*axisStates)[5] = signedShortData(rawData[12], rawData[13]);
+ }
+ }
+ else if ((rawDataSize >= 7) && (rawData[0] == 0x02)) // Rotation
+ {
+ // We could parse this via HidP_GetData(), but that won't work for buttons (see below)
+ (*axisStates)[3] = signedShortData(rawData[1], rawData[2]);
+ (*axisStates)[4] = signedShortData(rawData[3], rawData[4]);
+ (*axisStates)[5] = signedShortData(rawData[5], rawData[6]);
+ *updated = true;
+
+ if ((rawDataSize >= 14) && (rawData[7] == 0x01)) // rotation data may have translation appended to it
+ {
+ (*axisStates)[0] = signedShortData(rawData[8], rawData[9]);
+ (*axisStates)[1] = signedShortData(rawData[10], rawData[11]);
+ (*axisStates)[2] = signedShortData(rawData[12], rawData[13]);
+ }
+ }
+ else if ((rawDataSize >= 2) && (rawData[0] == 0x03)) // Buttons
+ {
+ // We CAN'T parse buttons via HidP_GetData(), because 3DConnexion devices produce button state in a
+ // different report from the axes, so when the last button is released you simply get no data. We could
+ // interpret "empty data list" as "the last button has been released", but that seems dangerous. So we
+ // roll our own parsing for now, even though it may not work for other devices. --advornik 2012-08-01
+
+ size_t currentByte = 1; // Byte 0 specifies the packet type; data starts at byte 1
+ unsigned char currentBit = 0x01;
+ for (size_t i = 0; i < (*buttonStates).size(); ++i)
+ {
+ (*buttonStates)[i] = ((rawData[currentByte] & currentBit) != 0);
+ if (currentBit < 0x80)
+ {
+ currentBit = currentBit << 1;
+ }
+ else
+ {
+ currentBit = 0x01;
+ ++currentByte;
+ if (currentByte >= rawDataSize) // out of data?
+ {
+ break;
+ }
+ }
+ }
+ *updated = true;
+ }
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.h b/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.h
new file mode 100644
index 0000000..51b5e99
--- /dev/null
+++ b/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.h
@@ -0,0 +1,120 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_MULTIAXIS_WIN32_WDKHIDDEVICEHANDLE_H
+#define SURGSIM_DEVICES_MULTIAXIS_WIN32_WDKHIDDEVICEHANDLE_H
+
+#include <string>
+#include <memory>
+#include <array>
+#include <vector>
+
+#include "SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h"
+
+
+// The following structure is defined by the Windows HID API, but we don't want to include the whole thing here.
+struct _HIDP_CAPS;
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+class Logger;
+}; // namespace Framework
+
+namespace Device
+{
+
+/// Access to an input/HID device using the HID API from the Windows Driver Kit.
+/// \sa SystemInputDeviceHandle
+class WdkHidDeviceHandle : public SystemInputDeviceHandle
+{
+public:
+ /// Destructor.
+ ~WdkHidDeviceHandle();
+
+ /// Enumerates input devices.
+ /// \param logger The logger to be used during enumeration.
+ /// \return A list of device paths.
+ static std::vector<std::string> enumeratePaths(SurgSim::Framework::Logger* logger);
+
+ /// Opens the given path and creates an access wrapper for the device.
+ /// \param path Full pathname for the device.
+ /// \param logger The logger to be used by the device.
+ /// \return The created device object, or an empty unique_ptr on failure.
+ static std::unique_ptr<WdkHidDeviceHandle> open(const std::string& path,
+ std::shared_ptr<SurgSim::Framework::Logger> logger);
+
+ virtual std::string getDeviceName() const override;
+
+ virtual bool getDeviceIds(int* vendorId, int* productId) const override;
+
+ virtual bool hasTranslationAndRotationAxes() const override;
+
+ virtual bool updateStates(AxisStates* axisStates, ButtonStates* buttonStates, bool* updated) override;
+
+ virtual void prepareForShutdown() override;
+
+private:
+ /// Constructor.
+ /// Cannot be called directly.
+ /// \sa open
+ explicit WdkHidDeviceHandle(std::shared_ptr<SurgSim::Framework::Logger>&& logger);
+
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ WdkHidDeviceHandle(const WdkHidDeviceHandle& other) /*= delete*/;
+ WdkHidDeviceHandle& operator=(const WdkHidDeviceHandle& other) /*= delete*/;
+
+ /// Gets the device capabilities.
+ /// \param [out] capabilities The capabilities data for the device.
+ /// \return true if it succeeds.
+ bool getCapabilities(struct _HIDP_CAPS* capabilities) const;
+
+ /// Starts an asynchronous read from the device.
+ /// Updates the internal flags to indicate the state of the device I/O.
+ /// \return true if an asynchronous read has been started, or the read has already completed synchronously.
+ bool startAsynchronousRead();
+
+ /// Checks if an asynchronous read from the device has completed.
+ /// Updates the internal flags to indicate the state of the device I/O.
+ /// \param [out] numBytesRead If the function returns true, the number of bytes read from the device.
+ /// \return true if data has been received; false if the asynchronous read is still pending or an error has
+ /// occurred.
+ bool finishAsynchronousRead(size_t* numBytesRead);
+
+ /// Cancels an asynchronous read from the device.
+ /// Should be executed in the context of the thread that called startAsynchronousRead.
+ void cancelAsynchronousRead();
+
+ /// Decode the raw state update data received from the device.
+ /// \param rawData Raw state update data.
+ /// \param rawDataSize Size of the raw state update data.
+ /// \param [in,out] axisStates The states for each axis of the device.
+ /// \param [in,out] buttonStates The states for each device button.
+ /// \param [out] updated True if any states were actually updated. (Note that even if this value is true, the
+ /// states may not have changed value; one or more states could have been updated to the same value.)
+ void decodeStateUpdates(const unsigned char* rawData, size_t rawDataSize,
+ AxisStates* axisStates, ButtonStates* buttonStates, bool* updated);
+
+
+ struct State;
+ std::unique_ptr<State> m_state;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_MULTIAXIS_WIN32_WDKHIDDEVICEHANDLE_H
diff --git a/SurgSim/Devices/Novint/CMakeLists.txt b/SurgSim/Devices/Novint/CMakeLists.txt
new file mode 100644
index 0000000..8c98bea
--- /dev/null
+++ b/SurgSim/Devices/Novint/CMakeLists.txt
@@ -0,0 +1,64 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+find_package(NovintHdalSdk REQUIRED)
+
+link_directories(${Boost_LIBRARY_DIRS})
+
+set(LIBS
+ ${Boost_LIBRARIES}
+ ${NOVINT_HDAL_SDK_LIBRARIES}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${NOVINT_HDAL_SDK_INCLUDE_DIR}"
+)
+
+set(NOVINT_DEVICE_SOURCES
+ Novint7DofDevice.cpp
+ NovintCommonDevice.cpp
+ NovintDevice.cpp
+ NovintScaffold.cpp
+)
+
+set(NOVINT_DEVICE_HEADERS
+ Novint7DofDevice.h
+ NovintCommonDevice.h
+ NovintDevice.h
+ NovintScaffold.h
+)
+
+# TODO(advornik): the installation should NOT copy all the headers...
+surgsim_add_library(
+ NovintDevice
+ "${NOVINT_DEVICE_SOURCES}" "${NOVINT_DEVICE_HEADERS}"
+ SurgSim/Devices/Novint
+)
+
+target_link_libraries(NovintDevice ${LIBS})
+
+if(BUILD_TESTING)
+ # The unit tests will be built whenever the library is built.
+ add_subdirectory(UnitTests)
+
+ if(GLUT_FOUND)
+ add_subdirectory(VisualTest)
+ endif(GLUT_FOUND)
+endif()
+
+# Put NovintDevice into folder "Devices"
+set_target_properties(NovintDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Novint/Novint.dox b/SurgSim/Devices/Novint/Novint.dox
new file mode 100644
index 0000000..46e31fe
--- /dev/null
+++ b/SurgSim/Devices/Novint/Novint.dox
@@ -0,0 +1,16 @@
+/*!
+
+\page Novint Novint: Haptic Device
+
+The Novint Falcon (by Novint Technologies, Inc.) is a 3D force-feedback haptic device. The user holds a grip that measures the hand's position, and can exert forces on the hand.
+
+Novint Technologies, Inc., only provides Novint Falcon driver support for Windows.
+
+Dependencies:
+- Windows:
+ - Requires a more recent HDAL SDK than is publicly available (currently 2.1.3 is publicly available) http://home.novint.com/index.php/support/downloads
+ -# Install the Falcon drivers.
+ -# Install the SDK.
+ -# Run NDSSetter as administrator (executable provided with the HDAL SDK) to set an environment variable named NOVINT_DEVICE_SUPPORT to the directory where the SDK was installed.
+ -# Add the directory containing hd.dll to the environment path (e.g., <tt>C:\\Program Files (x86)\\novint\\HDAL_SDK_X.X.XX.XX\\bin</tt>).
+*/
\ No newline at end of file
diff --git a/SurgSim/Devices/Novint/Novint7DofDevice.cpp b/SurgSim/Devices/Novint/Novint7DofDevice.cpp
new file mode 100644
index 0000000..0b5a855
--- /dev/null
+++ b/SurgSim/Devices/Novint/Novint7DofDevice.cpp
@@ -0,0 +1,62 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Novint/Novint7DofDevice.h"
+
+#include <iostream>
+#include <iomanip>
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Devices/Novint/NovintScaffold.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::RigidTransform3d;
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+Novint7DofDevice::Novint7DofDevice(const std::string& uniqueName, const std::string& initializationName) :
+ NovintCommonDevice(uniqueName, initializationName)
+{
+}
+
+
+Novint7DofDevice::~Novint7DofDevice()
+{
+}
+
+
+bool Novint7DofDevice::is7DofDevice() const
+{
+ return true;
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Novint/Novint7DofDevice.h b/SurgSim/Devices/Novint/Novint7DofDevice.h
new file mode 100644
index 0000000..524fe3a
--- /dev/null
+++ b/SurgSim/Devices/Novint/Novint7DofDevice.h
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_NOVINT_NOVINT7DOFDEVICE_H
+#define SURGSIM_DEVICES_NOVINT_NOVINT7DOFDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Devices/Novint/NovintCommonDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+/// A class implementing the communication with a Novint Falcon with the Open Surgery Grip 7-DoF device.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Device pose (units are meters). |
+/// | bool | "button1" | %Always false (there are no buttons present). |
+/// | bool | "button2" | %Always false (there are no buttons present). |
+/// | bool | "button3" | %Always false (there are no buttons present). |
+/// | bool | "button4" | %Always false (there are no buttons present). |
+/// | bool | "isHomed" | %Device homing status. |
+/// | bool | "isHeld" | %Device homing status. |
+///
+/// \par Application output used by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | vector | "force" | %Device output force (units are newtons). |
+/// | vector | "torque" | %Device output torque (units are newton-meters). |
+/// | bool | "gravityCompensation" | Enable or disable hardware gravity compensation. |
+///
+/// \sa NovintHapticDevice
+/// \sa NovintCommonDevice, SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class Novint7DofDevice : public NovintCommonDevice
+{
+public:
+ /// Constructor.
+ ///
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ /// \param initializationName The name passed to HDAL when initializing the device. This should match a
+ /// configured Novint device; alternately, an empty string indicates the default device.
+ Novint7DofDevice(const std::string& uniqueName, const std::string& initializationName);
+
+ /// Destructor.
+ virtual ~Novint7DofDevice();
+
+private:
+ virtual bool is7DofDevice() const override;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_NOVINT_NOVINT7DOFDEVICE_H
diff --git a/SurgSim/Devices/Novint/NovintCommonDevice.cpp b/SurgSim/Devices/Novint/NovintCommonDevice.cpp
new file mode 100644
index 0000000..0e22c8b
--- /dev/null
+++ b/SurgSim/Devices/Novint/NovintCommonDevice.cpp
@@ -0,0 +1,132 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Novint/NovintCommonDevice.h"
+
+#include <iostream>
+#include <iomanip>
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Devices/Novint/NovintScaffold.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::RigidTransform3d;
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+NovintCommonDevice::NovintCommonDevice(const std::string& uniqueName, const std::string& initializationName) :
+ SurgSim::Input::CommonDevice(uniqueName, NovintScaffold::buildDeviceInputData()),
+ m_initializationName(initializationName), m_positionScale(1.0), m_orientationScale(1.0)
+{
+}
+
+
+NovintCommonDevice::~NovintCommonDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+
+std::string NovintCommonDevice::getInitializationName() const
+{
+ return m_initializationName;
+}
+
+
+bool NovintCommonDevice::initialize()
+{
+ SURGSIM_ASSERT(! isInitialized());
+ std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold);
+
+ if (! scaffold->registerDevice(this))
+ {
+ return false;
+ }
+
+ m_scaffold = std::move(scaffold);
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
+ return true;
+}
+
+
+bool NovintCommonDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized());
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ bool ok = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return ok;
+}
+
+
+bool NovintCommonDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+void NovintCommonDevice::setPositionScale(double scale)
+{
+ m_positionScale = scale;
+ if (m_scaffold)
+ {
+ m_scaffold->setPositionScale(this, m_positionScale);
+ }
+}
+
+double NovintCommonDevice::getPositionScale() const
+{
+ return m_positionScale;
+}
+
+void NovintCommonDevice::setOrientationScale(double scale)
+{
+ m_orientationScale = scale;
+ if (m_scaffold)
+ {
+ m_scaffold->setOrientationScale(this, m_orientationScale);
+ }
+}
+
+double NovintCommonDevice::getOrientationScale() const
+{
+ return m_orientationScale;
+}
+
+bool NovintCommonDevice::is7DofDevice() const
+{
+ return false;
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Novint/NovintCommonDevice.h b/SurgSim/Devices/Novint/NovintCommonDevice.h
new file mode 100644
index 0000000..43fff8d
--- /dev/null
+++ b/SurgSim/Devices/Novint/NovintCommonDevice.h
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_NOVINT_NOVINTCOMMONDEVICE_H
+#define SURGSIM_DEVICES_NOVINT_NOVINTCOMMONDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+class NovintScaffold;
+
+
+/// A class implementing the communication with a generic Novint Falcon device.
+///
+/// \sa NovintDevice, Novint7DofHapticDevice
+/// \sa SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class NovintCommonDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ ///
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ /// \param initializationName The name passed to HDAL when initializing the device. This should match a
+ /// configured Novint device; alternately, an empty string indicates the default device.
+ NovintCommonDevice(const std::string& uniqueName, const std::string& initializationName);
+
+ /// Destructor.
+ virtual ~NovintCommonDevice();
+
+ /// Gets the name used by the Novint device configuration to refer to this device.
+ /// Note that this may or may not be the same as the device name retrieved by getName().
+ /// An empty string indicates the default device.
+ /// \return The initialization name.
+ std::string getInitializationName() const;
+
+ virtual bool initialize() override;
+
+ virtual bool finalize() override;
+
+ /// Check whether this device is initialized.
+ bool isInitialized() const;
+
+ /// Sets the position scale for this device.
+ /// The position scale controls how much the pose changes for a given device translation.
+ /// The default value for a raw device tries to correspond to the actual physical motion of the device.
+ /// \param scale The multiplicative factor to apply to the position.
+ void setPositionScale(double scale);
+ /// Gets the position scale for this device.
+ double getPositionScale() const;
+
+ /// Sets the orientation scale for this device.
+ /// The orientation scale controls how much the pose changes for a given device rotation.
+ /// The default value for a raw device tries to correspond to the actual physical motion of the device.
+ /// \param scale The multiplicative factor to apply to the rotation angles.
+ void setOrientationScale(double scale);
+ /// Gets the orientation scale for this device.
+ double getOrientationScale() const;
+
+private:
+ friend class NovintScaffold;
+
+ /// Query if this object represents a 7 degree of freedom hardware device.
+ /// \return true if 7 degree of freedom device, false if not.
+ virtual bool is7DofDevice() const;
+
+ /// The scaffold handles all the communication with the SDK.
+ std::shared_ptr<NovintScaffold> m_scaffold;
+ std::string m_initializationName;
+
+ /// Scale factor for the position axes; stored locally before the device is initialized.
+ double m_positionScale;
+ /// Scale factor for the orientation axes; stored locally before the device is initialized.
+ double m_orientationScale;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_NOVINT_NOVINTCOMMONDEVICE_H
diff --git a/SurgSim/Devices/Novint/NovintDevice.cpp b/SurgSim/Devices/Novint/NovintDevice.cpp
new file mode 100644
index 0000000..943e22c
--- /dev/null
+++ b/SurgSim/Devices/Novint/NovintDevice.cpp
@@ -0,0 +1,56 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Novint/NovintDevice.h"
+
+#include <iostream>
+#include <iomanip>
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Devices/Novint/NovintScaffold.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::RigidTransform3d;
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+NovintDevice::NovintDevice(const std::string& uniqueName, const std::string& initializationName) :
+ NovintCommonDevice(uniqueName, initializationName)
+{
+}
+
+
+NovintDevice::~NovintDevice()
+{
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Novint/NovintDevice.h b/SurgSim/Devices/Novint/NovintDevice.h
new file mode 100644
index 0000000..8573efe
--- /dev/null
+++ b/SurgSim/Devices/Novint/NovintDevice.h
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_NOVINT_NOVINTDEVICE_H
+#define SURGSIM_DEVICES_NOVINT_NOVINTDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Devices/Novint/NovintCommonDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+/// A class implementing the communication with a Novint Falcon device.
+///
+/// This should provide basic support for any device that can communicate using the Novint HDAL SDK toolkit, such as
+/// the off-the-shelf Novint Falcon haptic gaming controller. Note that certain devices may require device-specific
+/// support in the code to enable particular hardware features. In particular, the Novint Falcon with the Open Surgery
+/// Grip will not be able to produce torques unless it is accessed using the Novint7DofHapticDevice class.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Device pose (units are meters). |
+/// | bool | "button1" | %State of the first device button if present. |
+/// | bool | "button2" | %State of the second device button if present. |
+/// | bool | "button3" | %State of the third device button if present. |
+/// | bool | "button4" | %State of the third device button if present. |
+/// | bool | "isHomed" | %Device homing status. |
+/// Note that \c button1 through \c 4 correspond to the buttons 0 through 3 provided by the
+/// HDAL SDK, but a custom Novint device might have fewer than 4 buttons.
+///
+/// \par Application output used by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | vector | "force" | %Device output force (units are newtons). |
+/// | bool | "gravityCompensation" | Enable or disable hardware gravity compensation. |
+///
+/// \sa Novint7DofHapticDevice
+/// \sa NovintCommonDevice, SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class NovintDevice : public NovintCommonDevice
+{
+public:
+ /// Constructor.
+ ///
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ /// \param initializationName The name passed to HDAL when initializing the device. This should match a
+ /// configured Novint device; alternately, an empty string indicates the default device.
+ NovintDevice(const std::string& uniqueName, const std::string& initializationName);
+
+ /// Destructor.
+ virtual ~NovintDevice();
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_NOVINT_NOVINTDEVICE_H
diff --git a/SurgSim/Devices/Novint/NovintScaffold.cpp b/SurgSim/Devices/Novint/NovintScaffold.cpp
new file mode 100644
index 0000000..352e700
--- /dev/null
+++ b/SurgSim/Devices/Novint/NovintScaffold.cpp
@@ -0,0 +1,1198 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include "SurgSim/Devices/Novint/NovintScaffold.h"
+
+#include <vector>
+#include <memory>
+#include <algorithm>
+#include <array>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+#include <boost/thread/thread.hpp>
+
+#include <hdl/hdl.h>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/Novint/NovintDevice.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Clock.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Framework::Clock;
+using SurgSim::Math::makeRotationMatrix;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Math::Matrix66d;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+class NovintScaffold::Handle
+{
+public:
+ Handle() :
+ m_deviceHandle(HDL_INVALID_HANDLE),
+ m_scaffold(NovintScaffold::getOrCreateSharedInstance())
+ {
+ }
+
+ Handle(const std::string& deviceName, const std::string& initializationName) :
+ m_deviceHandle(HDL_INVALID_HANDLE),
+ m_scaffold(NovintScaffold::getOrCreateSharedInstance())
+ {
+ create(deviceName, initializationName);
+ }
+
+ ~Handle()
+ {
+ SURGSIM_ASSERT(! isValid()) << "Expected destroy() to be called before Handle object destruction.";
+ }
+
+ bool isValid() const
+ {
+ return (m_deviceHandle != HDL_INVALID_HANDLE);
+ }
+
+ bool create(const std::string& deviceName, const std::string& initializationName)
+ {
+ SURGSIM_ASSERT(! isValid());
+
+ HDLDeviceHandle deviceHandle = HDL_INVALID_HANDLE;
+ std::string hdalName = initializationName;
+ const char* hdalNameToPassSdk = hdalName.c_str();
+ if (hdalName.length() == 0)
+ {
+ hdalName = "Default Falcon";
+ hdalNameToPassSdk = nullptr; // This is how the HDAL API initializes default Falcon.
+ }
+ deviceHandle = hdlInitNamedDevice(hdalNameToPassSdk);
+
+ if (m_scaffold->checkForFatalError("Failed to initialize"))
+ {
+ // HDAL error message already logged
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << std::endl <<
+ " Device name: '" << deviceName << "'" << std::endl <<
+ " HDAL device name: '" << hdalName << "'" << std::endl;
+ return false;
+ }
+ else if (deviceHandle == HDL_INVALID_HANDLE)
+ {
+ SURGSIM_LOG_SEVERE(m_scaffold->getLogger()) << "Novint: Failed to initialize '" << deviceName << "'" <<
+ std::endl <<
+ " Error details: unknown (HDAL returned an invalid handle)" << std::endl <<
+ " HDAL device name: '" << hdalName << "'" << std::endl;
+ return false;
+ }
+
+ m_deviceHandle = deviceHandle;
+ return true;
+ }
+
+ bool destroy()
+ {
+ SURGSIM_ASSERT(isValid());
+
+ HDLDeviceHandle deviceHandle = m_deviceHandle;
+ if (deviceHandle == HDL_INVALID_HANDLE)
+ {
+ return false;
+ }
+ m_deviceHandle = HDL_INVALID_HANDLE;
+
+ hdlUninitDevice(deviceHandle);
+ m_scaffold->checkForFatalError("Couldn't disable device");
+ return true;
+ }
+
+ HDLDeviceHandle get() const
+ {
+ SURGSIM_ASSERT(isValid());
+ return m_deviceHandle;
+ }
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ Handle(const Handle&) /*= delete*/;
+ Handle& operator=(const Handle&) /*= delete*/;
+
+ /// The HDAL device handle (or HDL_INVALID_HANDLE if not valid).
+ HDLDeviceHandle m_deviceHandle;
+ /// The scaffold.
+ std::shared_ptr<NovintScaffold> m_scaffold;
+};
+
+
+class NovintScaffold::Callback
+{
+public:
+ Callback() :
+ m_callbackHandle(0),
+ m_haveCallback(false),
+ m_scaffold(NovintScaffold::getOrCreateSharedInstance())
+ {
+ create();
+ }
+
+ ~Callback()
+ {
+ if (m_haveCallback)
+ {
+ destroy();
+ }
+ }
+
+ bool isValid() const
+ {
+ return m_haveCallback;
+ }
+
+ bool create()
+ {
+ SURGSIM_ASSERT(! m_haveCallback);
+
+ const bool isCallbackNonblocking = false;
+ m_callbackHandle = hdlCreateServoOp(run, m_scaffold.get(), isCallbackNonblocking);
+ if (m_scaffold->checkForFatalError("Couldn't run haptic callback"))
+ {
+ return false;
+ }
+ m_haveCallback = true;
+ return true;
+ }
+
+ bool destroy()
+ {
+ SURGSIM_ASSERT(m_haveCallback);
+ hdlDestroyServoOp(m_callbackHandle);
+ if (m_scaffold->checkForFatalError("Couldn't stop haptic callback"))
+ {
+ return false;
+ }
+ m_haveCallback = false;
+ return true;
+ }
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ Callback(const Callback&) /*= delete*/;
+ Callback& operator=(const Callback&) /*= delete*/;
+
+ /// The callback wrapper passed to HDAL.
+ /// \param [in,out] data The user data (in our case, the scaffold pointer).
+ /// \return HD_CALLBACK_CONTINUE to wait for the next frame, or HD_CALLBACK_DONE to terminate further calls.
+ static HDLServoOpExitCode run(void* data);
+
+ /// The haptic loop callback handle.
+ HDLOpHandle m_callbackHandle;
+ /// True if the callback has been created (and not destroyed).
+ bool m_haveCallback;
+ /// The scaffold.
+ std::shared_ptr<NovintScaffold> m_scaffold;
+};
+
+
+struct NovintScaffold::DeviceData
+{
+ /// Initialize the state.
+ DeviceData(const std::string& apiName, NovintCommonDevice* device) :
+ initializationName(apiName),
+ deviceObject(device),
+ isPositionHomed(false),
+ isOrientationHomed(false),
+ isDeviceHomed(false),
+ isDeviceHeld(false),
+ isDevice7Dof(device->is7DofDevice()),
+ isDeviceRollAxisReversed(false),
+ eulerAngleOffsetRoll(0.0),
+ eulerAngleOffsetYaw(0.0),
+ eulerAngleOffsetPitch(0.0),
+ forwardPointingPoseThreshold(0.9),
+ torqueScale(Vector3d::Constant(1.0)),
+ positionScale(device->getPositionScale()),
+ orientationScale(device->getOrientationScale()),
+ position(Vector3d::Zero()),
+ jointAngles(Vector3d::Zero()),
+ force(Vector3d::Zero()),
+ torque(Vector4d::Zero()),
+ orientationTransform(RigidTransform3d::Identity()),
+ scaledPose(RigidTransform3d::Identity())
+ {
+ buttonStates.fill(false);
+ }
+
+
+ /// The maximum number of buttons supported by any device object.
+ static const size_t MAX_NUM_BUTTONS = 4;
+
+ /// Type used to store button states.
+ typedef std::array<bool, MAX_NUM_BUTTONS> ButtonStates;
+
+
+ /// The HDAL device name.
+ const std::string initializationName;
+ /// The corresponding device object.
+ NovintCommonDevice* const deviceObject;
+
+ /// The device handle wrapper.
+ NovintScaffold::Handle deviceHandle;
+ /// Time of the initialization of the handle.
+ Clock::time_point initializationTime;
+
+ /// The joint angles for the device orientation.
+ Vector3d jointAngles;
+ /// The button state read from the device.
+ ButtonStates buttonStates;
+ /// The homing state read from the device.
+ bool isPositionHomed;
+ /// The homing state read from the device.
+ bool isOrientationHomed;
+ /// The homing state read from the device.
+ bool isDeviceHomed;
+ /// The proximity state read from the device.
+ bool isDeviceHeld;
+ /// True if this is a 7DoF device.
+ bool isDevice7Dof;
+ /// True if the roll axis of a 7DoF device has reverse polarity because the device is left-handed.
+ bool isDeviceRollAxisReversed;
+
+ /// The offset added to the roll Euler angle.
+ double eulerAngleOffsetRoll;
+ /// The offset added to the yaw Euler angle.
+ double eulerAngleOffsetYaw;
+ /// The offset added to the pitch Euler angle.
+ double eulerAngleOffsetPitch;
+ /// The threshold to determine if the device is pointing forwards before unlocking orientation.
+ double forwardPointingPoseThreshold;
+ /// The scaling factors for the torque axes.
+ Vector3d torqueScale;
+
+ /// The position value from the device.
+ Vector3d position;
+ /// The orientation value from the device. If the device is not 7Dof the orientation is always Identity.
+ RigidTransform3d orientationTransform;
+ /// The pose value from the device, after scaling.
+ RigidTransform3d scaledPose;
+
+ /// The force value to be written to the device.
+ Vector3d force;
+ /// The torque value to be written to the device.
+ Vector4d torque;
+
+ /// Scale factor for the position axes.
+ double positionScale;
+ /// Scale factor for the orientation axes.
+ double orientationScale;
+ /// The mutex that protects the externally modifiable parameters.
+ boost::mutex parametersMutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ DeviceData(const DeviceData&) /*= delete*/;
+ DeviceData& operator=(const DeviceData&) /*= delete*/;
+};
+
+
+struct NovintScaffold::StateData
+{
+public:
+ /// Initialize the state.
+ StateData() : isApiInitialized(false)
+ {
+ }
+
+ /// True if the API has been initialized (and not finalized).
+ bool isApiInitialized;
+
+ /// Wrapper for the haptic loop callback handle.
+ std::unique_ptr<NovintScaffold::Callback> callback;
+
+ /// The list of known devices.
+ std::list<std::unique_ptr<NovintScaffold::DeviceData>> activeDeviceList;
+
+ /// The mutex that protects the list of known devices.
+ boost::mutex mutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ StateData(const StateData&) /*= delete*/;
+ StateData& operator=(const StateData&) /*= delete*/;
+};
+
+
+HDLServoOpExitCode NovintScaffold::Callback::run(void* data)
+{
+ NovintScaffold* scaffold = static_cast<NovintScaffold*>(data);
+ if (! scaffold->runHapticFrame())
+ {
+ //...do something?...
+ }
+
+ // Should return HDL_SERVOOP_CONTINUE to wait for the next frame, or HDL_SERVOOP_EXIT to terminate the calls.
+ return HDL_SERVOOP_CONTINUE;
+}
+
+
+
+template <typename T>
+static inline T clampToRange(T value, T rangeMin, T rangeMax)
+{
+ if (value < rangeMin)
+ return rangeMin;
+ if (value > rangeMax)
+ return rangeMax;
+ return value;
+}
+
+
+
+NovintScaffold::NovintScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
+ m_logger(logger), m_state(new StateData)
+{
+ if (! m_logger)
+ {
+ m_logger = SurgSim::Framework::Logger::getLogger("Novint device");
+ m_logger->setThreshold(m_defaultLogLevel);
+ }
+
+ {
+ // Drain the HDAL error stack
+ HDLError errorCode = hdlGetError();
+ while (errorCode != HDL_NO_ERROR)
+ {
+ errorCode = hdlGetError();
+ }
+ }
+
+ SURGSIM_LOG_DEBUG(m_logger) << "Novint: Shared scaffold created.";
+}
+
+
+NovintScaffold::~NovintScaffold()
+{
+ if (m_state->callback)
+ {
+ destroyHapticLoop();
+ }
+ // The following block controls the duration of the mutex being locked.
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (! m_state->activeDeviceList.empty())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Novint: Destroying scaffold while devices are active!?!";
+ // do anything special with each device?
+ m_state->activeDeviceList.clear();
+ }
+
+ if (m_state->isApiInitialized)
+ {
+ finalizeSdk();
+ }
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Novint: Shared scaffold destroyed.";
+}
+
+
+bool NovintScaffold::registerDevice(NovintCommonDevice* device)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (! m_state->isApiInitialized)
+ {
+ if (! initializeSdk())
+ {
+ return false;
+ }
+ }
+
+ // Make sure the object is unique.
+ auto sameObject = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ SURGSIM_ASSERT(sameObject == m_state->activeDeviceList.end()) << "Novint: Tried to register a device" <<
+ " which is already present!";
+
+ // Make sure the name is unique.
+ const std::string deviceName = device->getName();
+ auto sameName = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [&deviceName](const std::unique_ptr<DeviceData>& info) { return info->deviceObject->getName() == deviceName; });
+ if (sameName != m_state->activeDeviceList.end())
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Novint: Tried to register a device when the same name is" <<
+ " already present!";
+ return false;
+ }
+
+ // Make sure the initialization name is unique.
+ const std::string initializationName = device->getInitializationName();
+ auto sameInitializationName = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [&initializationName](const std::unique_ptr<DeviceData>& info)
+ { return info->deviceObject->getInitializationName() == initializationName; });
+ if (sameInitializationName != m_state->activeDeviceList.end())
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Novint: Tried to register a device when the same initialization" <<
+ " (HDAL) name is already present!";
+ return false;
+ }
+
+ // Construct the object, start its thread, then move it to the list.
+ // Note that since Visual Studio 2010 doesn't support multi-argument emplace_back() for STL containers, storing a
+ // list of unique_ptr results in nicer code than storing a list of DeviceData values directly.
+ std::unique_ptr<DeviceData> info(new DeviceData(initializationName, device));
+ if (! initializeDeviceState(info.get()))
+ {
+ return false; // message already printed
+ }
+ info->initializationTime = Clock::now();
+ m_state->activeDeviceList.emplace_back(std::move(info));
+
+ if (m_state->activeDeviceList.size() == 1)
+ {
+ // If this is the first device, create the haptic loop as well.
+ createHapticLoop();
+ }
+ return true;
+}
+
+
+bool NovintScaffold::unregisterDevice(const NovintCommonDevice* const device)
+{
+ std::unique_ptr<DeviceData> savedInfo;
+ bool haveOtherDevices = false;
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ savedInfo = std::move(*matching);
+ m_state->activeDeviceList.erase(matching);
+ // the iterator is now invalid but that's OK
+ }
+ haveOtherDevices = (m_state->activeDeviceList.size() > 0);
+ }
+
+ bool status = true;
+ if (! savedInfo)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Novint: Attempted to release a non-registered device.";
+ status = false;
+ }
+ else
+ {
+ // The HDAL seems to do bad things (and the CRT complains) if we uninitialize the device too soon.
+ const int MINIMUM_LIFETIME_MILLISECONDS = 500;
+ Clock::time_point earliestEndTime =
+ savedInfo->initializationTime + boost::chrono::milliseconds(MINIMUM_LIFETIME_MILLISECONDS);
+ boost::this_thread::sleep_until(earliestEndTime);
+
+ // The destroy-pop-create structure of this code mirrors the structure of the OpenHaptics code, and
+ // probably isn't necessary when using the HDAL.
+ destroyHapticLoop();
+
+ finalizeDeviceState(savedInfo.get());
+ savedInfo.reset(nullptr);
+
+ if (haveOtherDevices)
+ {
+ createHapticLoop();
+ }
+ }
+ return status;
+}
+
+
+bool NovintScaffold::initializeDeviceState(DeviceData* info)
+{
+ SURGSIM_ASSERT(! info->deviceHandle.isValid());
+
+ if (! info->deviceHandle.create(info->deviceObject->getName(), info->deviceObject->getInitializationName()))
+ {
+ return false; // message was already printed
+ }
+
+ // Select the handle.
+ hdlMakeCurrent(info->deviceHandle.get());
+ checkForFatalError("Couldn't enable the handle");
+
+ if (info->isDevice7Dof)
+ {
+ int gripStatus[2] = { 0, 0 };
+ // OSG2 grips report their "handedness" in the LSB of the second raw status byte
+ hdlGripGetAttributes (HDL_GRIP_STATUS, 2, gripStatus);
+ if (checkForFatalError("Cannot get grip status"))
+ {
+ // HDL reported an error. An error message was already logged.
+ return false;
+ }
+ bool leftHanded = ((gripStatus[1] & 0x01) != 0);
+ if (leftHanded)
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "'" << info->initializationName << "' is LEFT-handed.";
+ info->isDeviceRollAxisReversed = true; // sigh
+ // I wish we had someplace to put these instead of hardcoding.
+ info->eulerAngleOffsetRoll = 0;
+ info->eulerAngleOffsetYaw = -75. * M_PI / 180.;
+ info->eulerAngleOffsetPitch = -50. * M_PI / 180.;
+ }
+ else
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "'" << info->initializationName << "' is right-handed.";
+ info->isDeviceRollAxisReversed = false;
+ // I wish we had someplace to put these instead of hardcoding.
+ info->eulerAngleOffsetRoll = 0;
+ info->eulerAngleOffsetYaw = +75. * M_PI / 180.;
+ info->eulerAngleOffsetPitch = +50. * M_PI / 180.;
+ }
+ }
+
+ return true;
+}
+
+
+bool NovintScaffold::finalizeDeviceState(DeviceData* info)
+{
+ bool status = false;
+ if (info->deviceHandle.isValid())
+ {
+ status = info->deviceHandle.destroy();
+ }
+ return status;
+}
+
+
+bool NovintScaffold::updateDevice(DeviceData* info)
+{
+ const SurgSim::DataStructures::DataGroup& outputData = info->deviceObject->getOutputData();
+
+ boost::lock_guard<boost::mutex> lock(info->parametersMutex);
+
+ // TODO(bert): this code should cache the access indices.
+
+ hdlMakeCurrent(info->deviceHandle.get()); // This device is now "current", and all hdlXxx calls apply to it.
+ bool fatalError = checkForFatalError(false, "hdlMakeCurrent()");
+
+ // Receive the current device position (in meters), orientation transform, and button state bitmap.
+ hdlGripGetAttributev(HDL_GRIP_POSITION, 0, info->position.data());
+ fatalError = checkForFatalError(fatalError, "hdlGripGetAttributev(HDL_GRIP_POSITION)");
+
+ info->buttonStates.fill(false);
+ hdlGripGetAttributesb(HDL_GRIP_BUTTON, static_cast<int>(info->buttonStates.size()), info->buttonStates.data());
+ fatalError = checkForFatalError(fatalError, "hdlGripGetAttributesb(HDL_GRIP_BUTTON)");
+
+ // Get the additional 7DoF data if available.
+ if (info->isDevice7Dof)
+ {
+ // We compute the device orientation from the joint angles, for two reasons. The first that it lets us
+ // compensate for recurrent bugs in the HDAL grip code. The second is that we'll need the joint angles in
+ // order to correctly generate joint torques.
+ double angles[4];
+ hdlGripGetAttributesd(HDL_GRIP_ANGLE, 4, angles);
+ fatalError = checkForFatalError(fatalError, "hdlGripGetAttributesd(HDL_GRIP_ANGLE)");
+
+ // The zero values are NOT the home orientation.
+ info->jointAngles[0] = angles[0] + info->eulerAngleOffsetRoll;
+ info->jointAngles[1] = angles[1] + info->eulerAngleOffsetYaw;
+ info->jointAngles[2] = angles[2] + info->eulerAngleOffsetPitch;
+
+ // For the Falcon 7DoF grip, the axes are perpendicular and the joint angles are Euler angles:
+ Matrix33d rotationX = makeRotationMatrix(info->jointAngles[0] * info->orientationScale,
+ Vector3d(Vector3d::UnitX()));
+ Matrix33d rotationY = makeRotationMatrix(info->jointAngles[1] * info->orientationScale,
+ Vector3d(Vector3d::UnitY()));
+ Matrix33d rotationZ = makeRotationMatrix(info->jointAngles[2] * info->orientationScale,
+ Vector3d(Vector3d::UnitZ()));
+ Matrix33d orientation = rotationY * rotationZ * rotationX;
+ // Put the result into the orientation transform
+ info->orientationTransform.linear() = orientation;
+ }
+
+ checkDeviceHoming(info);
+
+ info->force.setZero();
+ info->torque.setZero();
+ if (info->isDeviceHomed)
+ {
+ bool desiredGravityCompensation = false;
+ if (outputData.booleans().get("gravityCompensation", &desiredGravityCompensation))
+ {
+ setGravityCompensation(info, desiredGravityCompensation);
+ }
+
+ calculateForceAndTorque(info);
+ }
+
+ // Set the force command (in newtons).
+ hdlGripSetAttributev(HDL_GRIP_FORCE, 0, info->force.data()); // 2nd arg is index; output force is always "vector #0"
+ fatalError = checkForFatalError(fatalError, "hdlGripSetAttributev(HDL_GRIP_FORCE)");
+
+ // Set the torque vector. Also set the jaw squeeze torque (as 4th element of the array)-- though this is not used
+ // anywhere at the moment.
+ // The 2nd arg to this call is the count; we're setting 4 doubles.
+ hdlGripSetAttributesd(HDL_GRIP_TORQUE, 4, info->torque.data());
+ fatalError = checkForFatalError(fatalError, "hdlGripSetAttributesd(HDL_GRIP_TORQUE)");
+
+ setInputData(info);
+
+ return !fatalError;
+}
+
+void NovintScaffold::checkDeviceHoming(DeviceData* info)
+{
+ unsigned int deviceStateBitmask = hdlGetState();
+ info->isPositionHomed = ((deviceStateBitmask & HDAL_NOT_CALIBRATED) == 0);
+
+ if (info->isDevice7Dof)
+ {
+ // The homing state is communicated using the button information.
+ info->isOrientationHomed = info->buttonStates[0] && info->buttonStates[1];
+ // So is the state of whether the device is currently held (proximity sensor).
+ info->isDeviceHeld = info->buttonStates[2];
+ // There are no ACTUAL buttons on the 7DoF Falcons, so we clear the button buffer.
+ info->buttonStates.fill(false);
+ }
+ else
+ {
+ // The 3-DoF device doesn't need the orientation homing shenanigans...
+ info->isOrientationHomed = true;
+ info->isDeviceHomed = info->isPositionHomed;
+ info->isDeviceHeld = true; // ...I guess
+ }
+
+ if (info->isPositionHomed && info->isOrientationHomed && ! info->isDeviceHomed)
+ {
+ // Wait until the tool is pointed forwards (i.e. perpendicular to the Falcon centerline) before proclaiming the
+ // whole device homed.
+ Vector3d forwardDirection = Vector3d::UnitX();
+ double forwardMetric = forwardDirection.dot(info->orientationTransform.linear() * forwardDirection);
+
+ if (forwardMetric >= info->forwardPointingPoseThreshold)
+ {
+ // It looks like everything is ready!
+ info->isDeviceHomed = true;
+ }
+ }
+
+ if (! info->isPositionHomed)
+ {
+ info->position.setZero();
+ }
+ if (! info->isOrientationHomed)
+ {
+ info->orientationTransform.setIdentity();
+ }
+
+ info->scaledPose.translation() = info->position * info->positionScale;
+ info->scaledPose.linear() = info->orientationTransform.linear();
+}
+
+void NovintScaffold::calculateForceAndTorque(DeviceData* info)
+{
+ typedef Eigen::Matrix<double, 6, 1> Vector6d;
+ const SurgSim::DataStructures::DataGroup& outputData = info->deviceObject->getOutputData();
+
+ // Set the DeviceData's force to the nominal force, if provided.
+ Vector3d nominalForce = Vector3d::Zero();
+ outputData.vectors().get(SurgSim::DataStructures::Names::FORCE, &nominalForce);
+ info->force = nominalForce;
+
+ // If the springJacobian was provided, multiply with the change in position since the output data was set,
+ // to get a delta force. This way a linearized output force is calculated at haptic update rates.
+ Vector6d deltaPosition;
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType springJacobian;
+ bool havespringJacobian =
+ outputData.matrices().get(SurgSim::DataStructures::Names::SPRING_JACOBIAN, &springJacobian);
+ if (havespringJacobian)
+ {
+ RigidTransform3d poseForNominal = info->scaledPose;
+ outputData.poses().get(SurgSim::DataStructures::Names::INPUT_POSE, &poseForNominal);
+
+ Vector3d rotationVector = Vector3d::Zero();
+ SurgSim::Math::computeRotationVector(info->scaledPose, poseForNominal, &rotationVector);
+
+ SurgSim::Math::setSubVector(info->scaledPose.translation() - poseForNominal.translation(), 0, 3,
+ &deltaPosition);
+ SurgSim::Math::setSubVector(rotationVector, 1, 3, &deltaPosition);
+
+ info->force += springJacobian.block<3,6>(0, 0) * deltaPosition;
+ }
+
+ // If the damperJacobian was provided, calculate a delta force based on the change in velocity.
+ Vector6d deltaVelocity;
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType damperJacobian;
+ bool havedamperJacobian =
+ outputData.matrices().get(SurgSim::DataStructures::Names::DAMPER_JACOBIAN, &damperJacobian);
+ if (havedamperJacobian)
+ {
+ // TODO(ryanbeasley): consider adding a velocity filter setting to NovintDevice/DeviceData.
+ Vector3d linearVelocity = Vector3d::Zero();
+ Vector3d angularVelocity = Vector3d::Zero();
+
+ Vector3d linearVelocityForNominal = linearVelocity;
+ outputData.vectors().get(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY, &linearVelocityForNominal);
+ Vector3d angularVelocityForNominal = angularVelocity;
+ outputData.vectors().get(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY, &angularVelocityForNominal);
+
+ SurgSim::Math::setSubVector(linearVelocity - linearVelocityForNominal, 0, 3, &deltaVelocity);
+ SurgSim::Math::setSubVector(angularVelocity - angularVelocityForNominal, 1, 3, &deltaVelocity);
+
+ info->force += damperJacobian.block<3,6>(0, 0) * deltaVelocity;
+ }
+
+ // Calculate the torque command if applicable (and convert newton-meters to command counts).
+ if (info->isDevice7Dof)
+ {
+ Vector3d nominalTorque = Vector3d::Zero();
+ outputData.vectors().get(SurgSim::DataStructures::Names::TORQUE, &nominalTorque);
+ Vector3d torque = nominalTorque;
+
+ if (havespringJacobian)
+ {
+ torque += springJacobian.block<3,6>(3, 0) * deltaPosition;
+ }
+
+ if (havedamperJacobian)
+ {
+ torque += damperJacobian.block<3,6>(3, 0) * deltaVelocity;
+ }
+
+ // We have the torque vector in newton-meters. Sadly, what we need is the torque command counts FOR EACH MOTOR
+ // AXIS, not for each Cartesian axis. Which means we need to go back to calculations with joint angles.
+ // For the Falcon 7DoF grip, the axes are perpendicular and the joint angles are Euler angles:
+ Matrix33d rotationX = makeRotationMatrix(info->jointAngles[0], Vector3d(Vector3d::UnitX()));
+ Matrix33d rotationY = makeRotationMatrix(info->jointAngles[1], Vector3d(Vector3d::UnitY()));
+ Matrix33d rotationZ = makeRotationMatrix(info->jointAngles[2], Vector3d(Vector3d::UnitZ()));
+ // NB: the order of rotations is (rotY * rotZ * rotX), not XYZ!
+ // Construct the joint axes for the CURRENT pose of the device.
+ Vector3d jointAxisY = Vector3d::UnitY();
+ Vector3d jointAxisZ = rotationY * Vector3d::UnitZ();
+ Vector3d jointAxisX = rotationY * (rotationZ * Vector3d::UnitX());
+ // To convert from Cartesian space to motor-axis space, we assemble the axes into a basis matrix and invert it.
+ Matrix33d basisMatrix;
+ basisMatrix.col(0) = jointAxisX;
+ basisMatrix.col(1) = jointAxisY;
+ basisMatrix.col(2) = jointAxisZ;
+ double basisDeterminant = fabs(basisMatrix.determinant());
+
+ // Also construct a "fake" X axis orthogonal with the other two, to be used when the pose is degenerate.
+ // Note that the Y and Z axes are always perpendicular for the Falcon 7DoF, so the normalize() can't fail and
+ // is basically unnecessary, but...
+ Vector3d fakeAxisX = jointAxisY.cross(jointAxisZ).normalized();
+ Matrix33d fakeBasisMatrix;
+ fakeBasisMatrix.col(0) = fakeAxisX;
+ fakeBasisMatrix.col(1) = jointAxisY;
+ fakeBasisMatrix.col(2) = jointAxisZ;
+
+ const double mediumBasisDeterminantThreshold = 0.6;
+ const double smallBasisDeterminantThreshold = 0.4;
+
+ Matrix33d decompositionMatrix;
+ if (basisDeterminant >= mediumBasisDeterminantThreshold)
+ {
+ // All is well!
+ decompositionMatrix = basisMatrix.inverse();
+ }
+ else if (basisDeterminant >= smallBasisDeterminantThreshold)
+ {
+ // If the determinant is "medium" but not "small", the device is in a near-degenerate configuration.
+ // Which axes are going to be commanded may be hugely dependent on small changes in the pose.
+ // We want to gradually decrease the amount of roll torque produced near the degenerate point.
+ double ratio = ((basisDeterminant - smallBasisDeterminantThreshold) /
+ (mediumBasisDeterminantThreshold - smallBasisDeterminantThreshold));
+ // The computed ratio has to be 0 <= ratio < 1. We just use linear drop-off.
+
+ // The "fake" basis matrix replaces the X axis with a fake (so it's always invertible), but the output X
+ // torque is then meaningless.
+ Matrix33d fakeDecompositionMatrix = fakeBasisMatrix.inverse();
+ fakeDecompositionMatrix.row(0) = Vector3d::Zero();
+
+ decompositionMatrix = basisMatrix.inverse() * ratio + fakeDecompositionMatrix * (1.-ratio);
+ }
+ else
+ {
+ // If the determinant is small, the matrix may not be invertible.
+ // The "fake" basis matrix replaces the X axis with a fake (so it's always invertible), but the output X
+ // torque is then meaningless.
+ decompositionMatrix = fakeBasisMatrix.inverse();
+ decompositionMatrix.row(0) = Vector3d::Zero();
+ // Moreover, near the degenerate position the X axis free-spins but is aligned with Y,
+ // so we want to reduce Y torques as well.
+ //double ratio = (basisDeterminant / smallBasisDeterminantThreshold);
+ double ratio = 0;
+ // The computed ratio has to be 0 <= ratio < 1. We just use linear drop-off.
+ decompositionMatrix.row(1) *= ratio;
+ }
+ Vector3d axisTorqueVector = decompositionMatrix * torque;
+
+ // Unit conversion factors for the Falcon 7DoF. THIS SHOULD BE PARAMETRIZED!
+ const double axisTorqueMin = -2000;
+ const double axisTorqueMax = +2000;
+ // roll axis: torque = 17.6 mNm when command = 2000 (but flipped in left grip!)
+ const double rollTorqueScale = axisTorqueMax / 17.6e-3;
+ // yaw axis: torque = 47.96 mNm when command = 2000
+ const double yawTorqueScale = axisTorqueMax / 47.96e-3;
+ // pitch axis: torque = 47.96 mNm when command = 2000
+ const double pitchTorqueScale = axisTorqueMax / 47.96e-3;
+
+ info->torque[0] = clampToRange(rollTorqueScale * info->torqueScale.x() * axisTorqueVector.x(),
+ axisTorqueMin, axisTorqueMax);
+ info->torque[1] = clampToRange(yawTorqueScale * info->torqueScale.y() * axisTorqueVector.y(),
+ axisTorqueMin, axisTorqueMax);
+ info->torque[2] = clampToRange(pitchTorqueScale * info->torqueScale.z() * axisTorqueVector.z(),
+ axisTorqueMin, axisTorqueMax);
+ info->torque[3] = 0;
+
+ if (info->isDeviceRollAxisReversed) // commence swearing.
+ {
+ info->torque[0] = -info->torque[0];
+ }
+ }
+}
+
+
+void NovintScaffold::setInputData(DeviceData* info)
+{
+ SurgSim::DataStructures::DataGroup& inputData = info->deviceObject->getInputData();
+ inputData.poses().set(SurgSim::DataStructures::Names::POSE, info->scaledPose);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_1, info->buttonStates[0]);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_2, info->buttonStates[1]);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_3, info->buttonStates[2]);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_4, info->buttonStates[3]);
+ inputData.booleans().set(SurgSim::DataStructures::Names::IS_HOMED, info->isDeviceHomed);
+ inputData.booleans().set(SurgSim::DataStructures::Names::IS_POSITION_HOMED, info->isPositionHomed);
+ inputData.booleans().set(SurgSim::DataStructures::Names::IS_ORIENTATION_HOMED, info->isOrientationHomed);
+}
+
+
+bool NovintScaffold::initializeSdk()
+{
+ SURGSIM_ASSERT(! m_state->isApiInitialized);
+
+ // nothing to do!
+
+ m_state->isApiInitialized = true;
+ return true;
+}
+
+
+bool NovintScaffold::finalizeSdk()
+{
+ SURGSIM_ASSERT(m_state->isApiInitialized);
+
+ // nothing to do!
+
+ m_state->isApiInitialized = false;
+ return true;
+}
+
+
+bool NovintScaffold::runHapticFrame()
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ for (auto it = m_state->activeDeviceList.begin(); it != m_state->activeDeviceList.end(); ++it)
+ {
+ (*it)->deviceObject->pullOutput();
+ if (updateDevice((*it).get()))
+ {
+ (*it)->deviceObject->pushInput();
+ }
+ }
+
+ return true;
+}
+
+
+bool NovintScaffold::createHapticLoop()
+{
+ SURGSIM_ASSERT(! m_state->callback);
+
+ if (! startScheduler())
+ {
+ return false;
+ }
+
+ std::unique_ptr<Callback> callback(new Callback);
+ if (! callback->isValid())
+ {
+ stopScheduler();
+ return false;
+ }
+
+ m_state->callback = std::move(callback);
+ return true;
+}
+
+
+bool NovintScaffold::destroyHapticLoop()
+{
+ SURGSIM_ASSERT(m_state->callback);
+
+ checkForFatalError("Error prior to stopping haptic callback"); // NOT considered an error for return code!
+
+ bool didDestroy = m_state->callback->destroy();
+ m_state->callback.reset(nullptr);
+
+ bool didStop = stopScheduler();
+
+ return didDestroy && didStop;
+}
+
+
+bool NovintScaffold::startScheduler()
+{
+ hdlStart();
+ if (checkForFatalError("Couldn't start the scheduler"))
+ {
+ return false;
+ }
+ return true;
+}
+
+
+bool NovintScaffold::stopScheduler()
+{
+ hdlStop();
+ if (checkForFatalError("Couldn't stop the scheduler"))
+ {
+ return false;
+ }
+ return true;
+}
+
+
+bool NovintScaffold::getGravityCompensation(const NovintScaffold::DeviceData* info, bool* gravityCompensationState)
+{
+ bool state1 = true;
+ hdlGripGetAttributeb(HDL_GRIP_GRAVITY_COMP, 1, &state1);
+ if (checkForFatalError("Cannot get gravity compensation (#1)"))
+ {
+ return false; // HDAL reported an error; an error message was already logged.
+ }
+
+ bool state2 = false;
+ hdlGripGetAttributeb(HDL_GRIP_GRAVITY_COMP, 1, &state2);
+ if (checkForFatalError("Cannot get gravity compensation (#2)"))
+ {
+ return false; // HDAL reported an error; an error message was already logged.
+ }
+
+ if (state1 == true && state2 == false)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "getting gravity compensation state for '" << info->deviceObject->getName() <<
+ "' does nothing!";
+ return false;
+ }
+ else if (state1 != state2)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "getting gravity compensation state for '" << info->deviceObject->getName() <<
+ "' keeps changing?!?";
+ return false;
+ }
+
+ *gravityCompensationState = state1;
+ return true;
+}
+
+
+bool NovintScaffold::enforceGravityCompensation(const NovintScaffold::DeviceData* info, bool gravityCompensationState)
+{
+ bool initialState;
+ bool isInitialStateValid = getGravityCompensation(info, &initialState);
+
+ const int maxAttempts = 20;
+ for (int i = 0; i < maxAttempts; ++i)
+ {
+ bool state = gravityCompensationState;
+ hdlGripSetAttributeb(HDL_GRIP_GRAVITY_COMP, 1, &state);
+ if (checkForFatalError("Cannot set gravity compensation state"))
+ {
+ return false; // HDAL reported an error; an error message was already logged.
+ }
+
+ if (! getGravityCompensation(info, &state))
+ {
+ return false; // HDAL reported an error; an error message was already logged.
+ }
+ else if (state == gravityCompensationState)
+ {
+ // If the state has been changed, log a message.
+ if (isInitialStateValid && (initialState != gravityCompensationState))
+ {
+ if (gravityCompensationState)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "gravity compensation for '" << info->deviceObject->getName() <<
+ "' changed to ENABLED.";
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(m_logger) << "gravity compensation for '" << info->deviceObject->getName() <<
+ "' changed to disabled.";
+ }
+ }
+ return true;
+ }
+ }
+
+ SURGSIM_LOG_WARNING(m_logger) << "failed to set gravity compensation for '" << info->deviceObject->getName() <<
+ "' to " << (gravityCompensationState ? "enabled" : "disabled") << " after " << maxAttempts << " attempts";
+ return false;
+}
+
+
+bool NovintScaffold::setGravityCompensation(const NovintScaffold::DeviceData* info, bool gravityCompensationState)
+{
+ bool initialState;
+ bool isInitialStateValid = getGravityCompensation(info, &initialState);
+
+ if (isInitialStateValid && (initialState == gravityCompensationState))
+ {
+ return true; // no need to do anything
+ }
+
+ return enforceGravityCompensation(info, gravityCompensationState);
+}
+
+
+static std::string convertErrorCodeToString(HDLError errorCode)
+{
+ // Convert a HDLError to text. The text was cut+pasted from the comments in Novint's hdlErrors.h file.
+ switch (errorCode)
+ {
+ case HDL_ERROR_STACK_OVERFLOW:
+ return "Overflow of error stack";
+ case HDL_ERROR_INTERNAL:
+ return "HDAL internal error";
+ case HDL_ERROR_INIT_FAILED:
+ return "Device initialization error";
+ case HDL_INIT_INI_NOT_FOUND:
+ return "Could not find configuration file";
+ case HDL_INIT_INI_DLL_STRING_NOT_FOUND:
+ return "No DLL name in configuration file";
+ case HDL_INIT_INI_MANUFACTURER_NAME_STRING_NOT_FOUND:
+ return "No MANUFACTURER_NAME value in configuration file";
+ case HDL_INIT_DLL_LOAD_ERROR:
+ return "Could not load driver DLL";
+ case HDL_INIT_DEVICE_FAILURE:
+ return "Failed to initialize device";
+ case HDL_INIT_DEVICE_ALREADY_INITED:
+ return "Device already initialized";
+ case HDL_INIT_DEVICE_NOT_CONNECTED:
+ return "Requested device not connected";
+ case HDL_SERVO_START_ERROR:
+ return "Could not start servo thread";
+ default:
+ return "<unknown>";
+ }
+}
+
+
+bool NovintScaffold::checkForFatalError(const char* message)
+{
+ HDLError errorCode = hdlGetError();
+ if (errorCode == HDL_NO_ERROR)
+ {
+ return false;
+ }
+
+ // The HDAL maintains an error stack, so in theory there could be more than one error pending.
+ // We do head recursion to get them all in the correct order, and hope we don't overrun the stack...
+ bool anotherFatalError = checkForFatalError(message);
+
+ bool isFatal = (errorCode != HDL_ERROR_STACK_OVERFLOW);
+
+ SURGSIM_LOG_SEVERE(m_logger) << "Novint: " << message << std::endl <<
+ " Error text: '" << convertErrorCodeToString(errorCode) << "'" << std::endl <<
+ " Error code: 0x" << std::hex << std::setw(4) << std::setfill('0') << errorCode << std::endl;
+
+ return (isFatal || anotherFatalError);
+}
+
+SurgSim::DataStructures::DataGroup NovintScaffold::buildDeviceInputData()
+{
+ DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_1);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_2);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_3);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_4);
+ builder.addBoolean(SurgSim::DataStructures::Names::IS_HOMED);
+ builder.addBoolean(SurgSim::DataStructures::Names::IS_POSITION_HOMED);
+ builder.addBoolean(SurgSim::DataStructures::Names::IS_ORIENTATION_HOMED);
+ return builder.createData();
+}
+
+void NovintScaffold::setPositionScale(const NovintCommonDevice* device, double scale)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
+ (*matching)->positionScale = scale;
+ }
+}
+
+void NovintScaffold::setOrientationScale(const NovintCommonDevice* device, double scale)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
+ (*matching)->orientationScale = scale;
+ }
+}
+
+
+std::shared_ptr<NovintScaffold> NovintScaffold::getOrCreateSharedInstance()
+{
+ static SurgSim::Framework::SharedInstance<NovintScaffold> sharedInstance;
+ return sharedInstance.get();
+}
+
+void NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel)
+{
+ m_defaultLogLevel = logLevel;
+}
+
+std::shared_ptr<SurgSim::Framework::Logger> NovintScaffold::getLogger() const
+{
+ return m_logger;
+}
+
+SurgSim::Framework::LogLevel NovintScaffold::m_defaultLogLevel = SurgSim::Framework::LOG_LEVEL_INFO;
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Novint/NovintScaffold.h b/SurgSim/Devices/Novint/NovintScaffold.h
new file mode 100644
index 0000000..3929d58
--- /dev/null
+++ b/SurgSim/Devices/Novint/NovintScaffold.h
@@ -0,0 +1,227 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_NOVINT_NOVINTSCAFFOLD_H
+#define SURGSIM_DEVICES_NOVINT_NOVINTSCAFFOLD_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+class NovintCommonDevice;
+class NovintDevice;
+
+
+/// A class that manages Novint Falcon devices.
+///
+/// This should support any device that can communicate using the Novint HDAL SDK toolkit, such as the
+/// off-the-shelf Novint Falcon gaming controller and the Novint Falcon with the Open Surgery Grip.
+///
+/// \sa SurgSim::Device::NovintDevice, SurgSim::Device::Novint7DofDevice
+/// \sa SurgSim::Device::NovintCommonDevice
+class NovintScaffold
+{
+public:
+ /// Constructor.
+ /// \param logger (optional) The logger to be used for the scaffold object and the devices it manages.
+ /// If unspecified or empty, a console logger will be created and used.
+ explicit NovintScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger = nullptr);
+
+ /// Destructor.
+ ~NovintScaffold();
+
+ /// Gets the logger used by this object and the devices it manages.
+ /// \return The logger.
+ std::shared_ptr<SurgSim::Framework::Logger> getLogger() const;
+
+ /// Gets or creates the scaffold shared by all NovintDevice and Novint7DofDevice instances.
+ /// The scaffold is managed using a SingleInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<NovintScaffold> getOrCreateSharedInstance();
+
+ /// Sets the default log level.
+ /// Has no effect unless called before a scaffold is created (i.e. before the first device).
+ /// \param logLevel The log level.
+ static void setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel);
+
+private:
+
+ /// Internal shared state data type.
+ struct StateData;
+ /// Internal per-device information.
+ struct DeviceData;
+ /// Wrapper for the haptic loop callback.
+ class Callback;
+ /// Wrapper for the HDAL device handle.
+ class Handle;
+
+ friend class NovintCommonDevice;
+ friend class NovintDevice;
+
+ /// Registers the specified device object.
+ /// If successful, the device object will become connected to an unused controller.
+ ///
+ /// \param device The device object to be used, which should have a unique name.
+ /// \return True if the initialization succeeds, false if it fails.
+ bool registerDevice(NovintCommonDevice* device);
+
+ /// Unregisters the specified device object.
+ /// The corresponding controller will become unused, and can be re-registered later.
+ ///
+ /// \param device The device object.
+ /// \return true on success, false on failure.
+ bool unregisterDevice(const NovintCommonDevice* device);
+
+ /// Initializes a single device, creating the necessary HDAL resources.
+ /// \param [in,out] info The device data.
+ /// \return true on success.
+ bool initializeDeviceState(DeviceData* info);
+
+ /// Finalizes a single device, destroying the necessary HDAL resources.
+ /// \param [in,out] info The device data.
+ /// \return true on success.
+ bool finalizeDeviceState(DeviceData* info);
+
+ /// Updates the device information for a single device.
+ /// \param info The device data.
+ /// \return true on success.
+ bool updateDevice(DeviceData* info);
+
+ /// Checks whether a device has been homed. If the position and/or orientation have not been homed, zeros the
+ /// respective Values. Call this before setting the data to send to the Input Component. The DeviceData's
+ /// parameter mutex should be locked before this function is called.
+ /// \param info The device data.
+ void checkDeviceHoming(DeviceData* info);
+
+ /// Calculates forces, and torques if the device is a 7Dof, and sends them to the HDAL. The force to output is
+ /// composed of a vector named "force" in the output data, plus contributions from two optional Jacobians.
+ /// If the matrix "springJacobian" is provided in the output data, a spring force & torque are generated from
+ /// its product with the difference between the current pose and the pose in the output data named "inputPose".
+ /// A damping force & torque are generated similarly. The intention is for a Behavior to calculate a nominal
+ /// force & torque as well as the desired linearized changes to the force & torque based on changes to the input's
+ /// pose & velocities. Then the linearized deltas to the output force & torque can be calculated at the haptic
+ /// update rates, thereby smoothing the output response to motion.
+ /// \param info The device data.
+ /// \note The DeviceData's parameter mutex should be locked before this function is called.
+ void calculateForceAndTorque(DeviceData* info);
+
+ /// Sets the input DataGroup, which will be pushed to the InputComponent
+ /// \param info The device data
+ void setInputData(DeviceData* info);
+
+ /// Initializes the HDAL SDK.
+ /// \return true on success.
+ bool initializeSdk();
+
+ /// Finalizes (de-initializes) the HDAL SDK.
+ /// \return true on success.
+ bool finalizeSdk();
+
+ /// Executes the operations for a single haptic frame.
+ /// Should only be called from the context of a HDAL callback.
+ /// \return true on success.
+ bool runHapticFrame();
+
+ /// Creates the haptic loop callback.
+ /// \return true on success.
+ bool createHapticLoop();
+
+ /// Destroys the haptic loop callback.
+ /// Should be called while NOT holding the internal device list mutex, to prevent deadlock.
+ /// \return true on success.
+ bool destroyHapticLoop();
+
+ /// Starts the HDAL scheduler.
+ /// \return true on success.
+ bool startScheduler();
+
+ /// Stops the HDAL scheduler.
+ /// \return true on success.
+ bool stopScheduler();
+
+ /// Gets the gravity compensation flag for the current device.
+ /// \param info The device data.
+ /// \param [out] gravityCompensationState State of the gravity compensation flag.
+ /// \return true if it succeeds, false if it fails.
+ bool getGravityCompensation(const DeviceData* info, bool* gravityCompensationState);
+
+ /// Attempts to force the gravity compensation flag for the current device to the specified value.
+ ///
+ /// Sets the flag repeatedly, until it reports the desired value, in order to work around the problem where
+ /// attempts to set the gravity compensation do not always change the actual gravity compensation flag in the
+ /// device.
+ ///
+ /// Logs a message if the state is known to have been changed.
+ ///
+ /// \param info The device data.
+ /// \param gravityCompensationState Desired state of the gravity compensation flag.
+ /// \return true if it succeeds, false if it fails.
+ bool enforceGravityCompensation(const DeviceData* info, bool gravityCompensationState);
+
+ /// Sets the gravity compensation flag for the current device, unless it's already set as desired.
+ /// \param info The device data.
+ /// \param gravityCompensationState Desired state of the gravity compensation flag.
+ /// \return true if it succeeds, false if it fails.
+ bool setGravityCompensation(const DeviceData* info, bool gravityCompensationState);
+
+ /// Check for HDAL errors, display them, and signal fatal errors.
+ /// Exactly equivalent to <code>checkForFatalError(false, message)</code>.
+ /// \param message An additional descriptive message.
+ /// \return true if there was a fatal error; false if everything is OK.
+ bool checkForFatalError(const char* message);
+
+ /// Check for HDAL errors, display them, and signal fatal errors.
+ /// Exactly equivalent to <code>checkForFatalError(message) || previousError</code>, but less nasty to read.
+ /// \param previousError True if a previous error has occurred.
+ /// \param message An additional descriptive message.
+ /// \return true if there was a fatal error or if previousError is true; false if everything is OK.
+ bool checkForFatalError(bool previousError, const char* message)
+ {
+ bool newError = checkForFatalError(message);
+ return previousError || newError;
+ }
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static SurgSim::DataStructures::DataGroup buildDeviceInputData();
+
+ /// Sets the position scale for this device.
+ /// \param device A pointer to the device.
+ /// \param scale The multiplicative factor to apply to the position.
+ void setPositionScale(const NovintCommonDevice* device, double scale);
+
+ /// Sets the orientation scale for this device.
+ /// \param device A pointer to the device.
+ /// \param scale The multiplicative factor to apply to the rotation angles.
+ void setOrientationScale(const NovintCommonDevice* device, double scale);
+
+ /// Logger used by the scaffold and all devices.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+ /// Internal scaffold state.
+ std::unique_ptr<StateData> m_state;
+
+ /// The default logging level.
+ static SurgSim::Framework::LogLevel m_defaultLogLevel;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_NOVINT_NOVINTSCAFFOLD_H
diff --git a/SurgSim/Devices/Novint/UnitTests/CMakeLists.txt b/SurgSim/Devices/Novint/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..69cbac2
--- /dev/null
+++ b/SurgSim/Devices/Novint/UnitTests/CMakeLists.txt
@@ -0,0 +1,38 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ Novint7DofDeviceTest.cpp
+ NovintDeviceTest.cpp
+ NovintScaffoldTest.cpp
+)
+
+set(LIBS
+ NovintDevice
+ SurgSimInput
+ SurgSimFramework
+ SurgSimDataStructures
+ ${Boost_LIBRARIES}
+ ${NOVINT_HDAL_SDK_LIBRARIES}
+)
+
+surgsim_add_unit_tests(NovintDeviceTest)
+
+set_target_properties(NovintDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Novint/UnitTests/Novint7DofDeviceTest.cpp b/SurgSim/Devices/Novint/UnitTests/Novint7DofDeviceTest.cpp
new file mode 100644
index 0000000..b8328a9
--- /dev/null
+++ b/SurgSim/Devices/Novint/UnitTests/Novint7DofDeviceTest.cpp
@@ -0,0 +1,219 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Novint7DofDevice class.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/Novint/Novint7DofDevice.h"
+//#include "SurgSim/Devices/Novint/NovintScaffold.h" // only needed if calling setDefaultLogLevel()
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Framework/Clock.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Device::Novint7DofDevice;
+using SurgSim::Device::NovintScaffold;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Framework::Clock;
+using SurgSim::Testing::MockInputOutput;
+
+// Define common device names used in the Novint device tests.
+extern const char* const NOVINT_TEST_DEVICE_NAME;
+extern const char* const NOVINT_TEST_DEVICE_NAME_2;
+
+TEST(Novint7DofDeviceTest, CreateUninitializedDevice)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<Novint7DofDevice> device =
+ std::make_shared<Novint7DofDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+}
+
+TEST(Novint7DofDeviceTest, CreateAndInitializeDevice)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<Novint7DofDevice> device =
+ std::make_shared<Novint7DofDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+// Note: this should work if the "Default Falcon" device can be initialized... but we have no reason to think it can,
+// so I'm going to disable the test.
+TEST(Novint7DofDeviceTest, DISABLED_CreateAndInitializeDefaultDevice)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<Novint7DofDevice> device = std::make_shared<Novint7DofDevice>("TestFalcon", "");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+TEST(Novint7DofDeviceTest, Name)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<Novint7DofDevice> device =
+ std::make_shared<Novint7DofDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_EQ("TestFalcon", device->getName());
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ EXPECT_EQ("TestFalcon", device->getName());
+}
+
+static void testCreateDeviceSeveralTimes(bool doSleep)
+{
+ for (int i = 0; i < 6; ++i)
+ {
+ std::shared_ptr<Novint7DofDevice> device =
+ std::make_shared<Novint7DofDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ if (doSleep)
+ {
+ boost::this_thread::sleep_until(Clock::now() + boost::chrono::milliseconds(100));
+ }
+ // the device will be destroyed here
+ }
+}
+
+TEST(Novint7DofDeviceTest, CreateDeviceSeveralTimes)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ testCreateDeviceSeveralTimes(true);
+}
+
+TEST(Novint7DofDeviceTest, CreateSeveralDevices)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<Novint7DofDevice> device1 =
+ std::make_shared<Novint7DofDevice>("Novint1", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ // We can't check what happens with the scaffolds, since those are no longer a part of the device's API...
+
+ std::shared_ptr<Novint7DofDevice> device2 =
+ std::make_shared<Novint7DofDevice>("Novint2", NOVINT_TEST_DEVICE_NAME_2);
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ if (! device2->initialize())
+ {
+ std::cerr << "[Warning: second Novint did not come up; is it plugged in?]" << std::endl;
+ }
+}
+
+TEST(Novint7DofDeviceTest, CreateDevicesWithSameName)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<Novint7DofDevice> device1 =
+ std::make_shared<Novint7DofDevice>("Novint", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ std::shared_ptr<Novint7DofDevice> device2 =
+ std::make_shared<Novint7DofDevice>("Novint", NOVINT_TEST_DEVICE_NAME_2);
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate name.";
+}
+
+TEST(Novint7DofDeviceTest, CreateDevicesWithSameInitializationName)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<Novint7DofDevice> device1 =
+ std::make_shared<Novint7DofDevice>("Novint1", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ std::shared_ptr<Novint7DofDevice> device2 =
+ std::make_shared<Novint7DofDevice>("Novint2", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate initialization name.";
+}
+
+TEST(Novint7DofDeviceTest, InputConsumer)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<Novint7DofDevice> device =
+ std::make_shared<Novint7DofDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device->addInputConsumer(consumer));
+
+ // Adding the same input consumer again should fail.
+ EXPECT_FALSE(device->addInputConsumer(consumer));
+
+ // Sleep for a second, to see how many times the consumer is invoked.
+ // (A Novint device is supposed to run at 1KHz.)
+ boost::this_thread::sleep_until(Clock::now() + boost::chrono::milliseconds(10000));
+
+ EXPECT_TRUE(device->removeInputConsumer(consumer));
+
+ // Removing the same input consumer again should fail.
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+
+ // Check the number of invocations.
+ EXPECT_GE(consumer->m_numTimesReceivedInput, 10*700);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 10*1300);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData(SurgSim::DataStructures::Names::POSE));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_1));
+}
+
+TEST(Novint7DofDeviceTest, OutputProducer)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<Novint7DofDevice> device =
+ std::make_shared<Novint7DofDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ std::shared_ptr<MockInputOutput> producer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_TRUE(device->setOutputProducer(producer));
+
+ // Sleep for a second, to see how many times the producer is invoked.
+ // (A Novint Falcon device is supposed to run at 1KHz.)
+ boost::this_thread::sleep_until(Clock::now() + boost::chrono::milliseconds(10000));
+
+ EXPECT_TRUE(device->removeOutputProducer(producer));
+
+ // Removing the same input producer again should fail.
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+
+ // Check the number of invocations.
+ EXPECT_GE(producer->m_numTimesRequestedOutput, 10*700);
+ EXPECT_LE(producer->m_numTimesRequestedOutput, 10*1300);
+}
diff --git a/SurgSim/Devices/Novint/UnitTests/NovintDeviceTest.cpp b/SurgSim/Devices/Novint/UnitTests/NovintDeviceTest.cpp
new file mode 100644
index 0000000..4398104
--- /dev/null
+++ b/SurgSim/Devices/Novint/UnitTests/NovintDeviceTest.cpp
@@ -0,0 +1,209 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 NovintDevice class.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/Novint/NovintDevice.h"
+//#include "SurgSim/Devices/Novint/NovintScaffold.h" // only needed if calling setDefaultLogLevel()
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Framework/Clock.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Device::NovintDevice;
+using SurgSim::Device::NovintScaffold;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Framework::Clock;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Testing::MockInputOutput;
+
+// Define common device names used in the Novint device tests.
+extern const char* const NOVINT_TEST_DEVICE_NAME = "FALCON_HTHR_R";
+extern const char* const NOVINT_TEST_DEVICE_NAME_2 = "FALCON_FRANKEN_L";
+//extern const char* const NOVINT_TEST_DEVICE_NAME = "FALCON_BURRv3_1";
+//extern const char* const NOVINT_TEST_DEVICE_NAME_2 = "FALCON_BURRv3_2";
+
+TEST(NovintDeviceTest, CreateUninitializedDevice)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+}
+
+TEST(NovintDeviceTest, CreateAndInitializeDevice)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+// Note: this should work if the "Default Falcon" device can be initialized... but we have no reason to think it can,
+// so I'm going to disable the test.
+TEST(NovintDeviceTest, DISABLED_CreateAndInitializeDefaultDevice)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", "");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+TEST(NovintDeviceTest, Name)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_EQ("TestFalcon", device->getName());
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ EXPECT_EQ("TestFalcon", device->getName());
+}
+
+static void testCreateDeviceSeveralTimes(bool doSleep)
+{
+ for (int i = 0; i < 6; ++i)
+ {
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ if (doSleep)
+ {
+ boost::this_thread::sleep_until(Clock::now() + boost::chrono::milliseconds(100));
+ }
+ // the device will be destroyed here
+ }
+}
+
+TEST(NovintDeviceTest, CreateDeviceSeveralTimes)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ testCreateDeviceSeveralTimes(true);
+}
+
+TEST(NovintDeviceTest, CreateSeveralDevices)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintDevice> device1 = std::make_shared<NovintDevice>("Novint1", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ // We can't check what happens with the scaffolds, since those are no longer a part of the device's API...
+
+ std::shared_ptr<NovintDevice> device2 = std::make_shared<NovintDevice>("Novint2", NOVINT_TEST_DEVICE_NAME_2);
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ if (! device2->initialize())
+ {
+ std::cerr << "[Warning: second Novint did not come up; is it plugged in?]" << std::endl;
+ }
+}
+
+TEST(NovintDeviceTest, CreateDevicesWithSameName)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintDevice> device1 = std::make_shared<NovintDevice>("Novint", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ std::shared_ptr<NovintDevice> device2 = std::make_shared<NovintDevice>("Novint", NOVINT_TEST_DEVICE_NAME_2);
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate name.";
+}
+
+TEST(NovintDeviceTest, CreateDevicesWithSameInitializationName)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintDevice> device1 = std::make_shared<NovintDevice>("Novint1", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ std::shared_ptr<NovintDevice> device2 = std::make_shared<NovintDevice>("Novint2", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate initialization name.";
+}
+
+TEST(NovintDeviceTest, InputConsumer)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device->addInputConsumer(consumer));
+
+ // Adding the same input consumer again should fail.
+ EXPECT_FALSE(device->addInputConsumer(consumer));
+
+ // Sleep for a second, to see how many times the consumer is invoked.
+ // (A Novint device is supposed to run at 1KHz.)
+ boost::this_thread::sleep_until(Clock::now() + boost::chrono::milliseconds(10000));
+
+ EXPECT_TRUE(device->removeInputConsumer(consumer));
+
+ // Removing the same input consumer again should fail.
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+
+ // Check the number of invocations.
+ EXPECT_GE(consumer->m_numTimesReceivedInput, 10*700);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 10*1300);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData(SurgSim::DataStructures::Names::POSE));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_1));
+}
+
+TEST(NovintDeviceTest, OutputProducer)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ std::shared_ptr<MockInputOutput> producer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_TRUE(device->setOutputProducer(producer));
+
+ // Sleep for a second, to see how many times the producer is invoked.
+ // (A Novint Falcon device is supposed to run at 1KHz.)
+ boost::this_thread::sleep_until(Clock::now() + boost::chrono::milliseconds(10000));
+
+ EXPECT_TRUE(device->removeOutputProducer(producer));
+
+ // Removing the same input producer again should fail.
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+
+ // Check the number of invocations.
+ EXPECT_GE(producer->m_numTimesRequestedOutput, 10*700);
+ EXPECT_LE(producer->m_numTimesRequestedOutput, 10*1300);
+}
diff --git a/SurgSim/Devices/Novint/UnitTests/NovintScaffoldTest.cpp b/SurgSim/Devices/Novint/UnitTests/NovintScaffoldTest.cpp
new file mode 100644
index 0000000..fed6e5b
--- /dev/null
+++ b/SurgSim/Devices/Novint/UnitTests/NovintScaffoldTest.cpp
@@ -0,0 +1,190 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 NovintScaffold class and its device interactions.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/Novint/NovintDevice.h"
+#include "SurgSim/Devices/Novint/NovintScaffold.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+
+using SurgSim::Device::NovintDevice;
+using SurgSim::Device::NovintScaffold;
+
+
+// Use common device names defined in the NovintDeviceTest code.
+extern const char* const NOVINT_TEST_DEVICE_NAME;
+
+
+TEST(NovintScaffoldTest, CreateAndDestroyScaffold)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ std::weak_ptr<NovintScaffold> scaffold1 = scaffold;
+ {
+ std::shared_ptr<NovintScaffold> stillHaveScaffold = scaffold1.lock();
+ EXPECT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ EXPECT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+ {
+ std::shared_ptr<NovintScaffold> sameScaffold = NovintScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, sameScaffold) << "Unable to get scaffold from class";
+ EXPECT_EQ(scaffold, sameScaffold) << "Scaffold mismatch!";
+ }
+
+ scaffold.reset();
+ {
+ std::shared_ptr<NovintScaffold> dontHaveScaffold = scaffold1.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ }
+
+ scaffold = NovintScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created the 2nd time!";
+ std::weak_ptr<NovintScaffold> scaffold2 = scaffold;
+ {
+ std::shared_ptr<NovintScaffold> stillHaveScaffold = scaffold2.lock();
+ ASSERT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ ASSERT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+}
+
+TEST(NovintScaffoldTest, ScaffoldLifeCycle)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::weak_ptr<NovintScaffold> lastScaffold;
+ {
+ std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ lastScaffold = scaffold;
+ }
+ {
+ std::shared_ptr<NovintScaffold> dontHaveScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ lastScaffold.reset();
+ }
+
+ {
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_NE(nullptr, device) << "Creation failed. Is a Novint device plugged in?";
+ // note: the device is NOT initialized!
+ {
+ std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<NovintScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The device has not been initialized, so it should NOT be hanging on to the device!
+ {
+ std::shared_ptr<NovintScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+ // the ("empty") device is about to get destroyed
+ }
+
+ {
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ {
+ std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<NovintScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The same scaffold is supposed to still be around because of the device
+ {
+ std::shared_ptr<NovintScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+
+ std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ EXPECT_EQ(sameScaffold, scaffold);
+ }
+ // the device and the scaffold are about to get destroyed
+ }
+
+ {
+ std::shared_ptr<NovintScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+
+ {
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Didn't this work a moment ago?";
+ std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+
+ std::shared_ptr<NovintScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+}
+
+
+TEST(NovintScaffoldTest, CreateDeviceSeveralTimes)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::weak_ptr<NovintScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ EXPECT_EQ(nullptr, lastScaffold.lock());
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold;
+ // the device and the scaffold will be destroyed here
+ }
+}
+
+
+TEST(NovintScaffoldTest, CreateDeviceSeveralTimesWithScaffoldRef)
+{
+ //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NovintScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ if (! lastScaffold)
+ {
+ lastScaffold = scaffold;
+ }
+ EXPECT_EQ(lastScaffold, scaffold);
+ // the device will be destroyed here, but the scaffold stays around because we have a shared_ptr to it.
+ }
+}
diff --git a/SurgSim/Devices/Novint/VisualTest/CMakeLists.txt b/SurgSim/Devices/Novint/VisualTest/CMakeLists.txt
new file mode 100644
index 0000000..ff6737f
--- /dev/null
+++ b/SurgSim/Devices/Novint/VisualTest/CMakeLists.txt
@@ -0,0 +1,67 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+)
+
+set(EXAMPLE_SOURCES
+ main.cpp
+)
+
+set(EXAMPLE_HEADERS
+)
+
+set(SEVEN_DOF_EXAMPLE_SOURCES
+ falcon_7dof_main.cpp
+)
+
+set(SEVEN_DOF_EXAMPLE_HEADERS
+)
+
+set(LIBS
+ NovintDevice
+ IdentityPoseDevice
+ VisualTestCommon
+ SurgSimInput
+ SurgSimFramework
+ SurgSimDataStructures
+ ${NOVINT_HDAL_SDK_LIBRARIES}
+ ${Boost_LIBRARIES}
+ ${OPENGL_LIBRARIES}
+)
+
+add_executable(NovintVisualTest
+ ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
+target_link_libraries(NovintVisualTest ${LIBS})
+surgsim_show_ide_folders(
+ "${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
+
+add_executable(Novint7DofVisualTest
+ ${SEVEN_DOF_EXAMPLE_SOURCES} ${SEVEN_DOF_EXAMPLE_HEADERS})
+target_link_libraries(Novint7DofVisualTest ${LIBS})
+surgsim_show_ide_folders(
+ "${SEVEN_DOF_EXAMPLE_SOURCES}" "${SEVEN_DOF_EXAMPLE_HEADERS}")
+
+# Put Novint7DofVisualTest into folder "Devices"
+set_target_properties(Novint7DofVisualTest PROPERTIES FOLDER "Devices")
+
+# Put NovintVisualTest into folder "Devices"
+set_target_properties(NovintVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Novint/VisualTest/falcon_7dof_main.cpp b/SurgSim/Devices/Novint/VisualTest/falcon_7dof_main.cpp
new file mode 100644
index 0000000..5c0194d
--- /dev/null
+++ b/SurgSim/Devices/Novint/VisualTest/falcon_7dof_main.cpp
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Devices/Novint/Novint7DofDevice.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+
+#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
+
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Device::Novint7DofDevice;
+using SurgSim::Device::IdentityPoseDevice;
+
+
+// Define the HDAL name of the device to use.
+static const char* const NOVINT_DEVICE_NAME = "FALCON_HTHR_R";
+//static const char* const NOVINT_DEVICE_NAME = "FALCON_FRANKEN_L";
+
+
+int main(int argc, char** argv)
+{
+ std::shared_ptr<Novint7DofDevice> toolDevice =
+ std::make_shared<Novint7DofDevice>("Novint7DofDevice", NOVINT_DEVICE_NAME);
+
+ // The square is controlled by a second device. For a simple test, we're using an IdentityPoseDevice--
+ // a pretend device that doesn't actually move.
+ std::shared_ptr<DeviceInterface> squareDevice = std::make_shared<IdentityPoseDevice>("IdentityPoseDevice");
+
+ runToolSquareTest(toolDevice, squareDevice,
+ //2345678901234567890123456789012345678901234567890123456789012345678901234567890
+ "Move the Novint Falcon device to move the sphere tool.");
+
+ std::cout << std::endl << "Exiting." << std::endl;
+ // Cleanup and shutdown will happen automatically as objects go out of scope.
+
+ return 0;
+}
diff --git a/SurgSim/Devices/Novint/VisualTest/main.cpp b/SurgSim/Devices/Novint/VisualTest/main.cpp
new file mode 100644
index 0000000..a8f59c9
--- /dev/null
+++ b/SurgSim/Devices/Novint/VisualTest/main.cpp
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Devices/Novint/NovintDevice.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+
+#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
+
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Device::NovintDevice;
+using SurgSim::Device::IdentityPoseDevice;
+
+
+// Define the HDAL name of the device to use.
+static const char* const NOVINT_DEVICE_NAME = ""; // An empty name will instantiate the default falcon.
+//static const char* const NOVINT_DEVICE_NAME = "FALCON_HTHR_R";
+//static const char* const NOVINT_DEVICE_NAME = "FALCON_FRANKEN_L";
+//static const char* const NOVINT_DEVICE_NAME = "FALCON_BURRv3_1";
+//static const char* const NOVINT_DEVICE_NAME = "FALCON_BURRv3_2";
+
+
+int main(int argc, char** argv)
+{
+ std::shared_ptr<NovintDevice> toolDevice = std::make_shared<NovintDevice>("NovintDevice", NOVINT_DEVICE_NAME);
+
+ // The square is controlled by a second device. For a simple test, we're using an IdentityPoseDevice--
+ // a pretend device that doesn't actually move.
+ std::shared_ptr<DeviceInterface> squareDevice = std::make_shared<IdentityPoseDevice>("IdentityPoseDevice");
+
+ runToolSquareTest(toolDevice, squareDevice,
+ //2345678901234567890123456789012345678901234567890123456789012345678901234567890
+ "Move the Novint Falcon device to move the sphere tool.");
+
+ std::cout << std::endl << "Exiting." << std::endl;
+ // Cleanup and shutdown will happen automatically as objects go out of scope.
+
+ return 0;
+}
diff --git a/SurgSim/Devices/Phantom/CMakeLists.txt b/SurgSim/Devices/Phantom/CMakeLists.txt
new file mode 100644
index 0000000..4b28fe2
--- /dev/null
+++ b/SurgSim/Devices/Phantom/CMakeLists.txt
@@ -0,0 +1,62 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+find_package(OpenHaptics REQUIRED)
+
+link_directories(${Boost_LIBRARY_DIRS})
+
+set(LIBS
+ ${Boost_LIBRARIES}
+ ${OPENHAPTICS_LIBRARIES}
+ SurgSimFramework
+ SurgSimInput
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${OPENHAPTICS_INCLUDE_DIR}"
+)
+
+set(PHANTOM_DEVICE_SOURCES
+ PhantomDevice.cpp
+ PhantomScaffold.cpp
+)
+
+set(PHANTOM_DEVICE_HEADERS
+ PhantomDevice.h
+ PhantomScaffold.h
+)
+
+# TODO(advornik): the installation should NOT copy all the headers...
+surgsim_add_library(
+ PhantomDevice
+ "${PHANTOM_DEVICE_SOURCES}" "${PHANTOM_DEVICE_HEADERS}"
+ SurgSim/Devices/Phantom
+)
+
+target_link_libraries(PhantomDevice ${LIBS})
+
+if(BUILD_TESTING)
+ # The unit tests will be built whenever the library is built.
+ add_subdirectory(UnitTests)
+
+ if(GLUT_FOUND)
+ add_subdirectory(VisualTest)
+ endif(GLUT_FOUND)
+endif()
+
+# Put PhantomDevice into folder "Devices"
+set_target_properties(PhantomDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Phantom/Phantom.dox b/SurgSim/Devices/Phantom/Phantom.dox
new file mode 100644
index 0000000..c45a3c1
--- /dev/null
+++ b/SurgSim/Devices/Phantom/Phantom.dox
@@ -0,0 +1,14 @@
+/*!
+
+\page Phantom Phantom: Haptic Devices
+
+The Geomagic haptic devices measure 6DOF positions and orientations, and can exert forces on the user's hand (some models exert 3DOF forces, others 6DOF).
+
+Supported models: Geomagic Touch is tested. Any model/configuration allowed by the OpenHaptics library (for Linux http://www.geomagic.com/en/products/open-haptics/specifications-2/specifications-2-21/ ) is expected to work.
+
+Dependencies:
+- Device drivers
+- OpenHaptics http://www.geomagic.com/en/products/open-haptics/overview/
+ - An environment variable named 3DTOUCH_BASE or OH_SDK_BASE must be set to the directory containing <tt>(include\\)HD\hd.h</tt>
+
+*/
\ No newline at end of file
diff --git a/SurgSim/Devices/Phantom/PhantomDevice.cpp b/SurgSim/Devices/Phantom/PhantomDevice.cpp
new file mode 100644
index 0000000..b1ec74b
--- /dev/null
+++ b/SurgSim/Devices/Phantom/PhantomDevice.cpp
@@ -0,0 +1,83 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Phantom/PhantomDevice.h"
+
+#include "SurgSim/Devices/Phantom/PhantomScaffold.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+PhantomDevice::PhantomDevice(const std::string& uniqueName, const std::string& initializationName) :
+ SurgSim::Input::CommonDevice(uniqueName, PhantomScaffold::buildDeviceInputData()),
+ m_initializationName(initializationName)
+{
+}
+
+
+PhantomDevice::~PhantomDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+
+std::string PhantomDevice::getInitializationName() const
+{
+ return m_initializationName;
+}
+
+
+bool PhantomDevice::initialize()
+{
+ SURGSIM_ASSERT(! isInitialized());
+ std::shared_ptr<PhantomScaffold> scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold);
+
+ if (! scaffold->registerDevice(this))
+ {
+ return false;
+ }
+
+ m_scaffold = std::move(scaffold);
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
+ return true;
+}
+
+
+bool PhantomDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized());
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ bool ok = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return ok;
+}
+
+
+bool PhantomDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Phantom/PhantomDevice.h b/SurgSim/Devices/Phantom/PhantomDevice.h
new file mode 100644
index 0000000..cc61915
--- /dev/null
+++ b/SurgSim/Devices/Phantom/PhantomDevice.h
@@ -0,0 +1,93 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_PHANTOM_PHANTOMDEVICE_H
+#define SURGSIM_DEVICES_PHANTOM_PHANTOMDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+class PhantomScaffold;
+
+
+/// A class implementing the communication with a SensAble/Geomagic PHANTOM device.
+///
+/// This should support any device that can communicate using the OpenHaptics 3.x toolkit, such as the
+/// PHANTOM Omni (a.k.a. Geomagic Touch), PHANTOM Desktop (a.k.a. Geomagic Touch X), and the PHANTOM Premium
+/// series devices.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Device pose (units are meters). |
+/// | bool | "button1" | %State of the first device button. |
+/// | bool | "button2" | %State of the second device button if present. |
+/// | bool | "button3" | %State of the third device button (probably doesn't exist). |
+/// | bool | "button4" | %State of the third device button (probably doesn't exist). |
+/// Note that \c button1 through \c 4 correspond to the \c HD_DEVICE_BUTTON_1 through \c 4 provided by the
+/// OpenHaptics API, but your PHANTOM device likely has fewer than 4 buttons. On one-button PHANTOM devices,
+/// the button state can be accessed through \c button1. On a PHANTOM Omni or a Geomagic Touch, \c button1
+/// corresponds to the front (blue) stylus button, and \c button2 to the rear (white/gray) stylus button.
+///
+/// \par Application output used by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | vector | "force" | %Device output force (units are newtons). |
+/// | vector | "torque" | %Device output torque (units are Nm). |
+///
+/// \sa SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class PhantomDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ ///
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ /// \param initializationName The name passed to HDAPI when initializing the device. This should match a
+ /// configured PHANTOM device; alternately, an empty string indicates the default device.
+ PhantomDevice(const std::string& uniqueName, const std::string& initializationName);
+
+ /// Destructor.
+ virtual ~PhantomDevice();
+
+ /// Gets the name used by the Phantom device configuration to refer to this device.
+ /// Note that this may or may not be the same as the device name retrieved by getName().
+ /// An empty string indicates the default device.
+ /// \return The initialization name.
+ std::string getInitializationName() const;
+
+ virtual bool initialize() override;
+
+ virtual bool finalize() override;
+
+ /// Check whether this device is initialized.
+ bool isInitialized() const;
+
+private:
+ friend class PhantomScaffold;
+
+ std::shared_ptr<PhantomScaffold> m_scaffold;
+ std::string m_initializationName;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_PHANTOM_PHANTOMDEVICE_H
diff --git a/SurgSim/Devices/Phantom/PhantomScaffold.cpp b/SurgSim/Devices/Phantom/PhantomScaffold.cpp
new file mode 100644
index 0000000..91ab750
--- /dev/null
+++ b/SurgSim/Devices/Phantom/PhantomScaffold.cpp
@@ -0,0 +1,744 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Phantom/PhantomScaffold.h"
+
+#include <vector>
+#include <memory>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+
+#include <HD/hd.h>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/Phantom/PhantomDevice.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+class PhantomScaffold::Handle
+{
+public:
+ Handle() :
+ m_deviceHandle(HD_INVALID_HANDLE),
+ m_scaffold(PhantomScaffold::getOrCreateSharedInstance())
+ {
+ }
+
+ Handle(const std::string& deviceName, const std::string& initializationName) :
+ m_deviceHandle(HD_INVALID_HANDLE),
+ m_scaffold(PhantomScaffold::getOrCreateSharedInstance())
+ {
+ create(deviceName, initializationName);
+ }
+
+ ~Handle()
+ {
+ SURGSIM_ASSERT(! isValid()) << "Expected destroy() to be called before Handle object destruction.";
+ }
+
+ bool isValid() const
+ {
+ return (m_deviceHandle != HD_INVALID_HANDLE);
+ }
+
+ bool create(const std::string& deviceName, const std::string& initializationName)
+ {
+ SURGSIM_ASSERT(! isValid());
+
+ HHD deviceHandle = HD_INVALID_HANDLE;
+ if (initializationName.length() > 0)
+ {
+ deviceHandle = hdInitDevice(initializationName.c_str());
+ }
+ else
+ {
+ deviceHandle = hdInitDevice(HD_DEFAULT_DEVICE);
+ }
+
+ if (m_scaffold->checkForFatalError("Failed to initialize"))
+ {
+ // HDAPI error message already logged
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << std::endl <<
+ " Device name: '" << deviceName << "'" << std::endl <<
+ " OpenHaptics device name: '" << initializationName << "'" << std::endl;
+ return false;
+ }
+ else if (deviceHandle == HD_INVALID_HANDLE)
+ {
+ SURGSIM_LOG_SEVERE(m_scaffold->getLogger()) << "Phantom: Failed to initialize '" << deviceName << "'" <<
+ std::endl <<
+ " Error details: unknown (HDAPI returned an invalid handle)" << std::endl <<
+ " OpenHaptics device name: '" << initializationName << "'" << std::endl;
+ return false;
+ }
+
+ m_deviceHandle = deviceHandle;
+ return true;
+ }
+
+ bool destroy()
+ {
+ SURGSIM_ASSERT(isValid());
+
+ HHD deviceHandle = m_deviceHandle;
+ if (deviceHandle == HD_INVALID_HANDLE)
+ {
+ return false;
+ }
+ m_deviceHandle = HD_INVALID_HANDLE;
+
+ hdDisableDevice(deviceHandle);
+ m_scaffold->checkForFatalError("Couldn't disable device");
+ return true;
+ }
+
+ HHD get() const
+ {
+ SURGSIM_ASSERT(isValid());
+ return m_deviceHandle;
+ }
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ Handle(const Handle&) /*= delete*/;
+ Handle& operator=(const Handle&) /*= delete*/;
+
+ /// The OpenHaptics device handle (or HD_INVALID_HANDLE if not valid).
+ HHD m_deviceHandle;
+ /// The scaffold.
+ std::shared_ptr<PhantomScaffold> m_scaffold;
+};
+
+
+class PhantomScaffold::Callback
+{
+public:
+ Callback() :
+ m_callbackHandle(0),
+ m_haveCallback(false),
+ m_scaffold(PhantomScaffold::getOrCreateSharedInstance())
+ {
+ create();
+ }
+
+ ~Callback()
+ {
+ if (m_haveCallback)
+ {
+ destroy();
+ }
+ }
+
+ bool isValid() const
+ {
+ return m_haveCallback;
+ }
+
+ bool create()
+ {
+ SURGSIM_ASSERT(! m_haveCallback);
+ m_callbackHandle = hdScheduleAsynchronous(run, m_scaffold.get(), HD_DEFAULT_SCHEDULER_PRIORITY);
+ if (m_scaffold->checkForFatalError("Couldn't run haptic callback"))
+ {
+ return false;
+ }
+ m_haveCallback = true;
+ return true;
+ }
+
+ bool destroy()
+ {
+ SURGSIM_ASSERT(m_haveCallback);
+ hdUnschedule(m_callbackHandle);
+ if (m_scaffold->checkForFatalError("Couldn't stop haptic callback"))
+ {
+ return false;
+ }
+ m_haveCallback = false;
+ return true;
+ }
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ Callback(const Callback&) /*= delete*/;
+ Callback& operator=(const Callback&) /*= delete*/;
+
+ /// The callback wrapper passed to OpenHaptics.
+ /// \param [in,out] data The user data (in our case, the scaffold pointer).
+ /// \return HD_CALLBACK_CONTINUE to wait for the next frame, or HD_CALLBACK_DONE to terminate further calls.
+ static HDCallbackCode HDCALLBACK run(void* data);
+
+ /// The haptic loop callback handle.
+ HDSchedulerHandle m_callbackHandle;
+ /// True if the callback has been created (and not destroyed).
+ bool m_haveCallback;
+ /// The scaffold.
+ std::shared_ptr<PhantomScaffold> m_scaffold;
+};
+
+
+struct PhantomScaffold::DeviceData
+{
+ /// Initialize the state.
+ DeviceData(const std::string& apiName, PhantomDevice* device) :
+ initializationName(apiName),
+ deviceObject(device),
+ position(Vector3d::Zero()),
+ linearVelocity(Vector3d::Zero()),
+ scaledPose(RigidTransform3d::Identity()),
+ force(Vector3d::Zero()),
+ torque(Vector3d::Zero()),
+ buttonsBuffer(0)
+ {
+ }
+
+ /// The OpenHaptics device name.
+ const std::string initializationName;
+ /// The corresponding device object.
+ PhantomDevice* const deviceObject;
+
+ /// The device handle wrapper.
+ PhantomScaffold::Handle deviceHandle;
+
+ /// The raw button state read from the device.
+ int buttonsBuffer;
+
+ /// The position value from the device.
+ Vector3d position;
+ /// The linear velocity value from the device.
+ Vector3d linearVelocity;
+ /// The pose value from the device, after scaling.
+ RigidTransform3d scaledPose;
+
+ /// The force value to be written to the device, in Newtons.
+ Vector3d force;
+
+ /// The torque value to be written to the device, in milliNewton-meters.
+ Vector3d torque;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ DeviceData(const DeviceData&) /*= delete*/;
+ DeviceData& operator=(const DeviceData&) /*= delete*/;
+};
+
+
+struct PhantomScaffold::StateData
+{
+public:
+ /// Initialize the state.
+ StateData() : isApiInitialized(false)
+ {
+ }
+
+ /// True if the API has been initialized (and not finalized).
+ bool isApiInitialized;
+
+ /// Wrapper for the haptic loop callback handle.
+ std::unique_ptr<PhantomScaffold::Callback> callback;
+
+ /// The list of known devices.
+ std::list<std::unique_ptr<PhantomScaffold::DeviceData>> activeDeviceList;
+
+ /// The mutex that protects the list of known devices.
+ boost::mutex mutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ StateData(const StateData&) /*= delete*/;
+ StateData& operator=(const StateData&) /*= delete*/;
+};
+
+
+HDCallbackCode HDCALLBACK PhantomScaffold::Callback::run(void* data)
+{
+ PhantomScaffold* scaffold = static_cast<PhantomScaffold*>(data);
+ if (! scaffold->runHapticFrame())
+ {
+ //...do something?...
+ }
+
+ // Should return HD_CALLBACK_CONTINUE to wait for the next frame, or HD_CALLBACK_DONE to terminate the calls.
+ return HD_CALLBACK_CONTINUE;
+}
+
+
+
+PhantomScaffold::PhantomScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
+ m_logger(logger), m_state(new StateData)
+{
+ if (! m_logger)
+ {
+ m_logger = SurgSim::Framework::Logger::getLogger("Phantom device");
+ m_logger->setThreshold(m_defaultLogLevel);
+ }
+
+ {
+ // Drain the HDAPI error stack
+ HDErrorInfo error = hdGetError();
+ while (error.errorCode != HD_SUCCESS)
+ {
+ error = hdGetError();
+ }
+ }
+
+ SURGSIM_LOG_DEBUG(m_logger) << "Phantom: Shared scaffold created.";
+}
+
+
+PhantomScaffold::~PhantomScaffold()
+{
+ if (m_state->callback)
+ {
+ destroyHapticLoop();
+ }
+ // The following block controls the duration of the mutex being locked.
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (! m_state->activeDeviceList.empty())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Phantom: Destroying scaffold while devices are active!?!";
+ // do anything special with each device?
+ m_state->activeDeviceList.clear();
+ }
+
+ if (m_state->isApiInitialized)
+ {
+ finalizeSdk();
+ }
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Phantom: Shared scaffold destroyed.";
+}
+
+
+bool PhantomScaffold::registerDevice(PhantomDevice* device)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (! m_state->isApiInitialized)
+ {
+ if (! initializeSdk())
+ {
+ return false;
+ }
+ }
+
+ // Make sure the object is unique.
+ auto sameObject = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ SURGSIM_ASSERT(sameObject == m_state->activeDeviceList.end()) << "Phantom: Tried to register a device" <<
+ " which is already present!";
+
+ // Make sure the name is unique.
+ const std::string deviceName = device->getName();
+ auto sameName = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [&deviceName](const std::unique_ptr<DeviceData>& info) { return info->deviceObject->getName() == deviceName; });
+ if (sameName != m_state->activeDeviceList.end())
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Phantom: Tried to register a device when the same name is" <<
+ " already present!";
+ return false;
+ }
+
+ // Make sure the initialization name is unique.
+ const std::string initializationName = device->getInitializationName();
+ auto sameInitializationName = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [&deviceName](const std::unique_ptr<DeviceData>& info)
+ { return info->deviceObject->getInitializationName() == deviceName; });
+ if (sameInitializationName != m_state->activeDeviceList.end())
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Phantom: Tried to register a device when the same initialization" <<
+ " (OpenHaptics) name is already present!";
+ return false;
+ }
+
+ // Construct the object, start its thread, then move it to the list.
+ // Note that since Visual Studio 2010 doesn't support multi-argument emplace_back() for STL containers, storing a
+ // list of unique_ptr results in nicer code than storing a list of DeviceData values directly.
+ std::unique_ptr<DeviceData> info(new DeviceData(initializationName, device));
+ if (! initializeDeviceState(info.get()))
+ {
+ return false; // message already printed
+ }
+ m_state->activeDeviceList.emplace_back(std::move(info));
+
+ if (m_state->activeDeviceList.size() == 1)
+ {
+ // If this is the first device, create the haptic loop as well.
+ // The haptic loop should be created AFTER initializing the device, or OpenHaptics will complain.
+ createHapticLoop();
+ }
+ return true;
+}
+
+
+bool PhantomScaffold::unregisterDevice(const PhantomDevice* const device)
+{
+ std::unique_ptr<DeviceData> savedInfo;
+ bool haveOtherDevices = false;
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ savedInfo = std::move(*matching);
+ m_state->activeDeviceList.erase(matching);
+ // the iterator is now invalid but that's OK
+ }
+ haveOtherDevices = (m_state->activeDeviceList.size() > 0);
+ }
+
+ bool status = true;
+ if (! savedInfo)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Phantom: Attempted to release a non-registered device.";
+ status = false;
+ }
+ else
+ {
+ // If you attempt to destroy the device while the haptic callback is active, you see lots of nasty errors
+ // under OpenHaptics 3.0. The solution seems to be to disable the haptic callback when destroying the device.
+ destroyHapticLoop();
+
+ finalizeDeviceState(savedInfo.get());
+ savedInfo.reset(nullptr);
+
+ if (haveOtherDevices)
+ {
+ // If there are other devices left, we need to recreate the haptic callback.
+ // If there aren't, we don't need the callback... and moreover, trying to create it will fail.
+ createHapticLoop();
+ }
+ }
+ return status;
+}
+
+
+bool PhantomScaffold::initializeDeviceState(DeviceData* info)
+{
+ SURGSIM_ASSERT(! info->deviceHandle.isValid());
+
+ if (! info->deviceHandle.create(info->deviceObject->getName(), info->deviceObject->getInitializationName()))
+ {
+ return false; // message was already printed
+ }
+
+ // Enable forces.
+ hdMakeCurrentDevice(info->deviceHandle.get());
+ hdEnable(HD_FORCE_OUTPUT);
+ checkForFatalError("Couldn't enable forces");
+
+ return true;
+}
+
+
+bool PhantomScaffold::finalizeDeviceState(DeviceData* info)
+{
+ bool status = false;
+ if (info->deviceHandle.isValid())
+ {
+ status = info->deviceHandle.destroy();
+ }
+ return status;
+}
+
+
+bool PhantomScaffold::updateDevice(PhantomScaffold::DeviceData* info)
+{
+ //boost::lock_guard<boost::mutex> lock(info->parametersMutex);
+
+ hdBeginFrame(info->deviceHandle.get());
+
+ // Receive the current device position (in millimeters!), pose transform, and button state bitmap.
+ hdGetDoublev(HD_CURRENT_POSITION, info->position.data());
+ info->scaledPose.translation() = info->position * 0.001; // convert from millimeters to meters!
+
+ hdGetDoublev(HD_CURRENT_VELOCITY, info->linearVelocity.data());
+ //TODO(ryanbeasley): convert HD_CURRENT_ANGULAR_VELOCITY to a rotation vector and store in info->angularVelocity.
+
+ Eigen::Matrix<double, 4, 4, Eigen::ColMajor> transform;
+ hdGetDoublev(HD_CURRENT_TRANSFORM, transform.data());
+ info->scaledPose.linear() = transform.block<3,3>(0, 0); // store orientation in a RigidTransform3d
+
+ hdGetIntegerv(HD_CURRENT_BUTTONS, &(info->buttonsBuffer));
+
+ calculateForceAndTorque(info);
+
+ // Set the force command (in newtons).
+ hdSetDoublev(HD_CURRENT_FORCE, info->force.data());
+
+ // Set the torque command.
+ hdSetDoublev(HD_CURRENT_GIMBAL_TORQUE, info->torque.data());
+
+ hdEndFrame(info->deviceHandle.get());
+
+ setInputData(info);
+
+ bool fatalError = checkForFatalError("Error in device update");
+
+ return !fatalError;
+}
+
+
+void PhantomScaffold::calculateForceAndTorque(PhantomScaffold::DeviceData* info)
+{
+ typedef Eigen::Matrix<double, 6, 1> Vector6d;
+ const SurgSim::DataStructures::DataGroup& outputData = info->deviceObject->getOutputData();
+
+ // Get the nominal force and torque, if provided.
+ Vector3d nominalForce = Vector3d::Zero();
+ outputData.vectors().get(SurgSim::DataStructures::Names::FORCE, &nominalForce);
+ Vector3d nominalTorque = Vector3d::Zero();
+ Vector6d nominalForceAndTorque = Vector6d::Zero();
+ SurgSim::Math::setSubVector(nominalForce, 0, 3, &nominalForceAndTorque);
+
+ // If the springJacobian was provided, multiply with the change in position since the output data was set,
+ // to get a delta force & torque. This way a linearized output force & torque is calculated at haptic update rates.
+ Vector6d forceAndTorqueFromDeltaPosition = Vector6d::Zero();
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType springJacobian;
+ if (outputData.matrices().get(SurgSim::DataStructures::Names::SPRING_JACOBIAN, &springJacobian))
+ {
+ RigidTransform3d poseForNominal = info->scaledPose;
+ outputData.poses().get(SurgSim::DataStructures::Names::INPUT_POSE, &poseForNominal);
+
+ Vector3d rotationVector = Vector3d::Zero();
+ SurgSim::Math::computeRotationVector(info->scaledPose, poseForNominal, &rotationVector);
+
+ Vector6d deltaPosition;
+ SurgSim::Math::setSubVector(info->scaledPose.translation() - poseForNominal.translation(), 0, 3,
+ &deltaPosition);
+ SurgSim::Math::setSubVector(rotationVector, 1, 3, &deltaPosition);
+
+ forceAndTorqueFromDeltaPosition = springJacobian * deltaPosition;
+ }
+
+ // If the damperJacobian was provided, calculate a delta force & torque based on the change in velocity.
+ Vector6d forceAndTorqueFromDeltaVelocity = Vector6d::Zero();
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType damperJacobian;
+ if (outputData.matrices().get(SurgSim::DataStructures::Names::DAMPER_JACOBIAN, &damperJacobian))
+ {
+ Vector3d angularVelocity = Vector3d::Zero();
+
+ Vector3d linearVelocityForNominal = info->linearVelocity;
+ outputData.vectors().get(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY, &linearVelocityForNominal);
+ Vector3d angularVelocityForNominal = angularVelocity;
+ outputData.vectors().get(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY, &angularVelocityForNominal);
+
+ Vector6d deltaVelocity;
+ SurgSim::Math::setSubVector(info->linearVelocity - linearVelocityForNominal, 0, 3, &deltaVelocity);
+ SurgSim::Math::setSubVector(angularVelocity - angularVelocityForNominal, 1, 3, &deltaVelocity);
+
+ forceAndTorqueFromDeltaVelocity = damperJacobian * deltaVelocity;
+ }
+
+ Vector6d forceAndTorque = nominalForceAndTorque + forceAndTorqueFromDeltaPosition +
+ forceAndTorqueFromDeltaVelocity;
+
+ info->force = SurgSim::Math::getSubVector(forceAndTorque, 0, 3);
+ // convert the torque from Newton-meters to milliNewton-meters as expected by the hardware library.
+ info->torque = SurgSim::Math::getSubVector(forceAndTorque, 1, 3) * 1000.0;
+}
+
+
+void PhantomScaffold::setInputData(DeviceData* info)
+{
+ // TODO(bert): this code should cache the access indices.
+ SurgSim::DataStructures::DataGroup& inputData = info->deviceObject->getInputData();
+ inputData.poses().set(SurgSim::DataStructures::Names::POSE, info->scaledPose);
+ inputData.vectors().set(SurgSim::DataStructures::Names::LINEAR_VELOCITY, info->linearVelocity);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_1,
+ (info->buttonsBuffer & HD_DEVICE_BUTTON_1) != 0);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_2,
+ (info->buttonsBuffer & HD_DEVICE_BUTTON_2) != 0);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_3,
+ (info->buttonsBuffer & HD_DEVICE_BUTTON_3) != 0);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_4,
+ (info->buttonsBuffer & HD_DEVICE_BUTTON_4) != 0);
+}
+
+
+bool PhantomScaffold::initializeSdk()
+{
+ SURGSIM_ASSERT(! m_state->isApiInitialized);
+
+ // nothing to do!
+
+ m_state->isApiInitialized = true;
+ return true;
+}
+
+
+bool PhantomScaffold::finalizeSdk()
+{
+ SURGSIM_ASSERT(m_state->isApiInitialized);
+
+ // nothing to do!
+
+ m_state->isApiInitialized = false;
+ return true;
+}
+
+
+bool PhantomScaffold::runHapticFrame()
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ for (auto it = m_state->activeDeviceList.begin(); it != m_state->activeDeviceList.end(); ++it)
+ {
+ (*it)->deviceObject->pullOutput();
+ if (updateDevice((*it).get()))
+ {
+ (*it)->deviceObject->pushInput();
+ }
+ }
+
+ return true;
+}
+
+
+bool PhantomScaffold::createHapticLoop()
+{
+ SURGSIM_ASSERT(! m_state->callback);
+
+ if (! startScheduler())
+ {
+ return false;
+ }
+
+ std::unique_ptr<Callback> callback(new Callback);
+ if (! callback->isValid())
+ {
+ stopScheduler();
+ return false;
+ }
+
+ m_state->callback = std::move(callback);
+ return true;
+}
+
+
+bool PhantomScaffold::destroyHapticLoop()
+{
+ SURGSIM_ASSERT(m_state->callback);
+
+ checkForFatalError("Error prior to stopping haptic callback"); // NOT considered an error for return code!
+
+ bool didDestroy = m_state->callback->destroy();
+ m_state->callback.reset(nullptr);
+
+ bool didStop = stopScheduler();
+
+ return didDestroy && didStop;
+}
+
+
+bool PhantomScaffold::startScheduler()
+{
+ hdStartScheduler();
+ if (checkForFatalError("Couldn't start the scheduler"))
+ {
+ return false;
+ }
+ return true;
+}
+
+
+bool PhantomScaffold::stopScheduler()
+{
+ hdStopScheduler();
+ if (checkForFatalError("Couldn't stop the scheduler"))
+ {
+ return false;
+ }
+ return true;
+}
+
+
+bool PhantomScaffold::checkForFatalError(const char* message)
+{
+ HDErrorInfo error = hdGetError();
+ if (error.errorCode == HD_SUCCESS)
+ {
+ return false;
+ }
+
+ // The HD API maintains an error stack, so in theory there could be more than one error pending.
+ // We do head recursion to get them all in the correct order, and hope we don't overrun the stack...
+ bool anotherFatalError = checkForFatalError(message);
+
+ bool isFatal = ((error.errorCode != HD_WARM_MOTORS) &&
+ (error.errorCode != HD_EXCEEDED_MAX_FORCE) &&
+ (error.errorCode != HD_EXCEEDED_MAX_FORCE_IMPULSE) &&
+ (error.errorCode != HD_EXCEEDED_MAX_VELOCITY) &&
+ (error.errorCode != HD_FORCE_ERROR));
+
+ SURGSIM_LOG_SEVERE(m_logger) << "Phantom: " << message << std::endl <<
+ " Error text: '" << hdGetErrorString(error.errorCode) << "'" << std::endl <<
+ " Error code: 0x" << std::hex << std::setw(4) << std::setfill('0') << error.errorCode <<
+ " (internal: " << std::dec << error.internalErrorCode << ")" << std::endl;
+
+ return (isFatal || anotherFatalError);
+}
+
+SurgSim::DataStructures::DataGroup PhantomScaffold::buildDeviceInputData()
+{
+ DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ builder.addVector(SurgSim::DataStructures::Names::LINEAR_VELOCITY);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_1);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_2);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_3);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_4);
+ return builder.createData();
+}
+
+std::shared_ptr<PhantomScaffold> PhantomScaffold::getOrCreateSharedInstance()
+{
+ static SurgSim::Framework::SharedInstance<PhantomScaffold> sharedInstance;
+ return sharedInstance.get();
+}
+
+void PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel)
+{
+ m_defaultLogLevel = logLevel;
+}
+
+SurgSim::Framework::LogLevel PhantomScaffold::m_defaultLogLevel = SurgSim::Framework::LOG_LEVEL_INFO;
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Phantom/PhantomScaffold.h b/SurgSim/Devices/Phantom/PhantomScaffold.h
new file mode 100644
index 0000000..a3ae215
--- /dev/null
+++ b/SurgSim/Devices/Phantom/PhantomScaffold.h
@@ -0,0 +1,175 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_PHANTOM_PHANTOMSCAFFOLD_H
+#define SURGSIM_DEVICES_PHANTOM_PHANTOMSCAFFOLD_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+class PhantomDevice;
+
+/// A class that manages Sensable PHANTOM devices.
+///
+/// This should support any PHANTOM device that can communicate using OpenHaptics 3.0 toolkit, such as PHANTOM
+/// Omni, PHANTOM Desktop, and the PHANTOM Premium series devices. The implementation is currently limited to
+/// 3DoF haptic output (forces only, no torques).
+///
+/// \sa SurgSim::Device::PhantomDevice
+class PhantomScaffold
+{
+public:
+ /// Constructor.
+ /// \param logger (optional) The logger to be used for the scaffold object and the devices it manages.
+ /// If unspecified or empty, a console logger will be created and used.
+ explicit PhantomScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger = nullptr);
+
+ /// Destructor.
+ ~PhantomScaffold();
+
+ /// Gets the logger used by this object and the devices it manages.
+ /// \return The logger.
+ std::shared_ptr<SurgSim::Framework::Logger> getLogger() const
+ {
+ return m_logger;
+ }
+
+ /// Gets or creates the scaffold shared by all PhantomDevice instances.
+ /// The scaffold is managed using a SingleInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<PhantomScaffold> getOrCreateSharedInstance();
+
+ /// Sets the default log level.
+ /// Has no effect unless called before a scaffold is created (i.e. before the first device).
+ /// \param logLevel The log level.
+ static void setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel);
+
+private:
+
+ /// Internal shared state data type.
+ struct StateData;
+ /// Interal per-device information.
+ struct DeviceData;
+ /// Wrapper for the haptic loop callback.
+ class Callback;
+ /// Wrapper for the OpenHaptics device handle.
+ class Handle;
+
+ friend class PhantomDevice;
+
+ /// Registers the specified device object.
+ /// If successful, the device object will become connected to an unused controller.
+ ///
+ /// \param device The device object to be used, which should have a unique name.
+ /// \return True if the initialization succeeds, false if it fails.
+ bool registerDevice(PhantomDevice* device);
+
+ /// Unregisters the specified device object.
+ /// The corresponding controller will become unused, and can be re-registered later.
+ ///
+ /// \param device The device object.
+ /// \return true on success, false on failure.
+ bool unregisterDevice(const PhantomDevice* device);
+
+ /// Initializes a single device, creating the necessary HDAPI resources.
+ /// \param [in,out] info The device data.
+ /// \return true on success.
+ bool initializeDeviceState(DeviceData* info);
+
+ /// Finalizes a single device, destroying the necessary HDAPI resources.
+ /// \param [in,out] info The device data.
+ /// \return true on success.
+ bool finalizeDeviceState(DeviceData* info);
+
+ /// Updates the device information for a single device.
+ /// \param info The device data.
+ /// \return true on success.
+ bool updateDevice(DeviceData* info);
+
+ /// Calculates forces and torques and sends them to the device library. The force to output is
+ /// composed of a vector named "force" in the output data, plus contributions from two optional Jacobians.
+ /// If the matrix "springJacobian" is provided in the output data, a spring force & torque are generated from
+ /// its product with the difference between the current pose and the pose in the output data named "inputPose".
+ /// A damping force & torque are generated similarly. The intention is for a Behavior to calculate a nominal
+ /// force & torque as well as the desired linearized changes to the force & torque based on changes to the input's
+ /// pose & velocities. Then the linearized deltas to the output force & torque can be calculated at the haptic
+ /// update rates, thereby smoothing the output response to motion.
+ /// \param info The device data.
+ void calculateForceAndTorque(DeviceData* info);
+
+ /// Sets the input DataGroup, which will be pushed to the InputComponent
+ /// \param info The device data
+ void setInputData(DeviceData* info);
+
+ /// Initializes the OpenHaptics SDK.
+ /// \return true on success.
+ bool initializeSdk();
+
+ /// Finalizes (de-initializes) the OpenHaptics SDK.
+ /// \return true on success.
+ bool finalizeSdk();
+
+ /// Executes the operations for a single haptic frame.
+ /// Should only be called from the context of an OpenHaptics callback.
+ /// \return true on success.
+ bool runHapticFrame();
+
+ /// Creates the haptic loop callback.
+ /// \return true on success.
+ bool createHapticLoop();
+
+ /// Destroys the haptic loop callback.
+ /// Should be called while NOT holding the internal device list mutex, to prevent deadlock.
+ /// \return true on success.
+ bool destroyHapticLoop();
+
+ /// Starts the OpenHaptics scheduler.
+ /// \return true on success.
+ bool startScheduler();
+
+ /// Stops the OpenHaptics scheduler.
+ /// \return true on success.
+ bool stopScheduler();
+
+ /// Check for OpenHaptics HDAPI errors, display them, and signal fatal errors.
+ /// \param message An additional descriptive message.
+ /// \return true if there was a fatal error; false if everything is OK.
+ bool checkForFatalError(const char* message);
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static SurgSim::DataStructures::DataGroup buildDeviceInputData();
+
+
+
+ /// Logger used by the scaffold and all devices.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+ /// Internal scaffold state.
+ std::unique_ptr<StateData> m_state;
+
+ /// The default logging level.
+ static SurgSim::Framework::LogLevel m_defaultLogLevel;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_PHANTOM_PHANTOMSCAFFOLD_H
diff --git a/SurgSim/Devices/Phantom/UnitTests/CMakeLists.txt b/SurgSim/Devices/Phantom/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..a591401
--- /dev/null
+++ b/SurgSim/Devices/Phantom/UnitTests/CMakeLists.txt
@@ -0,0 +1,37 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ PhantomDeviceTest.cpp
+ PhantomScaffoldTest.cpp
+)
+
+set(LIBS
+ PhantomDevice
+ SurgSimInput
+ SurgSimFramework
+ SurgSimDataStructures
+ ${Boost_LIBRARIES}
+ ${OPENHAPTICS_LIBRARIES}
+)
+
+surgsim_add_unit_tests(PhantomDeviceTest)
+
+set_target_properties(PhantomDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Phantom/UnitTests/PhantomDeviceTest.cpp b/SurgSim/Devices/Phantom/UnitTests/PhantomDeviceTest.cpp
new file mode 100644
index 0000000..ab41c8d
--- /dev/null
+++ b/SurgSim/Devices/Phantom/UnitTests/PhantomDeviceTest.cpp
@@ -0,0 +1,199 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 PhantomDevice class.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/Phantom/PhantomDevice.h"
+#include "SurgSim/Devices/Phantom/PhantomScaffold.h" // only needed if calling setDefaultLogLevel()
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Device::PhantomDevice;
+using SurgSim::Device::PhantomScaffold;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Testing::MockInputOutput;
+
+TEST(PhantomDeviceTest, CreateUninitializedDevice)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+}
+
+TEST(PhantomDeviceTest, CreateAndInitializeDevice)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+TEST(PhantomDeviceTest, CreateAndInitializeDefaultDevice)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+TEST(PhantomDeviceTest, Name)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_EQ("TestPhantom", device->getName());
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+ EXPECT_EQ("TestPhantom", device->getName());
+}
+
+static void testCreateDeviceSeveralTimes(bool doSleep)
+{
+ for (int i = 0; i < 6; ++i)
+ {
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+ if (doSleep)
+ {
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(100));
+ }
+ // the device will be destroyed here
+ }
+}
+
+TEST(PhantomDeviceTest, CreateDeviceSeveralTimes)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ testCreateDeviceSeveralTimes(true);
+}
+
+TEST(PhantomDeviceTest, CreateSeveralDevices)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomDevice> device1 = std::make_shared<PhantomDevice>("Phantom1", "Default PHANToM");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+
+ // We can't check what happens with the scaffolds, since those are no longer a part of the device's API...
+
+ std::shared_ptr<PhantomDevice> device2 = std::make_shared<PhantomDevice>("Phantom2", "Second PHANToM");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ if (! device2->initialize())
+ {
+ std::cerr << "[Warning: second Phantom did not come up; is it plugged in?]" << std::endl;
+ }
+}
+
+TEST(PhantomDeviceTest, CreateDevicesWithSameName)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomDevice> device1 = std::make_shared<PhantomDevice>("Phantom", "Default PHANToM");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+
+ std::shared_ptr<PhantomDevice> device2 = std::make_shared<PhantomDevice>("Phantom", "Second PHANToM");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate name.";
+}
+
+TEST(PhantomDeviceTest, CreateDevicesWithSameInitializationName)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomDevice> device1 = std::make_shared<PhantomDevice>("Phantom1", "Default PHANToM");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+
+ std::shared_ptr<PhantomDevice> device2 = std::make_shared<PhantomDevice>("Phantom2", "Default PHANToM");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate initialization name.";
+}
+
+TEST(PhantomDeviceTest, InputConsumer)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device->addInputConsumer(consumer));
+
+ // Adding the same input consumer again should fail.
+ EXPECT_FALSE(device->addInputConsumer(consumer));
+
+ // Sleep for a second, to see how many times the consumer is invoked.
+ // (A Phantom device is supposed to run at 1KHz.)
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeInputConsumer(consumer));
+
+ // Removing the same input consumer again should fail.
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+
+ // Check the number of invocations.
+ EXPECT_GE(consumer->m_numTimesReceivedInput, 700);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 1300);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData(SurgSim::DataStructures::Names::POSE));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_1));
+}
+
+TEST(PhantomDeviceTest, OutputProducer)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+
+ std::shared_ptr<MockInputOutput> producer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_TRUE(device->setOutputProducer(producer));
+
+ // Sleep for a second, to see how many times the producer is invoked.
+ // (A Phantom device is supposed to run at 1KHz.)
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeOutputProducer(producer));
+
+ // Removing the same input producer again should fail.
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+
+ // Check the number of invocations.
+ EXPECT_GE(producer->m_numTimesRequestedOutput, 700);
+ EXPECT_LE(producer->m_numTimesRequestedOutput, 1300);
+}
diff --git a/SurgSim/Devices/Phantom/UnitTests/PhantomScaffoldTest.cpp b/SurgSim/Devices/Phantom/UnitTests/PhantomScaffoldTest.cpp
new file mode 100644
index 0000000..488b726
--- /dev/null
+++ b/SurgSim/Devices/Phantom/UnitTests/PhantomScaffoldTest.cpp
@@ -0,0 +1,185 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 PhantomScaffold class and its device interactions.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/Phantom/PhantomDevice.h"
+#include "SurgSim/Devices/Phantom/PhantomScaffold.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+
+using SurgSim::Device::PhantomDevice;
+using SurgSim::Device::PhantomScaffold;
+
+TEST(PhantomScaffoldTest, CreateAndDestroyScaffold)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomScaffold> scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ std::weak_ptr<PhantomScaffold> scaffold1 = scaffold;
+ {
+ std::shared_ptr<PhantomScaffold> stillHaveScaffold = scaffold1.lock();
+ EXPECT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ EXPECT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+ {
+ std::shared_ptr<PhantomScaffold> sameScaffold = PhantomScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, sameScaffold) << "Unable to get scaffold from class";
+ EXPECT_EQ(scaffold, sameScaffold) << "Scaffold mismatch!";
+ }
+
+ scaffold.reset();
+ {
+ std::shared_ptr<PhantomScaffold> dontHaveScaffold = scaffold1.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ }
+
+ scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created the 2nd time!";
+ std::weak_ptr<PhantomScaffold> scaffold2 = scaffold;
+ {
+ std::shared_ptr<PhantomScaffold> stillHaveScaffold = scaffold2.lock();
+ ASSERT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ ASSERT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+}
+
+TEST(PhantomScaffoldTest, ScaffoldLifeCycle)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::weak_ptr<PhantomScaffold> lastScaffold;
+ {
+ std::shared_ptr<PhantomScaffold> scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ lastScaffold = scaffold;
+ }
+ {
+ std::shared_ptr<PhantomScaffold> dontHaveScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ lastScaffold.reset();
+ }
+
+ {
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_NE(nullptr, device) << "Creation failed. Is a Phantom device plugged in?";
+ // note: the device is NOT initialized!
+ {
+ std::shared_ptr<PhantomScaffold> scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<PhantomScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The device has not been initialized, so it should NOT be hanging on to the device!
+ {
+ std::shared_ptr<PhantomScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+ // the ("empty") device is about to get destroyed
+ }
+
+ {
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+ {
+ std::shared_ptr<PhantomScaffold> scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<PhantomScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The same scaffold is supposed to still be around because of the device
+ {
+ std::shared_ptr<PhantomScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+
+ std::shared_ptr<PhantomScaffold> scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ EXPECT_EQ(sameScaffold, scaffold);
+ }
+ // the device and the scaffold are about to get destroyed
+ }
+
+ {
+ std::shared_ptr<PhantomScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+
+ {
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Didn't this work a moment ago?";
+ std::shared_ptr<PhantomScaffold> scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+
+ std::shared_ptr<PhantomScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+}
+
+
+TEST(PhantomScaffoldTest, CreateDeviceSeveralTimes)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::weak_ptr<PhantomScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ EXPECT_EQ(nullptr, lastScaffold.lock());
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+ std::shared_ptr<PhantomScaffold> scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold;
+ // the device and the scaffold will be destroyed here
+ }
+}
+
+
+TEST(PhantomScaffoldTest, CreateDeviceSeveralTimesWithScaffoldRef)
+{
+ //PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<PhantomScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
+ std::shared_ptr<PhantomScaffold> scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ if (! lastScaffold)
+ {
+ lastScaffold = scaffold;
+ }
+ EXPECT_EQ(lastScaffold, scaffold);
+ // the device will be destroyed here, but the scaffold stays around because we have a shared_ptr to it.
+ }
+}
diff --git a/SurgSim/Devices/Phantom/VisualTest/CMakeLists.txt b/SurgSim/Devices/Phantom/VisualTest/CMakeLists.txt
new file mode 100644
index 0000000..f9a6d8b
--- /dev/null
+++ b/SurgSim/Devices/Phantom/VisualTest/CMakeLists.txt
@@ -0,0 +1,52 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+)
+
+set(EXAMPLE_SOURCES
+ main.cpp
+)
+
+set(EXAMPLE_HEADERS
+)
+
+add_executable(PhantomVisualTest
+ ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
+surgsim_show_ide_folders(
+ "${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
+
+set(LIBS
+ PhantomDevice
+ IdentityPoseDevice
+ VisualTestCommon
+ SurgSimInput
+ SurgSimFramework
+ SurgSimDataStructures
+ ${OPENHAPTICS_LIBRARIES}
+ ${Boost_LIBRARIES}
+ ${OPENGL_LIBRARIES}
+)
+
+target_link_libraries(PhantomVisualTest ${LIBS})
+
+# Put PhantomVisualTest into folder "Devices"
+set_target_properties(PhantomVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Phantom/VisualTest/main.cpp b/SurgSim/Devices/Phantom/VisualTest/main.cpp
new file mode 100644
index 0000000..cf11f78
--- /dev/null
+++ b/SurgSim/Devices/Phantom/VisualTest/main.cpp
@@ -0,0 +1,45 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Devices/Phantom/PhantomDevice.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+
+#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
+
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Device::PhantomDevice;
+using SurgSim::Device::IdentityPoseDevice;
+
+
+int main(int argc, char** argv)
+{
+ std::shared_ptr<PhantomDevice> toolDevice = std::make_shared<PhantomDevice>("PhantomDevice", "Default PHANToM");
+
+ // The square is controlled by a second device. For a simple test, we're using an IdentityPoseDevice--
+ // a pretend device that doesn't actually move.
+ std::shared_ptr<DeviceInterface> squareDevice = std::make_shared<IdentityPoseDevice>("IdentityPoseDevice");
+
+ runToolSquareTest(toolDevice, squareDevice,
+ //2345678901234567890123456789012345678901234567890123456789012345678901234567890
+ "Move the Phantom device to move the sphere tool.");
+
+ std::cout << std::endl << "Exiting." << std::endl;
+ // Cleanup and shutdown will happen automatically as objects go out of scope.
+
+ return 0;
+}
diff --git a/SurgSim/Devices/Sixense/CMakeLists.txt b/SurgSim/Devices/Sixense/CMakeLists.txt
new file mode 100644
index 0000000..be79c7d
--- /dev/null
+++ b/SurgSim/Devices/Sixense/CMakeLists.txt
@@ -0,0 +1,60 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+find_package(SixenseSdk REQUIRED)
+
+link_directories(${Boost_LIBRARY_DIRS})
+
+set(LIBS
+ ${Boost_LIBRARIES}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${SIXENSE_SDK_INCLUDE_DIR}"
+)
+
+set(SIXENSE_DEVICE_SOURCES
+ SixenseDevice.cpp
+ SixenseScaffold.cpp
+ SixenseThread.cpp
+)
+
+set(SIXENSE_DEVICE_HEADERS
+ SixenseDevice.h
+ SixenseScaffold.h
+ SixenseThread.h
+)
+
+# TODO(advornik): the installation should NOT copy all the headers...
+surgsim_add_library(
+ SixenseDevice
+ "${SIXENSE_DEVICE_SOURCES}" "${SIXENSE_DEVICE_HEADERS}"
+ SurgSim/Devices/Sixense
+)
+
+target_link_libraries(SixenseDevice ${LIBS})
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+
+ if(GLUT_FOUND)
+ add_subdirectory(VisualTest)
+ endif(GLUT_FOUND)
+endif()
+
+# Put SixenseDevice into folder "Devices"
+set_target_properties(SixenseDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Sixense/Sixense.dox b/SurgSim/Devices/Sixense/Sixense.dox
new file mode 100644
index 0000000..2a18a5b
--- /dev/null
+++ b/SurgSim/Devices/Sixense/Sixense.dox
@@ -0,0 +1,11 @@
+/*!
+
+\page Sixense Sixense: Motion and Orientation Tracking Device
+
+The Razer Hydra (by Sixense Entertainment and Razer USA) is a device that tracks position and orientation of two controllers using magnetic fields.
+
+Dependencies:
+- Requires the Sixense SDK, available through Steam http://sixense.com/hardware/sixensesdk
+- An environment variable named SIXENSE_SDK_PATH or SIXENSE_ROOT must be set to the directory containing <tt>(include\\)sixense.h</tt>
+
+*/
\ No newline at end of file
diff --git a/SurgSim/Devices/Sixense/SixenseDevice.cpp b/SurgSim/Devices/Sixense/SixenseDevice.cpp
new file mode 100644
index 0000000..6dcea1d
--- /dev/null
+++ b/SurgSim/Devices/Sixense/SixenseDevice.cpp
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Sixense/SixenseDevice.h"
+
+#include "SurgSim/Devices/Sixense/SixenseScaffold.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+SixenseDevice::SixenseDevice(const std::string& uniqueName) :
+ SurgSim::Input::CommonDevice(uniqueName, SixenseScaffold::buildDeviceInputData())
+{
+}
+
+
+SixenseDevice::~SixenseDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+
+bool SixenseDevice::initialize()
+{
+ SURGSIM_ASSERT(! isInitialized());
+ std::shared_ptr<SixenseScaffold> scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold);
+
+ if (! scaffold->registerDevice(this))
+ {
+ return false;
+ }
+
+ m_scaffold = std::move(scaffold);
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
+ return true;
+}
+
+bool SixenseDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized());
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ bool ok = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return ok;
+}
+
+
+bool SixenseDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Sixense/SixenseDevice.h b/SurgSim/Devices/Sixense/SixenseDevice.h
new file mode 100644
index 0000000..2f4e697
--- /dev/null
+++ b/SurgSim/Devices/Sixense/SixenseDevice.h
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_SIXENSE_SIXENSEDEVICE_H
+#define SURGSIM_DEVICES_SIXENSE_SIXENSEDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+class SixenseScaffold;
+
+
+/// A class implementing the communication with one Sixense controller, for example one of the two on the Razer Hydra.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Device pose (units are meters). |
+/// | scalar | "trigger" | State of the analog trigger button (0 = not pressed, 1 = fully pressed). |
+/// | scalar | "joystickX" | Joystick X position (0 = center, -1 = fully left, +1 = fully right). |
+/// | scalar | "joystickY" | Joystick Y position (0 = center, -1 = fully down/near, +1 = up/far). |
+/// | bool | "buttonTrigger" | True if the analog trigger button is pressed, i.e. its value is non-zero. |
+/// | bool | "buttonBumper" | True if the bumper button (next to the trigger) is pressed. |
+/// | bool | "button1" | True if the button marked "1" is pressed. |
+/// | bool | "button2" | True if the button marked "2" is pressed. |
+/// | bool | "button3" | True if the button marked "3" is pressed. |
+/// | bool | "button4" | True if the button marked "4" is pressed. |
+/// | bool | "buttonStart" | True if the "start" button is pressed. |
+/// | bool | "buttonJoystick" | True if the joystick is pressed down as a button ("into" the controller). |
+///
+/// \par Application output used by the device: none.
+///
+/// \sa SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class SixenseDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ ///
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ explicit SixenseDevice(const std::string& uniqueName);
+
+ /// Destructor.
+ virtual ~SixenseDevice();
+
+ virtual bool initialize() override;
+
+ virtual bool finalize() override;
+
+ /// Check wheter this device is initialized.
+ bool isInitialized() const;
+
+private:
+ friend class SixenseScaffold;
+
+ std::shared_ptr<SixenseScaffold> m_scaffold;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_SIXENSE_SIXENSEDEVICE_H
diff --git a/SurgSim/Devices/Sixense/SixenseScaffold.cpp b/SurgSim/Devices/Sixense/SixenseScaffold.cpp
new file mode 100644
index 0000000..5cf6676
--- /dev/null
+++ b/SurgSim/Devices/Sixense/SixenseScaffold.cpp
@@ -0,0 +1,496 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Sixense/SixenseScaffold.h"
+
+#include <vector>
+#include <list>
+#include <memory>
+#include <algorithm>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+
+#include <sixense.h>
+
+#include "SurgSim/Devices/Sixense/SixenseDevice.h"
+#include "SurgSim/Devices/Sixense/SixenseThread.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector3f;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::RigidTransform3d;
+
+
+namespace SurgSim
+{
+namespace Device
+{
+
+
+struct SixenseScaffold::DeviceData
+{
+public:
+ /// Initialize the data.
+ DeviceData(int baseIndex, int controllerIndex, SixenseDevice* device) :
+ deviceBaseIndex(baseIndex),
+ deviceControllerIndex(controllerIndex),
+ deviceObject(device)
+ {
+ }
+
+ /// The index of the Sixense base unit for this device.
+ const int deviceBaseIndex;
+ /// The index of the Sixense controller for this device.
+ const int deviceControllerIndex;
+ /// The corresponding device object.
+ SixenseDevice* const deviceObject;
+
+private:
+ // prohibit copy construction and assignment
+ DeviceData(const DeviceData&);
+ DeviceData& operator=(const DeviceData&);
+};
+
+struct SixenseScaffold::StateData
+{
+public:
+ /// Initialize the state.
+ StateData() : isApiInitialized(false)
+ {
+ }
+
+ /// True if the API has been initialized (and not finalized).
+ bool isApiInitialized;
+
+ /// Processing thread.
+ std::unique_ptr<SixenseThread> thread;
+
+ /// The list of known devices.
+ std::list<std::unique_ptr<SixenseScaffold::DeviceData>> activeDeviceList;
+
+ /// The mutex that protects the list of known devices.
+ boost::mutex mutex;
+
+private:
+ // prohibit copy construction and assignment
+ StateData(const StateData&);
+ StateData& operator=(const StateData&);
+};
+
+
+SixenseScaffold::SixenseScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
+ m_logger(logger), m_state(new StateData)
+{
+ if (! m_logger)
+ {
+ m_logger = SurgSim::Framework::Logger::getLogger("Sixense/Hydra device");
+ m_logger->setThreshold(m_defaultLogLevel);
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Sixense/Hydra: Shared scaffold created.";
+}
+
+
+SixenseScaffold::~SixenseScaffold()
+{
+ // The thread needs to be torn down while NOT holding the mutex, to avoid deadlock.
+ if (m_state->thread)
+ {
+ destroyThread();
+ }
+
+ // The following block controls the duration of the mutex being locked.
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (! m_state->activeDeviceList.empty())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Sixense/Hydra: Destroying scaffold while devices are active!?!";
+ // do anything special with each device?
+ m_state->activeDeviceList.clear();
+ }
+
+ if (m_state->isApiInitialized)
+ {
+ finalizeSdk();
+ }
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Sixense/Hydra: Shared scaffold destroyed.";
+}
+
+
+bool SixenseScaffold::registerDevice(SixenseDevice* device)
+{
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (! m_state->isApiInitialized)
+ {
+ if (! initializeSdk())
+ {
+ return false;
+ }
+ }
+ }
+
+ boost::chrono::steady_clock::time_point tick = boost::chrono::steady_clock::now();
+ int numUsedDevicesSeen = 0;
+ bool fatalError = false;
+
+ bool deviceFound = findUnusedDeviceAndRegister(device, &numUsedDevicesSeen, &fatalError);
+ if (! deviceFound && ! fatalError && (numUsedDevicesSeen == 0) && (m_startupDelayMilliseconds > 0))
+ {
+ // Unfortunately, right after sixenseInit() the library has not yet completed its device discovery!
+ // That means that calls to sixenseIsBaseConnected(), sixenseIsControllerEnabled(), etc. will return
+ // *false* even if the base/controller is actually present.
+ //
+ // One way to deal with that would be to dynamically handle any (or no) connected devices, the way the
+ // example code does. But we'd like to report missing devices to the calling code as soon as possible.
+ //
+ // Another is to simply sleep and retry a few times here. Ugh.
+ // --advornik 2012-08-03
+ boost::chrono::steady_clock::time_point retryEnd =
+ tick + boost::chrono::milliseconds(m_startupDelayMilliseconds);
+ while (true)
+ {
+ tick += boost::chrono::milliseconds(m_startupRetryIntervalMilliseconds);
+ boost::this_thread::sleep_until(tick);
+ tick = boost::chrono::steady_clock::now(); // finding a device may take > 100ms, so fix up the time.
+
+ deviceFound = findUnusedDeviceAndRegister(device, &numUsedDevicesSeen, &fatalError);
+ if (deviceFound || fatalError || (numUsedDevicesSeen > 0) || (tick >= retryEnd))
+ {
+ break;
+ }
+ }
+ }
+
+ if (! deviceFound)
+ {
+ if (fatalError)
+ {
+ // error information was hopefully already logged
+ SURGSIM_LOG_DEBUG(m_logger) << "Sixense/Hydra: Registering device failed due to earlier fatal error.";
+ }
+ else if (numUsedDevicesSeen > 0)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Sixense/Hydra: Failed to find any unused controllers!";
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Sixense/Hydra: Failed to find any devices." <<
+ " Are the base and controllers plugged in?";
+ }
+ return false;
+ }
+
+ return true;
+}
+
+
+bool SixenseScaffold::unregisterDevice(const SixenseDevice* const device)
+{
+ bool found = false;
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ if (matching != m_state->activeDeviceList.end())
+ {
+ m_state->activeDeviceList.erase(matching);
+ // the iterator is now invalid but that's OK
+ found = true;
+ }
+ }
+
+ if (! found)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Sixense/Hydra: Attempted to release a non-registered device.";
+ }
+ return found;
+}
+
+bool SixenseScaffold::runInputFrame()
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ for (auto it = m_state->activeDeviceList.begin(); it != m_state->activeDeviceList.end(); ++it)
+ {
+ // We don't call it->deviceObject->pullOutput() because we don't use any output at all.
+ if (updateDevice(**it))
+ {
+ (*it)->deviceObject->pushInput();
+ }
+ }
+
+ return true;
+}
+
+bool SixenseScaffold::updateDevice(const SixenseScaffold::DeviceData& info)
+{
+ //const SurgSim::DataStructures::DataGroup& outputData = info.deviceObject->getOutputData();
+
+ int status = sixenseSetActiveBase(info.deviceBaseIndex);
+ if (status != SIXENSE_SUCCESS)
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Sixense/Hydra: Could not activate base unit #" << info.deviceBaseIndex <<
+ " to read device '" << info.deviceObject->getName() << "'! (status = " << status << ")";
+ return false;
+ }
+
+ sixenseControllerData data;
+ status = sixenseGetNewestData(info.deviceControllerIndex, &data);
+ if (status != SIXENSE_SUCCESS)
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Sixense/Hydra: Could not get data from controller #" <<
+ info.deviceBaseIndex << "," << info.deviceControllerIndex << " for device '" <<
+ info.deviceObject->getName() << "'! (status = " << status << ")";
+ return false;
+ }
+
+ // Use Eigen::Map to make the raw API output values look like Eigen data types
+ Eigen::Map<Vector3f> position(data.pos);
+ Eigen::Map<Eigen::Matrix<float, 3, 3, Eigen::ColMajor>> orientation(&(data.rot_mat[0][0]));
+
+ RigidTransform3d pose;
+ pose.makeAffine();
+ pose.linear() = orientation.cast<double>();
+ pose.translation() = position.cast<double>() * 0.001; // convert from millimeters to meters!
+
+ // TODO(bert): this code should cache the access indices.
+ SurgSim::DataStructures::DataGroup& inputData = info.deviceObject->getInputData();
+ inputData.poses().set(SurgSim::DataStructures::Names::POSE, pose);
+ inputData.scalars().set("trigger", data.trigger);
+ inputData.scalars().set("joystickX", data.joystick_x);
+ inputData.scalars().set("joystickY", data.joystick_y);
+ inputData.booleans().set("buttonTrigger", (data.trigger > 0));
+ inputData.booleans().set("buttonBumper", (data.buttons & SIXENSE_BUTTON_BUMPER) != 0);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_1, (data.buttons & SIXENSE_BUTTON_1) != 0);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_2, (data.buttons & SIXENSE_BUTTON_2) != 0);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_3, (data.buttons & SIXENSE_BUTTON_3) != 0);
+ inputData.booleans().set(SurgSim::DataStructures::Names::BUTTON_4, (data.buttons & SIXENSE_BUTTON_4) != 0);
+ inputData.booleans().set("buttonStart", (data.buttons & SIXENSE_BUTTON_START) != 0);
+ inputData.booleans().set("buttonJoystick", (data.buttons & SIXENSE_BUTTON_JOYSTICK) != 0);
+
+ return true;
+}
+
+bool SixenseScaffold::initializeSdk()
+{
+ SURGSIM_ASSERT(! m_state->isApiInitialized);
+
+ int status = sixenseInit();
+ if (status != SIXENSE_SUCCESS)
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Sixense/Hydra: Could not initialize the Sixense library (status = " <<
+ status << ")";
+ return false;
+ }
+
+ m_state->isApiInitialized = true;
+ return true;
+}
+
+bool SixenseScaffold::finalizeSdk()
+{
+ SURGSIM_ASSERT(m_state->isApiInitialized);
+
+ int status = sixenseExit();
+ if (status != SIXENSE_SUCCESS)
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Sixense/Hydra: Could not shut down the Sixense library (status = " <<
+ status << ")";
+ return false;
+ }
+
+ m_state->isApiInitialized = false;
+ return true;
+}
+
+bool SixenseScaffold::findUnusedDeviceAndRegister(SixenseDevice* device, int* numUsedDevicesSeen, bool* fatalError)
+{
+ *numUsedDevicesSeen = 0;
+ *fatalError = false;
+
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ // Make sure the object is unique.
+ auto sameObject = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ SURGSIM_ASSERT(sameObject == m_state->activeDeviceList.end()) << "Sixense/Hydra: Tried to register a device" <<
+ " which is already present!";
+
+ // Make sure the name is unique.
+ const std::string deviceName = device->getName();
+ auto sameName = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [&deviceName](const std::unique_ptr<DeviceData>& info) { return info->deviceObject->getName() == deviceName; });
+ if (sameName != m_state->activeDeviceList.end())
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Sixense/Hydra: Tried to register a device when the same name is" <<
+ " already present!";
+ *fatalError = true;
+ return false;
+ }
+
+ const int maxNumBases = sixenseGetMaxBases();
+
+ for (int b = 0; b < maxNumBases; ++b)
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "Sixense/Hydra: scanning base #" << b << " (of total " << maxNumBases << ")";
+ if (! sixenseIsBaseConnected(b))
+ {
+ continue;
+ }
+
+ int status = sixenseSetActiveBase(b);
+ if (status != SIXENSE_SUCCESS)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Sixense/Hydra: Could not activate connected base #" << b <<
+ " (status = " << status << ")";
+ continue;
+ }
+
+ const int maxNumControllers = sixenseGetMaxControllers();
+ for (int c = 0; c < maxNumControllers; ++c)
+ {
+ if (! sixenseIsControllerEnabled(c))
+ {
+ continue;
+ }
+
+ sixenseControllerData data;
+ status = sixenseGetNewestData(c, &data);
+ if (status != SIXENSE_SUCCESS)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Sixense/Hydra: Could not get data from enabled controller #" <<
+ b << "," << c << " (status = " << status << ")";
+ continue;
+ }
+
+ SURGSIM_LOG_DEBUG(m_logger) << "Sixense/Hydra: scanning controller #" << b << "," << c <<
+ " (of total " << maxNumControllers << ")";
+
+ if (registerIfUnused(b, c, device, numUsedDevicesSeen))
+ {
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+bool SixenseScaffold::registerIfUnused(int baseIndex, int controllerIndex, SixenseDevice* device,
+ int* numUsedDevicesSeen)
+{
+ // Check existing devices.
+ auto sameIndices = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [baseIndex, controllerIndex](const std::unique_ptr<DeviceData>& info)
+ { return ((info->deviceBaseIndex == baseIndex) && (info->deviceControllerIndex == controllerIndex)); });
+ if (sameIndices != m_state->activeDeviceList.end())
+ {
+ // We found an existing device for this controller.
+ ++(*numUsedDevicesSeen);
+ return false;
+ }
+
+ // The controller is not yet in use.
+
+ if (! m_state->thread)
+ {
+ createThread();
+ }
+
+ // Construct the object, start its thread, then move it to the list.
+ // Note that since Visual Studio 2010 doesn't support multi-argument emplace_back() for STL containers, storing a
+ // list of unique_ptr results in nicer code than storing a list of DeviceData values directly.
+ std::unique_ptr<DeviceData> info(new DeviceData(baseIndex, controllerIndex, device));
+ m_state->activeDeviceList.emplace_back(std::move(info));
+
+ return true;
+}
+
+bool SixenseScaffold::createThread()
+{
+ SURGSIM_ASSERT(! m_state->thread);
+
+ std::unique_ptr<SixenseThread> thread(new SixenseThread(this));
+ thread->start();
+ m_state->thread = std::move(thread);
+
+ return true;
+}
+
+bool SixenseScaffold::destroyThread()
+{
+ SURGSIM_ASSERT(m_state->thread);
+
+ std::unique_ptr<SixenseThread> thread = std::move(m_state->thread);
+ thread->stop();
+ thread.release();
+
+ return true;
+}
+
+SurgSim::DataStructures::DataGroup SixenseScaffold::buildDeviceInputData()
+{
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ builder.addScalar("trigger");
+ builder.addScalar("joystickX");
+ builder.addScalar("joystickY");
+ builder.addBoolean("buttonTrigger");
+ builder.addBoolean("buttonBumper");
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_1);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_2);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_3);
+ builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_4);
+ builder.addBoolean("buttonStart");
+ builder.addBoolean("buttonJoystick");
+ return builder.createData();
+}
+
+std::shared_ptr<SixenseScaffold> SixenseScaffold::getOrCreateSharedInstance()
+{
+ // Using an explicit creation function gets around problems with accessing the private constructor.
+ static auto creator =
+ []() { return std::shared_ptr<SixenseScaffold>(new SixenseScaffold); }; // NOLINT(readability/braces)
+ static SurgSim::Framework::SharedInstance<SixenseScaffold> sharedInstance(creator);
+ return sharedInstance.get();
+}
+
+void SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel)
+{
+ m_defaultLogLevel = logLevel;
+}
+
+SurgSim::Framework::LogLevel SixenseScaffold::m_defaultLogLevel = SurgSim::Framework::LOG_LEVEL_INFO;
+
+int SixenseScaffold::m_startupDelayMilliseconds = 6000;
+int SixenseScaffold::m_startupRetryIntervalMilliseconds = 100;
+
+
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Sixense/SixenseScaffold.h b/SurgSim/Devices/Sixense/SixenseScaffold.h
new file mode 100644
index 0000000..17765d1
--- /dev/null
+++ b/SurgSim/Devices/Sixense/SixenseScaffold.h
@@ -0,0 +1,154 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_SIXENSE_SIXENSESCAFFOLD_H
+#define SURGSIM_DEVICES_SIXENSE_SIXENSESCAFFOLD_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+class SixenseDevice;
+class SixenseThread;
+
+/// A class that manages Sixense devices, such as the Razer Hydra.
+///
+/// \sa SurgSim::Device::SixenseDevice
+class SixenseScaffold
+{
+public:
+ /// Destructor.
+ ~SixenseScaffold();
+
+ /// Gets the logger used by this object and the devices it manages.
+ /// \return The logger.
+ std::shared_ptr<SurgSim::Framework::Logger> getLogger() const
+ {
+ return m_logger;
+ }
+
+ /// Gets or creates the scaffold shared by all SixenseDevice instances.
+ /// The scaffold is managed using a SingleInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<SixenseScaffold> getOrCreateSharedInstance();
+
+ /// Sets the default log level.
+ /// Has no effect unless called before a scaffold is created (i.e. before the first device).
+ /// \param logLevel The log level.
+ static void setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel);
+
+private:
+ /// Internal shared state data type.
+ struct StateData;
+ /// Interal per-device information.
+ struct DeviceData;
+
+ friend class SixenseDevice;
+ friend class SixenseThread;
+ friend struct StateData;
+
+ /// Constructor.
+ /// \param logger (optional) The logger to be used for the scaffold object and the devices it manages.
+ /// If unspecified or empty, a console logger will be created and used.
+ explicit SixenseScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger = nullptr);
+
+ /// Registers the specified device object.
+ /// If successful, the device object will become connected to an unused controller.
+ ///
+ /// \param device The device object to be used, which should have a unique name.
+ /// \return True if the initialization succeeds, false if it fails.
+ bool registerDevice(SixenseDevice* device);
+
+ /// Unregisters the specified device object.
+ /// The corresponding controller will become unused, and can be re-registered later.
+ ///
+ /// \param device The device object.
+ /// \return true on success, false on failure.
+ bool unregisterDevice(const SixenseDevice* device);
+
+ /// Executes the operations for a single input frame.
+ /// Should only be called from the context of the input loop thread.
+ /// \return true on success.
+ bool runInputFrame();
+
+ /// Updates the device information for a single device.
+ /// \return true on success.
+ bool updateDevice(const DeviceData& info);
+
+ /// Initializes the Sixense SDK.
+ /// \return true on success.
+ bool initializeSdk();
+
+ /// Finalizes (de-initializes) the Sixense SDK.
+ /// \return true on success.
+ bool finalizeSdk();
+
+ /// Scans controllers that are present in the system, and if an unused one is found, register a device for it.
+ ///
+ /// \param device The device object to register if an unused device is found.
+ /// \param [out] numUsedDevicesSeen The number of devices that were found during the scan but were already
+ /// in use. Can be used if the scan fails to determine the error message that should be displayed.
+ /// \param [out] fatalError Set to true if an error (such as a duplicate device name) prevented device
+ /// registration such that retrying will not help; false otherwise.
+ /// \return true on success.
+ bool findUnusedDeviceAndRegister(SixenseDevice* device, int* numUsedDevicesSeen, bool* fatalError);
+
+ /// Register a device object given a (baseIndex, controllerIndex) pair, if the same pair is not already in use.
+ ///
+ /// \param baseIndex Index of the base unit.
+ /// \param controllerIndex Index of the controller within the base unit.
+ /// \param device The device object to register if the index pair is in fact unused.
+ /// \param [in,out] numUsedDevicesSeen The number of devices that were found during the scan but were
+ /// already in use; incremented when an unused device is seen.
+ /// \return true on success.
+ bool registerIfUnused(int baseIndex, int controllerIndex, SixenseDevice* device, int* numUsedDevicesSeen);
+
+ /// Creates the input loop thread.
+ /// \return true on success.
+ bool createThread();
+
+ /// Destroys the input loop thread.
+ /// Should be called while NOT holding the internal device list mutex, to prevent deadlock.
+ /// \return true on success.
+ bool destroyThread();
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static SurgSim::DataStructures::DataGroup buildDeviceInputData();
+
+
+
+ /// Logger used by the scaffold and all devices.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+ /// Internal scaffold state.
+ std::unique_ptr<StateData> m_state;
+
+ /// The default logging level.
+ static SurgSim::Framework::LogLevel m_defaultLogLevel;
+ /// How long we're willing to wait for devices to be detected, in milliseconds.
+ static int m_startupDelayMilliseconds;
+ /// How long to wait between trying to detect devices, in milliseconds.
+ static int m_startupRetryIntervalMilliseconds;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_SIXENSE_SIXENSESCAFFOLD_H
diff --git a/SurgSim/Devices/Sixense/SixenseThread.cpp b/SurgSim/Devices/Sixense/SixenseThread.cpp
new file mode 100644
index 0000000..be61d3f
--- /dev/null
+++ b/SurgSim/Devices/Sixense/SixenseThread.cpp
@@ -0,0 +1,46 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/Sixense/SixenseThread.h"
+
+#include "SurgSim/Devices/Sixense/SixenseScaffold.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+SixenseThread::~SixenseThread()
+{
+}
+
+bool SixenseThread::doUpdate(double dt)
+{
+ m_scaffold->runInputFrame();
+ return true;
+}
+
+bool SixenseThread::doInitialize()
+{
+ return true;
+}
+
+bool SixenseThread::doStartUp()
+{
+ return true;
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Sixense/SixenseThread.h b/SurgSim/Devices/Sixense/SixenseThread.h
new file mode 100644
index 0000000..3379478
--- /dev/null
+++ b/SurgSim/Devices/Sixense/SixenseThread.h
@@ -0,0 +1,57 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_SIXENSE_SIXENSETHREAD_H
+#define SURGSIM_DEVICES_SIXENSE_SIXENSETHREAD_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Framework/BasicThread.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+class SixenseScaffold;
+
+/// A class implementing the thread context for sampling Sixense devices.
+/// \sa SurgSim::Device::SixenseScaffold
+class SixenseThread : public SurgSim::Framework::BasicThread
+{
+public:
+ explicit SixenseThread(SixenseScaffold* scaffold) :
+ BasicThread("Sixense thread"),
+ m_scaffold(scaffold)
+ {
+ setRate(120.0); // The Hydra runs at 60Hz, but running at 60Hz here could introduce up to 16.6ms extra latency
+ }
+
+ virtual ~SixenseThread();
+
+protected:
+ virtual bool doInitialize() override;
+ virtual bool doStartUp() override;
+ virtual bool doUpdate(double dt) override;
+
+private:
+ SixenseScaffold* m_scaffold;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_SIXENSE_SIXENSETHREAD_H
diff --git a/SurgSim/Devices/Sixense/UnitTests/CMakeLists.txt b/SurgSim/Devices/Sixense/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..5fbe7f9
--- /dev/null
+++ b/SurgSim/Devices/Sixense/UnitTests/CMakeLists.txt
@@ -0,0 +1,47 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ SixenseDeviceTest.cpp
+ SixenseScaffoldTest.cpp
+)
+
+set(LIBS
+ SixenseDevice
+ SurgSimInput
+ SurgSimFramework
+ SurgSimDataStructures
+ ${Boost_LIBRARIES}
+ ${SIXENSE_SDK_LIBRARIES}
+)
+
+set(UNIT_TEST_SHARED_RELEASE_LIBS
+ ${SIXENSE_SDK_sixense_SHARED_RELEASE}
+ ${SIXENSE_SDK_sixense_utils_SHARED_RELEASE}
+)
+set(UNIT_TEST_SHARED_DEBUG_LIBS
+ ${SIXENSE_SDK_sixense_SHARED_DEBUG}
+ ${SIXENSE_SDK_sixense_utils_SHARED_DEBUG}
+ ${SIXENSE_SDK_DeviceDLL_SHARED_DEBUG} # crazy debug-only dependency
+)
+
+surgsim_add_unit_tests(SixenseDeviceTest)
+
+set_target_properties(SixenseDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Sixense/UnitTests/SixenseDeviceTest.cpp b/SurgSim/Devices/Sixense/UnitTests/SixenseDeviceTest.cpp
new file mode 100644
index 0000000..53d098c
--- /dev/null
+++ b/SurgSim/Devices/Sixense/UnitTests/SixenseDeviceTest.cpp
@@ -0,0 +1,217 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 SixenseDevice class.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/Sixense/SixenseDevice.h"
+//#include "SurgSim/Devices/Sixense/SixenseScaffold.h" // only needed if calling SixenseScaffold::setDefaultLogLevel()
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Device::SixenseDevice;
+using SurgSim::Device::SixenseScaffold;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Testing::MockInputOutput;
+
+TEST(SixenseDeviceTest, CreateUninitializedDevice)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+}
+
+TEST(SixenseDeviceTest, CreateAndInitializeDevice)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+TEST(SixenseDeviceTest, Name)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_EQ("TestSixense", device->getName());
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+ EXPECT_EQ("TestSixense", device->getName());
+}
+
+TEST(SixenseDeviceTest, CreateDeviceSeveralTimes)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ for (int i = 0; i < 6; ++i)
+ {
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+ // the device will be destroyed here
+ }
+}
+
+TEST(SixenseDeviceTest, CreateSeveralDevices)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<SixenseDevice> device1 = std::make_shared<SixenseDevice>("Sixense1");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+
+ std::shared_ptr<SixenseDevice> device2 = std::make_shared<SixenseDevice>("Sixense2");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device2->initialize()) << "Initialization failed for second controller." <<
+ " Is only one controller plugged in?";
+
+ // We can't check what happens with the scaffolds, since those are no longer a part of the device's API...
+
+ std::shared_ptr<SixenseDevice> device3 = std::make_shared<SixenseDevice>("Sixense3");
+ ASSERT_TRUE(device3 != nullptr) << "Device creation failed.";
+ if (! device3->initialize())
+ {
+ std::cerr << "[Warning: third Sixense/Hydra controller did not come up; is it plugged in?]" << std::endl;
+ }
+}
+
+TEST(SixenseDeviceTest, CreateDevicesWithSameName)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<SixenseDevice> device1 = std::make_shared<SixenseDevice>("Sixense");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+
+ std::shared_ptr<SixenseDevice> device2 = std::make_shared<SixenseDevice>("Sixense");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate name.";
+}
+
+// Create a string representation from an int.
+// C++11 adds std::to_string() to do this for various types, but VS2010 only half-supports that.
+template <typename T>
+inline std::string makeString(T value)
+{
+ std::ostringstream out;
+ out << value;
+ return out.str();
+}
+
+TEST(SixenseDeviceTest, CreateAllDevices)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::vector<std::shared_ptr<SixenseDevice>> devices;
+
+ for (int i = 1; ; ++i)
+ {
+ std::string name = "Sixense" + makeString(i);
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>(name);
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ if (! device->initialize())
+ {
+ break;
+ }
+ devices.emplace_back(std::move(device));
+ }
+
+ std::cout << devices.size() << " devices initialized." << std::endl;
+ ASSERT_GT(devices.size(), 0U) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+ EXPECT_GT(devices.size(), 1U) << "Controller #2 initialization failed. Is only one controller plugged in?";
+}
+
+TEST(SixenseDeviceTest, InputConsumer)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer->m_numTimesInitializedInput);
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+ EXPECT_EQ(0, consumer->m_numTimesInitializedInput);
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device->addInputConsumer(consumer));
+
+ // Adding the same input consumer again should fail.
+ EXPECT_FALSE(device->addInputConsumer(consumer));
+
+ // Sleep for a second, to see how many times the consumer is invoked.
+ // (A Sixense device updates internally at 60Hz, but our code currently runs at 120Hz to reduce latency.)
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeInputConsumer(consumer));
+
+ // Removing the same input consumer again should fail.
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+
+ // Check the number of invocations.
+ EXPECT_EQ(1, consumer->m_numTimesInitializedInput);
+ EXPECT_GE(consumer->m_numTimesReceivedInput, 100);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 140);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData(SurgSim::DataStructures::Names::POSE));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.scalars().hasData("trigger"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.scalars().hasData("joystickX"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.scalars().hasData("joystickY"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData("buttonTrigger"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData("buttonBumper"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_1));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_2));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_3));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData(SurgSim::DataStructures::Names::BUTTON_4));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData("buttonStart"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.booleans().hasData("buttonJoystick"));
+}
+
+TEST(SixenseDeviceTest, OutputProducer)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+
+ std::shared_ptr<MockInputOutput> producer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_TRUE(device->setOutputProducer(producer));
+
+ // Sleep for a second, to see how many times the producer is invoked.
+ // (A Sixense device is does not request any output.)
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeOutputProducer(producer));
+
+ // Removing the same input producer again should fail.
+ EXPECT_FALSE(device->removeOutputProducer(producer));
+
+ // Check the number of invocations.
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+}
diff --git a/SurgSim/Devices/Sixense/UnitTests/SixenseScaffoldTest.cpp b/SurgSim/Devices/Sixense/UnitTests/SixenseScaffoldTest.cpp
new file mode 100644
index 0000000..7d85345
--- /dev/null
+++ b/SurgSim/Devices/Sixense/UnitTests/SixenseScaffoldTest.cpp
@@ -0,0 +1,185 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 SixenseScaffold class and its device interactions.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/Sixense/SixenseDevice.h"
+#include "SurgSim/Devices/Sixense/SixenseScaffold.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+
+using SurgSim::Device::SixenseDevice;
+using SurgSim::Device::SixenseScaffold;
+
+TEST(SixenseScaffoldTest, CreateAndDestroyScaffold)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<SixenseScaffold> scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ std::weak_ptr<SixenseScaffold> scaffold1 = scaffold;
+ {
+ std::shared_ptr<SixenseScaffold> stillHaveScaffold = scaffold1.lock();
+ EXPECT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ EXPECT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+ {
+ std::shared_ptr<SixenseScaffold> sameScaffold = SixenseScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, sameScaffold) << "Unable to get scaffold from class";
+ EXPECT_EQ(scaffold, sameScaffold) << "Scaffold mismatch!";
+ }
+
+ scaffold.reset();
+ {
+ std::shared_ptr<SixenseScaffold> dontHaveScaffold = scaffold1.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ }
+
+ scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created the 2nd time!";
+ std::weak_ptr<SixenseScaffold> scaffold2 = scaffold;
+ {
+ std::shared_ptr<SixenseScaffold> stillHaveScaffold = scaffold2.lock();
+ ASSERT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ ASSERT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+}
+
+TEST(SixenseScaffoldTest, ScaffoldLifeCycle)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::weak_ptr<SixenseScaffold> lastScaffold;
+ {
+ std::shared_ptr<SixenseScaffold> scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ lastScaffold = scaffold;
+ }
+ {
+ std::shared_ptr<SixenseScaffold> dontHaveScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ lastScaffold.reset();
+ }
+
+ {
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_NE(nullptr, device) << "Creation failed. Is a Sixense/Hydra device plugged in?";
+ // note: the device is NOT initialized!
+ {
+ std::shared_ptr<SixenseScaffold> scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<SixenseScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The device has not been initialized, so it should NOT be hanging on to the device!
+ {
+ std::shared_ptr<SixenseScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+ // the ("empty") device is about to get destroyed
+ }
+
+ {
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+ {
+ std::shared_ptr<SixenseScaffold> scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<SixenseScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The same scaffold is supposed to still be around because of the device
+ {
+ std::shared_ptr<SixenseScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+
+ std::shared_ptr<SixenseScaffold> scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ EXPECT_EQ(sameScaffold, scaffold);
+ }
+ // the device and the scaffold are about to get destroyed
+ }
+
+ {
+ std::shared_ptr<SixenseScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+
+ {
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Didn't this work a moment ago?";
+ std::shared_ptr<SixenseScaffold> scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+
+ std::shared_ptr<SixenseScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+}
+
+
+TEST(SixenseScaffoldTest, CreateDeviceSeveralTimes)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::weak_ptr<SixenseScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ EXPECT_EQ(nullptr, lastScaffold.lock());
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+ std::shared_ptr<SixenseScaffold> scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold;
+ // the device and the scaffold will be destroyed here
+ }
+}
+
+
+TEST(SixenseScaffoldTest, CreateDeviceSeveralTimesWithScaffoldRef)
+{
+ //SixenseScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<SixenseScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ std::shared_ptr<SixenseDevice> device = std::make_shared<SixenseDevice>("TestSixense");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Sixense/Hydra device plugged in?";
+ std::shared_ptr<SixenseScaffold> scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ if (! lastScaffold)
+ {
+ lastScaffold = scaffold;
+ }
+ EXPECT_EQ(lastScaffold, scaffold);
+ // the device will be destroyed here, but the scaffold stays around because we have a shared_ptr to it.
+ }
+}
diff --git a/SurgSim/Devices/Sixense/VisualTest/CMakeLists.txt b/SurgSim/Devices/Sixense/VisualTest/CMakeLists.txt
new file mode 100644
index 0000000..7508268
--- /dev/null
+++ b/SurgSim/Devices/Sixense/VisualTest/CMakeLists.txt
@@ -0,0 +1,65 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+)
+
+set(EXAMPLE_SOURCES
+ main.cpp
+)
+
+set(EXAMPLE_HEADERS
+)
+
+add_executable(SixenseVisualTest
+ ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
+
+surgsim_show_ide_folders(
+ "${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
+
+set(LIBS
+ SixenseDevice
+ IdentityPoseDevice
+ VisualTestCommon
+ SurgSimInput
+ SurgSimFramework
+ SurgSimDataStructures
+ ${Boost_LIBRARIES}
+ ${SIXENSE_SDK_LIBRARIES}
+ ${OPENGL_LIBRARIES}
+)
+
+target_link_libraries(SixenseVisualTest ${LIBS})
+
+
+# Copy the device DLLs into the build directory
+surgsim_copy_to_target_directory_for_release(SixenseVisualTest
+ ${SIXENSE_SDK_sixense_SHARED_RELEASE}
+ ${SIXENSE_SDK_sixense_utils_SHARED_RELEASE}
+)
+
+surgsim_copy_to_target_directory_for_debug(SixenseVisualTest
+ ${SIXENSE_SDK_sixense_SHARED_DEBUG}
+ ${SIXENSE_SDK_sixense_utils_SHARED_DEBUG}
+ ${SIXENSE_SDK_DeviceDLL_SHARED_DEBUG} # crazy debug-only dependency
+)
+
+set_target_properties(SixenseVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Sixense/VisualTest/main.cpp b/SurgSim/Devices/Sixense/VisualTest/main.cpp
new file mode 100644
index 0000000..b7700ac
--- /dev/null
+++ b/SurgSim/Devices/Sixense/VisualTest/main.cpp
@@ -0,0 +1,44 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Devices/Sixense/SixenseDevice.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+
+#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
+
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Device::SixenseDevice;
+using SurgSim::Device::IdentityPoseDevice;
+
+
+int main(int argc, char** argv)
+{
+ std::shared_ptr<DeviceInterface> toolDevice = std::make_shared<SixenseDevice>("SixenseDevice1");
+ std::shared_ptr<DeviceInterface> squareDevice = std::make_shared<SixenseDevice>("SixenseDevice2");
+
+ runToolSquareTest(toolDevice, squareDevice,
+ //2345678901234567890123456789012345678901234567890123456789012345678901234567890
+ "Move the Razer Hydra controllers, but keep them above the base unit!\n"
+ "\n"
+ "One controller will control the sphere tool, the other the square.");
+
+ std::cout << std::endl << "Exiting." << std::endl;
+ // Cleanup and shutdown will happen automatically as objects go out of scope.
+
+ return 0;
+}
diff --git a/SurgSim/Devices/TrackIR/CMakeLists.txt b/SurgSim/Devices/TrackIR/CMakeLists.txt
new file mode 100644
index 0000000..c03da61
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/CMakeLists.txt
@@ -0,0 +1,68 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+find_package(OptiTrack REQUIRED)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${OPENSCENEGRAPH_INCLUDE_DIR}"
+ "${OPTITRACK_INCLUDE_DIR}"
+)
+
+set(TRACKIR_DEVICE_SOURCES
+ TrackIRDevice.cpp
+ TrackIRThread.cpp
+)
+
+set(TRACKIR_DEVICE_HEADERS
+ TrackIRDevice.h
+ TrackIRScaffold.h
+ TrackIRThread.h
+)
+
+set(TRACKIR_DEVICE_LINUX_SOURCES
+ linux/TrackIRScaffold.cpp
+)
+
+set(TRACKIR_DEVICE_WIN_SOURCES
+ win32/TrackIRScaffold.cpp
+)
+
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ list(APPEND TRACKIR_DEVICE_SOURCES ${TRACKIR_DEVICE_LINUX_SOURCES})
+else()
+ list(APPEND TRACKIR_DEVICE_SOURCES ${TRACKIR_DEVICE_WIN_SOURCES})
+endif()
+
+surgsim_add_library(
+ TrackIRDevice
+ "${TRACKIR_DEVICE_SOURCES}" "${TRACKIR_DEVICE_HEADERS}"
+ SurgSim/Devices/TrackIR
+)
+
+if(BUILD_TESTING)
+ # The unit tests will be built whenever the library is built.
+ add_subdirectory(UnitTests)
+
+ if(GLUT_FOUND)
+ add_subdirectory(VisualTest)
+ endif(GLUT_FOUND)
+endif()
+
+# Natural Point Camera SDK needs this preprocessor definition to work properly
+add_definitions( -DCAMERALIBRARY_IMPORTS )
+
+# Put TrackIRDevice into folder "Devices"
+set_target_properties(TrackIRDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/TrackIR/TrackIR.dox b/SurgSim/Devices/TrackIR/TrackIR.dox
new file mode 100644
index 0000000..cf990e4
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/TrackIR.dox
@@ -0,0 +1,22 @@
+/*!
+
+\page TrackIR TrackIR: Optical Tracker
+
+NaturalPoint's product list contains various motion capture cameras/systems to measure position and orientation of tracked objects.
+
+Supported devices:
+- Linux:
+ - NaturalPoint's TrackIR is tested. Other devices supported by linux-track (SmartNav, Wiimote, and some webcams) may work https://code.google.com/p/linux-track/wiki/InputDevices
+- Windows:
+ - NaturalPoint's TrackIR is tested. All OptiTrack and SmartNav models are expected to work.
+
+Dependencies:
+ - Linux: Requires linuxtrack https://code.google.com/p/linux-track/
+ - Change file 51-TIR.rules to (replace the "simquest" with your group name): SUBSYSTEM=="usb", ATTRS{idVendor}=="131d", GROUP="simquest", MODE="0666"
+ - Then move it to /lib/udev/rules.d/ and restart the udev service.
+ - Run ltr_gui (requires Wine).
+ - (Probably optional) An environment variable named NP_CAMERASDK may be set to the directory containing <tt>(include/)linuxtrack.h</tt>
+ - Windows: Requires NaturalPoint's Camera SDK https://www.naturalpoint.com/optitrack/downloads/developer-tools.html
+ - An environment variable named NP_CAMERASDK must be set to the directory containing <tt>(include\\)cameralibrary.h</tt>
+
+*/
\ No newline at end of file
diff --git a/SurgSim/Devices/TrackIR/TrackIRDevice.cpp b/SurgSim/Devices/TrackIR/TrackIRDevice.cpp
new file mode 100644
index 0000000..ebff529
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/TrackIRDevice.cpp
@@ -0,0 +1,112 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/TrackIR/TrackIRDevice.h"
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Devices/TrackIR/TrackIRScaffold.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+TrackIRDevice::TrackIRDevice(const std::string& uniqueName) :
+ SurgSim::Input::CommonDevice(uniqueName, TrackIRScaffold::buildDeviceInputData())
+{
+}
+
+TrackIRDevice::~TrackIRDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+
+bool TrackIRDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized()) << "TrackIR device already initialized.";
+ std::shared_ptr<TrackIRScaffold> scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold) << "TrackIRDevice::initialize(): Failed to obtain a TrackIR scaffold.";
+
+ if (!scaffold->registerDevice(this))
+ {
+ return false;
+ }
+
+ m_scaffold = std::move(scaffold);
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
+ return true;
+}
+
+bool TrackIRDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized()) << "TrackIR device already finalized.";
+ SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ bool ok = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return ok;
+}
+
+bool TrackIRDevice::isInitialized() const
+{
+ return (nullptr != m_scaffold);
+}
+
+
+void TrackIRDevice::setPositionScale(double scale)
+{
+ m_positionScale = scale;
+ if (m_scaffold)
+ {
+ m_scaffold->setPositionScale(this, m_positionScale);
+ }
+}
+
+double TrackIRDevice::getPositionScale() const
+{
+ return m_positionScale;
+}
+
+
+void TrackIRDevice::setOrientationScale(double scale)
+{
+ m_orientationScale = scale;
+ if (m_scaffold)
+ {
+ m_scaffold->setOrientationScale(this, m_orientationScale);
+ }
+}
+
+double TrackIRDevice::getOrientationScale() const
+{
+ return m_orientationScale;
+}
+
+
+double TrackIRDevice::defaultPositionScale()
+{
+ return 1.0;
+}
+
+double TrackIRDevice::defaultOrientationScale()
+{
+ return 1.0;
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/TrackIR/TrackIRDevice.h b/SurgSim/Devices/TrackIR/TrackIRDevice.h
new file mode 100644
index 0000000..b620f94
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/TrackIRDevice.h
@@ -0,0 +1,95 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_TRACKIR_TRACKIRDEVICE_H
+#define SURGSIM_DEVICES_TRACKIR_TRACKIRDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+class TrackIRScaffold;
+
+/// A class implementing the communication with Natural Point TrackIR camera.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Device pose (units are meters). |
+///
+/// \par Application output used by the device: none.
+///
+/// \sa SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class TrackIRDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ /// \param uniqueName A unique name for the device.
+ explicit TrackIRDevice(const std::string& uniqueName);
+
+ /// Destructor.
+ virtual ~TrackIRDevice();
+
+ /// Initialize this device, register it with the scaffold.
+ /// \return True on success; false otherwise.
+ virtual bool initialize() override;
+ /// Finalize this device, unregister this device from the scaffold.
+ /// \return True on success; false otherwise.
+ virtual bool finalize() override;
+
+ /// Check whether this device is initialized.
+ /// \return True if this device is initialized; false otherwise.
+ bool isInitialized() const;
+
+ /// Sets the position scale for this device.
+ /// The position scale controls how much the pose changes for a given device translation.
+ /// The default value for a raw device tries to correspond to the actual physical motion of the device.
+ void setPositionScale(double scale);
+ /// Gets the position scale for this device.
+ double getPositionScale() const;
+
+ /// Sets the orientation scale for this device.
+ /// The orientation scale controls how much the pose changes for a given device rotation.
+ /// The default value for a raw device tries to correspond to the actual physical motion of the device.
+ void setOrientationScale(double scale);
+ /// Gets the orientation scale for this device.
+ double getOrientationScale() const;
+
+private:
+ friend class TrackIRScaffold;
+
+ // Returns the default position scale
+ static double defaultPositionScale();
+ // Returns the default rotation scale
+ static double defaultOrientationScale();
+
+ /// Scale factor for the position axes; stored locally before the device is initialized.
+ double m_positionScale;
+ /// Scale factor for the orientation axes; stored locally before the device is initialized.
+ double m_orientationScale;
+
+ /// Communication with hardware is handled by scaffold.
+ std::shared_ptr<TrackIRScaffold> m_scaffold;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_TRACKIR_TRACKIRDEVICE_H
diff --git a/SurgSim/Devices/TrackIR/TrackIRScaffold.h b/SurgSim/Devices/TrackIR/TrackIRScaffold.h
new file mode 100644
index 0000000..33a1128
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/TrackIRScaffold.h
@@ -0,0 +1,147 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_TRACKIR_TRACKIRSCAFFOLD_H
+#define SURGSIM_DEVICES_TRACKIR_TRACKIRSCAFFOLD_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Logger.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+class DataGroup;
+}
+
+namespace Device
+{
+class TrackIRDevice;
+
+/// A class that manages Natural Point TRACKIR devices.
+///
+/// \sa SurgSim::Device::TrackIRDevice
+class TrackIRScaffold
+{
+public:
+ /// Constructor.
+ /// \param logger (optional) The logger to be used for the scaffold object and the devices it manages.
+ /// If unspecified or empty, a console logger will be created and used.
+ explicit TrackIRScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger = nullptr);
+
+ /// Destructor.
+ ~TrackIRScaffold();
+
+ /// Gets the logger used by this object and the devices it manages.
+ /// \return The logger used by this scaffold.
+ std::shared_ptr<SurgSim::Framework::Logger> getLogger() const;
+
+ /// Gets or creates the scaffold shared by all TrackIRDevice instances.
+ /// The scaffold is managed using a SingleInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<TrackIRScaffold> getOrCreateSharedInstance();
+
+ /// Sets the default log level.
+ /// Has no effect unless called before a scaffold is created (i.e. before the first device).
+ /// \param logLevel The log level.
+ static void setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel);
+
+private:
+ /// Internal shared state data type.
+ struct StateData;
+ /// Internal per-device information.
+ struct DeviceData;
+
+ friend class TrackIRDevice;
+ friend class TrackIRThread;
+ friend struct StateData;
+
+ /// Registers the specified device object.
+ /// If successful, the device object will become connected to an unused controller.
+ ///
+ /// \param device The device object to be used, which should have a unique name.
+ /// \return True if the initialization succeeds, false if it fails.
+ bool registerDevice(TrackIRDevice* device);
+ /// Unregisters the specified device object.
+ /// The corresponding controller will become unused, and can be re-registered later.
+ ///
+ /// \param device The device object.
+ /// \return true on success, false on failure.
+ bool unregisterDevice(const TrackIRDevice* device);
+
+ /// Sets the position scale for the device.
+ /// \param device The device whose position scale will be set.
+ /// \param scale Scale of the position.
+ void setPositionScale(const TrackIRDevice* device, double scale);
+ /// Sets the orientation scale for the device.
+ /// \param device The device whose orientation scale will be set.
+ /// \param scale Scale of the orientation.
+ void setOrientationScale(const TrackIRDevice* device, double scale);
+
+ /// Initializes the OptiTrack SDK.
+ /// \return true on success.
+ bool initializeSdk();
+ /// Finalizes (de-initializes) the OptiTrack SDK.
+ /// \return true on success.
+ bool finalizeSdk();
+
+ /// Start the camera, it will start sending frames.
+ /// \param info DeviceData object which contains the camera to be started.
+ /// \return true on success.
+ bool startCamera(DeviceData* info);
+ /// Stop the camera, it will stop sending frames.
+ /// \param info DeviceData object which contains the camera to be stopped.
+ /// \return true on success.
+ bool stopCamera(DeviceData* info);
+
+ /// Executes the operations for a single input frame for a single device.
+ /// Should only be called from the context of the input loop thread.
+ /// \param info The internal device data.
+ /// \return true on success.
+ bool runInputFrame(DeviceData* info);
+
+ /// Updates the device information for a single device.
+ /// \param info The device data.
+ /// \return true on success.
+ bool updateDevice(DeviceData* info);
+
+ /// Creates a thread for the given DeviceData object
+ /// \param data A DeviceData object
+ /// \return true on success.
+ bool createPerDeviceThread(DeviceData* data);
+ /// Destroys the thread associated with the given DeviceData object
+ /// \param data A DeviceData object
+ /// \return true on success.
+ bool destroyPerDeviceThread(DeviceData* data);
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static SurgSim::DataStructures::DataGroup buildDeviceInputData();
+
+ /// Logger used by the scaffold and all devices.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+
+ /// The default logging level.
+ static SurgSim::Framework::LogLevel m_defaultLogLevel;
+
+ /// Internal scaffold state.
+ std::unique_ptr<StateData> m_state;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_TRACKIR_TRACKIRSCAFFOLD_H
diff --git a/SurgSim/Devices/TrackIR/TrackIRThread.cpp b/SurgSim/Devices/TrackIR/TrackIRThread.cpp
new file mode 100644
index 0000000..559fee8
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/TrackIRThread.cpp
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/TrackIR/TrackIRThread.h"
+
+#include "SurgSim/Devices/TrackIR/TrackIRScaffold.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+TrackIRThread::TrackIRThread(TrackIRScaffold* scaffold, TrackIRScaffold::DeviceData* deviceData) :
+ BasicThread("TrackIR thread"),
+ m_scaffold(scaffold),
+ m_deviceData(deviceData)
+{
+}
+
+TrackIRThread::~TrackIRThread()
+{
+}
+
+bool TrackIRThread::doInitialize()
+{
+ return true;
+}
+
+bool TrackIRThread::doStartUp()
+{
+ return true;
+}
+
+bool TrackIRThread::doUpdate(double dt)
+{
+ return m_scaffold->runInputFrame(m_deviceData);
+}
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/TrackIR/TrackIRThread.h b/SurgSim/Devices/TrackIR/TrackIRThread.h
new file mode 100644
index 0000000..7e3f011
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/TrackIRThread.h
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_DEVICES_TRACKIR_TRACKIRTHREAD_H
+#define SURGSIM_DEVICES_TRACKIR_TRACKIRTHREAD_H
+
+#include "SurgSim/Framework/BasicThread.h"
+#include "SurgSim/Devices/TrackIR/TrackIRScaffold.h"
+
+namespace SurgSim
+{
+namespace Device
+{
+
+/// A class implementing the thread context for sampling TrackIR devices.
+/// \sa SurgSim::Device::TrackIRScaffold
+class TrackIRThread : public SurgSim::Framework::BasicThread
+{
+public:
+ /// Constructor
+ /// TrackIR sample rate: 120FPS.
+ /// Default update rate is set by BasicThread constructor to 30Hz
+ /// \param scaffold The TrackIRScaffold updated by this thread
+ /// \param deviceData Corresponds to the TrackIRScaffold::DeviceData updated by this thread
+ TrackIRThread(TrackIRScaffold* scaffold, TrackIRScaffold::DeviceData* deviceData);
+
+ /// Destructor
+ virtual ~TrackIRThread();
+
+protected:
+ /// Initialize this thread.
+ /// \return True on success, false otherwise.
+ virtual bool doInitialize() override;
+ /// Start up this thread.
+ /// \return True on success, false otherwise.
+ virtual bool doStartUp() override;
+ /// Update work of this thread.
+ /// \param dt The time step.
+ /// \return True on success, false otherwise.
+ virtual bool doUpdate(double dt) override;
+
+private:
+ // Pointer to the scaffold which will be updated by this thread.
+ TrackIRScaffold* m_scaffold;
+ // Pointer to the DeviceData object which will be updated by the scaffold.
+ TrackIRScaffold::DeviceData* m_deviceData;
+};
+
+}; // namespace Device
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_TRACKIR_TRACKIRTHREAD_H
diff --git a/SurgSim/Devices/TrackIR/UnitTests/CMakeLists.txt b/SurgSim/Devices/TrackIR/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..197b5ba
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/UnitTests/CMakeLists.txt
@@ -0,0 +1,59 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+ ${gtest_SOURCE_DIR}
+)
+
+set(UNIT_TEST_SOURCES
+ TrackIRDeviceTest.cpp
+ TrackIRScaffoldTest.cpp
+)
+
+set(UNIT_TEST_HEADERS
+)
+
+set(LIBS
+ TrackIRDevice
+ SurgSimInput
+ SurgSimFramework
+ SurgSimDataStructures
+ gmock
+ gtest
+ ${Boost_LIBRARIES}
+ ${OPTITRACK_LIBRARY}
+)
+
+# The unit tests will be built whenever the library is built, but
+# running them at build time requires hardware and is disabled by
+# default.
+option(SURGSIM_TESTS_RUN_DEVICE_TRACKIR
+ "Run the TrackIR tests as part of unit tests. (Requires hardware and OptiTrack!)"
+ OFF)
+surgsim_add_unit_tests(TrackIRDeviceTest
+ RUN ${SURGSIM_TESTS_RUN_DEVICE_TRACKIR})
+
+# Copy the device DLLs into the build directory
+surgsim_copy_to_target_directory_for_release(TrackIRDeviceTest
+ ${OPTITRACK_SHARED_RELEASE}
+)
+surgsim_copy_to_target_directory_for_debug(TrackIRDeviceTest
+ ${OPTITRACK_SHARED_DEBUG}
+)
+
+# Put TrackIRDeviceTest into folder "Devices"
+set_target_properties(TrackIRDeviceTest PROPERTIES FOLDER "Devices")
\ No newline at end of file
diff --git a/SurgSim/Devices/TrackIR/UnitTests/TrackIRDeviceTest.cpp b/SurgSim/Devices/TrackIR/UnitTests/TrackIRDeviceTest.cpp
new file mode 100644
index 0000000..b49818d
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/UnitTests/TrackIRDeviceTest.cpp
@@ -0,0 +1,109 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 TrackIRDevice class.
+
+#include <memory>
+#include <string>
+
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Devices/TrackIR/TrackIRDevice.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Device::TrackIRDevice;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Input::InputConsumerInterface;
+using SurgSim::Testing::MockInputOutput;
+
+TEST(TrackIRDeviceTest, CreateAndInitializeDevice)
+{
+ std::shared_ptr<TrackIRDevice> device = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_TRUE(nullptr != device) << "Device creation failed.";
+
+ EXPECT_FALSE(device->isInitialized());
+ EXPECT_EQ("TrackIR", device->getName());
+
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a TrackIR device plugged in?";
+
+ EXPECT_TRUE(device->isInitialized());
+ EXPECT_EQ("TrackIR", device->getName());
+}
+
+TEST(TrackIRDeviceTest, FinalizeDevice)
+{
+ std::shared_ptr<TrackIRDevice> device = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_TRUE(nullptr != device) << "Device creation failed.";
+
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a TrackIR device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+ EXPECT_EQ("TrackIR", device->getName());
+
+ ASSERT_TRUE(device->finalize()) << "Finalization failed.";
+ EXPECT_FALSE(device->isInitialized());
+ EXPECT_EQ("TrackIR", device->getName());
+}
+
+TEST(TrackIRDeviceTest, CreateDevicesWithSameName)
+{
+ std::shared_ptr<TrackIRDevice> device1 = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_TRUE(nullptr != device1) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a TrackIR device plugged in?";
+
+ std::shared_ptr<TrackIRDevice> device2 = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_TRUE(nullptr != device2) << "Device creation failed.";
+ ASSERT_ANY_THROW(device2->initialize()) << "Initialization succeeded despite duplicate name.";
+}
+
+TEST(TrackIRDeviceTest, RegisterMoreThanOneDevice)
+{
+ std::shared_ptr<TrackIRDevice> device1 = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_TRUE(nullptr != device1) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a TrackIR device plugged in?";
+
+ std::shared_ptr<TrackIRDevice> device2 = std::make_shared<TrackIRDevice>("TrackIR2");
+ ASSERT_TRUE(nullptr != device2) << "Device creation failed.";
+ ASSERT_ANY_THROW(device2->initialize()) << "Two TrackIR cameras are registered.";
+}
+
+TEST(TrackIRDeviceTest, InputConsumer)
+{
+ std::shared_ptr<TrackIRDevice> device = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_TRUE(nullptr != device) << "Device creation failed.";
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a TrackIR device plugged in?";
+
+ std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+ EXPECT_TRUE(device->addInputConsumer(consumer));
+
+ // Sleep for one second, to see how many times the consumer is invoked.
+ // (TrackIR device sample rate is 120FPS.)
+ // (The thread to poll data out of TrackIR is running at default 30Hz.)
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(device->removeInputConsumer(consumer));
+
+ // Check the number of invocations.
+ EXPECT_GE(consumer->m_numTimesReceivedInput, 20);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 50);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().isValid());
+}
diff --git a/SurgSim/Devices/TrackIR/UnitTests/TrackIRScaffoldTest.cpp b/SurgSim/Devices/TrackIR/UnitTests/TrackIRScaffoldTest.cpp
new file mode 100644
index 0000000..c3def6c
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/UnitTests/TrackIRScaffoldTest.cpp
@@ -0,0 +1,193 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 TrackIRScaffold class and its device interactions.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Devices/TrackIR/TrackIRDevice.h"
+#include "SurgSim/Devices/TrackIR/TrackIRScaffold.h"
+
+using SurgSim::Device::TrackIRDevice;
+using SurgSim::Device::TrackIRScaffold;
+
+TEST(TrackIRScaffoldTest, CreateAndDestroyScaffold)
+{
+ std::shared_ptr<TrackIRScaffold> scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+
+ std::weak_ptr<TrackIRScaffold> weakScaffold = scaffold;
+ {
+ std::shared_ptr<TrackIRScaffold> stillHaveScaffold = weakScaffold.lock();
+ EXPECT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ EXPECT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+ {
+ std::shared_ptr<TrackIRScaffold> sameScaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, sameScaffold) << "Unable to get scaffold from class";
+ EXPECT_EQ(scaffold, sameScaffold) << "Scaffold mismatch!";
+ }
+
+ scaffold.reset();
+ {
+ std::shared_ptr<TrackIRScaffold> dontHaveScaffold = weakScaffold.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ }
+
+ scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created the 2nd time!";
+ std::weak_ptr<TrackIRScaffold> yetAnotherWeakScaffold = scaffold;
+ {
+ std::shared_ptr<TrackIRScaffold> stillHaveScaffold = yetAnotherWeakScaffold.lock();
+ ASSERT_NE(nullptr, stillHaveScaffold) << "Unable to get scaffold from weak ref (while strong ref exists)";
+ ASSERT_EQ(scaffold, stillHaveScaffold) << "Scaffold mismatch!";
+ }
+}
+
+TEST(TrackIRScaffoldTest, ScaffoldLifeCycle)
+{
+ std::weak_ptr<TrackIRScaffold> lastScaffold;
+ {
+ std::shared_ptr<TrackIRScaffold> scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ lastScaffold = scaffold;
+ }
+ {
+ std::shared_ptr<TrackIRScaffold> dontHaveScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ lastScaffold.reset();
+ }
+
+ {
+ std::shared_ptr<TrackIRDevice> device = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_NE(nullptr, device) << "Creation failed. Is a TrackIR device plugged in?";
+ // note: the device is NOT initialized!
+ {
+ std::shared_ptr<TrackIRScaffold> scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<TrackIRScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The device has not been initialized, so it should NOT be hanging on to the device!
+ {
+ std::shared_ptr<TrackIRScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+ // the ("empty") device is about to get destroyed
+ }
+
+ {
+ std::shared_ptr<TrackIRDevice> device = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a TrackIR device plugged in?";
+ {
+ std::shared_ptr<TrackIRScaffold> scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<TrackIRScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+ EXPECT_EQ(scaffold, sameScaffold);
+ }
+ // The same scaffold is supposed to still be around because of the device
+ {
+ std::shared_ptr<TrackIRScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+
+ std::shared_ptr<TrackIRScaffold> scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ EXPECT_EQ(sameScaffold, scaffold);
+ }
+ // the device and the scaffold are about to get destroyed
+ }
+
+ {
+ std::shared_ptr<TrackIRScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+
+ {
+ std::shared_ptr<TrackIRDevice> device = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Didn't this work a moment ago?";
+ std::shared_ptr<TrackIRScaffold> scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+
+ std::shared_ptr<TrackIRScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+}
+
+
+TEST(TrackIRScaffoldTest, CreateDeviceSeveralTimes)
+{
+ std::weak_ptr<TrackIRScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ EXPECT_EQ(nullptr, lastScaffold.lock());
+ std::shared_ptr<TrackIRDevice> device = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a TrackIR device plugged in?";
+ std::shared_ptr<TrackIRScaffold> scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold;
+ // the device and the scaffold will be destroyed here
+ }
+}
+
+TEST(TrackIRScaffoldTest, RegisterAndUnregisterDevice)
+{
+ std::shared_ptr<TrackIRScaffold> scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+
+ std::shared_ptr<TrackIRDevice> device = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a TrackIR device plugged in?";
+
+ ASSERT_TRUE(device->finalize()) << "Finalization failed.";
+ ASSERT_NE(nullptr, scaffold) << "The scaffold should NOT be destroyed!";
+
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a TrackIR device plugged in?";
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+}
+
+TEST(TrackIRScaffoldTest, CreateDeviceSeveralTimesWithScaffoldRef)
+{
+ std::shared_ptr<TrackIRScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ std::shared_ptr<TrackIRDevice> device = std::make_shared<TrackIRDevice>("TrackIR");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a TrackIR device plugged in?";
+ std::shared_ptr<TrackIRScaffold> scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ if (!lastScaffold)
+ {
+ lastScaffold = scaffold;
+ }
+ EXPECT_EQ(lastScaffold, scaffold);
+ // the device will be destroyed here, but the scaffold stays around because we have a shared_ptr to it.
+ }
+}
diff --git a/SurgSim/Devices/TrackIR/VisualTest/CMakeLists.txt b/SurgSim/Devices/TrackIR/VisualTest/CMakeLists.txt
new file mode 100644
index 0000000..af4617c
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/VisualTest/CMakeLists.txt
@@ -0,0 +1,50 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+set(EXAMPLE_SOURCES
+ main.cpp
+)
+
+set(EXAMPLE_HEADERS
+)
+
+add_executable(TrackIRVisualTest
+ ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
+surgsim_show_ide_folders(
+ "${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
+
+set(LIBS
+ IdentityPoseDevice
+ TrackIRDevice
+ VisualTestCommon
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ ${OPENGL_LIBRARIES}
+ ${OPTITRACK_LIBRARY}
+)
+
+target_link_libraries(TrackIRVisualTest ${LIBS})
+
+# Copy the device DLLs into the build directory
+surgsim_copy_to_target_directory_for_release(TrackIRVisualTest
+ ${OPTITRACK_SHARED_RELEASE}
+)
+surgsim_copy_to_target_directory_for_debug(TrackIRVisualTest
+ ${OPTITRACK_SHARED_DEBUG}
+)
+
+# Put TrackIRVisualTest into folder "Devices"
+set_target_properties(TrackIRVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/TrackIR/VisualTest/main.cpp b/SurgSim/Devices/TrackIR/VisualTest/main.cpp
new file mode 100644
index 0000000..aaecef1
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/VisualTest/main.cpp
@@ -0,0 +1,45 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Devices/TrackIR/TrackIRDevice.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+
+#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
+
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Device::TrackIRDevice;
+using SurgSim::Device::IdentityPoseDevice;
+
+
+int main(int argc, char** argv)
+{
+ std::shared_ptr<TrackIRDevice> toolDevice = std::make_shared<TrackIRDevice>("TrackIRDevice");
+
+ // The square is controlled by a second device. For a simple test, we're using an IdentityPoseDevice--
+ // a pretend device that doesn't actually move.
+ std::shared_ptr<DeviceInterface> squareDevice = std::make_shared<IdentityPoseDevice>("IdentityPoseDevice");
+
+ runToolSquareTest(toolDevice, squareDevice,
+ //2345678901234567890123456789012345678901234567890123456789012345678901234567890
+ "Move the TrackIR device to move the sphere tool.");
+
+ std::cout << std::endl << "Exiting." << std::endl;
+ // Cleanup and shutdown will happen automatically as objects go out of scope.
+
+ return 0;
+}
diff --git a/SurgSim/Devices/TrackIR/linux/TrackIRScaffold.cpp b/SurgSim/Devices/TrackIR/linux/TrackIRScaffold.cpp
new file mode 100644
index 0000000..e323305
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/linux/TrackIRScaffold.cpp
@@ -0,0 +1,399 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/TrackIR/TrackIRScaffold.h"
+
+#include <algorithm>
+#include <list>
+#include <memory>
+
+#include <boost/thread/locks.hpp>
+#include <boost/thread/mutex.hpp>
+
+#include <linuxtrack.h>
+
+#include "SurgSim/Devices/TrackIR/TrackIRDevice.h"
+#include "SurgSim/Devices/TrackIR/TrackIRThread.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Math::makeRotationMatrix;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Device
+{
+
+struct TrackIRScaffold::DeviceData
+{
+ /// Constructor
+ /// \param device Device to be wrapped
+ explicit DeviceData(TrackIRDevice* device) :
+ deviceObject(device),
+ thread(),
+ positionScale(TrackIRDevice::defaultPositionScale()),
+ orientationScale(TrackIRDevice::defaultOrientationScale())
+ {
+ }
+
+ /// The corresponding device object.
+ SurgSim::Device::TrackIRDevice* const deviceObject;
+ /// Processing thread.
+ std::unique_ptr<SurgSim::Device::TrackIRThread> thread;
+
+ /// Scale factor for the position axes; stored locally before the device is initialized.
+ double positionScale;
+ /// Scale factor for the orientation axes; stored locally before the device is initialized.
+ double orientationScale;
+
+ /// The mutex that protects the externally modifiable parameters.
+ boost::mutex parametersMutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ DeviceData(const DeviceData&) /*= delete*/;
+ DeviceData& operator=(const DeviceData&) /*= delete*/;
+};
+
+struct TrackIRScaffold::StateData
+{
+public:
+ /// Initialize the state.
+ StateData() : isApiInitialized(false)
+ {
+ }
+
+ /// True if the API has been initialized (and not finalized).
+ bool isApiInitialized;
+
+ /// The list of known devices.
+ std::list<std::unique_ptr<TrackIRScaffold::DeviceData>> activeDeviceList;
+
+ /// The mutex that protects the list of known devices.
+ boost::mutex mutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ StateData(const StateData&) /*= delete*/;
+ StateData& operator=(const StateData&) /*= delete*/;
+};
+
+
+TrackIRScaffold::TrackIRScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
+ m_logger(logger),
+ m_state(new StateData)
+{
+ if (!m_logger)
+ {
+ m_logger = SurgSim::Framework::Logger::getLogger("TrackIR device");
+ m_logger->setThreshold(m_defaultLogLevel);
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "TrackIR: Shared scaffold created.";
+}
+
+
+TrackIRScaffold::~TrackIRScaffold()
+{
+ // The following block controls the duration of the mutex being locked.
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (!m_state->activeDeviceList.empty())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "TrackIR: Destroying scaffold while devices are active!?!";
+ for (auto it = std::begin(m_state->activeDeviceList); it != std::end(m_state->activeDeviceList); ++it)
+ {
+ stopCamera((*it).get());
+ if ((*it)->thread)
+ {
+ destroyPerDeviceThread(it->get());
+ }
+ }
+ m_state->activeDeviceList.clear();
+ }
+
+ if (m_state->isApiInitialized)
+ {
+ if (!finalizeSdk())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Finalizing TrackIR SDK failed.";
+ }
+ }
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "TrackIR: Shared scaffold destroyed.";
+}
+
+std::shared_ptr<SurgSim::Framework::Logger> TrackIRScaffold::getLogger() const
+{
+ return m_logger;
+}
+
+
+bool TrackIRScaffold::registerDevice(TrackIRDevice* device)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (!m_state->isApiInitialized)
+ {
+ if (!initializeSdk())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to initialize TrackIR SDK in TrackIRScaffold::registerDevice(). "
+ << "Continuing without the TrackIR device.";
+ }
+ }
+
+ // Only proceed when initializationSdk() is successful.
+ if (m_state->isApiInitialized)
+ {
+ // Make sure the object is unique.
+ auto sameObject = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ SURGSIM_ASSERT(sameObject == m_state->activeDeviceList.end()) << "TrackIR: Tried to register a device" <<
+ " which is already registered!";
+
+ // Make sure the name is unique.
+ const std::string name = device->getName();
+ auto sameName = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [&name](const std::unique_ptr<DeviceData>& info) { return info->deviceObject->getName() == name; });
+ SURGSIM_ASSERT(sameName == m_state->activeDeviceList.end()) << "TrackIR: Tried to register a device" <<
+ " when the same name is already present!";
+
+ // The handling of multiple cameras could be done in different ways, each with trade-offs.
+ // Instead of choosing an approach now, we assert on attempting to use more than one camera.
+ SURGSIM_ASSERT(m_state->activeDeviceList.size() < 1) << "There is already an active TrackIR camera."
+ << " TrackIRScaffold only supports one TrackIR camera right now.";
+
+ std::unique_ptr<DeviceData> info(new DeviceData(device));
+ createPerDeviceThread(info.get());
+ SURGSIM_ASSERT(info->thread) << "Failed to create a per-device thread for TrackIR device: " <<
+ info->deviceObject->getName();
+
+ startCamera(info.get());
+ m_state->activeDeviceList.emplace_back(std::move(info));
+ }
+
+ return m_state->isApiInitialized;
+}
+
+
+bool TrackIRScaffold::unregisterDevice(const TrackIRDevice* const device)
+{
+ bool found = false;
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+
+ if (matching != m_state->activeDeviceList.end())
+ {
+ stopCamera((*matching).get());
+ if ((*matching)->thread)
+ {
+ destroyPerDeviceThread(matching->get());
+ }
+ m_state->activeDeviceList.erase(matching);
+ // the iterator is now invalid but that's OK
+ found = true;
+ }
+ }
+
+ if (!found)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "TrackIR: Attempted to release a non-registered device.";
+ }
+ return found;
+}
+
+void TrackIRScaffold::setPositionScale(const TrackIRDevice* device, double scale)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+
+ if (matching != m_state->activeDeviceList.end())
+ {
+ boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
+ (*matching)->positionScale = scale;
+ }
+}
+
+void TrackIRScaffold::setOrientationScale(const TrackIRDevice* device, double scale)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+
+ if (matching != m_state->activeDeviceList.end())
+ {
+ boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
+ (*matching)->orientationScale = scale;
+ }
+}
+
+bool TrackIRScaffold::runInputFrame(TrackIRScaffold::DeviceData* info)
+{
+ if (!updateDevice(info))
+ {
+ return false;
+ }
+ info->deviceObject->pushInput();
+ return true;
+}
+
+bool TrackIRScaffold::updateDevice(TrackIRScaffold::DeviceData* info)
+{
+ SurgSim::DataStructures::DataGroup& inputData = info->deviceObject->getInputData();
+
+ boost::lock_guard<boost::mutex> lock(info->parametersMutex);
+
+ float x = 0.0, y = 0.0, z = 0.0, yaw = 0.0, pitch = 0.0, roll = 0.0;
+ unsigned counter = 0; // Current camera frame number
+
+ // roll: rotation around X-axis
+ // yaw: rotation around Y-axis
+ // pitch: rotation around Z-axis
+ // Positions are reported in millimeters.
+ // Angles are in radians.
+ ltr_get_pose(&yaw, &pitch, &roll, &x, &y, &z, &counter);
+ Vector3d position(static_cast<double>(x) / 1000.0,
+ static_cast<double>(y) / 1000.0,
+ static_cast<double>(z) / 1000.0); // Convert millimeter to meter
+ // Scale Position
+ position *= info->positionScale;
+
+ Matrix33d rotationX = makeRotationMatrix(
+ info->orientationScale * static_cast<double>(-roll), Vector3d(Vector3d::UnitX()));
+ Matrix33d rotationY = makeRotationMatrix(
+ info->orientationScale * static_cast<double>(yaw), Vector3d(Vector3d::UnitY()));
+ Matrix33d rotationZ = makeRotationMatrix(
+ info->orientationScale * static_cast<double>(pitch), Vector3d(Vector3d::UnitZ()));
+ // Rotation order is intrinsic/local XYZ
+ Matrix33d orientation = rotationX * rotationY * rotationZ;
+
+ RigidTransform3d pose;
+ pose.linear() = orientation;
+ pose.translation() = position;
+
+ inputData.poses().set(SurgSim::DataStructures::Names::POSE, pose);
+
+ return true;
+}
+
+bool TrackIRScaffold::initializeSdk()
+{
+ SURGSIM_ASSERT(!m_state->isApiInitialized) << "TrackIR API already initialized.";
+
+ //Initialize the tracking using Default profile
+ ltr_init(NULL);
+
+ //Wait for TrackIR initialization
+ ltr_state_type state;
+ int timeout = 100; // Wait for 10 seconds before quit
+ while(timeout > 0)
+ {
+ state = ltr_get_tracking_state();
+ if(state != RUNNING)
+ {
+ usleep(100000); //sleep 0.1s
+ }
+ else
+ {
+ m_state->isApiInitialized = true;
+ break;
+ }
+ --timeout;
+ };
+
+ return m_state->isApiInitialized;
+}
+
+bool TrackIRScaffold::finalizeSdk()
+{
+ SURGSIM_ASSERT(m_state->isApiInitialized) << "TrackIR API already finalized.";
+
+ ltr_shutdown();
+ ltr_state_type state;
+ state = ltr_get_tracking_state();
+ if (state == STOPPED)
+ {
+ m_state->isApiInitialized = false;
+ }
+ return !m_state->isApiInitialized;
+}
+
+bool TrackIRScaffold::createPerDeviceThread(DeviceData* deviceData)
+{
+ SURGSIM_ASSERT(!deviceData->thread) << "Device " << deviceData->deviceObject->getName() << " already has a thread.";
+
+ std::unique_ptr<TrackIRThread> thread(new TrackIRThread(this, deviceData));
+ thread->start();
+ deviceData->thread = std::move(thread);
+
+ return true;
+}
+
+bool TrackIRScaffold::destroyPerDeviceThread(DeviceData* deviceData)
+{
+ SURGSIM_ASSERT(deviceData->thread) << "No thread attached to device " << deviceData->deviceObject->getName();
+
+ std::unique_ptr<TrackIRThread> thread = std::move(deviceData->thread);
+ thread->stop();
+ thread.reset();
+
+ return true;
+}
+
+bool TrackIRScaffold::startCamera(DeviceData* info)
+{
+ return true;
+}
+
+bool TrackIRScaffold::stopCamera(DeviceData* info)
+{
+ return true;
+}
+
+SurgSim::DataStructures::DataGroup TrackIRScaffold::buildDeviceInputData()
+{
+ DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ return builder.createData();
+}
+
+std::shared_ptr<TrackIRScaffold> TrackIRScaffold::getOrCreateSharedInstance()
+{
+ static SurgSim::Framework::SharedInstance<TrackIRScaffold> sharedInstance;
+ return sharedInstance.get();
+}
+
+void TrackIRScaffold::setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel)
+{
+ m_defaultLogLevel = logLevel;
+}
+
+SurgSim::Framework::LogLevel TrackIRScaffold::m_defaultLogLevel = SurgSim::Framework::LOG_LEVEL_INFO;
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/TrackIR/win32/TrackIRScaffold.cpp b/SurgSim/Devices/TrackIR/win32/TrackIRScaffold.cpp
new file mode 100644
index 0000000..b5f6d4a
--- /dev/null
+++ b/SurgSim/Devices/TrackIR/win32/TrackIRScaffold.cpp
@@ -0,0 +1,473 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Devices/TrackIR/TrackIRScaffold.h"
+
+#include <algorithm>
+#include <list>
+#include <memory>
+
+#include <boost/thread/locks.hpp>
+#include <boost/thread/mutex.hpp>
+
+#include <cameralibrary.h>
+
+#include "SurgSim/Devices/TrackIR/TrackIRDevice.h"
+#include "SurgSim/Devices/TrackIR/TrackIRThread.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Math::makeRotationMatrix;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Device
+{
+
+struct TrackIRScaffold::DeviceData
+{
+ /// Constructor
+ /// \param device Device to be wrapped
+ /// \param cameraID The camera identifier
+ explicit DeviceData(TrackIRDevice* device, int cameraID) :
+ deviceObject(device),
+ thread(),
+ vector(CameraLibrary::cModuleVector::Create()),
+ vectorProcessor(new CameraLibrary::cModuleVectorProcessing())
+ {
+ CameraLibrary::CameraList list;
+ list.Refresh();
+ camera = CameraLibrary::CameraManager::X().GetCamera(list[cameraID].UID());
+
+ SURGSIM_ASSERT(nullptr != camera) << "Failed to obtain a camera from CameraLibrary.";
+ camera->SetVideoType(CameraLibrary::BitPackedPrecisionMode);
+
+ CameraLibrary::cVectorProcessingSettings vectorProcessorSettings;
+ vectorProcessorSettings = *(vectorProcessor->Settings());
+ vectorProcessorSettings.Arrangement = CameraLibrary::cVectorSettings::VectorClip;
+ vectorProcessorSettings.ShowPivotPoint = false;
+ vectorProcessorSettings.ShowProcessed = false;
+ vectorProcessorSettings.ScaleTranslationX = device->defaultPositionScale();
+ vectorProcessorSettings.ScaleTranslationY = device->defaultPositionScale();
+ vectorProcessorSettings.ScaleTranslationZ = device->defaultPositionScale();
+ vectorProcessorSettings.ScaleRotationPitch = device->defaultOrientationScale();
+ vectorProcessorSettings.ScaleRotationYaw = device->defaultOrientationScale();
+ vectorProcessorSettings.ScaleRotationRoll = device->defaultOrientationScale();
+ vectorProcessor->SetSettings(vectorProcessorSettings);
+
+ //== Plug in focal length in (mm) by converting it from pixels -> mm
+ CameraLibrary::cVectorSettings vectorSettings;
+ vectorSettings = *(vector->Settings());
+ vectorSettings.Arrangement = CameraLibrary::cVectorSettings::VectorClip;
+ vectorSettings.Enabled = true;
+ camera->GetDistortionModel(lensDistortion);
+ vectorSettings.ImagerFocalLength = lensDistortion.HorizontalFocalLength /
+ static_cast<double>(camera->PhysicalPixelWidth()) *
+ camera->ImagerWidth();
+
+ vectorSettings.ImagerHeight = camera->ImagerHeight();
+ vectorSettings.ImagerWidth = camera->ImagerWidth();
+ vectorSettings.PrincipalX = camera->PhysicalPixelWidth() / 2.0;
+ vectorSettings.PrincipalY = camera->PhysicalPixelHeight() / 2.0;
+ vectorSettings.PixelWidth = camera->PhysicalPixelWidth();
+ vectorSettings.PixelHeight = camera->PhysicalPixelHeight();
+ vector->SetSettings(vectorSettings);
+ }
+
+ ~DeviceData()
+ {
+ camera->Release();
+ }
+
+ Core::DistortionModel lensDistortion;
+ CameraLibrary::Camera* camera;
+ CameraLibrary::cModuleVector* vector;
+ CameraLibrary::cModuleVectorProcessing* vectorProcessor;
+
+ /// The corresponding device object.
+ SurgSim::Device::TrackIRDevice* const deviceObject;
+ /// Processing thread.
+ std::unique_ptr<SurgSim::Device::TrackIRThread> thread;
+
+ /// The mutex that protects the externally modifiable parameters.
+ boost::mutex parametersMutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ DeviceData(const DeviceData&) /*= delete*/;
+ DeviceData& operator=(const DeviceData&) /*= delete*/;
+};
+
+struct TrackIRScaffold::StateData
+{
+public:
+ /// Initialize the state.
+ StateData() : isApiInitialized(false)
+ {
+ }
+
+ /// True if the API has been initialized (and not finalized).
+ bool isApiInitialized;
+
+ /// The list of known devices.
+ std::list<std::unique_ptr<TrackIRScaffold::DeviceData>> activeDeviceList;
+
+ /// The mutex that protects the list of known devices.
+ boost::mutex mutex;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ StateData(const StateData&) /*= delete*/;
+ StateData& operator=(const StateData&) /*= delete*/;
+};
+
+
+TrackIRScaffold::TrackIRScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
+ m_logger(logger),
+ m_state(new StateData)
+{
+ if (!m_logger)
+ {
+ m_logger = SurgSim::Framework::Logger::getLogger("TrackIR device");
+ m_logger->setThreshold(m_defaultLogLevel);
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "TrackIR: Shared scaffold created.";
+}
+
+
+TrackIRScaffold::~TrackIRScaffold()
+{
+ // The following block controls the duration of the mutex being locked.
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (!m_state->activeDeviceList.empty())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "TrackIR: Destroying scaffold while devices are active!?!";
+ for (auto it = std::begin(m_state->activeDeviceList); it != std::end(m_state->activeDeviceList); ++it)
+ {
+ stopCamera((*it).get());
+ if ((*it)->thread)
+ {
+ destroyPerDeviceThread(it->get());
+ }
+ }
+ m_state->activeDeviceList.clear();
+ }
+
+ if (m_state->isApiInitialized)
+ {
+ if (!finalizeSdk())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Finalizing TrackIR SDK failed.";
+ }
+ }
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "TrackIR: Shared scaffold destroyed.";
+}
+
+std::shared_ptr<SurgSim::Framework::Logger> TrackIRScaffold::getLogger() const
+{
+ return m_logger;
+}
+
+
+bool TrackIRScaffold::registerDevice(TrackIRDevice* device)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (!m_state->isApiInitialized)
+ {
+ if (!initializeSdk())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to initialize TrackIR SDK in TrackIRScaffold::registerDevice(). "
+ << "Continuing without the TrackIR device.";
+ }
+ }
+
+ // Only proceed when initializationSdk() is successful.
+ if (m_state->isApiInitialized)
+ {
+ // Make sure the object is unique.
+ auto sameObject = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+ SURGSIM_ASSERT(sameObject == m_state->activeDeviceList.end()) << "TrackIR: Tried to register a device" <<
+ " which is already registered!";
+
+ // Make sure the name is unique.
+ const std::string name = device->getName();
+ auto sameName = std::find_if(m_state->activeDeviceList.cbegin(), m_state->activeDeviceList.cend(),
+ [&name](const std::unique_ptr<DeviceData>& info) { return info->deviceObject->getName() == name; });
+ SURGSIM_ASSERT(sameName == m_state->activeDeviceList.end()) << "TrackIR: Tried to register a device" <<
+ " when the same name is already present!";
+
+ // The handling of multiple cameras could be done in different ways, each with trade-offs.
+ // Instead of choosing an approach now, we assert on attempting to use more than one camera.
+ SURGSIM_ASSERT(m_state->activeDeviceList.size() < 1) << "There is already an active TrackIR camera."
+ << " TrackIRScaffold only supports one TrackIR camera right now.";
+
+ CameraLibrary::CameraList cameraList;
+ cameraList.Refresh();
+ if (cameraList.Count() > static_cast<int>(m_state->activeDeviceList.size()))
+ {
+ int cameraID = static_cast<int>(m_state->activeDeviceList.size());
+ std::unique_ptr<DeviceData> info(new DeviceData(device, cameraID));
+ createPerDeviceThread(info.get());
+ SURGSIM_ASSERT(info->thread) << "Failed to create a per-device thread for TrackIR device: " <<
+ info->deviceObject->getName() << ", with ID number " << cameraID << ".";
+
+ startCamera(info.get());
+ m_state->activeDeviceList.emplace_back(std::move(info));
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Registration failed. Is a TrackIR device plugged in?";
+ m_state->isApiInitialized = false;
+ }
+ }
+
+ return m_state->isApiInitialized;
+}
+
+
+bool TrackIRScaffold::unregisterDevice(const TrackIRDevice* const device)
+{
+ bool found = false;
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+
+ if (matching != m_state->activeDeviceList.end())
+ {
+ stopCamera((*matching).get());
+ if ((*matching)->thread)
+ {
+ destroyPerDeviceThread(matching->get());
+ }
+ m_state->activeDeviceList.erase(matching);
+ // the iterator is now invalid but that's OK
+ found = true;
+ }
+ }
+
+ if (!found)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "TrackIR: Attempted to release a non-registered device.";
+ }
+ return found;
+}
+
+void TrackIRScaffold::setPositionScale(const TrackIRDevice* device, double scale)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+
+ if (matching != m_state->activeDeviceList.end())
+ {
+ boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
+ CameraLibrary::cVectorProcessingSettings* vectorProcessorSettings = (*matching)->vectorProcessor->Settings();
+ vectorProcessorSettings->ScaleTranslationX = scale;
+ vectorProcessorSettings->ScaleTranslationY = scale;
+ vectorProcessorSettings->ScaleTranslationZ = scale;
+ }
+}
+
+void TrackIRScaffold::setOrientationScale(const TrackIRDevice* device, double scale)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto matching = std::find_if(m_state->activeDeviceList.begin(), m_state->activeDeviceList.end(),
+ [device](const std::unique_ptr<DeviceData>& info) { return info->deviceObject == device; });
+
+ if (matching != m_state->activeDeviceList.end())
+ {
+ boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
+ CameraLibrary::cVectorProcessingSettings* vectorProcessorSettings = (*matching)->vectorProcessor->Settings();
+ vectorProcessorSettings->ScaleRotationPitch = scale;
+ vectorProcessorSettings->ScaleRotationYaw = scale;
+ vectorProcessorSettings->ScaleRotationRoll = scale;
+ }
+}
+
+bool TrackIRScaffold::runInputFrame(TrackIRScaffold::DeviceData* info)
+{
+ if (!updateDevice(info))
+ {
+ return false;
+ }
+ info->deviceObject->pushInput();
+ return true;
+}
+
+bool TrackIRScaffold::updateDevice(TrackIRScaffold::DeviceData* info)
+{
+ SurgSim::DataStructures::DataGroup& inputData = info->deviceObject->getInputData();
+
+ boost::lock_guard<boost::mutex> lock(info->parametersMutex);
+
+ CameraLibrary::Frame *frame = info->camera->GetFrame();
+ bool poseValid = false;
+ double x, y, z, pitch, yaw, roll;
+ if (frame)
+ {
+ info->vector->BeginFrame();
+ for(int i = 0; i < frame->ObjectCount(); ++i)
+ {
+ CameraLibrary::cObject *obj = frame->Object(i);
+ float xValue = obj->X();
+ float yValue = obj->Y();
+
+ Core::Undistort2DPoint(info->lensDistortion, xValue, yValue);
+ info->vector->PushMarkerData(xValue, yValue, obj->Area(), obj->Width(), obj->Height());
+ }
+ info->vector->Calculate();
+ info->vectorProcessor->PushData(info->vector);
+
+ // Vector Clip uses 3 markers to identify the pose, i.e. 6DOF
+ // Otherwise, the pose is considered as invalid.
+ if(info->vectorProcessor->MarkerCount() == 3)
+ {
+ info->vectorProcessor->GetOrientation(yaw, pitch, roll); // Rotations are Euler Angles in degrees.
+ info->vectorProcessor->GetPosition(x, y, z); // Positions are reported in millimeters.
+ poseValid = true;
+ }
+ frame->Release();
+ }
+
+ if (poseValid)
+ {
+ // Positions returned from CameraSDK are right-handed.
+ Vector3d position(x / 1000.0, y / 1000.0, z / 1000.0); // Convert millimeter to meter
+
+ // Euler conventions returned from CameraSDK are: left-handed, axis order intrinsic ZYX, X=roll, Y=yaw, Z=pitch.
+ // OSS uses right-handed coordinate system.
+ Matrix33d rotationX = makeRotationMatrix(-roll * M_PI / 180.0, Vector3d(Vector3d::UnitX()));
+ Matrix33d rotationY = makeRotationMatrix(yaw * M_PI / 180.0, Vector3d(Vector3d::UnitY()));
+ Matrix33d rotationZ = makeRotationMatrix(pitch * M_PI / 180.0, Vector3d(Vector3d::UnitZ()));
+ Matrix33d orientation = rotationZ * rotationY * rotationX;
+
+ RigidTransform3d pose;
+ pose.linear() = orientation;
+ pose.translation() = position;
+
+ inputData.poses().set(SurgSim::DataStructures::Names::POSE, pose);
+ }
+ else // Invalid pose. inputData.poses().hasData("pose") will be set to 'false'.
+ {
+ inputData.poses().reset(SurgSim::DataStructures::Names::POSE);
+ }
+
+ return true;
+}
+
+bool TrackIRScaffold::initializeSdk()
+{
+ SURGSIM_ASSERT(!m_state->isApiInitialized) << "TrackIR API already initialized.";
+
+ if (!CameraLibrary::CameraManager::X().AreCamerasInitialized())
+ {
+ CameraLibrary::CameraManager::X().WaitForInitialization();
+ }
+
+ if (CameraLibrary::CameraManager::X().GetCamera())
+ {
+ m_state->isApiInitialized = true;
+ }
+
+ return m_state->isApiInitialized;
+}
+
+bool TrackIRScaffold::finalizeSdk()
+{
+ SURGSIM_ASSERT(m_state->isApiInitialized) << "TrackIR API already finalized.";
+
+ if (!CameraLibrary::CameraManager::X().AreCamerasShutdown())
+ {
+ // Dec-17-2013-HW It's a bug in TrackIR CameraSDK that after calling CameraLibrary::CameraManager::X().Shutdown(),
+ // calls to CameraLibrary::CameraManager::X().WaitForInitialization will throw memory violation error.
+ //CameraLibrary::CameraManager::X().Shutdown();
+ }
+
+ m_state->isApiInitialized = false;
+ return !m_state->isApiInitialized;
+}
+
+bool TrackIRScaffold::createPerDeviceThread(DeviceData* deviceData)
+{
+ SURGSIM_ASSERT(!deviceData->thread) << "Device " << deviceData->deviceObject->getName() << " already has a thread.";
+
+ std::unique_ptr<TrackIRThread> thread(new TrackIRThread(this, deviceData));
+ thread->start();
+ deviceData->thread = std::move(thread);
+
+ return true;
+}
+
+bool TrackIRScaffold::destroyPerDeviceThread(DeviceData* deviceData)
+{
+ SURGSIM_ASSERT(deviceData->thread) << "No thread attached to device " << deviceData->deviceObject->getName();
+
+ std::unique_ptr<TrackIRThread> thread = std::move(deviceData->thread);
+ thread->stop();
+ thread.reset();
+
+ return true;
+}
+
+bool TrackIRScaffold::startCamera(DeviceData* info)
+{
+ info->camera->Start();
+ return info->camera->IsCameraRunning();
+}
+
+bool TrackIRScaffold::stopCamera(DeviceData* info)
+{
+ info->camera->Stop();
+ return !(info->camera->IsCameraRunning());
+}
+
+SurgSim::DataStructures::DataGroup TrackIRScaffold::buildDeviceInputData()
+{
+ DataGroupBuilder builder;
+ builder.addPose("pose");
+ return builder.createData();
+}
+
+std::shared_ptr<TrackIRScaffold> TrackIRScaffold::getOrCreateSharedInstance()
+{
+ static SurgSim::Framework::SharedInstance<TrackIRScaffold> sharedInstance;
+ return sharedInstance.get();
+}
+
+void TrackIRScaffold::setDefaultLogLevel(SurgSim::Framework::LogLevel logLevel)
+{
+ m_defaultLogLevel = logLevel;
+}
+
+SurgSim::Framework::LogLevel TrackIRScaffold::m_defaultLogLevel = SurgSim::Framework::LOG_LEVEL_INFO;
+
+}; // namespace Device
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/devices.dox b/SurgSim/Devices/devices.dox
new file mode 100644
index 0000000..f4ec6ac
--- /dev/null
+++ b/SurgSim/Devices/devices.dox
@@ -0,0 +1,15 @@
+/*!
+
+\page Devices Devices
+
+The 'Devices' namespace contains classes that add physical devices (e.g., mice, keyboards, data acquisition, and robots) to simulations. Some devices are always built, while others require specific cmake options to be set (e.g., BUILD_DEVICE_SIXENSE) due to required dependencies that may not exist in all build environments.
+
+Devices with Dependencies:
+- \subpage LabJack
+- \subpage MultiAxis
+- \subpage Novint
+- \subpage Phantom
+- \subpage Sixense
+- \subpage TrackIR
+
+*/
\ No newline at end of file
diff --git a/SurgSim/Framework/Accessible-inl.h b/SurgSim/Framework/Accessible-inl.h
new file mode 100644
index 0000000..e8846c9
--- /dev/null
+++ b/SurgSim/Framework/Accessible-inl.h
@@ -0,0 +1,63 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_ACCESSIBLE_INL_H
+#define SURGSIM_FRAMEWORK_ACCESSIBLE_INL_H
+
+#include "SurgSim/Framework/Assert.h"
+
+template <class T>
+bool SurgSim::Framework::Accessible::getValue(const std::string& name, T* value) const
+{
+ bool result = false;
+ auto functors = m_functors.find(name);
+ if (value != nullptr && functors != m_functors.end() && functors->second.getter != nullptr)
+ {
+ try
+ {
+ *value = boost::any_cast<T>(functors->second.getter());
+ result = true;
+ }
+ catch (boost::bad_any_cast exception)
+ {
+
+ }
+ }
+ return result;
+}
+
+template <class T>
+T SurgSim::Framework::Accessible::getValue(const std::string& name) const
+{
+ T result;
+ try
+ {
+ result = boost::any_cast<T>(getValue(name));
+ }
+ catch (boost::bad_any_cast exception)
+ {
+ SURGSIM_FAILURE() << "Failure to cast to the given type. <" << exception.what() << ">";
+ return T();
+ }
+ return result;
+}
+
+template <class T>
+T SurgSim::Framework::convert(boost::any val)
+{
+ return boost::any_cast<T>(val);
+}
+
+#endif
diff --git a/SurgSim/Framework/Accessible.cpp b/SurgSim/Framework/Accessible.cpp
new file mode 100644
index 0000000..02d377a
--- /dev/null
+++ b/SurgSim/Framework/Accessible.cpp
@@ -0,0 +1,200 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Math/Matrix.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+Accessible::Accessible()
+{
+
+}
+
+Accessible::~Accessible()
+{
+
+}
+
+template <>
+boost::any Accessible::getValue(const std::string& name) const
+{
+ auto functors = m_functors.find(name);
+ if (functors != std::end(m_functors) && functors->second.getter != nullptr)
+ {
+ return functors->second.getter();
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Can't get property: " << name << "." << ((functors == std::end(m_functors)) ?
+ "Property not found." : "No getter defined for property.");
+ return boost::any();
+ }
+}
+
+boost::any Accessible::getValue(const std::string& name) const
+{
+ return getValue<boost::any>(name);
+}
+
+
+void Accessible::setValue(const std::string& name, const boost::any& value)
+
+{
+ auto functors = m_functors.find(name);
+ if (functors != std::end(m_functors) && functors->second.setter != nullptr)
+ {
+ functors->second.setter(value);
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Can't set property: " << name << "." << ((functors == std::end(m_functors)) ?
+ "Property not found." : "No setter defined for property.");
+ }
+}
+
+
+void Accessible::setGetter(const std::string& name, GetterType func)
+{
+ SURGSIM_ASSERT(func != nullptr) << "Getter functor can't be nullptr";
+
+ m_functors[name].getter = func;
+}
+
+void Accessible::setSetter(const std::string& name, SetterType func)
+{
+ SURGSIM_ASSERT(func != nullptr) << "Setter functor can't be nullptr";
+
+ m_functors[name].setter = func;
+}
+
+void Accessible::setAccessors(const std::string& name, GetterType getter, SetterType setter)
+{
+ setGetter(name, getter);
+ setSetter(name, setter);
+}
+
+
+void Accessible::removeAccessors(const std::string& name)
+{
+ auto functors = m_functors.find(name);
+ if (functors != std::end(m_functors))
+ {
+ functors->second.setter = nullptr;
+ functors->second.getter = nullptr;
+ }
+}
+
+
+bool Accessible::isReadable(const std::string& name) const
+{
+ auto functors = m_functors.find(name);
+ return (functors != m_functors.end() && functors->second.getter != nullptr);
+}
+
+bool Accessible::isWriteable(const std::string& name) const
+{
+ auto functors = m_functors.find(name);
+ return (functors != m_functors.end() && functors->second.setter != nullptr);
+}
+
+void Accessible::setSerializable(const std::string& name, EncoderType encoder, DecoderType decoder)
+{
+ SURGSIM_ASSERT(encoder != nullptr) << "Encoder functor can't be nullptr.";
+ SURGSIM_ASSERT(decoder != nullptr) << "Decoder functor can't be nullptr.";
+
+ m_functors[name].encoder = encoder;
+ m_functors[name].decoder = decoder;
+}
+
+YAML::Node Accessible::encode() const
+{
+ YAML::Node result;
+ for (auto functors = m_functors.cbegin(); functors != m_functors.cend(); ++functors)
+ {
+ auto encoder = functors->second.encoder;
+ if (encoder != nullptr)
+ {
+ result[functors->first] = encoder();
+ }
+ }
+ return result;
+}
+
+void Accessible::decode(const YAML::Node& node)
+{
+ SURGSIM_ASSERT(node.IsMap()) << "Node to decode accessible has to be map.";
+ for (auto functors = m_functors.cbegin(); functors != m_functors.cend(); ++functors)
+ {
+ auto decoder = functors->second.decoder;
+ if (decoder != nullptr)
+ {
+ YAML::Node temporary = node[functors->first];
+ if (!temporary.IsNull() && temporary.IsDefined())
+ {
+ try
+ {
+ decoder(&temporary);
+ }
+ catch (std::exception e)
+ {
+ SURGSIM_FAILURE() << e.what();
+ }
+ }
+ }
+ }
+}
+
+void Accessible::forwardProperty(const std::string& name, const Accessible& target, const std::string& targetValueName)
+{
+ Functors functors;
+ auto found = target.m_functors.find(targetValueName);
+ if (found != target.m_functors.end())
+ {
+ functors.getter = found->second.getter;
+ functors.setter = found->second.setter;
+ m_functors[name] = std::move(functors);
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Target does not have any setters or getters on property " << targetValueName;
+ }
+}
+
+template<>
+SurgSim::Math::Matrix44f convert(boost::any val)
+{
+
+ SurgSim::Math::Matrix44f floatResult;
+ // Use try in case this conversion was created using a Matrix44f, in which case the any_cast will
+ // still fail and throw an exception
+ try
+ {
+ SurgSim::Math::Matrix44d result = boost::any_cast<SurgSim::Math::Matrix44d>(val);
+ floatResult = result.cast<float>();
+ }
+ catch (boost::bad_any_cast&)
+ {
+ floatResult = boost::any_cast<SurgSim::Math::Matrix44f>(val);
+ }
+ return floatResult;
+}
+
+
+}; // Framework
+}; // SurgSim
diff --git a/SurgSim/Framework/Accessible.h b/SurgSim/Framework/Accessible.h
new file mode 100644
index 0000000..b929ef7
--- /dev/null
+++ b/SurgSim/Framework/Accessible.h
@@ -0,0 +1,223 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_ACCESSIBLE_H
+#define SURGSIM_FRAMEWORK_ACCESSIBLE_H
+
+#include <string>
+#include <memory>
+#include <unordered_map>
+#include <functional>
+#include <boost/any.hpp>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Math/Matrix.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// Mixin class for enabling a property system on OSS classes, the instance still needs to initialize properties in
+/// the constructor by using either addSetter, addGetter, addAccessors or the macro for each member variable
+/// that should be made accessible.
+class Accessible
+{
+public:
+
+ /// Default Constructor
+ Accessible();
+
+ /// Destructor
+ ~Accessible();
+
+ typedef std::function<boost::any(void)> GetterType;
+ typedef std::function<void (boost::any)> SetterType;
+
+ typedef std::function<YAML::Node(void)> EncoderType;
+ typedef std::function<void(const YAML::Node*)> DecoderType;
+
+
+
+ /// Retrieves the value with the name by executing the getter if it is found and tries to convert
+ /// it to the given type.
+ /// \throws SurgSim::Framework::AssertionFailure If the conversion fails or the property cannot be found.
+ /// \tparam T The requested type for the property.
+ /// \param name The name of the property.
+ /// \return The value of the property if the getter was found
+ template <class T>
+ T getValue(const std::string& name) const;
+
+ /// Retrieves the value with the name by executing the getter if it is found.
+ /// \throws SurgSim::Framework::AssertionFailure if the property cannot be found
+ /// \param name The name of the property.
+ /// \return The value of the property if the getter was found
+ boost::any getValue(const std::string& name) const;
+
+
+ /// Retrieves the value with the name by executing the getter if it is found, and converts it to
+ /// the type of the output parameter. This does not throw.
+ /// \tparam T the type of the property, usually can be deduced automatically
+ /// \param name The name of the property.
+ /// \param [out] value If non-null, will receive the value of the given property.
+ /// \return true if value != nullptr and the getter can be found.
+ template <class T>
+ bool getValue(const std::string& name, T* value) const;
+
+ /// Sets a value of a property that has setter.
+ /// \throws SurgSim::Framework::AssertionFailure If the property cannot be found.
+ /// \param name The name of the property.
+ /// \param value The value that it should be set to.
+ void setValue(const std::string& name, const boost::any& value);
+
+ /// Check whether a property is readable
+ /// \param name Name of the property to be checked.
+ /// \return true if the property exists and has a getter
+ bool isReadable(const std::string& name) const;
+
+ /// Check whether a property is writable
+ /// \param name Name of the property to be checked.
+ /// \return true if the property exists and has a setter
+ bool isWriteable(const std::string& name) const;
+
+ /// Sets a getter for a given property.
+ /// \throws SurgSim::Framework::AssertionFailure if func is a nullptr.
+ /// \param name The name of the property.
+ /// \param func The getter function.
+ void setGetter(const std::string& name, GetterType func);
+
+ /// Sets a setter for a given property.
+ /// \throws SurgSim::Framework::AssertionFailure if func is a nullptr.
+ /// \param name The name of the property.
+ /// \param func The setter function.
+ void setSetter(const std::string& name, SetterType func);
+
+ /// Sets the accessors getter and setter in one function.
+ /// \throws SurgSim::Framework::AssertionFailure if either getter or setter is a nullptr.
+ /// \param name The name of the property.
+ /// \param getter The getter.
+ /// \param setter The setter.
+ void setAccessors(const std::string& name, GetterType getter, SetterType setter);
+
+ /// Removes all the accessors (getter and setter) for a given property
+ /// \param name The name of the property
+ void removeAccessors(const std::string& name);
+
+ /// Adds a property with the given name that uses the targets accessors, in effect forwarding the value
+ /// to the target
+ /// \note This will copy the appropriate calls into the local function table of this accessible, in effect
+ /// exposing a pointer to the target, if the target goes out of scope, the behavior is undefined
+ /// \throws SurgSim::Framework::AssertionFailure if the target does not contain the property named in this call.
+ /// \param name The name of the new property
+ /// \param target The instance that provides the actual property
+ /// \param targetProperty The name of the property that should be used.
+ void forwardProperty(const std::string& name, const Accessible& target, const std::string& targetProperty);
+
+ /// Sets the functions used to convert data from and to a YAML::Node. Will throw and exception
+ /// if the data type that is passed to YAML cannot be converted into a YAML::Node
+ /// \param name The name of the property.
+ /// \param encoder The function to be used to put the property into the node.
+ /// \param decoder The function to be used to read the property from the node and set it
+ /// in the instance.
+ void setSerializable(const std::string& name, EncoderType encoder, DecoderType decoder);
+
+ /// Encode this Accessible to a YAML::Node
+ /// \return The encoded version of this instance.
+ YAML::Node encode() const;
+
+ /// Decode this Accessible from a YAML::Node, will throw an exception if the data type cannot
+ /// be converted.
+ /// \throws SurgSim::Framework::AssertionFailure if node is not of YAML::NodeType::Map.
+ /// \param node The node that carries the data to be, properties with names that don't
+ /// match up with properties in the Accessible are ignored
+ void decode(const YAML::Node& node);
+
+private:
+
+ /// @{
+ /// Prevent default copy construction and default assignment
+ Accessible(const Accessible& other) /*= delete*/;
+ Accessible& operator=(const Accessible& other) /*= delete*/;
+ /// @}
+
+ /// Private struct to keep the map under control
+ struct Functors
+ {
+ GetterType getter;
+ SetterType setter;
+ EncoderType encoder;
+ DecoderType decoder;
+ };
+
+ std::unordered_map<std::string, Functors> m_functors;
+
+};
+
+/// Public struct to pair an accessible with its appropriate property
+struct Property
+{
+ std::weak_ptr<Accessible> accessible;
+ std::string name;
+};
+
+template <>
+boost::any Accessible::getValue(const std::string& name) const;
+
+
+/// Wrap boost::any_cast to use in std::bind, for some reason it does not work by itself. This function will
+/// throw an exception if the cast does not work, this usually means that the types do not match up at all.
+/// \tparam T target type for conversion.
+/// \param val The value to be converted.
+/// \return An object converted from boost::any to T, will throw an exception if the conversion fails
+template <class T>
+T convert(boost::any val);
+
+/// Specialization for convert<T>() to correctly cast Matrix44d to Matrix44f, will throw if the val is not casteable to
+/// Matrix44[fd]. This is necessary as we need Matrix44f as outputs in some cases but all our Matrices are Matrix44d.
+/// This lets the user define a property that does a type conversion, without having to implement an accessor.
+/// \param val The value to be converted, should be a Matrix44[df].
+/// \return A matrix val converted to Matrix44f.
+template <>
+SurgSim::Math::Matrix44f convert(boost::any val);
+
+/// A macro to register getter and setter for a property that is readable and writeable,
+/// order of getter and setter agrees with 'RW'. Note that the property should not be quoted in the original
+/// macro call.
+#define SURGSIM_ADD_RW_PROPERTY(class, type, property, getter, setter) \
+ setAccessors(#property, \
+ std::bind(&class::getter, this),\
+ std::bind(&class::setter, this, std::bind(SurgSim::Framework::convert<type>,std::placeholders::_1)))
+
+/// A macro to register a getter for a property that is read only
+#define SURGSIM_ADD_RO_PROPERTY(class, type, property, getter) \
+ setGetter(#property, \
+ std::bind(&class::getter, this))
+
+/// A macro to register a serializable property, this needs to support reading, writing and all the
+/// conversions to and from YAML::Node
+#define SURGSIM_ADD_SERIALIZABLE_PROPERTY(class, type, property, getter, setter) \
+ setAccessors(#property, \
+ std::bind(&class::getter, this),\
+ std::bind(&class::setter, this, std::bind(SurgSim::Framework::convert<type>,std::placeholders::_1)));\
+ setSerializable(#property,\
+ std::bind(&YAML::convert<type>::encode, std::bind(&class::getter, this)),\
+ std::bind(&class::setter, this, std::bind(&YAML::Node::as<type>,std::placeholders::_1)))
+
+}; // Framework
+}; // SurgSim
+
+#include "SurgSim/Framework/Accessible-inl.h"
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Framework/ApplicationData.cpp b/SurgSim/Framework/ApplicationData.cpp
new file mode 100644
index 0000000..1c5f289
--- /dev/null
+++ b/SurgSim/Framework/ApplicationData.cpp
@@ -0,0 +1,171 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Log.h"
+
+#include <algorithm>
+
+#include <boost/filesystem.hpp>
+#include <boost/filesystem/fstream.hpp>
+
+using boost::filesystem::path;
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+ApplicationData::ApplicationData(const std::vector<std::string>& paths)
+{
+ setPaths(paths);
+}
+
+ApplicationData::ApplicationData(const std::string& configurationFileName)
+{
+ path filePath(configurationFileName);
+ SURGSIM_ASSERT(boost::filesystem::exists(filePath)) <<
+ "ApplicationdData could not find configuration file " << configurationFileName << " " <<
+ "the application is probably not going to be able to find it's data files";
+
+ std::vector<std::string> paths;
+ boost::filesystem::ifstream in(filePath);
+ std::string line;
+ while (! in.eof())
+ {
+ getline(in, line);
+ paths.push_back(line);
+ // Skip possible Trailing newlines
+ in >> std::ws;
+ }
+ setPaths(paths);
+}
+
+ApplicationData::~ApplicationData()
+{
+}
+
+std::string ApplicationData::findFile(const std::string& fileName) const
+{
+ std::string result;
+
+ if (!isValidFilename(fileName))
+ {
+ return "";
+ }
+
+ path file(fileName);
+ for (auto it = m_paths.cbegin(); it != m_paths.cend(); ++it)
+ {
+ path filePath(*it);
+ filePath /= fileName;
+ if (boost::filesystem::exists(filePath))
+ {
+ result = filePath.make_preferred().string();
+ break;
+ }
+ }
+ return result;
+}
+
+
+bool ApplicationData::tryFindFile(const std::string& fileName, std::string* target) const
+{
+ bool result = false;
+ std::string resultName = findFile(fileName);
+ if (resultName != "")
+ {
+ *target = std::move(resultName);
+ result = true;
+ }
+ return result;
+}
+
+
+bool ApplicationData::setPaths(const std::vector<std::string>& paths)
+{
+ bool result = true;
+ m_paths.clear();
+ for (auto it = paths.cbegin(); it != paths.cend(); ++it)
+ {
+ result = addPath(*it) && result;
+ }
+ return result;
+}
+
+bool ApplicationData::addPath(const std::string& pathName)
+{
+ bool result = false;
+
+ if (! isValidFilename(pathName))
+ {
+ return false;
+ }
+
+ path newPath(pathName);
+ if (boost::filesystem::exists(newPath) && boost::filesystem::is_directory(newPath))
+ {
+ newPath = boost::filesystem::canonical(newPath).make_preferred();
+ if (std::find(m_paths.cbegin(), m_paths.cend(), newPath) == m_paths.cend())
+ {
+ m_paths.push_back(newPath);
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(Logger::getDefaultLogger()) <<
+ "ApplicationsData::addPath: Trying to add duplicate path " << pathName;
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(Logger::getDefaultLogger()) <<
+ "ApplicationData, trying to add nonexistent or non directory path to search list " << newPath;
+ }
+ return result;
+}
+
+std::vector<std::string> ApplicationData::getPaths() const
+{
+ std::vector<std::string> result;
+ for (auto it = m_paths.cbegin(); it != m_paths.cend(); ++it)
+ {
+ result.push_back(it->string());
+ }
+ return result;
+}
+
+bool ApplicationData::isValidFilename(const std::string& fileName) const
+{
+ bool result = true;
+
+ if (fileName.empty())
+ {
+ result = false;
+ }
+
+ size_t index = fileName.find("\\");
+ if (index != std::string::npos)
+ {
+ SURGSIM_LOG_WARNING(Logger::getDefaultLogger()) << __FUNCTION__ <<
+ " Backslashes encountered in the path, this path cannot be used " << fileName <<
+ " to be useful it needs to be rewritten using '/'.";
+ result = false;
+ }
+ return result;
+}
+
+}; // Framework
+}; // SurgSim
diff --git a/SurgSim/Framework/ApplicationData.h b/SurgSim/Framework/ApplicationData.h
new file mode 100644
index 0000000..643ab0a
--- /dev/null
+++ b/SurgSim/Framework/ApplicationData.h
@@ -0,0 +1,103 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_APPLICATIONDATA_H
+#define SURGSIM_FRAMEWORK_APPLICATIONDATA_H
+
+#include <string>
+#include <vector>
+
+#if !defined(SURGSIM_PARSED_BY_DOXYGEN) // do not generate documentation for Boost stuff!
+namespace boost
+{
+namespace filesystem
+{
+class path;
+} // namespace filesystem
+} // namespace boost
+#endif // defined(SURGSIM_PARSED_BY_DOXYGEN)
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// Enable searching for files in a given list of paths, give access to the current directory and
+/// wrap boost::filesystem functionality
+class ApplicationData
+{
+public:
+
+ /// Constructor, class is immutable, pass a list of paths to be used for searching duplicate
+ /// paths will be eliminated, invalid paths will be eliminated, the order of the paths given here
+ /// is the order that will be used in searching, the first occurrance of a file within a given
+ /// path will be used.
+ /// \param paths The list of search paths.
+ explicit ApplicationData(const std::vector<std::string>& paths);
+
+ /// Reads the search paths from a given configuration file
+ /// \param configFile The configuration file for reading the search paths.
+ explicit ApplicationData(const std::string& configFile);
+
+ ~ApplicationData();
+
+ /// Gets the search paths.
+ /// \return The paths, passed in the constructor with duplicates and invalid paths
+ /// removed. All paths returned will be absolute and in system format.
+ std::vector<std::string> getPaths() const;
+
+ /// Searches for the first occurrence of fileName amongst the given paths, the
+ /// search is shallow, only direct content of the directories in the path list
+ /// will be used as search candidates.
+ /// \param fileName Filename of the file.
+ /// \return The absolute path to the file in system format i.e c:\\xxx\\yyy\\file.txt for
+ /// windows and /xxx/yyy/file.txt for all other systems. An empty string will be
+ /// returned if the file cannot be found.
+ std::string findFile(const std::string& fileName) const;
+
+ /// Searches for the first occurrence of fileName amongst the given paths, see findFile() for details.
+ /// If the file is found the full pathName will be sent in target and true returned. If the file is
+ /// not found the result will be false and the content of target will not change.
+ /// \param fileName Filename of the file.
+ /// \param target The location for the converted filename if it was found.
+ /// \return true if the file is found, false otherwise.
+ bool tryFindFile(const std::string& fileName, std::string* target) const;
+
+ /// Checks if the filename is acceptable
+ /// \param fileName Filename to be checked.
+ /// \return true if the name is valid, false otherwise.
+ bool isValidFilename(const std::string& fileName) const;
+
+private:
+ /// Adds a single path to the list of search paths.
+ /// \param pathName Full pathname.
+ /// \return true if it succeeds, false if the given path does not exist or if it is
+ /// already in the list of paths.
+ bool addPath(const std::string& pathName);
+
+ /// Sets the list of search paths to be used for finding the location of files.
+ /// Eliminates duplicate paths and paths that do not exist
+ /// \param paths The paths to be used for finding files.
+ /// \return true if it succeeds, false if one or more of the paths
+ /// do not exist in the system or are duplicated.
+ bool setPaths(const std::vector<std::string>& paths);
+
+ std::vector<boost::filesystem::path> m_paths;
+};
+
+}; // Framework
+}; // SurgSim
+
+#endif // SURGSIM_FRAMEWORK_APPLICATIONDATA_H
diff --git a/SurgSim/Framework/Assert.h b/SurgSim/Framework/Assert.h
new file mode 100644
index 0000000..2be83a6
--- /dev/null
+++ b/SurgSim/Framework/Assert.h
@@ -0,0 +1,106 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 header that provides the assertion API.
+/// \ingroup assertAPI
+/// \sa assertAPI
+
+#ifndef SURGSIM_FRAMEWORK_ASSERT_H
+#define SURGSIM_FRAMEWORK_ASSERT_H
+
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/Framework/AssertMessage.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// \defgroup assertAPI Assertions
+/// The assertion API used by OpenSurgSim code.
+/// \sa loggingAPI
+/// @{
+
+/// \defgroup assertInternals Internal assertion helpers
+/// Not meant for public consumption.
+
+
+#if !defined(SURGSIM_ASSERT_LOGGER)
+/// Logger used to log asserts.
+/// The default logger is used if no other logger is defined.
+/// \ingroup assertInternals
+#define SURGSIM_ASSERT_LOGGER ::SurgSim::Framework::Logger::getDefaultLogger()
+#endif
+
+/// Helper macro to determine the function name currently being compiled.
+/// Tries to provide some readable but information-rich version of the name, as provided by different compilers.
+/// \ingroup assertInternals
+/// \hideinitializer
+#if defined(__func__)
+#define SURGSIM_CURRENT_FUNCTION __func__
+#elif defined(__FUNCSIG__)
+#define SURGSIM_CURRENT_FUNCTION __FUNCSIG__
+#elif defined(__PRETTY_FUNCTION__)
+#define SURGSIM_CURRENT_FUNCTION __PRETTY_FUNCTION__
+#elif defined(__FUNCTION__)
+#define SURGSIM_CURRENT_FUNCTION __FUNCTION__
+#else
+#define SURGSIM_CURRENT_FUNCTION "???"
+#endif
+
+/// Helper macros to turn its argument into a quoted string constant.
+/// \ingroup assertInternals
+/// \param x Argument to convert into a string
+/// \return Quoted string constant. Note that macros such as __LINE__ may be quoted literally rather than expanded.
+#define SURGSIM_MAKE_STRING(x) #x
+
+/// Assert that \c condition is true. If not, abort program execution, printing an error message that will include
+/// \c failText, the condition, source file/line, etc.
+/// \param condition Condition to test
+/// \return Stream to output extra assert information
+///
+/// Example:
+/// SURGSIM_ASSERT(index >= 0) << "bad index: " << index;
+#define SURGSIM_ASSERT(condition) \
+ if ((condition)) \
+ { \
+ } \
+ else \
+ /* important: no curly braces around this! */ \
+ ::SurgSim::Framework::AssertMessage(SURGSIM_ASSERT_LOGGER) << "*** Assertion failed: " << \
+ SURGSIM_MAKE_STRING(condition) << " ***" << std::endl << \
+ " in " << SURGSIM_CURRENT_FUNCTION << std::endl << \
+ " at " << __FILE__ << ":" << __LINE__ << std::endl
+
+/// Report that something very bad has happened and abort program execution. An error message will be printed, and will
+/// include \c failText, source file/line, etc.
+/// This is pretty similar to SURGSIM_ASSERT(true, ...), except "true" won't be included in the resulting message. =)
+/// \return Stream to output extra failure information
+///
+/// Example:
+/// SURGSIM_FAILURE() << failText;
+#define SURGSIM_FAILURE() \
+ ::SurgSim::Framework::AssertMessage(SURGSIM_ASSERT_LOGGER) << "*** Failure ***" << std::endl << \
+ " in " << SURGSIM_CURRENT_FUNCTION << std::endl << \
+ " at " << __FILE__ << ":" << __LINE__ << std::endl
+
+
+/// @}
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_ASSERT_H
diff --git a/SurgSim/Framework/AssertMessage.cpp b/SurgSim/Framework/AssertMessage.cpp
new file mode 100644
index 0000000..5d3700b
--- /dev/null
+++ b/SurgSim/Framework/AssertMessage.cpp
@@ -0,0 +1,59 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/AssertMessage.h"
+
+#if defined(_WIN32)
+#include <windows.h>
+#else // not defined(_WIN32)
+#include <stdlib.h>
+#endif // not defined(_WIN32)
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+
+void AssertMessage::setFailureCallback(AssertMessage::DeathCallback callback)
+{
+ m_killMeNow = callback;
+}
+
+AssertMessage::DeathCallback AssertMessage::getFailureCallback()
+{
+ return m_killMeNow;
+}
+
+void AssertMessage::throwException(const std::string& errorMessage)
+{
+ throw AssertionFailure(errorMessage);
+}
+
+void AssertMessage::killApplication(const std::string& errorMessage)
+{
+#if defined(_WIN32)
+ DebugBreak();
+#else // not defined(_WIN32)
+ abort();
+#endif // not defined(_WIN32)
+}
+
+AssertMessage::DeathCallback AssertMessage::m_killMeNow = AssertMessage::throwException;
+
+
+}; // namespace Framework
+}; // namespace SurgSim
diff --git a/SurgSim/Framework/AssertMessage.h b/SurgSim/Framework/AssertMessage.h
new file mode 100644
index 0000000..27ebbe2
--- /dev/null
+++ b/SurgSim/Framework/AssertMessage.h
@@ -0,0 +1,124 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_ASSERTMESSAGE_H
+#define SURGSIM_FRAMEWORK_ASSERTMESSAGE_H
+
+#include <memory>
+
+#include "SurgSim/Framework/LogMessageBase.h"
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+
+/// An exception class thrown by SURGSIM_ASSERT() failures and SURGSIM_FAILURE().
+/// \ingroup assertAPI
+class AssertionFailure : public std::runtime_error
+{
+public:
+ /// Constructor
+ /// \param message Exception message
+ explicit AssertionFailure(const std::string& message) : std::runtime_error(message)
+ {
+ }
+};
+
+
+/// An internal message class used for assertion failures. Dies after logging.
+/// \ingroup assertInternals
+class AssertMessage : public LogMessageBase
+{
+public:
+ /// The type used for the callback function that is triggered after an assertion has failed.
+ typedef void (*DeathCallback)(const std::string& message);
+
+ /// Constructor.
+ /// \param logger %Logger used to log this message.
+ explicit AssertMessage(Logger* logger) : LogMessageBase(logger, LOG_LEVEL_CRITICAL)
+ {
+ }
+
+ /// Constructor.
+ /// \param logger %Logger used to log this message.
+ explicit AssertMessage(const std::unique_ptr<Logger>& logger) : LogMessageBase(logger.get(), LOG_LEVEL_CRITICAL)
+ {
+ }
+
+ /// Constructor.
+ /// \param logger %Logger used to log this message.
+ explicit AssertMessage(const std::shared_ptr<Logger>& logger) : LogMessageBase(logger.get(), LOG_LEVEL_CRITICAL)
+ {
+ }
+
+ /// Destructor, which may throw an exception if the failure behavior does
+#ifdef _MSC_VER
+ ~AssertMessage() throw(...) // Visual Studio does not support noexcept. The throw(...) is optional.
+#else
+ ~AssertMessage() noexcept(false) /// C++11 introduced noexcept
+#endif
+ {
+ flush();
+ m_killMeNow(getMessage());
+ }
+
+ /// After an assertion has failed, call some arbitrary function.
+ /// The callback function should cause the application (or at least the current thread) to terminate.
+ ///
+ /// Thread-unsafe if called concurrently from multiple threads, or concurrently with a failing assertion.
+ static void setFailureCallback(DeathCallback callback);
+
+ /// Get the callback that will currently be called after an assertion has failed.
+ /// Thread-unsafe if called concurrently from multiple threads, or concurrently with a failing assertion.
+ /// \return The callback.
+ static DeathCallback getFailureCallback();
+
+ /// After an assertion has failed, throw a C++ exception.
+ /// Thread-unsafe if called concurrently from multiple threads, or concurrently with a failing assertion.
+ static void setFailureBehaviorToThrow()
+ {
+ setFailureCallback(throwException);
+ }
+
+ /// After an assertion has failed, enter the debugger or kill the application in a system-dependent way.
+ /// Thread-unsafe if called concurrently from multiple threads, or concurrently with a failing assertion.
+ static void setFailureBehaviorToDeath()
+ {
+ setFailureCallback(killApplication);
+ }
+
+private:
+ /// Kill the application by throwing an exception.
+ /// \param errorMessage Message describing the error.
+ static void throwException(const std::string& errorMessage);
+
+ /// Enter the debugger or kill the application in a system-dependent way.
+ /// \param errorMessage Message describing the error (which will be ignored).
+ static void killApplication(const std::string& errorMessage);
+
+
+ /// The callback function that is triggered after an assertion has failed.
+ /// Thread-unsafe if called concurrently from multiple threads.
+ static DeathCallback m_killMeNow;
+};
+
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_ASSERTMESSAGE_H
diff --git a/SurgSim/Framework/Asset.cpp b/SurgSim/Framework/Asset.cpp
new file mode 100644
index 0000000..5a7afcb
--- /dev/null
+++ b/SurgSim/Framework/Asset.cpp
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Asset.h"
+
+#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Runtime.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+Asset::Asset() : m_fileName()
+{
+}
+
+Asset::~Asset()
+{
+}
+
+void Asset::load(const std::string& fileName, const SurgSim::Framework::ApplicationData& data)
+{
+ m_fileName = fileName;
+ SURGSIM_ASSERT(!m_fileName.empty()) << "File name is empty";
+
+ std::string path = data.findFile(m_fileName);
+
+ SURGSIM_ASSERT(!path.empty()) << "Can not locate file " << m_fileName;
+ SURGSIM_ASSERT(doLoad(path)) << "Failed to load file " << m_fileName;
+}
+
+void Asset::load(const std::string& fileName)
+{
+ load(fileName, *SurgSim::Framework::Runtime::getApplicationData());
+}
+
+std::string Asset::getFileName() const
+{
+ return m_fileName;
+}
+
+void Asset::serializeFileName(SurgSim::Framework::Accessible* accesible)
+{
+ // Special treatment to let std::bind() deal with overloaded function.
+ auto resolvedOverloadFunction = static_cast<void(Asset::*)(const std::string&)>(&Asset::load);
+
+ accesible->setAccessors("FileName",
+ std::bind(&Asset::getFileName, this),
+ std::bind(resolvedOverloadFunction, this,
+ std::bind(SurgSim::Framework::convert<std::string>, std::placeholders::_1)));
+
+ accesible->setSerializable("FileName",
+ std::bind(&YAML::convert<std::string>::encode, std::bind(&Asset::getFileName, this)),
+ std::bind(resolvedOverloadFunction, this,
+ std::bind(&YAML::Node::as<std::string>, std::placeholders::_1)));
+}
+
+}; // Framework
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Framework/Asset.h b/SurgSim/Framework/Asset.h
new file mode 100644
index 0000000..815826e
--- /dev/null
+++ b/SurgSim/Framework/Asset.h
@@ -0,0 +1,80 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_ASSET_H
+#define SURGSIM_FRAMEWORK_ASSET_H
+
+#include <string>
+
+namespace SurgSim
+{
+namespace Framework
+{
+class Accessible;
+class ApplicationData;
+class AssetTest;
+
+/// This class is used to facilitate file loading. It uses the static ApplicationData
+/// in SurgSim::Framework::Runtime to load file.
+/// Classes not in SurgSim::Framework::Component hierarchy should inherit this class in
+/// order to load a file.
+class Asset
+{
+ friend AssetTest;
+public:
+ /// Constructor
+ Asset();
+
+ /// Destructor
+ virtual ~Asset();
+
+ /// Load a file with given name using 'data' as look up path(s).
+ /// If 'fileName' is not empty and the file is found, this method calls 'doLoad()' to load the file.
+ /// Assertions will fail if 'fileName' is empty or file is not found or file loading is unsuccessful.
+ /// \note As a side effect, the name of the file will be recorded in
+ /// \note Asset::m_fileName and can be retrieved by Asset::getFileName().
+ /// \param fileName Name of the file to be loaded.
+ /// \param data ApplicationData which provides the runtime look up path(s).
+ void load(const std::string& fileName, const SurgSim::Framework::ApplicationData& data);
+
+ /// Overloaded function using SurgSim::Framework::Runtime::getApplicationData() as look up path(s).
+ /// \param fileName Name of the file to be loaded.
+ void load(const std::string& fileName);
+
+ /// Return the name of file loaded by this class.
+ /// \return Name of the file loaded by this class.
+ std::string getFileName() const;
+
+protected:
+ /// Derived classes will overwrite this method to do actual loading.
+ /// \note This method is not required to do any check on the validity or the existence of the file.
+ /// \param filePath Absolute path to the file.
+ /// \return True if loading is successful; Otherwise, false.
+ virtual bool doLoad(const std::string& filePath) = 0;
+
+ /// Derived classes (which also inherit from SurgSim::Framework::Accessible) should call this function
+ /// with 'this' pointer as the parameter in their constructors to register file name property for serialization.
+ /// \param accessible 'this' pointer of derived class.
+ void serializeFileName(SurgSim::Framework::Accessible* accessible);
+
+private:
+ /// Name of the file to be loaded.
+ std::string m_fileName;
+};
+
+} // namespace Framework
+} // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_ASSET_H
\ No newline at end of file
diff --git a/SurgSim/Framework/Barrier.cpp b/SurgSim/Framework/Barrier.cpp
new file mode 100644
index 0000000..517ad4f
--- /dev/null
+++ b/SurgSim/Framework/Barrier.cpp
@@ -0,0 +1,59 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Based on barrier.hpp from Boost 1.51
+// Copyright (C) 2002-2003
+// David Moore, William E. Kempf
+// Copyright (C) 2007-8 Anthony Williams
+//
+// Which was distributed under the Boost Software License, Version 1.0.
+// (See accomanying NOTICES or a copy at http://www.boost.org/LICENSE_1_0.txt)
+
+#include "SurgSim/Framework/Barrier.h"
+
+SurgSim::Framework::Barrier::Barrier(size_t count) :
+ m_threshold(count),
+ m_count(count),
+ m_generation(0),
+ m_success(true)
+{
+ SURGSIM_ASSERT(count != 0) << "Barrier constructor count cannot be zero";
+}
+
+bool SurgSim::Framework::Barrier::wait(bool success)
+{
+ boost::mutex::scoped_lock lock(m_mutex);
+ size_t gen = m_generation;
+ m_success = m_success && success;
+
+ --m_count;
+
+ if (m_count == 0)
+ {
+ m_generation++;
+ m_count = m_threshold;
+ m_successResult = m_success;
+ m_success = true;
+ m_cond.notify_all();
+ return m_successResult;
+ }
+
+ while (gen == m_generation)
+ {
+ m_cond.wait(lock);
+ }
+ return m_successResult;
+}
+
diff --git a/SurgSim/Framework/Barrier.h b/SurgSim/Framework/Barrier.h
new file mode 100644
index 0000000..d87f475
--- /dev/null
+++ b/SurgSim/Framework/Barrier.h
@@ -0,0 +1,76 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Based on barrier.hpp from Boost 1.51
+// Copyright (C) 2002-2003
+// David Moore, William E. Kempf
+// Copyright (C) 2007-8 Anthony Williams
+//
+// Which was distributed under the Boost Software License, Version 1.0.
+// (See accomanying NOTICES or a copy at http://www.boost.org/LICENSE_1_0.txt)
+
+#ifndef SURGSIM_FRAMEWORK_BARRIER_H
+#define SURGSIM_FRAMEWORK_BARRIER_H
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition_variable.hpp>
+#include <string>
+#include <stdexcept>
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// Barrier class, synchronize a set of threads to wait at a common point, all
+/// threads will wait at Barrier::wait(val) until the number of threads calling
+/// wait is equal to the number given in the constructor.
+/// Additionally wait will return a boolean AND over all the values passed into
+/// the wait function, this can be used to signal a failure condition across
+/// threads.
+class Barrier
+{
+public:
+ /// Construct the barrier.
+ /// \param count Number of threads to synchronize, can't be 0.
+ explicit Barrier(size_t count);
+
+ /// Waits until all \a count threads have called wait.
+ ///
+ /// The wait calls in all of the threads waiting on a barrier will return
+ /// the same value. This return value will be true if the \c success
+ /// argument was true in \em all of the threads; if any thread passes false,
+ /// the return value will be false.
+ /// \param success a value indicating if this thread has been successful, used to determine the return
+ /// value across all threads.
+ /// \return true if all threads claimed success, false otherwise.
+ bool wait(bool success);
+
+private:
+ boost::mutex m_mutex;
+ boost::condition_variable m_cond;
+ size_t m_threshold;
+ size_t m_count;
+ size_t m_generation;
+ bool m_success;
+ bool m_successResult;
+};
+
+} // namespace Framework
+} // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Framework/BasicSceneElement.cpp b/SurgSim/Framework/BasicSceneElement.cpp
new file mode 100644
index 0000000..fa58d0c
--- /dev/null
+++ b/SurgSim/Framework/BasicSceneElement.cpp
@@ -0,0 +1,39 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+BasicSceneElement::BasicSceneElement(const std::string& name) :
+ SurgSim::Framework::SceneElement(name)
+{
+}
+
+BasicSceneElement::~BasicSceneElement()
+{
+}
+
+bool BasicSceneElement::doInitialize()
+{
+ return true;
+}
+
+}
+}
+
diff --git a/SurgSim/Framework/BasicSceneElement.h b/SurgSim/Framework/BasicSceneElement.h
new file mode 100644
index 0000000..060bfba
--- /dev/null
+++ b/SurgSim/Framework/BasicSceneElement.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_BASICSCENEELEMENT_H
+#define SURGSIM_FRAMEWORK_BASICSCENEELEMENT_H
+
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Framework/Macros.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+
+/// Simple concrete implementation of a scene element that does not have any higher logic
+class BasicSceneElement : public SurgSim::Framework::SceneElement
+{
+public:
+ /// Constructor
+ /// \name Name of the scene element
+ explicit BasicSceneElement(const std::string& name);
+ /// Destructor
+ virtual ~BasicSceneElement();
+
+ SURGSIM_CLASSNAME(SurgSim::Framework::BasicSceneElement);
+
+protected:
+ /// Initializes the scene element
+ /// \return True if succeeds, false if fails
+ virtual bool doInitialize() override;
+
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_BASICSCENEELEMENT_H
diff --git a/SurgSim/Framework/BasicThread.cpp b/SurgSim/Framework/BasicThread.cpp
new file mode 100644
index 0000000..73a067b
--- /dev/null
+++ b/SurgSim/Framework/BasicThread.cpp
@@ -0,0 +1,255 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/BasicThread.h"
+
+#include <boost/thread.hpp>
+#include <boost/thread/barrier.hpp>
+#include <boost/ref.hpp>
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Clock.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Runtime.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+BasicThread::BasicThread(const std::string& name) :
+ m_name(name),
+ m_period(1.0 / 30),
+ m_isIdle(false),
+ m_isInitialized(false),
+ m_isRunning(false),
+ m_stopExecution(false),
+ m_isSynchronous(false)
+{
+}
+
+#ifdef _MSC_VER
+BasicThread::~BasicThread() throw(...) // Visual Studio does not support noexcept. The throw(...) is optional.
+#else
+BasicThread::~BasicThread() noexcept(false) /// C++11 introduced noexcept
+#endif
+{
+ // Still need to stop thread to get a clean exit
+ if (m_isRunning || m_thisThread.joinable())
+ {
+ SURGSIM_FAILURE() <<
+ "A BasicThread instance destructor was called while the thread was still running or " <<
+ "in the process of being stopped, this is currently not supported. If this was intentional " <<
+ "call stop() before destruction of the thread. If this is unintentional, make sure to prevent " <<
+ "the destructor from being called.";
+ }
+}
+
+bool BasicThread::isInitialized()
+{
+ return m_isInitialized;
+}
+
+bool BasicThread::isRunning() const
+{
+ return m_isRunning;
+}
+
+bool BasicThread::initialize()
+{
+ m_isInitialized = doInitialize();
+ return m_isInitialized;
+}
+
+
+bool BasicThread::startUp()
+{
+ return doStartUp();
+}
+
+void BasicThread::start(std::shared_ptr<Barrier> startupBarrier, bool isSynchronized)
+{
+ m_startupBarrier = startupBarrier;
+ m_stopExecution = false;
+ m_isRunning = false;
+ m_isSynchronous = isSynchronized;
+
+ // Start the thread with a reference to this
+ // prevents making a copy
+ m_thisThread = boost::thread(boost::ref(*this));
+}
+
+boost::thread& BasicThread::getThread()
+{
+ return m_thisThread;
+}
+
+void BasicThread::operator()()
+{
+ bool success = executeInitialization();
+ if (! success)
+ {
+ return;
+ }
+
+ boost::chrono::duration<double> frameTime(0.0);
+ boost::chrono::duration<double> sleepTime(0.0);
+ Clock::time_point start;
+
+ m_isRunning = true;
+ while (m_isRunning && !m_stopExecution)
+ {
+ if (! m_isSynchronous)
+ {
+ // Check for frameTime being > desired update period report error, adjust ...
+ if (m_period > frameTime)
+ {
+ sleepTime = m_period - frameTime;
+ boost::this_thread::sleep_until(Clock::now() + sleepTime);
+ }
+ start = Clock::now();
+ if (!m_isIdle)
+ {
+ m_isRunning = doUpdate(m_period.count());
+ }
+ frameTime = Clock::now() - start;
+ }
+ else
+ {
+ // HS-2014-feb-21 This is not thread safe, if setSynchronous(false) is called while the thread is in the
+ // _not_ in the wait state, the thread will exit without having issued a wait, this will cause the
+ // all the threads that are waiting to indefinitely wait as there is one less thread on the barrier
+ // #threadsafety
+ bool success = waitForBarrier(true);
+ if (success && !m_isIdle)
+ {
+ m_isRunning = doUpdate(m_period.count());
+ }
+ if (! success || !m_isRunning)
+ {
+ m_isRunning = false;
+ m_isSynchronous = false;
+ }
+ }
+ }
+
+ doBeforeStop();
+
+ m_isRunning = false;
+ m_stopExecution = false;
+}
+
+void BasicThread::stop()
+{
+ m_stopExecution = true;
+
+ if (! m_isSynchronous)
+ {
+ if (! m_thisThread.joinable())
+ {
+ SURGSIM_LOG_INFO(Logger::getDefaultLogger()) << "Thread " << getName() <<
+ " is detached, cannot wait for it to stop.";
+ }
+ else
+ {
+ m_thisThread.join();
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(Logger::getDefaultLogger()) << "Thread " << getName() <<
+ " is in synchronouse mode, stop with a barrier->wait(false).";
+ }
+}
+
+void BasicThread::setIdle(bool isIdle)
+{
+ m_isIdle = isIdle;
+}
+
+bool BasicThread::isIdle()
+{
+ return m_isIdle;
+}
+
+std::string BasicThread::getName() const
+{
+ return m_name;
+}
+
+bool BasicThread::executeInitialization()
+{
+ bool success = true;
+
+ success = initialize();
+ SURGSIM_ASSERT(success) << "Initialization has failed for thread " << getName();
+ SURGSIM_LOG_INFO(Logger::getDefaultLogger()) << "Initialization has succeeded for thread " << getName();
+ // Waits for all the threads to init and then proceeds
+ // If one of the other thread asserts and ends this does not matter
+ // as the process will be taken down
+ success = waitForBarrier(success);
+
+ if (!success)
+ {
+ return success;
+ }
+
+ success = startUp();
+
+ SURGSIM_ASSERT(success) << "Startup has failed for thread " << getName();
+ SURGSIM_LOG_INFO(Logger::getDefaultLogger()) << "Startup has succeeded for thread " << getName();
+
+ // Waits for all the threads to startup and then proceeds
+ success = waitForBarrier(success);
+
+ return success;
+}
+
+bool BasicThread::waitForBarrier(bool success)
+{
+ if (m_startupBarrier != nullptr)
+ {
+ success = m_startupBarrier->wait(success);
+ }
+ return success;
+}
+
+bool BasicThread::setSynchronous(bool val)
+{
+ if (m_startupBarrier != nullptr)
+ {
+ m_isSynchronous = val;
+ }
+ return m_isSynchronous;
+}
+
+bool BasicThread::isSynchronous()
+{
+ return m_isSynchronous;
+}
+
+bool BasicThread::doUpdate(double dt)
+{
+ return true;
+}
+
+void SurgSim::Framework::BasicThread::doBeforeStop()
+{
+}
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+
+
diff --git a/SurgSim/Framework/BasicThread.h b/SurgSim/Framework/BasicThread.h
new file mode 100644
index 0000000..a3124de
--- /dev/null
+++ b/SurgSim/Framework/BasicThread.h
@@ -0,0 +1,169 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_BASICTHREAD_H
+#define SURGSIM_FRAMEWORK_BASICTHREAD_H
+
+#include <memory>
+#include <string>
+
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+
+#include "SurgSim/Framework/Barrier.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+class Component;
+class Runtime;
+
+/// Basic thread implementation, tries to maintain a constant rate, supplies
+/// startup an initialization, can be synchronized with other threads at startup
+/// after calling doRun() a thread be be set off and doInit() and doStartup() will
+/// be called in succession. If given a startup barrier the sequence will pause at
+/// both steps until all other threads are done with these steps.
+/// Initialization can be further customized by implementing a executeInitialization() function.
+/// When a barrier was used to start up the thread it can also be used to run the thread in a
+/// synchronous fashion. Use setIsSynchrous(true) to switch the thread over, after that the thread
+/// will wait for the barrier to trigger before it executes another update. When running asynchronously the thread
+/// cannot be stopped with the stop() call, a barrier wait with an argument of false has to be used
+/// to stop the thread. The thread can be set back to asynchronous execution, one last barrier wait after
+/// the switch has to be executed for the thread to come out of the wait.
+class BasicThread
+{
+public:
+ explicit BasicThread(const std::string& name = "Unknown Thread");
+#ifdef _MSC_VER
+ virtual ~BasicThread() throw(...); // Visual Studio does not support noexcept. The throw(...) is optional.
+#else
+ virtual ~BasicThread() noexcept(false); /// C++11 introduced noexcept
+#endif
+
+ /// Live cycle functions, public implementation.
+ /// All of these have virtual partners as private functions
+
+ /// Start the thread from the outside, this will call the private
+ /// run() function that can be overridden for each implementor of this
+ /// interface.
+ /// \param startupBarrier is a barrier it synchronizes a group of thread that should go through their startup
+ /// sequence in step.
+ /// \param isSynchronous when true the thread will wait on the barrier after each call to update(dt), this
+ /// means that only one step will be performed at a time
+ void start(std::shared_ptr<Barrier> startupBarrier = nullptr, bool isSynchronous = false);
+
+ /// Stopping the execution, blocks until the running thread has actually stopped,
+ /// \note When the thread is in synchronous mode, it needs to be stopped with a call to
+ /// the barrier wait function with an argument of false, of course it can always be stopped
+ /// by going back to asynchronous mode and then calling stop
+ void stop();
+
+ /// Set/Unset the thread in an idle state (doUpdate() called or not in the update() method)
+ /// \param isIdle True to set the thread in an idle state, false otherwise
+ void setIdle(bool isIdle);
+
+ /// Query if this thread is in idle state or not
+ /// \return true if the thread is in idle state, false otherwise.
+ bool isIdle();
+
+ /// Query if this object is initialized.
+ /// \return true if initialized, false if not.
+ bool isInitialized();
+
+ /// Query if this object is running.
+ /// \return true if the threads update() function is being called
+ bool isRunning() const;
+
+ /// This is what boost::thread executes on thread creation.
+ void operator()();
+
+ /// \return the boost threading object
+ boost::thread& getThread();
+
+ /// \return the name of the thread
+ std::string getName() const;
+
+ /// Set the update rate of the thread
+ /// \param val rate in hertz (updates per second) of the thread
+ void setRate(double val)
+ {
+ m_period = boost::chrono::duration<double>(1.0 / val);
+ }
+
+ /// Sets the thread to synchronized execution in concert with the startup
+ /// barrier, the startup barrier has to exist for this call to succeed.
+ /// When the thread is set to run synchronized it will only execute one update at a time
+ /// and then wait for the startup barrier to wake it up again.
+ /// \param val if true the thread will need to be controlled via the barrier.
+ /// \return the actual value of isSynchronous()
+ /// \note HS-2013-nov-01 Currently mostly for use in unit tests and debugging, when multiple thread with differing
+ /// rates are being synchronized the call rates will not correspond to the expected rates.
+ bool setSynchronous(bool val);
+
+ /// Query if this object is synchronized.
+ /// \return true if synchronized, false if not.
+ bool isSynchronous();
+
+protected:
+
+ /// Trigger the initialization of this object, this will be called before all other threads doStartup()
+ /// are called
+ /// \return true on success
+ bool initialize();
+
+ /// Trigger the startup of this object, this will be called after all other threads doInit() was called
+ /// the thread will only enter the run loop triggering upated() if all threads doInit() and doStartup()
+ /// returned true
+ /// \return true on success
+ bool startUp();
+
+ bool waitForBarrier(bool success);
+
+ virtual bool executeInitialization();
+
+
+private:
+ std::string m_name;
+
+ boost::thread m_thisThread;
+ boost::chrono::duration<double> m_period;
+ std::shared_ptr<Barrier> m_startupBarrier;
+
+ bool m_isIdle;
+ bool m_isInitialized;
+ bool m_isRunning;
+ bool m_stopExecution;
+ bool m_isSynchronous;
+
+ virtual bool doInitialize() = 0;
+ virtual bool doStartUp() = 0;
+
+ /// Implementation of actual work function for this thread, this has a default implementation to handle
+ /// destruction better, as it could be called while the thread is under destruction, if left unimplemented
+ /// this would trigger a call to a pure virtual function.
+ /// \return false when the thread is done, this will stop execution
+ virtual bool doUpdate(double dt);
+
+ /// Prepares the thread for its execution to be stopped
+ /// \note Called from this thread before joined
+ virtual void doBeforeStop();
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_BASICTHREAD_H
diff --git a/SurgSim/Framework/Behavior.h b/SurgSim/Framework/Behavior.h
new file mode 100644
index 0000000..9961942
--- /dev/null
+++ b/SurgSim/Framework/Behavior.h
@@ -0,0 +1,61 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_BEHAVIOR_H
+#define SURGSIM_FRAMEWORK_BEHAVIOR_H
+
+#include "SurgSim/Framework/Component.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// Fixed List of enums for the available manager types, do not explicitly assign values,
+/// MANAGER_TYPE_COUNT is used to determine the number of actual manager types
+enum {
+ MANAGER_TYPE_NONE = -1,
+ MANAGER_TYPE_BEHAVIOR,
+ MANAGER_TYPE_GRAPHICS,
+ MANAGER_TYPE_INPUT,
+ MANAGER_TYPE_PHYSICS,
+ MANAGER_TYPE_COUNT
+};
+
+/// Behaviors perform actions. They can update components, facilitate
+/// communication between components, and create new components. They are
+/// updated periodicly by the BehaviorManager through update() call.
+class Behavior: public Component
+{
+public:
+ explicit Behavior(const std::string& name) : Component(name)
+ {
+ }
+ virtual ~Behavior()
+ {
+ }
+
+ /// Update the behavior
+ /// \param dt The length of time (seconds) between update calls.
+ virtual void update(double dt) = 0;
+
+ /// Specifies which manger will handle this behavior
+ virtual int getTargetManagerType() const { return MANAGER_TYPE_BEHAVIOR; }
+};
+
+}; //namespace Framework
+}; //namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_BEHAVIOR_H
diff --git a/SurgSim/Framework/BehaviorManager.cpp b/SurgSim/Framework/BehaviorManager.cpp
new file mode 100644
index 0000000..0e3b6aa
--- /dev/null
+++ b/SurgSim/Framework/BehaviorManager.cpp
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/BehaviorManager.h"
+
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/Logger.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+BehaviorManager::BehaviorManager() : ComponentManager("Behavior Manager")
+{
+ m_logger = SurgSim::Framework::Logger::getLogger(getName());
+}
+
+BehaviorManager::~BehaviorManager()
+{
+
+}
+
+bool BehaviorManager::doInitialize()
+{
+ return true;
+}
+
+bool BehaviorManager::doStartUp()
+{
+ return true;
+}
+
+bool BehaviorManager::executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component)
+{
+ return false;
+}
+
+bool BehaviorManager::executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component)
+{
+ return false;
+}
+
+bool BehaviorManager::doUpdate(double dt)
+{
+ // Add all components that came in before the last update
+ processComponents();
+
+ // Process specific behaviors belongs to this manager
+ processBehaviors(dt);
+ return true;
+}
+
+int BehaviorManager::getType() const
+{
+ return MANAGER_TYPE_BEHAVIOR;
+}
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+
+
diff --git a/SurgSim/Framework/BehaviorManager.h b/SurgSim/Framework/BehaviorManager.h
new file mode 100644
index 0000000..e74cae6
--- /dev/null
+++ b/SurgSim/Framework/BehaviorManager.h
@@ -0,0 +1,54 @@
+ // This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_BEHAVIORMANAGER_H
+#define SURGSIM_FRAMEWORK_BEHAVIORMANAGER_H
+
+#include <memory>
+
+#include "SurgSim/Framework/ComponentManager.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// Manager to handle Behaviors. The manager will collect all the behaviors
+/// in the scene through addComponent/removeComponent calls. All the
+/// behaviors will be update once per period (default 30Hz) once the
+/// BehaviorManager is started.
+class BehaviorManager : public ComponentManager
+{
+public:
+ BehaviorManager();
+ ~BehaviorManager();
+
+ virtual int getType() const override;
+
+protected:
+ virtual bool executeAdditions(const std::shared_ptr<Component>& component) override;
+ virtual bool executeRemovals(const std::shared_ptr<Component>& component) override;
+
+private:
+ virtual bool doInitialize() override;
+ virtual bool doStartUp() override;
+ virtual bool doUpdate(double dt) override;
+};
+
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_BEHAVIORMANAGER_H
diff --git a/SurgSim/Framework/CMakeLists.txt b/SurgSim/Framework/CMakeLists.txt
new file mode 100644
index 0000000..f6c2311
--- /dev/null
+++ b/SurgSim/Framework/CMakeLists.txt
@@ -0,0 +1,107 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+set(SURGSIM_FRAMEWORK_SOURCES
+ Accessible.cpp
+ ApplicationData.cpp
+ AssertMessage.cpp
+ Asset.cpp
+ Barrier.cpp
+ BasicSceneElement.cpp
+ BasicThread.cpp
+ BehaviorManager.cpp
+ Component.cpp
+ ComponentManager.cpp
+ FrameworkConvert.cpp
+ Logger.cpp
+ LoggerManager.cpp
+ LogMessageBase.cpp
+ LogOutput.cpp
+ PoseComponent.cpp
+ Representation.cpp
+ Runtime.cpp
+ Scene.cpp
+ SceneElement.cpp
+ Timer.cpp
+ TransferPropertiesBehavior.cpp
+)
+
+set(SURGSIM_FRAMEWORK_HEADERS
+ Accessible.h
+ Accessible-inl.h
+ ApplicationData.h
+ Assert.h
+ AssertMessage.h
+ Asset.h
+ Barrier.h
+ BasicSceneElement.h
+ BasicThread.h
+ Behavior.h
+ BehaviorManager.h
+ Clock.h
+ Component.h
+ ComponentManager.h
+ ComponentManager-inl.h
+ FrameworkConvert.h
+ FrameworkConvert-inl.h
+ LockedContainer.h
+ Log.h
+ Logger.h
+ LoggerManager.h
+ LogMacros.h
+ LogMessage.h
+ LogMessageBase.h
+ LogOutput.h
+ Macros.h
+ ObjectFactory.h
+ ObjectFactory-inl.h
+ PoseComponent.h
+ Representation.h
+ ReuseFactory.h
+ Runtime.h
+ Scene.h
+ SceneElement.h
+ SceneElement-inl.h
+ SharedInstance.h
+ SharedInstance-inl.h
+ Timer.h
+ TransferPropertiesBehavior.h
+)
+
+surgsim_add_library(
+ SurgSimFramework
+ "${SURGSIM_FRAMEWORK_SOURCES}"
+ "${SURGSIM_FRAMEWORK_HEADERS}"
+ "SurgSim/Framework"
+)
+
+SET(LIBS
+ ${Boost_LIBRARIES}
+ ${YAML_CPP_LIBRARIES}
+)
+
+target_link_libraries(SurgSimFramework ${LIBS})
+add_dependencies(SurgSimFramework yaml-cpp)
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+endif()
+
+add_subdirectory(Documentation)
+
+
+# Put SurgSimFramework into folder "Framework"
+set_target_properties(SurgSimFramework PROPERTIES FOLDER "Framework")
diff --git a/SurgSim/Framework/Clock.h b/SurgSim/Framework/Clock.h
new file mode 100644
index 0000000..7395a85
--- /dev/null
+++ b/SurgSim/Framework/Clock.h
@@ -0,0 +1,36 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_CLOCK_H
+#define SURGSIM_FRAMEWORK_CLOCK_H
+
+#include <boost/chrono.hpp>
+
+/// \file
+/// Place for a simple wrapper around boost
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+ /// Wraps around the actual clock we are using.
+ typedef boost::chrono::system_clock Clock;
+
+}; // Framework
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Framework/Component.cpp b/SurgSim/Framework/Component.cpp
new file mode 100644
index 0000000..4709579
--- /dev/null
+++ b/SurgSim/Framework/Component.cpp
@@ -0,0 +1,192 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Component.h"
+
+#include <boost/uuid/random_generator.hpp>
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Framework/SceneElement.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+// Forward References
+class Runtime;
+class Scene;
+
+Component::Component(const std::string& name) :
+ m_name(name),
+ m_uuid(boost::uuids::random_generator()()),
+ m_didInit(false),
+ m_didWakeUp(false),
+ m_isInitialized(false),
+ m_isAwake(false),
+ m_isLocalActive(true)
+{
+ SURGSIM_ADD_RO_PROPERTY(Component, bool, IsActive, isActive);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Component, bool, IsLocalActive, isLocalActive, setLocalActive);
+}
+
+Component::~Component()
+{
+}
+
+std::string Component::getName() const
+{
+ return m_name;
+}
+
+void Component::setName(const std::string& name)
+{
+ m_name = name;
+}
+
+bool Component::isInitialized() const
+{
+ return m_isInitialized;
+}
+
+bool Component::initialize(const std::weak_ptr<Runtime>& runtime)
+{
+ SURGSIM_ASSERT(!m_didInit) << "Double initialization called in component " << getName();
+ m_runtime = runtime;
+
+ m_didInit = true;
+ m_isInitialized = doInitialize();
+
+ return m_isInitialized;
+}
+
+bool Component::isAwake() const
+{
+ return m_isAwake;
+}
+
+bool Component::wakeUp()
+{
+ SURGSIM_ASSERT(! m_didWakeUp) << "Double wakeup called on component." << getName();
+ SURGSIM_ASSERT(m_didInit) << "Component " << getName() << " was awoken without being initialized.";
+ SURGSIM_ASSERT(m_isInitialized) << "Wakeup called even though initialization failed on component." << getName();
+
+ m_didWakeUp = true;
+ m_isAwake = doWakeUp();
+
+ return m_isAwake;
+}
+
+void Component::setScene(std::weak_ptr<Scene> scene)
+{
+ m_scene = scene;
+}
+
+std::shared_ptr<Scene> Component::getScene()
+{
+ return m_scene.lock();
+}
+
+void Component::setSceneElement(std::weak_ptr<SceneElement> sceneElement)
+{
+ m_sceneElement = sceneElement;
+}
+
+std::shared_ptr<SceneElement> Component::getSceneElement()
+{
+ return m_sceneElement.lock();
+}
+
+std::shared_ptr<const SceneElement> Component::getSceneElement() const
+{
+ return m_sceneElement.lock();
+}
+
+std::shared_ptr<Runtime> Component::getRuntime() const
+{
+ return m_runtime.lock();
+}
+
+std::shared_ptr<const PoseComponent> Component::getPoseComponent() const
+{
+ SURGSIM_ASSERT(m_isInitialized) << "Can't access the pose component before initialization";
+ return m_sceneElement.lock()->getPoseComponent();
+}
+
+std::shared_ptr<PoseComponent> Component::getPoseComponent()
+{
+ SURGSIM_ASSERT(m_isInitialized) << "Can't access the pose component before initialization";
+ return m_sceneElement.lock()->getPoseComponent();
+}
+
+boost::uuids::uuid Component::getUuid() const
+{
+ return m_uuid;
+}
+
+Component::FactoryType& Component::getFactory()
+{
+ static FactoryType factory;
+ return factory;
+}
+
+std::string Component::getClassName() const
+{
+ SURGSIM_LOG_WARNING(Logger::getDefaultLogger()) << "getClassName() called on Component base class, this is wrong" <<
+ " in almost all cases, this means there is a class that does not have getClassName() defined.";
+ return "SurgSim::Framework::Component";
+}
+
+std::shared_ptr<Component> Component::getSharedPtr()
+{
+ std::shared_ptr<Component> result;
+ try
+ {
+ result = shared_from_this();
+ }
+ catch (const std::exception&)
+ {
+ SURGSIM_FAILURE() << "Component was not created as a shared_ptr.";
+ }
+ return result;
+}
+
+bool Component::isActive() const
+{
+ if (getSceneElement() != nullptr)
+ {
+ return getSceneElement()->isActive() && m_isLocalActive;
+ }
+ else
+ {
+ return m_isLocalActive;
+ }
+}
+
+void Component::setLocalActive(bool val)
+{
+ m_isLocalActive = val;
+}
+
+bool Component::isLocalActive() const
+{
+ return m_isLocalActive;
+}
+
+
+}; // namespace Framework
+}; // namespace SurgSim
diff --git a/SurgSim/Framework/Component.h b/SurgSim/Framework/Component.h
new file mode 100644
index 0000000..0c56760
--- /dev/null
+++ b/SurgSim/Framework/Component.h
@@ -0,0 +1,187 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_COMPONENT_H
+#define SURGSIM_FRAMEWORK_COMPONENT_H
+
+
+#include <string>
+#include <memory>
+
+#include <boost/uuid/uuid.hpp>
+
+#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+// Forward References
+class PoseComponent;
+class Runtime;
+class Scene;
+class SceneElement;
+
+/// Component is the main interface class to pass information to the system managers each will decide
+/// whether to handle a component of a given type or not. Components will get initialized by having
+/// doInit(), and doWakeUp() called in succession, all components together will have doInit() called before
+/// any component will recieve doWakeUp()
+class Component : public Accessible, public std::enable_shared_from_this<Component>
+{
+public:
+ /// Constructor
+ /// \param name Name of the component
+ explicit Component(const std::string& name);
+ /// Destructor
+ virtual ~Component();
+
+ /// Gets component name.
+ /// \return Name of this component.
+ std::string getName() const;
+
+ /// Sets the name of component.
+ /// \param name The name of this component.
+ void setName(const std::string& name);
+
+ /// Gets the id of the component
+ boost::uuids::uuid getUuid() const;
+
+ /// \return True if this component is initialized; otherwise, false.
+ bool isInitialized() const;
+
+ /// Initialize this component, this needs to be called before wakeUp() can be called.
+ /// This will be done automatically by the Scene hierarchy, either in SceneElement::addComponent(), if
+ /// SceneElement has already been added to the Scene, or through Scene::addSceneElement() on all Components
+ /// on the SceneElement.
+ /// \param runtime The runtime which contains this component.
+ /// \return True if this component is initialized successfully; otherwise, false.
+ bool initialize(const std::weak_ptr<Runtime>& runtime);
+
+ /// \return True if this component is awake; otherwise, false.
+ bool isAwake() const;
+
+ /// Wakeup this component, this will be called when the component is inserted into the ComponentManager that is
+ /// responsible for handling this component.
+ /// \return True if this component is woken up successfully; otherwise, false.
+ bool wakeUp();
+
+ /// Sets the scene.
+ /// \param scene The scene for this component
+ void setScene(std::weak_ptr<Scene> scene);
+
+ /// Gets the scene.
+ /// \return The scene for this component
+ std::shared_ptr<Scene> getScene();
+
+ /// Sets the scene element.
+ /// \param sceneElement The scene element for this component.
+ void setSceneElement(std::weak_ptr<SceneElement> sceneElement);
+
+ /// Gets the scene element.
+ /// \return The scene element for this component.
+ std::shared_ptr<SceneElement> getSceneElement();
+
+ /// Gets the scene element, constant version
+ /// \return The scene element for this component.
+ std::shared_ptr<const SceneElement> getSceneElement() const;
+
+ /// Get the runtime which contains this component.
+ /// \return The runtime which contains this component.
+ std::shared_ptr<Runtime> getRuntime() const;
+
+ /// The class name for this class, this being the base class it should
+ /// return SurgSim::Framework::Component but this would make missing implemenentations
+ /// of this hard to catch, therefore this calls SURGSIM_FAILURE.
+ /// \note Use the SURGSIM_CLASSNAME macro in derived classes.
+ /// \return The fully namespace qualified name of this class.
+ virtual std::string getClassName() const;
+
+ typedef SurgSim::Framework::ObjectFactory1<SurgSim::Framework::Component, std::string> FactoryType;
+
+ /// \return The static class factory that is being used in the conversion.
+ static FactoryType& getFactory();
+
+ /// Gets a shared pointer to this component.
+ /// \return The shared pointer.
+ std::shared_ptr<Component> getSharedPtr();
+
+ /// Interface to be implemented by derived classes
+ /// \return True if component is initialized successfully; otherwise, false.
+ virtual bool doInitialize() = 0;
+
+ /// Interface to be implemented by derived classes
+ /// \return True if component is woken up successfully; otherwise, false.
+ virtual bool doWakeUp() = 0;
+
+ /// \return True if this component is active and its SceneElement (if any) is also active;
+ /// Otherwise, false.
+ bool isActive() const;
+
+ /// Set the component's active state
+ /// \param val If true component is active, inactive if false.
+ virtual void setLocalActive(bool val);
+
+ /// \return True if this component is active
+ /// Otherwise, false.
+ bool isLocalActive() const;
+
+protected:
+ /// Get the PoseComponent for this component
+ /// \return The PoseComponent
+ virtual std::shared_ptr<PoseComponent> getPoseComponent();
+
+ /// Get the PoseComponent for this component, constant access
+ /// \return The PoseComponent
+ virtual std::shared_ptr<const PoseComponent> getPoseComponent() const;
+
+private:
+ /// Name of this component
+ std::string m_name;
+
+ /// Id of this component
+ boost::uuids::uuid m_uuid;
+
+ /// Runtime which contains this component
+ std::weak_ptr<Runtime> m_runtime;
+
+ /// Scene which contains this component
+ std::weak_ptr<Scene> m_scene;
+
+ /// SceneElement which contains this component
+ std::weak_ptr<SceneElement> m_sceneElement;
+
+ /// Indicates if doInitialize() has been called
+ bool m_didInit;
+
+ /// Indicates if doWakeup() has been called
+ bool m_didWakeUp;
+
+ /// Indicates if this component is initialized
+ bool m_isInitialized;
+
+ /// Indicates if this component is awake
+ bool m_isAwake;
+
+ /// Indicates if this component is active
+ bool m_isLocalActive;
+
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_COMPONENT_H
diff --git a/SurgSim/Framework/ComponentManager-inl.h b/SurgSim/Framework/ComponentManager-inl.h
new file mode 100644
index 0000000..6d68aab
--- /dev/null
+++ b/SurgSim/Framework/ComponentManager-inl.h
@@ -0,0 +1,83 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_COMPONENTMANAGER_INL_H
+#define SURGSIM_FRAMEWORK_COMPONENTMANAGER_INL_H
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// Executes the add component operation.
+/// \tparam T Type of the component to be added.
+/// \param component The component that is being added.
+/// \param [in,out] container The container that the component is being added to.
+/// \return The correctly cast component if it is of type T and does not exist in the container yet, nullptr otherwise.
+template<class T>
+std::shared_ptr<T> ComponentManager::tryAddComponent(std::shared_ptr<SurgSim::Framework::Component> component,
+ std::vector<std::shared_ptr<T>>* container)
+{
+ SURGSIM_ASSERT(component != nullptr) << "Trying to add a component that is null";
+ SURGSIM_ASSERT(container != nullptr) << "Trying to use a component container that is null";
+ std::shared_ptr<T> typedComponent = std::dynamic_pointer_cast<T>(component);
+ if (typedComponent != nullptr)
+ {
+ auto found = std::find(container->cbegin(), container->cend(), typedComponent);
+ if (found == container->cend())
+ {
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added component " << component->getName();
+ container->push_back(typedComponent);
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " component " << component->getName() <<
+ " already added to " << getName();
+ typedComponent = nullptr;
+ }
+ }
+ return typedComponent;
+};
+
+template<class T>
+bool ComponentManager::tryRemoveComponent(std::shared_ptr<SurgSim::Framework::Component> component,
+ std::vector<std::shared_ptr<T>>* container)
+{
+ SURGSIM_ASSERT(container != nullptr) << "Trying to use a component container that is null";
+ bool result = false;
+ std::shared_ptr<T> typedComponent = std::dynamic_pointer_cast<T>(component);
+ if (typedComponent != nullptr && container->size() != 0)
+ {
+ auto found = std::find(container->begin(), container->end(), typedComponent);
+ if (found != container->end())
+ {
+ container->erase(found);
+ SURGSIM_LOG_DEBUG(m_logger) << __FUNCTION__ << " Removed component " << typedComponent->getName();
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(m_logger) << SURGSIM_CURRENT_FUNCTION << " Unable to remove component " <<
+ typedComponent->getName() << ". Not found.";
+ }
+ }
+ return result;
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif
+
diff --git a/SurgSim/Framework/ComponentManager.cpp b/SurgSim/Framework/ComponentManager.cpp
new file mode 100644
index 0000000..41c6da4
--- /dev/null
+++ b/SurgSim/Framework/ComponentManager.cpp
@@ -0,0 +1,215 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/ComponentManager.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Runtime.h"
+
+#include <boost/thread/locks.hpp>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+ComponentManager::ComponentManager(const std::string& name /*= "Unknown Component Manager"*/) :
+ BasicThread(name), m_logger(Logger::getLogger(name))
+{
+}
+
+ComponentManager::~ComponentManager()
+{
+}
+
+void ComponentManager::setRuntime(std::shared_ptr<Runtime> val)
+{
+ m_runtime = val;
+}
+
+bool ComponentManager::enqueueAddComponent(const std::shared_ptr<Component>& component)
+{
+ boost::lock_guard<boost::mutex> lock(m_componentMutex);
+ m_componentAdditions.push_back(component);
+ return true;
+}
+
+bool ComponentManager::enqueueRemoveComponent(const std::shared_ptr<Component>& component)
+{
+ boost::lock_guard<boost::mutex> lock(m_componentMutex);
+ m_componentRemovals.push_back(component);
+ return true;
+}
+
+std::shared_ptr<Runtime> ComponentManager::getRuntime() const
+{
+ return m_runtime.lock();
+}
+
+void ComponentManager::processComponents()
+{
+ // Please note that the implementation of this function needs to mirror the executeInitialization() function.
+ // This is called from within the update() function, and executeInitialization() is called at startup
+ std::vector<std::shared_ptr<Component>> inflightAdditions;
+ std::vector<std::shared_ptr<Component>> inflightRemovals;
+ std::vector<std::shared_ptr<Component>> actualAdditions;
+
+ copyScheduledComponents(&inflightAdditions, &inflightRemovals);
+ actualAdditions.reserve(inflightAdditions.size());
+
+ if (!inflightAdditions.empty())
+ {
+ addComponents(std::begin(inflightAdditions), std::end(inflightAdditions), &actualAdditions);
+ wakeUpComponents(std::begin(actualAdditions), std::end(actualAdditions));
+ }
+
+ if (!inflightRemovals.empty())
+ {
+ removeComponents(std::begin(inflightRemovals), std::end(inflightRemovals));
+ }
+}
+
+void ComponentManager::processBehaviors(const double dt)
+{
+ auto it = std::begin(m_behaviors);
+ auto endIt = std::end(m_behaviors);
+ for ( ; it != endIt; ++it)
+ {
+ if ((*it)->isActive())
+ {
+ (*it)->update(dt);
+ }
+ }
+}
+
+bool ComponentManager::executeInitialization()
+{
+ // Please note that the implementation of this function needs to mirror processComponents()
+ // this function is called at startup whereas the other is called during the update call.
+
+ // Call BasicThread initialize to do the initialize and startup call
+ bool success = BasicThread::executeInitialization();
+ if (! success )
+ {
+ return success;
+ }
+
+ // Now Initialize and and wakeup all the components
+ std::vector<std::shared_ptr<Component>> inflightAdditions;
+ std::vector<std::shared_ptr<Component>> inflightRemovals;
+ std::vector<std::shared_ptr<Component>> actualAdditions;
+
+ copyScheduledComponents(&inflightAdditions, &inflightRemovals);
+ actualAdditions.reserve(inflightAdditions.size());
+
+ if (! inflightAdditions.empty())
+ {
+ addComponents(std::begin(inflightAdditions), std::end(inflightAdditions), &actualAdditions);
+ }
+
+ success = waitForBarrier(success);
+ if (! success)
+ {
+ return success;
+ }
+
+ if (! inflightAdditions.empty())
+ {
+ wakeUpComponents(std::begin(actualAdditions), std::end(actualAdditions));
+ }
+
+ if (! inflightRemovals.empty())
+ {
+ removeComponents(std::begin(inflightRemovals), std::end(inflightRemovals));
+ }
+
+ success = waitForBarrier(success);
+
+ return success;
+}
+
+void ComponentManager::copyScheduledComponents(
+ std::vector<std::shared_ptr<Component>>* inflightAdditions,
+ std::vector<std::shared_ptr<Component>>* inflightRemovals
+)
+{
+ // Lock for any more additions or removals and then copy to local storage
+ // this will insulate us from the actual add or remove call taking longer than it should
+ boost::lock_guard<boost::mutex> lock(m_componentMutex);
+ *inflightAdditions = std::move(m_componentAdditions);
+ m_componentAdditions.clear();
+
+ *inflightRemovals = std::move(m_componentRemovals);
+ m_componentRemovals.clear();
+}
+
+void ComponentManager::removeComponents(const std::vector<std::shared_ptr<Component>>::const_iterator& beginIt,
+ const std::vector<std::shared_ptr<Component>>::const_iterator& endIt)
+{
+ for(auto it = beginIt; it != endIt; ++it)
+ {
+ tryRemoveComponent(*it, &m_behaviors);
+ executeRemovals(*it);
+ }
+}
+
+void ComponentManager::addComponents(
+ const std::vector<std::shared_ptr<Component>>::const_iterator& beginIt,
+ const std::vector<std::shared_ptr<Component>>::const_iterator& endIt,
+ std::vector<std::shared_ptr<Component>>* actualAdditions)
+{
+ // Add All Components to the internal storage
+ for(auto it = beginIt; it != endIt; ++it)
+ {
+ std::shared_ptr<Behavior> behavior = std::dynamic_pointer_cast<Behavior>(*it);
+ if (behavior != nullptr && behavior->getTargetManagerType() == getType())
+ {
+ if (tryAddComponent(*it, &m_behaviors) != nullptr)
+ {
+ actualAdditions->push_back(*it);
+ }
+ }
+ else if (executeAdditions(*it))
+ {
+ actualAdditions->push_back(*it);
+ }
+ }
+}
+
+void ComponentManager::wakeUpComponents(const std::vector<std::shared_ptr<Component>>::const_iterator& beginIt,
+ const std::vector<std::shared_ptr<Component>>::const_iterator& endIt)
+{
+ for(auto it = beginIt; it != endIt; ++it)
+ {
+ if ( (*it)->isInitialized() && !(*it)->isAwake())
+ {
+ if ( !(*it)->wakeUp())
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Failed to wake up component " << (*it)->getName() << " in manager " <<
+ getName() << ". Component was not added to the manager!";
+ executeRemovals(*it);
+ }
+ }
+ }
+}
+
+/// Returns this manager's logger
+std::shared_ptr<SurgSim::Framework::Logger> ComponentManager::getLogger() const
+{
+ return m_logger;
+}
+
+}; // Framework
+}; // SurgSim
diff --git a/SurgSim/Framework/ComponentManager.h b/SurgSim/Framework/ComponentManager.h
new file mode 100644
index 0000000..fc9f59d
--- /dev/null
+++ b/SurgSim/Framework/ComponentManager.h
@@ -0,0 +1,182 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_COMPONENTMANAGER_H
+#define SURGSIM_FRAMEWORK_COMPONENTMANAGER_H
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <boost/thread/mutex.hpp>
+
+#include "SurgSim/Framework/BasicThread.h"
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Component.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+class Runtime;
+class Logger;
+
+/// Base Component Manager class. Component Managers manage a collection of components.
+/// The runtime will present each new component to the manager, and it is up to
+/// the manger to decide whether to handle a component of a given type or not.
+/// Adding and removing components is thread-safe, when the [add|remove]Component
+/// call is made, the component is added to an intermediary data structure, each
+/// ComponentManager implementation must call processComponents() to trigger the
+/// actual addition and removal. Each ComponentManager subclass needs to implement
+/// doAddComponent() and doRemoveComponent() to the actual addition and removal of
+/// components.
+/// ComponentManager implements a custom executeInitialization() method that lets the
+/// runtime schedule initialization of components that exist at the start of the simulation
+class ComponentManager : public BasicThread
+{
+public:
+
+ explicit ComponentManager(const std::string& name = "Unknown Component Manager");
+ virtual ~ComponentManager();
+
+ /// Queues a component to be added later.
+ /// \param component The component to be added.
+ /// \return true if the component was scheduled for addition, this does not indicate that
+ /// the component will actually be added to this manager
+ bool enqueueAddComponent(const std::shared_ptr<Component>& component);
+
+ /// Queues a component to be removed
+ /// \param component The component to be removed.
+ /// \return true if the component was scheduled for removal, this does not indicate that
+ /// the component will actually be removed from this manager
+ bool enqueueRemoveComponent(const std::shared_ptr<Component>& component);
+
+ /// @{
+ /// Runtime accessors
+ std::shared_ptr<Runtime> getRuntime() const;
+ void setRuntime(std::shared_ptr<Runtime> val);
+ /// @}
+
+protected:
+ /// Template version of the addComponent method.
+ /// \tparam T Specific type of the component that is being added.
+ /// \param component The component that needs to be added.
+ /// \param [in,out] container If non-null, the container that should receive the component if of the correct type.
+ /// \return the correctly cast component pointer if successful and the
+ /// component did not already exist in the container
+ template<class T>
+ std::shared_ptr<T> tryAddComponent(std::shared_ptr<SurgSim::Framework::Component> component,
+ std::vector<std::shared_ptr<T>>* container);
+
+ /// Template version of the removeComponent method.
+ /// \tparam T Specific type of the component that is being removed.
+ /// \param component The component that needs to be removed.
+ /// \param [in,out] container If non-null, the container, from which the component should be removed.
+ /// \return true if the component exists in the container or the component did not cast to T, otherwise.
+ template<class T>
+ bool tryRemoveComponent(std::shared_ptr<SurgSim::Framework::Component> component,
+ std::vector<std::shared_ptr<T>>* container);
+
+
+ /// Processes all the components that are scheduled for addition or removal, this needs to be called
+ /// inside the doUpdate() function.
+ void processComponents();
+ /// Processes behaviors
+ /// This needs to be called inside doUpdate() function in each 'sub' manager.
+ void processBehaviors(const double dt);
+
+ /// Returns the type of Manager
+ // Enum is defined in the beginning of this file
+ virtual int getType() const = 0;
+
+ /// Helper, blocks access to the additions and removal queue and copies the components
+ /// from there to the intermediate inflight queues, after this call, the incoming
+ /// queues will be empty.
+ void copyScheduledComponents(std::vector<std::shared_ptr<Component>>* inflightAdditions,
+ std::vector<std::shared_ptr<Component>>* inflightRemovals);
+
+ /// Returns this manager's logger
+ std::shared_ptr<SurgSim::Framework::Logger> getLogger() const;
+
+ /// Blocks protects addition and removal queues
+ boost::mutex m_componentMutex;
+
+ ///@{
+ /// Data structures
+ /// Contain components scheduled to be added/removed
+ std::vector<std::shared_ptr<Component>> m_componentAdditions;
+ std::vector<std::shared_ptr<Component>> m_componentRemovals;
+ ///@}
+
+ /// Logger for this class
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+
+ /// Collection of behaviors
+ // Each behavior will have a type to be matched with the corresponding manager
+ // Managers will only handle matching behaviors
+ std::vector<std::shared_ptr<SurgSim::Framework::Behavior>> m_behaviors;
+
+private:
+ /// Adds a component.
+ /// \param component The component to be added.
+ /// \return true if the component was scheduled for addition, this does not indicate that
+ /// the component will actually be added to this manager.
+ virtual bool executeAdditions(const std::shared_ptr<Component>& component) = 0;
+
+ /// Handle representations, override for each thread
+ /// \param component The component to be removed.
+ /// \return true if the component was scheduled for removal, this does not indicate that
+ /// the component will actually be removed from this manager.
+ virtual bool executeRemovals(const std::shared_ptr<Component>& component) = 0;
+
+ /// Overridden from BasicThread, extends the initialization to contain component initialization
+ /// including waiting for the other threads to conclude their component initialization and wakeup
+ virtual bool executeInitialization() override;
+
+ /// Delegates to doRemoveComponent to remove all the components in the indicated array.
+ /// \param beginIt The begin iterator.
+ /// \param endIt The end iterator.
+ void removeComponents(const std::vector<std::shared_ptr<Component>>::const_iterator& beginIt,
+ const std::vector<std::shared_ptr<Component>>::const_iterator& endIt);
+
+ /// Delegates to doAddComponent and calls initialize on all the components
+ /// \param beginIt The begin iterator.
+ /// \param endIt The end iterator.
+ /// \param[out] actualAdditions List of components actually added
+ void addComponents(
+ const std::vector<std::shared_ptr<Component>>::const_iterator& beginIt,
+ const std::vector<std::shared_ptr<Component>>::const_iterator& endIt,
+ std::vector<std::shared_ptr<Component>>* actualAdditions);
+
+ /// Wake all the components up, only the components that were successfully initialized get
+ /// the wakeup call, check for isAwake because there to catch multiple versions of the same
+ /// component from being awoken more than once. Will also remove components if they did not
+ /// wake up as expected
+ /// \param beginIt The begin iterator.
+ /// \param endIt The end iterator.
+ void wakeUpComponents(const std::vector<std::shared_ptr<Component>>::const_iterator& beginIt,
+ const std::vector<std::shared_ptr<Component>>::const_iterator& endIt);
+
+ std::weak_ptr<Runtime> m_runtime;
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#include "SurgSim/Framework/ComponentManager-inl.h"
+
+#endif // SURGSIM_FRAMEWORK_COMPONENTMANAGER_H
diff --git a/SurgSim/Framework/Documentation/CMakeLists.txt b/SurgSim/Framework/Documentation/CMakeLists.txt
new file mode 100644
index 0000000..93a3871
--- /dev/null
+++ b/SurgSim/Framework/Documentation/CMakeLists.txt
@@ -0,0 +1,22 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+set(SURGSIM_FRAMEWORK_DOCUMENTATION
+ framework.dox
+ serialization.dox
+)
+
+add_custom_target(SurgSimFrameworkDocumentation SOURCES ${SURGSIM_FRAMEWORK_DOCUMENTATION})
+set_target_properties(SurgSimFrameworkDocumentation PROPERTIES FOLDER "Framework")
\ No newline at end of file
diff --git a/SurgSim/Framework/Documentation/framework.dox b/SurgSim/Framework/Documentation/framework.dox
new file mode 100644
index 0000000..95754dc
--- /dev/null
+++ b/SurgSim/Framework/Documentation/framework.dox
@@ -0,0 +1,8 @@
+/*! \page Framework The OSS Framework
+
+\addtogroup Framework
+
+The 'Framework' namespace contains classes that are the basis of the OSS simulation framework. Most of the classes here are not used by themselves but are extended by the other libraries in OSS.
+
+\ref Serialization
+*/
diff --git a/SurgSim/Framework/Documentation/serialization.dox b/SurgSim/Framework/Documentation/serialization.dox
new file mode 100644
index 0000000..e2ad957
--- /dev/null
+++ b/SurgSim/Framework/Documentation/serialization.dox
@@ -0,0 +1,81 @@
+/*! \page Serialization Serialization
+
+Loading and storing of instances from the OpenSurgSim framework utilizes the facilities provided by YAML-cpp, the duck typing and introspection capabilities provided by SurgSim::Framework::Accessible and when necessary a generic class registration and factory mechanism provided by SurgSim::Framework::ObjectFactory. Together these classes enable a serialization solution that can load and store instances of classes without the need of knowing the structure.
+
+By using YAML::Node, we can represent data in a form that can be written to file and read from a file in the YAML format. By converting instances to and from YAML::Node we can enable the loading and storing of instances to disk. Depending on the requirements more or less work needs to be done to enable the conversions. We will describe the various steps in order of increasing complexity.
+
+## Conventions
+
+For consistency we will agree on naming properties in CamelCase beginning with a capital letter, this concurs with the naming portion of the getter and setter. E.g. if a component has a public getter and setter getFileName() and setFileName() the property should be named 'FileName'. We reserve the names "ClassName", "Name" and "Uuid" for use in component serialization.
+
+## POD
+
+SurgSim and YAML-cpp already include conversions from and to YAML::node for all the POD types that should be needed, int, float, double, std::map, std::vector, ... are handle, likewise with the main Eigen classes Matrix and Vector. These can all be used with YAML::Node as such.
+
+ YAML::Node node;
+ node["value"] = 1;
+ int a = node["value"].as<int>();
+
+For more complex data types the conversion can be implemented as member functions, for consistency purposes the following signatures should be used:
+
+ YAML::node Class::encode();
+ void Class::decode(const YAML::Node& node);
+
+If an addition to the member class is not possible or the conversion using 'node.as<>()' is wished, it will be necessary to specialize the YAML::convert structure for the required class type that needs to be converted.
+
+ template <>
+ YAML::convert<Class>(...)
+
+After implementing either of these it should be possible to create a node structure from the class under development, and to fill the members of the class with values from the node.
+
+When restoring the class these approaches work best if the class that is being handled implements a default constructor, when it does the data in the given node does not have to be inspected or extracted before class creation. And a class can be filled with data as such.
+
+ Class a;
+ a.decode(dataNode);
+
+When the constructor needs more data things like the following might become necessary possibly making the process more brittle
+
+ int val = node["val"].as<int>();
+ Class a(val);
+ a.decode(dataNode);
+
+## Accessible
+
+An easy way to enable serialization for a class is to derive from SurgSim::Framework::Accessible, and to declare all the classes' properties that need to be serialized through the `SURGSIM_SERIALIZABLE_PROPERTY` macro. This macro does two things, it declares a read/write property on the instance, but it also declares a conversion from and to YAML::Node for this property. `Accessible` implements `encode()` and `decode()` in a way so that all the properties that were declared serializable [...]
+
+## ObjectFactory
+
+The above approaches work well when the type of the data that is being serialized is known, e.g. a list of objects of the same type, or the specific member variables of a class. An additional facility needs to be utilized when the objects that are being dealt with are of heterogeneous types, i.e. list of objects with same base type but different subtypes. In this case a factory class needs to be filled and utilized. The main purpose of the factory is to create an instance of a class from [...]
+
+### Registration
+
+For classes with limited subclasses it is usually pretty easy to execute all the calls to `register()` in one place, but sometimes this is not feasible and a more distributed approach is necessary. For Example when the classes are spread out over multiple libraries it can be hard to create an exhaustive list. In this case on can register the class through static initialisation using a macro. The following code used in the `.cpp` file can register a subclass in the superclasses factory if [...]
+
+ namespace
+ {
+ SURGSIM_REGISTER(BaseClass, DerivedClass)
+ }
+
+with `Baseclass` being the fully qualified name of the base class e.g. `SurgSim::Framework::Component` and `Derived` the name of the derived class without name space e.g. `OsgBoxRepresentation`.
+
+### Loading
+
+The key to restore the correct class under differing incoming classes is to implement the YAML conversion specialized to the base class pointer, when the decode function is called, the decode code can determine at runtime what instance needs to be created and call the appropriate factory function. After the correct instance has been created, a class member `decode()` function can be used to fill the appropriate member variables.
+
+## Instances vs. References
+
+When there is a need to load and store references to objects, when objects are being shared amongst other objects the serialization can be specialized to object instances and object pointers to indicate whether a reference to an object as opposed to the actual object data should be written.
+
+ template<>
+ YAML::convert<Class> ...
+
+ template<>
+ YAMLL::convert<std::shared_ptr<Class>>
+
+In the calling code the determination needs to be made whether the reference is serialized or the actual instance. Only the instance serialization should write out the data, the reference serializations should write out, a unique identifier for the instance and class information, if needed. When reading the unique identifier can be used to return a pointer to the shared instance in all places where the object is being used. When the actual data is found the normal `decode()` can be used [...]
+
+## Component Loading and Storing
+
+All of the above mechanisms are in place for loading and storing components. The component class implements a factory, it utilizes the split convert object to serialize shared component references from the inside of a component. The data for a component can be written out when the owning `SceneElement` writes its component. Additionally there is a registry data structure for components in the inside of the convert object that can be used to restore shared instances to components.
+
+*/
diff --git a/SurgSim/Framework/FrameworkConvert-inl.h b/SurgSim/Framework/FrameworkConvert-inl.h
new file mode 100644
index 0000000..b86ce8a
--- /dev/null
+++ b/SurgSim/Framework/FrameworkConvert-inl.h
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_FRAMEWORKCONVERT_INL_H
+#define SURGSIM_FRAMEWORK_FRAMEWORKCONVERT_INL_H
+
+#include <type_traits>
+#include "SurgSim/Framework/Assert.h"
+
+// Use of enable_if. this function is only created if 'T' is a subclass of SurgSim::Framework::Component
+// Which means for each subclass that is being deserialized, a new converter function is created
+template <class T>
+YAML::Node YAML::convert<std::shared_ptr<T>>::encode(
+ const typename std::enable_if <std::is_base_of <SurgSim::Framework::Component, T>::value,
+ std::shared_ptr<T> >::type rhs)
+{
+ return YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::encode(rhs);
+}
+
+template <class T>
+bool YAML::convert<std::shared_ptr<T>>::decode(const Node& node,
+ typename std::enable_if <std::is_base_of<SurgSim::Framework::Component, T>::value,
+ std::shared_ptr<T> >::type& rhs)
+{
+ std::shared_ptr<SurgSim::Framework::Component> temporary;
+ bool success = YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::decode(node, temporary);
+ if (success)
+ {
+ rhs = std::dynamic_pointer_cast<T>(temporary);
+ SURGSIM_ASSERT(rhs != nullptr) << "Failure to convert to target type in " << __FUNCTION__;
+ }
+ return success;
+}
+
+
+#endif
diff --git a/SurgSim/Framework/FrameworkConvert.cpp b/SurgSim/Framework/FrameworkConvert.cpp
new file mode 100644
index 0000000..bd976dc
--- /dev/null
+++ b/SurgSim/Framework/FrameworkConvert.cpp
@@ -0,0 +1,164 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/Scene.h"
+
+#include <boost/uuid/uuid_io.hpp>
+
+namespace
+{
+const std::string NamePropertyName = "Name";
+const std::string IdPropertyName = "Id";
+}
+
+namespace YAML
+{
+Node convert<std::shared_ptr<SurgSim::Framework::Component>>::encode(
+ const std::shared_ptr<SurgSim::Framework::Component> rhs)
+{
+ Node result;
+ if (nullptr != rhs)
+ {
+ Node data;
+ data[IdPropertyName] = to_string(rhs->getUuid());
+ data[NamePropertyName] = rhs->getName();
+ result[rhs->getClassName()] = data;
+ }
+ return result;
+}
+
+bool convert<std::shared_ptr<SurgSim::Framework::Component>>::decode(
+ const Node& node,
+ std::shared_ptr<SurgSim::Framework::Component>& rhs)
+{
+ bool result = false;
+
+ if (!node.IsMap())
+ {
+ return false;
+ }
+
+ Node data = node.begin()->second;
+ std::string className = node.begin()->first.as<std::string>();
+
+ if (data.IsMap() &&
+ data[IdPropertyName].IsDefined() &&
+ data[NamePropertyName].IsDefined())
+ {
+ if (rhs == nullptr)
+ {
+ std::string id = data[IdPropertyName].as<std::string>();
+ RegistryType& registry = getRegistry();
+ auto sharedComponent = registry.find(id);
+ if (sharedComponent != registry.end())
+ {
+ SURGSIM_ASSERT(data[NamePropertyName].as<std::string>() == sharedComponent->second->getName() &&
+ className == sharedComponent->second->getClassName())
+ << "The current node: " << std::endl << node << "has the same id as an instance "
+ << "already registered, but the name and/or the className are different. This is "
+ << "likely a problem with a manually assigned id.";
+ rhs = sharedComponent->second;
+ }
+ else
+ {
+ SurgSim::Framework::Component::FactoryType& factory =
+ SurgSim::Framework::Component::getFactory();
+
+ if (factory.isRegistered(className))
+ {
+ rhs = factory.create(className, data[NamePropertyName].as<std::string>());
+ getRegistry()[id] = rhs;
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Class " << className << " is not registered in the factory.";
+ }
+ }
+ }
+ rhs->decode(data);
+ result = true;
+ }
+ return result;
+}
+
+convert<std::shared_ptr<SurgSim::Framework::Component>>::RegistryType&
+ convert<std::shared_ptr<SurgSim::Framework::Component>>::getRegistry()
+{
+ static RegistryType registry;
+ return registry;
+}
+
+Node convert<SurgSim::Framework::Component>::encode(const SurgSim::Framework::Component& rhs)
+{
+ YAML::Node data(rhs.encode());
+ data[IdPropertyName] = to_string(rhs.getUuid());
+ data[NamePropertyName] = rhs.getName();
+
+ YAML::Node result;
+ result[rhs.getClassName()] = data;
+
+ return result;
+}
+
+
+Node convert<std::shared_ptr<SurgSim::Framework::SceneElement>>::encode(
+ const std::shared_ptr<SurgSim::Framework::SceneElement> rhs)
+{
+ SURGSIM_ASSERT(rhs != nullptr) << "Trying to encode nullptr SceneElement";
+ return rhs->encode(false);
+}
+
+bool convert<std::shared_ptr<SurgSim::Framework::SceneElement>>::decode(
+ const Node& node,
+ std::shared_ptr<SurgSim::Framework::SceneElement>& rhs)
+{
+ if (rhs == nullptr)
+ {
+ // For now only deal with BasicSceneElement classes
+ rhs = std::make_shared<SurgSim::Framework::BasicSceneElement>("");
+ }
+ return rhs->decode(node);
+}
+
+Node convert<SurgSim::Framework::SceneElement>::encode(
+ const SurgSim::Framework::SceneElement& rhs)
+{
+ return rhs.encode(true);
+}
+
+Node convert<std::shared_ptr<SurgSim::Framework::Scene>>::encode(
+ const std::shared_ptr<SurgSim::Framework::Scene> rhs)
+{
+ SURGSIM_ASSERT(rhs != nullptr) << "Trying to encode nullptr Scene";
+ return rhs->encode();
+}
+
+bool convert<std::shared_ptr<SurgSim::Framework::Scene>>::decode(
+ const Node& node,
+ std::shared_ptr<SurgSim::Framework::Scene>& rhs)
+{
+ bool result = false;
+ if (rhs != nullptr)
+ {
+ result = rhs->decode(node);
+ }
+ return result;
+}
+
+}
\ No newline at end of file
diff --git a/SurgSim/Framework/FrameworkConvert.h b/SurgSim/Framework/FrameworkConvert.h
new file mode 100644
index 0000000..d56ddb5
--- /dev/null
+++ b/SurgSim/Framework/FrameworkConvert.h
@@ -0,0 +1,117 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_FRAMEWORKCONVERT_H
+#define SURGSIM_FRAMEWORK_FRAMEWORKCONVERT_H
+
+#include <memory>
+#include <unordered_map>
+#include <yaml-cpp/yaml.h>
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+class Component;
+class SceneElement;
+class Scene;
+}
+}
+
+namespace YAML
+{
+
+/// Specializatio of YAML::convert for std::shared_ptr, this is used to redirect the serialization of a derived class
+/// to the specialization of the serialization for a base class, for example all subclasses of Component can use the
+/// Component serialization specialization, currently each redirection has to be implemented separately, there is
+/// probably a way to do this automatically.
+/// \tparam T class that should be converted from a shared ptr
+template <class T>
+struct convert<std::shared_ptr<T>>
+{
+ static YAML::Node encode(
+ const typename std::enable_if <std::is_base_of <SurgSim::Framework::Component, T>::value,
+ std::shared_ptr<T> >::type rhs);
+ static bool decode(
+ const Node& node,
+ typename std::enable_if <std::is_base_of<SurgSim::Framework::Component, T>::value,
+ std::shared_ptr<T> >::type& rhs);
+};
+
+/// Specialization of YAML::convert for std::shared_ptr<Component>, use this for to read in a component
+/// written by the convert<SurgSim::Framework::Component> converter, or a reference to a
+/// component written by this converter.
+/// This specialization, is slightly asymmetric, on encode it will only encode a components
+/// name, id and className. When decoding this conversion will check whether a component with
+/// the same id has already been encountered. If no a new instance will be created and stored
+/// in a local Registry. If yes, the entry from the registry will be returned, this makes sure
+/// that all references to the same id will use the correct, copy of the smart pointer.
+/// Additionally this class contains a class factory that can be used to generate the class from
+/// its name.
+template <>
+struct convert<std::shared_ptr<SurgSim::Framework::Component> >
+{
+ static Node encode(const std::shared_ptr<SurgSim::Framework::Component> rhs);
+ static bool decode(const Node& node, std::shared_ptr<SurgSim::Framework::Component>& rhs);
+
+private:
+
+ typedef std::unordered_map<std::string, std::shared_ptr<SurgSim::Framework::Component>> RegistryType;
+
+ /// \return The static registry for shared instances
+ static RegistryType& getRegistry();
+};
+
+
+
+
+/// Override of the convert structure for an Component, use this form to write out a full version
+/// of the component information, to decode a component use the other converter. This converter
+/// intentionally does not have a decode function.
+template<>
+struct convert<SurgSim::Framework::Component>
+{
+ static Node encode(const SurgSim::Framework::Component& rhs);
+};
+
+template<>
+struct convert<std::shared_ptr<SurgSim::Framework::SceneElement>>
+{
+ static Node encode(const std::shared_ptr<SurgSim::Framework::SceneElement> rhs);
+ static bool decode(const Node& node, std::shared_ptr<SurgSim::Framework::SceneElement>& rhs);
+};
+
+template<>
+struct convert<SurgSim::Framework::SceneElement>
+{
+ static Node encode(const SurgSim::Framework::SceneElement& rhs);
+};
+
+template<>
+struct convert<std::shared_ptr<SurgSim::Framework::Scene>>
+{
+ static Node encode(const std::shared_ptr<SurgSim::Framework::Scene> rhs);
+ static bool decode(const Node& node, std::shared_ptr<SurgSim::Framework::Scene>& rhs);
+};
+
+
+
+
+};
+
+#include "SurgSim/Framework/FrameworkConvert-inl.h"
+
+#endif // SURGSIM_FRAMEWORK_FRAMEWORKCONVERT_H
\ No newline at end of file
diff --git a/SurgSim/Framework/LockedContainer.h b/SurgSim/Framework/LockedContainer.h
new file mode 100644
index 0000000..b4ac09f
--- /dev/null
+++ b/SurgSim/Framework/LockedContainer.h
@@ -0,0 +1,202 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_LOCKEDCONTAINER_H
+#define SURGSIM_FRAMEWORK_LOCKEDCONTAINER_H
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// A simple thread-safe data container that can support multiple writers and readers.
+///
+/// The type of the contained data is determined by the template argument, and should satisfy the following:
+/// - It must be either default-constructable or copy-constructable or move-constructable. In other words,
+/// construction must be possible using either <code>T()</code> or <code>T(const T&)</code> or
+/// <code>T(T&&)</code>, or compiler-generated equivalents.
+/// - It must be either copy-assignable or move-assignable. In other words, assignment must be possible using
+/// either <code>operator=(const T&)</code> or <code>operator=(T&&)</code>, or compiler-generated
+/// equivalents. (If it is only move-assignable, then you can't get the value in the container without
+/// erasing it.)
+///
+/// Note that STL container types, plain-old-data structs, and most other things you might want to use satisfy
+/// those requirements.
+///
+/// The container will create and manage an extra internal instance of the data object.
+///
+/// The interface has been designed to be incredibly simple. The trade-off is that the overhead of reading or
+/// writing to the container is significant (Each write incurs either a copy or a move of the data, plus a mutex
+/// lock/unlock. Each read incurs a copy, plus a mutex lock/unlock. Applications that write and read heavily
+/// may also become mutex-bound.)
+///
+/// Writers write the data by calling the \ref set method, which copies or moves the data into internal storage.
+/// Readers read the data by calling the \ref get method, which copies the data from internal storage.
+///
+/// \tparam T Type of the data held by the LockedContainer.
+template <typename T>
+class LockedContainer
+{
+public:
+ /// Create the container and the data it contains.
+ ///
+ /// The data will be initialized using the default constructor.
+ LockedContainer() :
+ m_buffer(),
+ m_haveNewData(false)
+ {
+ }
+
+ /// Create the container and the data it contains.
+ ///
+ /// The data will be initialized using the copy constructor.
+ /// \param initialValue The initial value to be used.
+ explicit LockedContainer(const T& initialValue) :
+ m_buffer(initialValue),
+ m_haveNewData(false)
+ {
+ }
+
+ /// Create the container and the data it contains.
+ ///
+ /// The data in the active buffer will be initialized using the move constructor.
+ /// The data in the second, inactive buffer will be initialized using the default constructor.
+ /// \param initialValue The initial value to be moved into the active buffer.
+ explicit LockedContainer(T&& initialValue) :
+ m_buffer(std::move(initialValue)),
+ m_haveNewData(false)
+ {
+ }
+
+
+ /// Destroy the container and the data it contains.
+ ~LockedContainer()
+ {
+ }
+
+ /// Write (copy) new data into the container.
+ ///
+ /// The data will be copied into internal storage. If \ref set is called again before the next \ref get,
+ /// the first data will be overwritten and lost.
+ /// \param value The value to be written.
+ void set(const T& value)
+ {
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ m_buffer = value;
+ m_haveNewData = true;
+ }
+
+ /// Write (move) new data into the container.
+ ///
+ /// The data will be moved into internal storage. If \ref set is called again before the next \ref get, the
+ /// first data will be overwritten and lost.
+ /// \param value The value to be written.
+ void set(T&& value)
+ {
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ m_buffer = std::move(value);
+ m_haveNewData = true;
+ }
+
+ /// Read (copy) the data from the container.
+ /// \param [out] value The location to write the data from the container. The pointer must be non-null.
+ void get(T* value) const
+ {
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ m_haveNewData = false;
+ *value = m_buffer;
+ }
+
+ /// Move the data out of the container.
+ /// For types that support move assignment, the internal state of the container will be invalidated.
+ /// \param [out] value The location to write the data from the container. The pointer must be non-null.
+ void take(T* value)
+ {
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ m_haveNewData = false;
+ *value = std::move(m_buffer);
+ }
+
+ /// Read (copy) the data from the container if it has been modified since the last access.
+ /// If \ref set has not been called since the last \ref get, \ref take, \ref tryGetChanged or
+ /// \ref tryTakeChanged, the method returns \c false and doesn't modify the data.
+ ///
+ /// \param [out] value The location to write the data from the container if it has changed. The pointer
+ /// must be non-null.
+ /// \return true if there was new data (which may or may not be equal to the old). Note that the initial
+ /// value created when the object was constructed (if any) is not considered "new" data by this method.
+ bool tryGetChanged(T* value) const
+ {
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ if (! m_haveNewData)
+ {
+ return false;
+ }
+ else
+ {
+ m_haveNewData = false;
+ *value = m_buffer;
+ return true;
+ }
+ }
+
+ /// Move the data out of the container if it has been modified since the last access.
+ /// If \ref set has not been called since the last \ref get, \ref take, \ref tryGetChanged or
+ /// \ref tryTakeChanged, the method returns \c false and doesn't modify the data.
+ ///
+ /// \param [out] value The location to write the data from the container if it has changed. The pointer
+ /// must be non-null.
+ /// \return true if there was new data (which may or may not be equal to the old). Note that the initial
+ /// value created when the object was constructed (if any) is not considered "new" data by this method.
+ bool tryTakeChanged(T* value)
+ {
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ if (! m_haveNewData)
+ {
+ return false;
+ }
+ else
+ {
+ m_haveNewData = false;
+ *value = std::move(m_buffer);
+ return true;
+ }
+ }
+
+private:
+ /// Prevent copying
+ LockedContainer(const LockedContainer&);
+ /// Prevent assignment
+ LockedContainer& operator=(const LockedContainer&);
+
+
+ /// Internal buffer.
+ T m_buffer;
+
+ /// True if there data that has been written, but not yet pulled in by get, take, etc.
+ mutable bool m_haveNewData;
+
+ /// Mutex for synchronization of set() and get() calls.
+ mutable boost::mutex m_mutex;
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_LOCKEDCONTAINER_H
diff --git a/SurgSim/Framework/Log.h b/SurgSim/Framework/Log.h
new file mode 100644
index 0000000..bf5a920
--- /dev/null
+++ b/SurgSim/Framework/Log.h
@@ -0,0 +1,41 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 convenience header that provides the entirety of the logging API.
+/// \ingroup loggingAPI
+/// \sa loggingAPI
+
+#ifndef SURGSIM_FRAMEWORK_LOG_H
+#define SURGSIM_FRAMEWORK_LOG_H
+
+/// \defgroup loggingAPI Logging API
+/// The logging API used by OpenSurgSim code.
+/// \sa assertAPI
+/// @{
+
+/// \defgroup logInternals Internal logging helpers
+/// Not meant for public consumption.
+
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/Framework/LoggerManager.h"
+#include "SurgSim/Framework/LogMessage.h"
+#include "SurgSim/Framework/LogOutput.h"
+#include "SurgSim/Framework/LogMacros.h"
+#include "SurgSim/Framework/Assert.h"
+
+/// @}
+
+#endif // SURGSIM_FRAMEWORK_LOG_H
diff --git a/SurgSim/Framework/LogMacros.h b/SurgSim/Framework/LogMacros.h
new file mode 100644
index 0000000..323b58c
--- /dev/null
+++ b/SurgSim/Framework/LogMacros.h
@@ -0,0 +1,217 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Macros used for logging.
+/// \ingroup logInternals
+/// \sa loggingAPI
+
+#ifndef SURGSIM_FRAMEWORK_LOGMACROS_H
+#define SURGSIM_FRAMEWORK_LOGMACROS_H
+
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/Framework/LogMessage.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// \addtogroup loggingAPI
+/// @{
+
+/// Converts a short level name to the log level enum value.
+/// \param level Short log level name
+/// (\link SurgSim::Framework::LOG_LEVEL_DEBUG DEBUG\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_INFO INFO\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_WARNING WARNING\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_SEVERE SEVERE\endlink or
+/// \link SurgSim::Framework::LOG_LEVEL_CRITICAL CRITICAL\endlink).
+/// \return Log level \link SurgSim::Framework::LogLevel enum value\endlink.
+/// \ingroup logInternals
+#define SURGSIM_LOG_LEVEL(level) ::SurgSim::Framework::LOG_LEVEL_ ## level
+
+/// Logs a message to the specified \c logger with the short \c level name.
+/// \param logger Logger used to log the message
+/// \param level Level of this log message
+/// (\link SurgSim::Framework::LOG_LEVEL_DEBUG DEBUG\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_INFO INFO\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_WARNING WARNING\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_SEVERE SEVERE\endlink or
+/// \link SurgSim::Framework::LOG_LEVEL_CRITICAL CRITICAL\endlink).
+/// \return Stream to output the log message
+///
+/// \b Example
+/// ~~~~
+/// SURGSIM_LOG(logger, WARNING) << messageText;
+/// ~~~~
+#define SURGSIM_LOG(logger, level) \
+ if (SURGSIM_LOG_LEVEL(level) < (logger)->getThreshold()) \
+ { \
+ } \
+ else \
+ /* important: no curly braces around this! */ \
+ ::SurgSim::Framework::LogMessage((logger), SURGSIM_LOG_LEVEL(level))
+
+/// Logs a message to the specified \c logger at the \link SurgSim::Framework::LOG_LEVEL_DEBUG DEBUG\endlink level.
+/// \param logger Logger used to log the message
+/// \return Stream to output the log message
+///
+/// \b Example
+/// ~~~~
+/// SURGSIM_LOG_DEBUG(logger) << messageText;
+/// ~~~~
+#define SURGSIM_LOG_DEBUG(logger) SURGSIM_LOG(logger, DEBUG)
+
+/// Logs a message to the specified \c logger at the \link SurgSim::Framework::LOG_LEVEL_INFO INFO\endlink level.
+/// \param logger Logger used to log the message
+/// \return Stream to output the log message
+///
+/// \b Example
+/// ~~~~
+/// SURGSIM_LOG_INFO(logger) << messageText;
+/// ~~~~
+#define SURGSIM_LOG_INFO(logger) SURGSIM_LOG(logger, INFO)
+
+/// Logs a message to the specified \c logger at the \link SurgSim::Framework::LOG_LEVEL_WARNING WARNING\endlink level.
+/// \param logger Logger used to log the message
+/// \return Stream to output the log message
+///
+/// \b Example
+/// ~~~~
+/// SURGSIM_LOG_WARNING(logger) << messageText;
+/// ~~~~
+#define SURGSIM_LOG_WARNING(logger) SURGSIM_LOG(logger, WARNING)
+
+/// Logs a message to the specified \c logger at the \link SurgSim::Framework::LOG_LEVEL_SEVERE SEVERE\endlink level.
+/// \param logger Logger used to log the message
+/// \return Stream to output the log message
+///
+/// \b Example
+/// ~~~~
+/// SURGSIM_LOG_SEVERE(logger) << messageText;
+/// ~~~~
+#define SURGSIM_LOG_SEVERE(logger) SURGSIM_LOG(logger, SEVERE)
+
+/// Logs a message to the specified \c logger at the \link SurgSim::Framework::LOG_LEVEL_CRITICAL CRITICAL\endlink
+/// level.
+/// \param logger Logger used to log the message
+/// \return Stream to output the log message
+///
+/// \b Example
+/// ~~~~
+/// SURGSIM_LOG_CRITICAL(logger) << messageText;
+/// ~~~~
+#define SURGSIM_LOG_CRITICAL(logger) SURGSIM_LOG(logger, CRITICAL)
+
+
+/// Logs a message to the specified \c logger with the short \c level name if \c condition is true.
+/// \param condition Condition to test.
+/// \param logger Logger used to log the message.
+/// \param level Level of this log message
+/// (\link SurgSim::Framework::LOG_LEVEL_DEBUG DEBUG\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_INFO INFO\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_WARNING WARNING\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_SEVERE SEVERE\endlink or
+/// \link SurgSim::Framework::LOG_LEVEL_CRITICAL CRITICAL\endlink).
+/// \return Stream to output the log message.
+///
+/// \b Example
+/// ~~~~
+/// SURGSIM_LOG_IF(size >= 100, logger, INFO) << "size is " << size;
+/// ~~~~
+#define SURGSIM_LOG_IF(condition, logger, level) \
+ if (! (condition)) \
+ { \
+ } \
+ else \
+ /* important: no curly braces around this! */ \
+ SURGSIM_LOG(logger, level)
+
+/// Generate a variable name that should be unique within a file.
+/// The name will be unique unless this macro is used twice on the same source code line, e.g. due to macro expansion.
+/// Two macros are needed to make sure the argument is fully expanded.
+/// \ingroup logInternals
+#define SURGSIM_FLAG_VARIABLE_NAME_HELPER(base, line) base ## line
+/// Generate a variable name that should be unique within a file.
+/// The name will be unique unless this macro is used twice on the same source code line, e.g. due to macro expansion.
+/// Two macros are needed to make sure the argument is fully expanded.
+/// \ingroup logInternals
+#define SURGSIM_FLAG_VARIABLE_NAME(base, line) SURGSIM_FLAG_VARIABLE_NAME_HELPER(base, line)
+/// Define a variable name that depends on the line number in the source file where the macro is called from.
+/// We need this because we can't just add braces to create a scope; that would break using << to add more info.
+/// \ingroup logInternals
+#define SURGSIM_LOG_ONCE_VARIABLE SURGSIM_FLAG_VARIABLE_NAME(surgsimLogOnceFlag, __LINE__)
+
+/// Logs a message to the specified \c logger with the short \c level name, but only the first time this statement
+/// is reached.
+/// \param logger Logger used to log the message.
+/// \param level Level of this log message
+/// (\link SurgSim::Framework::LOG_LEVEL_DEBUG DEBUG\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_INFO INFO\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_WARNING WARNING\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_SEVERE SEVERE\endlink or
+/// \link SurgSim::Framework::LOG_LEVEL_CRITICAL CRITICAL\endlink).
+/// \return Stream to output the log message.
+///
+/// \b Example
+/// ~~~~
+/// SURGSIM_LOG_ONCE(logger, level) << messageTextShownOnce;
+/// ~~~~
+#define SURGSIM_LOG_ONCE(logger, level) \
+ static int SURGSIM_LOG_ONCE_VARIABLE = 0; \
+ if ((SURGSIM_LOG_ONCE_VARIABLE++) != 0) \
+ { \
+ } \
+ else \
+ /* important: no curly braces around this! */ \
+ SURGSIM_LOG(logger, level)
+
+
+/// Logs a message to the specified \c logger with the short \c level name if \c condition is true, but only the
+/// first time *this* particular condition is true.
+/// \param condition Condition to test
+/// \param logger Logger used to log the message
+/// \param level Level of this log message
+/// (\link SurgSim::Framework::LOG_LEVEL_DEBUG DEBUG\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_INFO INFO\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_WARNING WARNING\endlink,
+/// \link SurgSim::Framework::LOG_LEVEL_SEVERE SEVERE\endlink or
+/// \link SurgSim::Framework::LOG_LEVEL_CRITICAL CRITICAL\endlink).
+/// \return Stream to output the log message
+///
+/// \b Example
+/// ~~~~
+/// SURGSIM_LOG_ONCE_IF(condition, logger, level) << messageTextShownOnce;
+/// ~~~~
+#define SURGSIM_LOG_ONCE_IF(condition, logger, level) \
+ static int SURGSIM_LOG_ONCE_VARIABLE = 0; \
+ if (! (condition)) \
+ { \
+ } \
+ else if ((SURGSIM_LOG_ONCE_VARIABLE++) != 0) \
+ { \
+ } \
+ else \
+ /* important: no curly braces around this! */ \
+ SURGSIM_LOG(logger, level)
+
+
+/// @}
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_LOGMACROS_H
diff --git a/SurgSim/Framework/LogMessage.h b/SurgSim/Framework/LogMessage.h
new file mode 100644
index 0000000..c498e17
--- /dev/null
+++ b/SurgSim/Framework/LogMessage.h
@@ -0,0 +1,61 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_LOGMESSAGE_H
+#define SURGSIM_FRAMEWORK_LOGMESSAGE_H
+
+#include "SurgSim/Framework/LogMessageBase.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+class Logger;
+
+/// \addtogroup logInternals
+/// @{
+
+
+/// Specialization, handles flush on destruction
+class LogMessage : public LogMessageBase
+{
+public:
+
+ explicit LogMessage(Logger* logger, int level) : LogMessageBase(logger, level)
+ {
+ }
+
+ explicit LogMessage(const std::unique_ptr<Logger>& logger, int level) : LogMessageBase(logger.get(), level)
+ {
+ }
+
+ explicit LogMessage(const std::shared_ptr<Logger>& logger, int level) : LogMessageBase(logger.get(), level)
+ {
+ }
+
+ ~LogMessage()
+ {
+ flush();
+ }
+};
+
+
+/// @}
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_LOGMESSAGE_H
diff --git a/SurgSim/Framework/LogMessageBase.cpp b/SurgSim/Framework/LogMessageBase.cpp
new file mode 100644
index 0000000..8782288
--- /dev/null
+++ b/SurgSim/Framework/LogMessageBase.cpp
@@ -0,0 +1,56 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/LogMessageBase.h"
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+LogMessageBase::LogMessageBase(Logger* logger, int level)
+ : m_stream(), m_logger(logger)
+{
+ SURGSIM_ASSERT(logger) << "logger should not be a null pointer";
+ static std::string levelNames[5] = {"DEBUG ", "INFO ", "WARNING ", "SEVERE ","CRITICAL"};
+ std::time_t timeStamp;
+ std::time(&timeStamp);
+ ::tm tm;
+#ifdef _MSC_VER
+ localtime_s(&tm, &timeStamp);
+#else
+ localtime_r(&timeStamp, &tm);
+#endif
+ std::string levelName("NONE ");
+ if (level >= 0 && level <= LOG_LEVEL_CRITICAL)
+ {
+ levelName = levelNames[level];
+ }
+ char fillChar = m_stream.fill();
+ m_stream << std::setfill('0') <<
+ std::setw(2) << 1+tm.tm_mon << "." <<
+ std::setw(2) << tm.tm_mday << ' ' <<
+ std::setw(2) << tm.tm_hour << ':' <<
+ std::setw(2) << tm.tm_min << ':' <<
+ std::setw(2) << tm.tm_sec << ' ' <<
+ std::setfill(fillChar) <<
+ m_logger->getName() << " " <<
+ levelName << " ";
+}
+
+}; // namespace Framework
+}; // namespace SurgSim
diff --git a/SurgSim/Framework/LogMessageBase.h b/SurgSim/Framework/LogMessageBase.h
new file mode 100644
index 0000000..2dd3979
--- /dev/null
+++ b/SurgSim/Framework/LogMessageBase.h
@@ -0,0 +1,105 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_LOGMESSAGEBASE_H
+#define SURGSIM_FRAMEWORK_LOGMESSAGEBASE_H
+
+#include <string>
+#include <sstream>
+#include <iomanip>
+#include <ctime>
+
+#include "SurgSim/Framework/Logger.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// \addtogroup logInternals
+/// @{
+
+
+/// LogMessageBase is a base class to be used to customize messages for logging
+/// textual information can be put into a log message by using the << operator
+/// in general the message class will output all of its information when the
+/// destructor is being invoked, formats the incoming message to timestamp it
+/// and adds information about the logger
+class LogMessageBase
+{
+public:
+
+ /// Construct a LogMessage
+ /// \param logger The logger to be used
+ /// \param level The logging level for this message
+ LogMessageBase(Logger* logger, int level);
+
+ /// Destructor.
+ ~LogMessageBase()
+ {
+ }
+
+ /// Add the given input to the current log message.
+ /// \param input The input to be added to the current stream
+ template <typename T>
+ LogMessageBase& operator <<(T&& input)
+ {
+ m_stream << input;
+ return *this;
+ }
+
+ // A specialization for output manipulators (functions that apply to the stream).
+ // Otherwise overloaded manipulators like std::endl and std::endl don't work, since the compiler can't know
+ // what overloaded variant to apply.
+ LogMessageBase& operator <<(std::ios_base& (*manipulator)(std::ios_base&))
+ {
+ m_stream << *manipulator;
+ return *this;
+ }
+
+ // A specialization for output manipulators (functions that apply to the stream).
+ // Otherwise overloaded manipulators like std::hex and std::endl don't work, since the compiler can't know
+ // what overloaded variant to apply.
+ LogMessageBase& operator <<(std::ostream& (*manipulator)(std::ostream&))
+ {
+ m_stream << *manipulator;
+ return *this;
+ }
+
+protected:
+ /// \return the current content of the message to be logged
+ std::string getMessage()
+ {
+ return m_stream.str();
+ }
+
+ /// write the current message to the logger
+ void flush()
+ {
+ m_logger->writeMessage(m_stream.str());
+ }
+
+private:
+ std::ostringstream m_stream;
+ Logger* m_logger;
+};
+
+
+/// @}
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_LOGMESSAGEBASE_H
diff --git a/SurgSim/Framework/LogOutput.cpp b/SurgSim/Framework/LogOutput.cpp
new file mode 100644
index 0000000..167f6de
--- /dev/null
+++ b/SurgSim/Framework/LogOutput.cpp
@@ -0,0 +1,90 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/LogOutput.h"
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/Framework/Assert.h"
+
+#include <boost/thread/locks.hpp>
+
+#include <fstream>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+FileOutput::FileOutput(const std::string& filename) :
+ m_filename(filename)
+{
+ if (! m_stream.is_open())
+ {
+ m_stream.open(m_filename,std::ios_base::app);
+ }
+
+ SURGSIM_ASSERT(! m_stream.fail()) << "Failed to open '" << m_filename << "'!";
+}
+
+FileOutput::~FileOutput()
+{
+ m_stream.close();
+}
+
+bool SurgSim::Framework::FileOutput::writeMessage(const std::string& message)
+{
+ SURGSIM_ASSERT(m_stream.is_open() && ! m_stream.fail()) <<
+ "Error writing to " << m_filename;
+ {
+ boost::mutex::scoped_lock lock(m_mutex);
+ m_stream << message << std::endl;
+ }
+ return true;
+}
+
+
+StreamOutput::StreamOutput(std::ostream& ostream) : m_stream(ostream) //NOLINT
+{
+}
+
+StreamOutput::~StreamOutput()
+{
+ if (m_stream.fail())
+ {
+ //TODO(hscheirich) 2013-01-28: Still need to figure out default logging
+ throw("Default logging not implemented yet");
+ }
+}
+
+bool StreamOutput::writeMessage(const std::string& message)
+{
+ bool result = false;
+ if (!m_stream.fail())
+ {
+ boost::mutex::scoped_lock lock(m_mutex);
+ m_stream << message << std::endl;
+ result = true;
+ }
+ else
+ {
+ //TODO(hscheirich) 2013-01-28: Still need to figure out default logging
+ throw("Default logging not implemented yet");
+ }
+ return result;
+}
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+
diff --git a/SurgSim/Framework/LogOutput.h b/SurgSim/Framework/LogOutput.h
new file mode 100644
index 0000000..0d31fba
--- /dev/null
+++ b/SurgSim/Framework/LogOutput.h
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_LOGOUTPUT_H
+#define SURGSIM_FRAMEWORK_LOGOUTPUT_H
+
+#include <fstream>
+#include <boost/thread/mutex.hpp>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// Virtual Base class to define an interface for outputting logging information
+class LogOutput
+{
+public:
+ LogOutput()
+ {
+ }
+
+ virtual ~LogOutput()
+ {
+ }
+
+ /// \param message to be written out
+ /// \return true on success
+ virtual bool writeMessage(const std::string& message) = 0;
+
+};
+
+class NullOutput : public LogOutput
+{
+public:
+ virtual bool writeMessage(const std::string& message) {return true;}
+};
+
+
+/// Class to output logging information to a give file
+class FileOutput : public LogOutput
+{
+public:
+
+ /// Constructor
+ /// \param filename The filename to be used for writing
+ explicit FileOutput(const std::string& filename);
+
+ virtual ~FileOutput();
+
+ /// \param message to be written out
+ /// \return true on success
+ virtual bool writeMessage(const std::string& message) override;
+
+private:
+ std::string m_filename;
+ std::ofstream m_stream;
+ boost::mutex m_mutex;
+};
+
+/// Class to output logging information to a stream that can be passed
+/// into the constructor of the class
+class StreamOutput : public LogOutput
+{
+public:
+
+ /// Constructor
+ /// \param ostream stream to be used for writing
+ /// ostream parameter to be passed by non-const reference on purpose.
+ explicit StreamOutput(std::ostream& ostream); //NOLINT
+ virtual ~StreamOutput();
+
+ /// Writes a message to the stream.
+ /// \param message Message to be written to the stream
+ /// \return True on success
+ virtual bool writeMessage(const std::string& message) override;
+
+private:
+ std::ostream& m_stream;
+ boost::mutex m_mutex;
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_LOGOUTPUT_H
diff --git a/SurgSim/Framework/Logger.cpp b/SurgSim/Framework/Logger.cpp
new file mode 100644
index 0000000..4786590
--- /dev/null
+++ b/SurgSim/Framework/Logger.cpp
@@ -0,0 +1,36 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Logger.h"
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+ Logger::Logger(const std::string& name, std::shared_ptr<LogOutput> output) :
+ m_threshold(LOG_LEVEL_DEBUG), // include all logging levels
+ m_name(name),
+ m_output(output)
+ {
+ }
+
+ std::shared_ptr<LoggerManager> Logger::getLoggerManager()
+ {
+ static std::shared_ptr<LoggerManager> loggerManager = std::make_shared<LoggerManager>();
+ return loggerManager;
+ }
+}
+}
diff --git a/SurgSim/Framework/Logger.h b/SurgSim/Framework/Logger.h
new file mode 100644
index 0000000..9b3a606
--- /dev/null
+++ b/SurgSim/Framework/Logger.h
@@ -0,0 +1,142 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_LOGGER_H
+#define SURGSIM_FRAMEWORK_LOGGER_H
+
+#include "SurgSim/Framework/LoggerManager.h"
+#include "SurgSim/Framework/LogOutput.h"
+
+#include <string>
+#include <memory>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// \addtogroup loggingAPI
+/// @{
+
+/// Logging levels.
+/// Please note that most logging macros take an abbreviated version of these enumerations, without the leading
+/// \c LOG_LEVEL_, i.e. one of \c DEBUG, \c INFO, \c WARNING, \c SEVERE or \c CRITICAL .
+enum LogLevel
+{
+ /// Use at your discretion.
+ LOG_LEVEL_DEBUG,
+ /// Informational, notify of state changes.
+ LOG_LEVEL_INFO,
+ /// Something failed, but the impact of the failure is not know or minimal (e.g. purely visual).
+ LOG_LEVEL_WARNING,
+ /// Something failed and will impact functionality, some parts of the program will not function correctly.
+ LOG_LEVEL_SEVERE,
+ /// Used by assertion, after using this level the program will not be functional at all.
+ LOG_LEVEL_CRITICAL
+};
+
+/// An object that can be used to control logging parameters, such as verbosity and log output destination.
+class Logger
+{
+public:
+
+ friend class LoggerManager;
+
+ /// Destructor.
+ ~Logger()
+ {
+ }
+
+ /// Uses the contained instance of LogOutput to write the log message
+ /// \return true on success
+ /// \param message the message to be printed
+ bool writeMessage(const std::string& message)
+ {
+ return m_output->writeMessage(message);
+ }
+
+ /// Gets the logging threshold.
+ /// Anything message with less than this level will be ignored.
+ /// \return The threshold value.
+ int getThreshold() const
+ {
+ return m_threshold;
+ }
+
+ /// Sets the logging threshold.
+ /// Anything message with less than this level will be ignored.
+ /// \param val The value to be used as the threshold.
+ void setThreshold(int val)
+ {
+ m_threshold = val;
+ }
+
+ /// Gets the output object used by this logger.
+ /// \return The current output object used this logger.
+ std::shared_ptr<LogOutput> getOutput() const
+ {
+ return m_output;
+ }
+
+ /// Sets the output object used by this logger.
+ /// \param val The output object to be used.
+ void setOutput(std::shared_ptr<LogOutput> val)
+ {
+ m_output = val;
+ }
+
+ /// Gets this logger's name.
+ /// \return The name.
+ std::string getName( ) const
+ {
+ return m_name;
+ }
+
+ /// Get a logger by name from Logger Manager
+ /// \return A logger with given name.
+ static std::shared_ptr<Logger> getLogger(const std::string& name)
+ {
+ return getLoggerManager()->getLogger(name);
+ }
+
+ /// Get default logger
+ /// \return Default logger
+ static std::shared_ptr<Logger> getDefaultLogger()
+ {
+ return getLoggerManager()->getDefaultLogger();
+ }
+
+ /// Get the logger manager
+ /// \return Logger Manager that manages all loggers
+ static std::shared_ptr<LoggerManager> getLoggerManager();
+
+private:
+ /// Constructor.
+ /// \param name The name used for this logger.
+ /// \param output The LogOutput instance used to display or log the data.
+ Logger(const std::string& name, std::shared_ptr<LogOutput> output);
+
+ int m_threshold;
+ std::string m_name;
+ std::shared_ptr<LogOutput> m_output;
+};
+
+
+/// @}
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_LOGGER_H
diff --git a/SurgSim/Framework/LoggerManager.cpp b/SurgSim/Framework/LoggerManager.cpp
new file mode 100644
index 0000000..ecb2a2b
--- /dev/null
+++ b/SurgSim/Framework/LoggerManager.cpp
@@ -0,0 +1,111 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/Framework/LogOutput.h"
+#include "SurgSim/Framework/LoggerManager.h"
+
+#include <iostream>
+
+#include <boost/thread/locks.hpp>
+#include <boost/algorithm/string.hpp>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+LoggerManager::LoggerManager():
+ m_loggers(),
+ m_defaultOutput(std::make_shared<StreamOutput>(std::cerr)),
+ m_globalThreshold(LOG_LEVEL_WARNING),
+ m_mutex()
+{
+}
+
+LoggerManager::~LoggerManager()
+{
+}
+
+
+void LoggerManager::setDefaultOutput(std::shared_ptr<LogOutput> output)
+{
+ m_defaultOutput = output;
+}
+
+std::shared_ptr<LogOutput> LoggerManager::getDefaultOutput() const
+{
+ return m_defaultOutput;
+}
+
+
+void LoggerManager::setThreshold(int threshold)
+{
+ {
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ m_globalThreshold = threshold;
+ }
+ setThreshold("", threshold);
+}
+
+void LoggerManager::setThreshold(const std::string& path, int threshold)
+{
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ for (auto it = m_loggers.cbegin(); it != m_loggers.cend(); ++it)
+ {
+ if (boost::istarts_with(it->first, path))
+ {
+ if (it->second != nullptr)
+ {
+ it->second->setThreshold(threshold);
+ }
+ }
+ }
+}
+
+int LoggerManager::getThreshold() const
+{
+ return m_globalThreshold;
+}
+
+
+std::shared_ptr<Logger> LoggerManager::getLogger(const std::string& name)
+{
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+
+ auto it = m_loggers.find(name);
+ std::shared_ptr<Logger> result;
+
+ if (it != m_loggers.end() && (it->second != nullptr))
+ {
+ result = it->second;
+ }
+ else
+ {
+ result = std::make_shared<Logger>(Logger(name, m_defaultOutput));
+ result->setThreshold(m_globalThreshold);
+ m_loggers[name] = result;
+ }
+ return result;
+}
+
+
+std::shared_ptr<Logger> LoggerManager::getDefaultLogger()
+{
+ return getLogger("default");
+}
+
+}; // Framework
+}; // SurgSim
diff --git a/SurgSim/Framework/LoggerManager.h b/SurgSim/Framework/LoggerManager.h
new file mode 100644
index 0000000..188b46c
--- /dev/null
+++ b/SurgSim/Framework/LoggerManager.h
@@ -0,0 +1,93 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_LOGGERMANAGER_H
+#define SURGSIM_FRAMEWORK_LOGGERMANAGER_H
+
+#include <unordered_map>
+#include <boost/thread/mutex.hpp>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+class Logger;
+class LogOutput;
+
+/// Class to safely handle access to a group of loggers, manipulate the global
+/// logging threshold, and fetch logger(s) from a global pool
+class LoggerManager
+{
+public:
+ /// Constructor
+ LoggerManager();
+
+ /// Destructor
+ ~LoggerManager();
+
+ /// Sets/Changes default output.
+ /// \param output The output class to be used.
+ void setDefaultOutput(std::shared_ptr<LogOutput> output);
+
+ /// Return the default output
+ std::shared_ptr<LogOutput> getDefaultOutput() const;
+
+ /// Gets the default logger
+ /// \return The default logger.
+ std::shared_ptr<Logger> getDefaultLogger();
+
+ /// Gets a logger with a given name, creates a new one if none exists or the logger
+ /// has been deallocated.
+ /// \param name The name.
+ /// \return The logger.
+ std::shared_ptr<Logger> getLogger(const std::string& name);
+
+ /// Sets a threshold for all loggers.
+ /// \param threshold The threshold.
+ void setThreshold(int threshold);
+
+ /// Sets a threshold for a subgroup of loggers, the group is chosen by finding all loggers
+ /// whose pathname starts with the same string as the pathname given.
+ /// \param path Full pathname of the file.
+ /// \param threshold The threshold to use for these loggers.
+ void setThreshold(const std::string& path, int threshold);
+
+ /// Return the threshold used by all loggers
+ /// \return Threshold used by all the loggers.
+ int getThreshold() const;
+
+private:
+ /// Keep track of all the loggers
+ std::unordered_map<std::string, std::shared_ptr<Logger>> m_loggers;
+
+ /// Use for default output of the logger
+ std::shared_ptr<LogOutput> m_defaultOutput;
+
+ /// Threshold used by all loggers
+ int m_globalThreshold;
+
+ boost::mutex m_mutex;
+
+ // Aug 13, 2013
+ // VS2012 does not support "delete" now
+ LoggerManager(const LoggerManager&);
+ LoggerManager& operator=(const LoggerManager&);
+};
+
+}; // Framework
+}; // SurgSim
+
+#endif //SURGSIM_FRAMEWORK_LOGGERMANAGER_H
\ No newline at end of file
diff --git a/SurgSim/Framework/Macros.h b/SurgSim/Framework/Macros.h
new file mode 100644
index 0000000..3edf7e7
--- /dev/null
+++ b/SurgSim/Framework/Macros.h
@@ -0,0 +1,47 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_MACROS_H
+#define SURGSIM_FRAMEWORK_MACROS_H
+
+/// Declare the class name of a class with the appropriate function header,
+/// do not use quotes
+#define SURGSIM_CLASSNAME(ClassName) \
+ virtual std::string getClassName() const {return #ClassName;}
+
+/// GCC macro to write out an _Pragma statement inside a macro, disabled for other platforms
+#ifdef __GNUC__
+#define SURGSIM_DO_PRAGMA(x) _Pragma (#x)
+#else
+#define SURGSIM_DO_PRAGMA(x)
+#endif
+
+///@{
+/// Set of macros to create a unique name with a common basename
+#define SURGSIM_CONCATENATE_DETAIL(x, y) x##y
+#define SURGSIM_CONCATENATE(x, y) SURGSIM_CONCATENATE_DETAIL(x, y)
+#define SURGSIM_MAKE_UNIQUE(x) SURGSIM_CONCATENATE(x, __COUNTER__)
+///@}
+
+/// \note HS-2013-dec-23 The gcc and msvc compilers seem to have different requirements when a template class
+/// needs to be passed template parameters in a specialization, that extend the original template interface
+/// gcc needs the template<> statement before the new template parameters, msvc does not like it at all.
+#ifdef _GNUC_
+#define SURGSIM_DOUBLE_SPECIALIZATION template<>
+#else
+#define SURGSIM_DOUBLE_SPECIALIZATION
+#endif
+
+#endif
diff --git a/SurgSim/Framework/ObjectFactory-inl.h b/SurgSim/Framework/ObjectFactory-inl.h
new file mode 100644
index 0000000..0d66342
--- /dev/null
+++ b/SurgSim/Framework/ObjectFactory-inl.h
@@ -0,0 +1,100 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef SURGSIM_FRAMEWORK_OBJECTFACTORY_INL_H
+#define SURGSIM_FRAMEWORK_OBJECTFACTORY_INL_H
+
+#include "SurgSim/Framework/Assert.h"
+
+template <class Base>
+template <class Derived>
+bool SurgSim::Framework::ObjectFactory<Base>::registerClass(const std::string& className)
+{
+ boost::mutex::scoped_lock lock(m_mutex);
+ bool result = false;
+ if (m_constructors.find(className) == m_constructors.end())
+ {
+ m_constructors[className] = boost::factory<std::shared_ptr<Derived>>();
+ result = true;
+ };
+ return result;
+};
+
+template <class Base>
+std::shared_ptr<Base> SurgSim::Framework::ObjectFactory<Base>::create(const std::string& className)
+{
+ boost::mutex::scoped_lock lock(m_mutex);
+ auto it = m_constructors.find(className);
+ if (it == m_constructors.end())
+ {
+ SURGSIM_FAILURE() << "ObjectFactory does not know about class called " << className;
+ // gcc complains if there is no return
+ return nullptr;
+ }
+ return (it->second)();
+};
+
+
+template <typename Base>
+bool SurgSim::Framework::ObjectFactory<Base>::isRegistered(const std::string& className) const
+{
+ boost::mutex::scoped_lock lock(m_mutex);
+ auto it = m_constructors.find(className);
+ return (it != m_constructors.end());
+}
+
+
+template <typename Base, typename Parameter1>
+template <typename Derived>
+bool SurgSim::Framework::ObjectFactory1<Base, Parameter1>::registerClass(const std::string& className)
+{
+ boost::mutex::scoped_lock lock(m_mutex);
+ bool result = false;
+ if (m_constructors.find(className) == m_constructors.end())
+ {
+ m_constructors[className] = boost::factory<std::shared_ptr<Derived>>();
+ result = true;
+ };
+ return result;
+};
+
+template <typename Base, typename Parameter1>
+std::shared_ptr<Base> SurgSim::Framework::ObjectFactory1<Base, Parameter1>::create(
+ const std::string& className,
+ const Parameter1& val)
+{
+ boost::mutex::scoped_lock lock(m_mutex);
+ auto it = m_constructors.find(className);
+
+ if (it == m_constructors.end())
+ {
+ SURGSIM_FAILURE() << "ObjectFactory does not know about class called " << className;
+ // gcc complains if there is no return
+ return nullptr;
+ }
+ return (it->second)(val);
+};
+
+template <typename Base, typename Parameter1>
+bool SurgSim::Framework::ObjectFactory1<Base, Parameter1>::isRegistered(const std::string& className) const
+{
+ boost::mutex::scoped_lock lock(m_mutex);
+ auto it = m_constructors.find(className);
+ return (it != m_constructors.end());
+}
+
+
+#endif // SURGSIM_FRAMEWORK_OBJECTFACTORY_INL_H
\ No newline at end of file
diff --git a/SurgSim/Framework/ObjectFactory.h b/SurgSim/Framework/ObjectFactory.h
new file mode 100644
index 0000000..3343159
--- /dev/null
+++ b/SurgSim/Framework/ObjectFactory.h
@@ -0,0 +1,152 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_OBJECTFACTORY_H
+#define SURGSIM_FRAMEWORK_OBJECTFACTORY_H
+
+#include "SurgSim/Framework/Macros.h"
+
+#include <string>
+#include <map>
+#include <boost/function.hpp>
+#include <boost/functional/factory.hpp>
+#include <boost/thread/mutex.hpp>
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// An object factory, once a class is registered with the factory it can
+/// be used to create instances of registered classes. All the classes registered
+/// need to have a default constructor.
+/// \note The names used for registration and the actual c++ class names are independent
+/// the className parameter here is just an identifier for the class.
+/// \tparam Base the base class for all classes that can be registered with the factory.
+template <typename Base>
+class ObjectFactory
+{
+public:
+
+ /// Register a class with the factory.
+ /// \tparam T The specific type of the class to be registered.
+ /// \param className The name of this class.
+ /// \return true if the class was added, false if it already existed in the registry.
+ template <typename Derived>
+ bool registerClass(const std::string& className);
+
+ /// Create an instance of a class based on the specific class name.
+ /// \param className The class name that was used to register the class.
+ /// \return a shared pointer to the object of type className, fails with an
+ /// assertion otherwise.
+ std::shared_ptr<Base> create(const std::string& className);
+
+ /// Check whether the class is registered in the factory.
+ /// \param className Name of the class to check.
+ /// \return true if the factory has a constructor for this class.
+ bool isRegistered(const std::string& className) const;
+
+private:
+
+ typedef boost::function<std::shared_ptr<Base>()> Constructor;
+
+ /// All the constructors.
+ std::map<std::string, Constructor> m_constructors;
+
+ /// Threadsafety for registration
+ mutable boost::mutex m_mutex;
+
+};
+
+/// An object factory, once a class is registered with the factory it can
+/// be used to create instances of registered classes. All the classes registered
+/// need to have a one parameter constructor, the type for that parameter can be
+/// passed as a template parameter.
+/// \note The names used for registration and the actual c++ class names are independent
+/// the className parameter here is just an identifier for the class.
+/// \tparam Base The base class for all classes that can be registered with the factory.
+/// \tparam Parameter1 The class for the constructor parameter.
+template <typename Base, typename Parameter1>
+class ObjectFactory1
+{
+public:
+
+ /// Register a class with the factory.
+ /// \tparam T The specific type of the class to be registered.
+ /// \param className The name of this class.
+ /// \return true if the class was added, false if it already existed in the registry.
+ template <typename Derived>
+ bool registerClass(const std::string& className);
+
+ /// Create an instance of a class based on the specific class name, whose constructor takes 1 parameter.
+ /// \param className The class name.
+ /// \param val The value of the parameter.
+ /// \return a shared pointer to the object of type className instantiated with the given parameter,
+ /// fails with an assertion otherwise.
+ std::shared_ptr<Base> create(const std::string& className, const Parameter1& val);
+
+ /// Check whether the class is registered in the factory.
+ /// \param className Name of the class to check.
+ /// \return true if the factory has a constructor for this class
+ bool isRegistered(const std::string& className) const;
+
+private:
+
+ typedef boost::function<std::shared_ptr<Base>(Parameter1)> Constructor;
+
+ /// All the constructors.
+ std::map<std::string, Constructor> m_constructors;
+
+ /// Threadsafety for registration
+ mutable boost::mutex m_mutex;
+};
+
+};
+};
+
+#include "SurgSim/Framework/ObjectFactory-inl.h"
+
+/// Register a class with a factory that is in a base class, DerivedClass has to be of type BaseClass.
+/// The assignment is used to enable the execution of registerClass during static initialization time.
+/// This macro should be put under the same namespace in which 'ClassName' is, to avoid symbol clashes.
+/// 'BaseClass' should be the full name of the base class with namespace prefix,
+/// 'DerivedClass' is 'ClassName' with namespace prefixes,
+/// and 'ClassName' is the name of the class without namespace prefix.
+#define SURGSIM_REGISTER(BaseClass, DerivedClass, ClassName) \
+ bool SURGSIM_CONCATENATE(ClassName, Registered) = \
+ BaseClass::getFactory().registerClass<DerivedClass>(#DerivedClass); \
+
+/// Force compilation of the boolean symbol SURGSIM_CONCATENATE(ClassName, Registered) in SURGSIM_REGISTER macro,
+/// which in turn registers DerivedClass into BaseClass's ObjectFactory.
+/// After that, DerivedClass is linked to any code which includes its header.
+///
+/// Boolean symbol SURGSIM_CONCATENATE(ClassName, Registered) in SURGSIM_REGISTER macro is exposed as an
+/// extern variable in DerivedClass's header, and is referenced to initialize the static global variable
+/// SURGSIM_CONCATENATE(ClassName, IsRegistered) in the header.
+///
+/// This forces the compiler to include the definition of SURGSIM_CONCATENATE(ClassName, Registered)
+/// (defined most likely in the cpp file).
+/// The variable SURGSIM_CONCATENATE(ClassName, IsRegistered) will never be used, so the GCC warning is disabled.
+/// This macro should be put in the DerivedClass's header file, under the same namespace in which the DerivedClass is.
+/// 'ClassName' should be the name of the class without any prefix.
+#define SURGSIM_STATIC_REGISTRATION(ClassName) \
+ SURGSIM_DO_PRAGMA (GCC diagnostic push); \
+ SURGSIM_DO_PRAGMA (GCC diagnostic ignored "-Wunused-variable"); \
+ extern bool SURGSIM_CONCATENATE(ClassName, Registered); \
+ static bool SURGSIM_CONCATENATE(ClassName, IsRegistered) = SURGSIM_CONCATENATE(ClassName, Registered); \
+ SURGSIM_DO_PRAGMA (GCC diagnostic pop)
+
+#endif // SURGSIM_FRAMEWORK_OBJECTFACTORY_H
\ No newline at end of file
diff --git a/SurgSim/Framework/PoseComponent.cpp b/SurgSim/Framework/PoseComponent.cpp
new file mode 100644
index 0000000..b38ba29
--- /dev/null
+++ b/SurgSim/Framework/PoseComponent.cpp
@@ -0,0 +1,63 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/PoseComponent.h"
+
+#include "SurgSim/Math/MathConvert.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Framework::PoseComponent, PoseComponent);
+
+PoseComponent::PoseComponent(const std::string& name) : Component(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(PoseComponent, SurgSim::Math::RigidTransform3d, Pose, getPose, setPose);
+}
+
+void PoseComponent::setPose(const SurgSim::Math::RigidTransform3d& pose)
+{
+ m_pose = pose;
+}
+
+const SurgSim::Math::RigidTransform3d& PoseComponent::getPose() const
+{
+ return m_pose;
+}
+
+std::shared_ptr<const PoseComponent> PoseComponent::getPoseComponent() const
+{
+ return nullptr;
+}
+
+std::shared_ptr<PoseComponent> PoseComponent::getPoseComponent()
+{
+ return nullptr;
+}
+
+bool PoseComponent::doInitialize()
+{
+ return true;
+}
+
+bool PoseComponent::doWakeUp()
+{
+ return true;
+}
+
+
+}; // namespace Framework
+}; // namespace SurgSim
diff --git a/SurgSim/Framework/PoseComponent.h b/SurgSim/Framework/PoseComponent.h
new file mode 100644
index 0000000..eee5076
--- /dev/null
+++ b/SurgSim/Framework/PoseComponent.h
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_POSECOMPONENT_H
+#define SURGSIM_FRAMEWORK_POSECOMPONENT_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+SURGSIM_STATIC_REGISTRATION(PoseComponent);
+
+/// The PoseComponent holds a pose. It is used to group Representations that
+/// share a common pose, in a SceneElement for example.
+class PoseComponent : public Component
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit PoseComponent(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Framework::PoseComponent);
+
+ /// Set the Pose of the PoseComponent
+ /// \param pose The pose in world coordinates
+ void setPose(const SurgSim::Math::RigidTransform3d& pose);
+
+ /// Get the pose of the PoseComponent
+ /// \return The pose in world coordinates
+ const SurgSim::Math::RigidTransform3d& getPose() const;
+
+protected:
+ /// Get the PoseComponent for this component
+ /// A PoseComponent cannot have a PoseComponent, so this will
+ /// return nullptr.
+ /// \return The PoseComponent
+ virtual std::shared_ptr<PoseComponent> getPoseComponent() override;
+
+ /// Get the PoseComponent for this component, constant access
+ /// A PoseComponent cannot have a PoseComponent, so this will
+ /// return nullptr.
+ /// \return The PoseComponent
+ virtual std::shared_ptr<const PoseComponent> getPoseComponent() const override;
+
+private:
+ virtual bool doInitialize() override;
+ virtual bool doWakeUp() override;
+
+ SurgSim::Math::RigidTransform3d m_pose;
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_POSECOMPONENT_H
diff --git a/SurgSim/Framework/Representation.cpp b/SurgSim/Framework/Representation.cpp
new file mode 100644
index 0000000..3b31ebd
--- /dev/null
+++ b/SurgSim/Framework/Representation.cpp
@@ -0,0 +1,71 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Framework/Representation.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Math/MathConvert.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+Representation::Representation(const std::string& m_name) :
+ Component(m_name), m_localPose(SurgSim::Math::RigidTransform3d::Identity())
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, SurgSim::Math::RigidTransform3d, LocalPose, getLocalPose,
+ setLocalPose);
+ SURGSIM_ADD_RO_PROPERTY(Representation, SurgSim::Math::RigidTransform3d, Pose, getPose);
+}
+
+Representation::~Representation()
+{
+}
+
+bool Representation::doInitialize()
+{
+ return true;
+}
+
+bool Representation::doWakeUp()
+{
+ return true;
+}
+
+void Representation::setLocalPose(const SurgSim::Math::RigidTransform3d& pose)
+{
+ m_localPose = pose;
+}
+
+SurgSim::Math::RigidTransform3d Representation::getPose() const
+{
+ if (getSceneElement() != nullptr)
+ {
+ return getSceneElement()->getPose() * getLocalPose();
+ }
+ else
+ {
+ return getLocalPose();
+ }
+}
+
+SurgSim::Math::RigidTransform3d Representation::getLocalPose() const
+{
+ return m_localPose;
+}
+
+}; // namespace Framework
+}; // namespace SurgSim
diff --git a/SurgSim/Framework/Representation.h b/SurgSim/Framework/Representation.h
new file mode 100644
index 0000000..485a193
--- /dev/null
+++ b/SurgSim/Framework/Representation.h
@@ -0,0 +1,65 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_REPRESENTATION_H
+#define SURGSIM_FRAMEWORK_REPRESENTATION_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+
+/// Representations are manifestations of a SceneElement. For example, a
+/// SceneElement can be represented in graphics, physics, etc. Each of these
+/// representation will be derived from this class.
+class Representation : public Component
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit Representation(const std::string& name);
+ /// Destructor
+ virtual ~Representation();
+
+ /// Set the pose of the representation with respect to the Scene Element
+ /// \param pose The pose to set the representation to
+ virtual void setLocalPose(const SurgSim::Math::RigidTransform3d& pose);
+
+ /// Get the pose of the representation with respect to the Scene Element
+ /// \return The pose of this representation
+ virtual SurgSim::Math::RigidTransform3d getLocalPose() const;
+
+ /// Get the pose of the representation in world coordinates
+ /// \return The pose of this representation
+ virtual SurgSim::Math::RigidTransform3d getPose() const;
+
+private:
+ /// Local Pose of the Representation with respect to the SceneElement
+ SurgSim::Math::RigidTransform3d m_localPose;
+
+ virtual bool doInitialize() override;
+ virtual bool doWakeUp() override;
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_REPRESENTATION_H
diff --git a/SurgSim/Framework/ReuseFactory.h b/SurgSim/Framework/ReuseFactory.h
new file mode 100644
index 0000000..66e4a97
--- /dev/null
+++ b/SurgSim/Framework/ReuseFactory.h
@@ -0,0 +1,123 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_REUSEFACTORY_H
+#define SURGSIM_FRAMEWORK_REUSEFACTORY_H
+
+#include <memory>
+#include <stack>
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+
+/// Factory for acquiring new or unused existing instances of class T to reduce repeated deallocation and reallocation
+/// of objects with short lifespans.
+///
+/// Example Usage:
+/// \code{.cpp}
+/// {
+/// // Create an instance of the factory to provide instances of MyObject.
+/// ReuseFactory<MyObject> factory;
+/// // Get an instance of MyObject. This instance will be allocated as no unused objects are available yet.
+/// std::shared_ptr<MyObject> myObject = factory.getInstance();
+/// // Setup the provided object, as the state is not guaranteed to be initialized.
+/// myObject.set(...);
+/// }
+/// // When myObject goes out of scope, it will automatically be added back to the factory for reuse later.
+/// // This will return the previous, now unused, instance rather than allocating anew.
+/// std::shared_ptr<MyObject> myObject2 = factory.getInstance();
+/// // Setup the provided object, as the state is not guaranteed to be initialized.
+/// myObject2.set(...);
+/// \endcode
+///
+/// Limitations:
+/// - The class T must have a default constructor.
+/// - The state of returned objects are in no way reset, so the state will need to be setup after retrieving the
+/// instance from the factory.
+/// - Custom deleters for an instance of T cannot be specified, as a custom deleter is used to manage the unused
+/// objects.
+///
+/// \tparam T Instances of this class are provided by this factory
+template <class T>
+class ReuseFactory
+{
+ /// Custom Deleter is friended to manage unused objects rather than actually deleting them.
+ friend class Deleter;
+public:
+ /// Constructor. Initially no unused objects are available, so returned instances are new allocations.
+ ReuseFactory() {}
+ /// Destructor. Any remaining unused objects will be deleted.
+ ~ReuseFactory() {}
+
+ /// Get a new or previously deleted object of class T.
+ /// \return Valid shared pointer to an object of class T
+ std::shared_ptr<T> getInstance()
+ {
+ std::shared_ptr<T> object;
+
+ if (m_unusedObjects.empty())
+ {
+ object = std::shared_ptr<T>(new T(), Deleter(this));
+ }
+ else
+ {
+ object = std::shared_ptr<T>(m_unusedObjects.top().release(), Deleter(this));
+ m_unusedObjects.pop();
+ }
+
+ return object;
+ }
+
+private:
+ /// Custom deleter to keep unused objects for reuse, rather than actually deleting them.
+ class Deleter
+ {
+ public:
+ /// Constructor
+ /// \param factory ReuseFactory with the collection of unused object for reuse.
+ explicit Deleter(ReuseFactory* factory) : m_factory(factory)
+ {
+ }
+ /// Deletion method, adds the object to the ReuseFactory's collection.
+ /// \param unusedObject Object that is no longer referenced by any shared pointers
+ void operator()(T* unusedObject) const
+ {
+ m_factory->addUnused(unusedObject);
+ }
+ private:
+ /// ReuseFactory with the collection of unused objects for reuse.
+ ReuseFactory* m_factory;
+ };
+
+ /// Adds an object to the stack of unused objects.
+ /// This should only be called from Deleter.
+ /// \param unusedObject Object that is no longer referenced by any shared pointers
+ void addUnused(T* unusedObject)
+ {
+ m_unusedObjects.push(std::unique_ptr<T>(unusedObject));
+ }
+
+ /// Stack of objects that are available for reuse.
+ std::stack<std::unique_ptr<T>> m_unusedObjects;
+};
+
+}; // namespace Framework
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_REUSEFACTORY_H
diff --git a/SurgSim/Framework/Runtime.cpp b/SurgSim/Framework/Runtime.cpp
new file mode 100644
index 0000000..5fc90c9
--- /dev/null
+++ b/SurgSim/Framework/Runtime.cpp
@@ -0,0 +1,311 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <boost/thread/thread.hpp>
+#include <boost/thread/locks.hpp>
+
+#include "SurgSim/Framework/Runtime.h"
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Barrier.h"
+#include "SurgSim/Framework/ComponentManager.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Scene.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+std::shared_ptr<ApplicationData> Runtime::m_applicationData;
+
+Runtime::Runtime() :
+ m_isRunning(false),
+ m_isPaused(false)
+{
+ initSearchPaths("");
+}
+
+Runtime::Runtime(const std::string& configFilePath) :
+ m_isRunning(false),
+ m_isPaused(false)
+{
+ initSearchPaths(configFilePath);
+}
+
+Runtime::~Runtime()
+{
+ // Kill all threads
+ stop();
+}
+
+void Runtime::addManager(std::shared_ptr<ComponentManager> manager)
+{
+ SURGSIM_ASSERT(! m_isRunning) << "Cannot add a manager to the runtime once it is running";
+
+ if (manager != nullptr)
+ {
+ manager->setRuntime(getSharedPtr());
+ m_managers.push_back(manager);
+ }
+}
+
+std::shared_ptr<Scene> Runtime::getScene()
+{
+ if (m_scene == nullptr)
+ {
+ m_scene = std::make_shared<Scene>(getSharedPtr());
+ }
+ return m_scene;
+}
+
+
+bool Runtime::addSceneElement(std::shared_ptr<SceneElement> sceneElement)
+{
+ // Before the runtime has been started, adding components will be handled via
+ // preprocessSceneElements()
+ if (m_isRunning)
+ {
+ addComponents(sceneElement->getComponents());
+ }
+
+ return m_isRunning;
+}
+
+void Runtime::addComponents(const std::vector<std::shared_ptr<SurgSim::Framework::Component>>& components)
+{
+ auto componentsIt = std::begin(components);
+ auto componentsEnd = std::end(components);
+ for (; componentsIt != componentsEnd; ++componentsIt)
+ {
+ for (auto manager = std::begin(m_managers); manager != std::end(m_managers); ++manager)
+ {
+ if ((*componentsIt)->isInitialized())
+ {
+ (*manager)->enqueueAddComponent(*componentsIt);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(Logger::getLogger("Runtime")) <<
+ "Trying to add an uninitialized component.";
+ }
+ }
+ }
+}
+
+bool Runtime::execute()
+{
+ bool doExit = false;
+ if (start())
+ {
+ auto it = m_managers.cbegin();
+ while (!doExit)
+ {
+ // Watchdog, shut down all managers if one manager is done
+ boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+ for (it = m_managers.cbegin(); it != m_managers.cend(); ++it)
+ {
+ if (!(*it)->isRunning())
+ {
+ doExit = true;
+ break;
+ }
+ }
+ }
+ stop();
+ }
+ return true;
+}
+
+bool Runtime::start(bool paused)
+{
+ auto logger = Logger::getDefaultLogger();
+
+ // Gather all the Components from the currently known SceneElements to add them
+ // collectively.
+ // HS-2013-dec-12 This construct cause a bug as this also gathers the elements to be processed
+ // any sceneelements added after this call and before initialization is finished will get lost
+ preprocessSceneElements();
+
+ m_isRunning = true;
+ m_isPaused = paused;
+
+ std::vector<std::shared_ptr<ComponentManager>>::iterator it;
+ m_barrier.reset(new Barrier(m_managers.size() + 1));
+ for (it = m_managers.begin(); it != m_managers.end(); ++it)
+ {
+ (*it)->start(m_barrier, m_isPaused);
+ }
+
+ // Wait for all the managers to initialize
+ m_barrier->wait(true);
+ SURGSIM_LOG_INFO(logger) << "All managers doInit() succeeded";
+
+ // Wait for all the managers to startup
+ m_barrier->wait(true);
+ SURGSIM_LOG_INFO(logger) << "All managers doStartup() succeeded";
+
+ // Wait for all the components to initialize()
+ m_barrier->wait(true);
+ SURGSIM_LOG_INFO(logger) << "All component initialize() succeeded";
+
+ // Wait for all the components to wakeUp()
+ m_barrier->wait(true);
+ SURGSIM_LOG_INFO(logger) << "All component wakeUp() succeeded";
+ SURGSIM_LOG_INFO(logger) << "Scene is initialized. All managers updating";
+
+ return true;
+}
+
+bool Runtime::stop()
+{
+ if (isPaused())
+ {
+ resume();
+ }
+
+ m_isRunning = false;
+
+ std::vector<std::shared_ptr<ComponentManager>>::iterator it;
+ for (it = m_managers.begin(); it != m_managers.end(); ++it)
+ {
+ (*it)->stop();
+ }
+ return true;
+}
+
+void Runtime::pause()
+{
+ m_isPaused = true;
+ for (auto it = std::begin(m_managers); it != std::end(m_managers); ++it)
+ {
+ (*it)->setSynchronous(true);
+ }
+}
+
+void Runtime::resume()
+{
+ if (isPaused())
+ {
+ m_isPaused = false;
+ for (auto it = std::begin(m_managers); it != std::end(m_managers); ++it)
+ {
+ (*it)->setSynchronous(false);
+ }
+ // HS-2014-feb-21 if there are threads that are not waiting this will hang, this can happen if the above call
+ // to setSynchronous was made while the thread was executing code rather than waiting.
+ // #threadsafety
+ m_barrier->wait(true);
+ }
+}
+
+void Runtime::step()
+{
+ if (isPaused())
+ {
+ m_barrier->wait(true);
+ }
+}
+
+bool Runtime::isRunning() const
+{
+ return m_isRunning;
+}
+
+bool Runtime::isPaused() const
+{
+ return m_isPaused;
+}
+
+void Runtime::preprocessSceneElements()
+{
+ // Collect all the Components
+ std::vector<std::shared_ptr<Component>> newComponents;
+ auto sceneElements = getScene()->getSceneElements();
+ for (auto it = sceneElements.begin(); it != sceneElements.end(); ++it)
+ {
+ // Initialize should have been called by now, if the SceneElement is not initialized this means
+ // initialization failed
+ if ((*it)->isInitialized())
+ {
+ std::vector<std::shared_ptr<Component>> elementComponents = (*it)->getComponents();
+ newComponents.insert(newComponents.end(), elementComponents.begin(), elementComponents.end());
+ }
+ }
+
+ addComponents(newComponents);
+}
+
+std::shared_ptr<Runtime> Runtime::getSharedPtr()
+{
+ std::shared_ptr<Runtime> result;
+ try
+ {
+ result = shared_from_this();
+ }
+ catch (const std::exception&)
+ {
+ SURGSIM_FAILURE() << "Runtime was not created as a shared_ptr";
+ }
+ return result;
+}
+
+void Runtime::initSearchPaths(const std::string& configFilePath)
+{
+ if (configFilePath == "")
+ {
+ std::vector<std::string> paths;
+ paths.push_back(".");
+ m_applicationData = std::make_shared<ApplicationData>(paths);
+ }
+ else
+ {
+ m_applicationData = std::make_shared<ApplicationData>(configFilePath);
+ }
+}
+
+std::shared_ptr<const ApplicationData> Runtime::getApplicationData()
+{
+ SURGSIM_ASSERT(nullptr != m_applicationData) <<
+ "Runtime::getApplicationData() should be called after the Runtime is created.";
+ return m_applicationData;
+}
+
+void Runtime::addComponent(const std::shared_ptr<Component>& component)
+{
+ if (m_isRunning)
+ {
+ for (auto it = std::begin(m_managers); it != std::end(m_managers); ++it)
+ {
+ (*it)->enqueueAddComponent(component);
+ }
+ }
+}
+
+void Runtime::removeComponent(const std::shared_ptr<Component>& component)
+{
+ if (m_isRunning)
+ {
+ for (auto it = std::begin(m_managers); it != std::end(m_managers); ++it)
+ {
+ (*it)->enqueueRemoveComponent(component);
+ }
+ }
+}
+
+}; // namespace Framework
+}; // namespace SurgSim
diff --git a/SurgSim/Framework/Runtime.h b/SurgSim/Framework/Runtime.h
new file mode 100644
index 0000000..15fe277
--- /dev/null
+++ b/SurgSim/Framework/Runtime.h
@@ -0,0 +1,156 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_RUNTIME_H
+#define SURGSIM_FRAMEWORK_RUNTIME_H
+
+#include <vector>
+#include <memory>
+#include <string>
+#include <map>
+
+#include <boost/thread/mutex.hpp>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+class ApplicationData;
+class Barrier;
+class ComponentManager;
+class Component;
+class Logger;
+class Scene;
+class SceneElement;
+
+/// This class contains all the information about the runtime environment of
+/// the simulation, all the running threads, the state, while it is de facto a
+/// singleton it should be passed around if needed. Needs to be created as a
+/// shared object.
+class Runtime : public std::enable_shared_from_this<Runtime>
+{
+public:
+
+ /// Default constructor.
+ Runtime();
+
+ /// Constructor, initializes the search path with paths from the given file.
+ /// \param configFilePath Full pathname of the configuration file that contains paths,
+ /// one per line, to search for application data. If no config file
+ /// is given "." will be used as default path.
+ explicit Runtime(const std::string& configFilePath);
+
+ /// Destructor.
+ ~Runtime();
+
+ /// Add a worker thread, this should probably only be possible if the system is not running
+ void addManager(std::shared_ptr<ComponentManager> thread);
+
+ /// \return The scene to be used for this runtime. Use this for any kind of scene manipulation.
+ std::shared_ptr<Scene> getScene();
+
+ /// \returns The current scene
+ std::shared_ptr<Scene> getScene() const;
+
+ /// Adds a scene element
+ /// \param sceneElement The scene element.
+ /// \return true if it succeeds, false if it fails.
+ bool addSceneElement(std::shared_ptr<SceneElement> sceneElement);
+
+ /// Start all the threads and block until one of them quits
+ bool execute();
+
+ /// Start all the threads non returns after the startup as succeeded
+ /// \return true if it succeeds, false if it fails.
+ bool start(bool paused = false);
+
+ /// Pause all managers, this will set all managers to synchronous execution, they will all complete
+ /// their updates and then wait for step() to proceed, call resume to go back to uninterupted execution.
+ /// \note HS-2013-nov-01 this is mostly to be used as a facillity for testing and debugging, the threads
+ /// are not executed at the correct rates against each other, this is an issue that can be resolved
+ /// but is not necessary right now.
+ void pause();
+
+ /// Resume from pause, causes all managers to resume normal processing
+ /// \warning This function is not thread safe, if stop is called when there are threads that are not waiting,
+ /// this call will hang indefinitely.
+ void resume();
+
+ /// Make all managers execute 1 update loop, afterwards they will wait for another step() call or resume()
+ void step();
+
+ /// Stops the simulation.
+ /// The call will wait for all the threads to finish, except for any threads that have been detached.
+ /// \warning This function is not thread safe, if stop is called when there are threads that are not waiting,
+ /// this call will hang indefinitely.
+ /// \return true if it succeeds, false if it fails.
+ bool stop();
+
+ /// Query if this object is running.
+ /// \return true if running, false if not.
+ bool isRunning() const;
+
+ /// Query if this object is paused.
+ /// \return true if paused, false if not.
+ bool isPaused() const;
+
+ /// Gets application data for the runtime.
+ /// \return The application data.
+ static std::shared_ptr<const ApplicationData> getApplicationData();
+
+ /// Adds a component.
+ /// \param component The component.
+ void addComponent(const std::shared_ptr<Component>& component);
+
+ /// Removes the component described by component.
+ /// \param component The component.
+ void removeComponent(const std::shared_ptr<Component>& component);
+
+private:
+
+ /// Preprocess scene elements. This is called during the startup sequence
+ /// and installs all the Components of the SceneElements in the worker threads
+ void preprocessSceneElements();
+
+ /// Adds the components.
+ /// \param components The components.
+ /// \return true if it succeeds, false if it fails.
+ void addComponents(const std::vector<std::shared_ptr<SurgSim::Framework::Component>>& components);
+
+ /// Initializes the search paths.
+ /// \param configFilePath Full pathname of the configuration file, if path is empty
+ /// "." will be used as default path.
+ void initSearchPaths(const std::string& configFilePath);
+
+ /// Gets a shared pointer to the runtime.
+ /// \return The shared pointer.
+ std::shared_ptr<Runtime> getSharedPtr();
+
+ bool m_isRunning;
+ std::vector< std::shared_ptr<ComponentManager> > m_managers;
+ std::shared_ptr<Scene> m_scene;
+ static std::shared_ptr<ApplicationData> m_applicationData;
+
+ boost::mutex m_mutex;
+
+ std::shared_ptr<Barrier> m_barrier;
+ bool m_isPaused;
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_RUNTIME_H
diff --git a/SurgSim/Framework/Scene.cpp b/SurgSim/Framework/Scene.cpp
new file mode 100644
index 0000000..b248a8f
--- /dev/null
+++ b/SurgSim/Framework/Scene.cpp
@@ -0,0 +1,144 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest LLC.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Scene.h"
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/Log.h"
+
+#include <utility>
+#include <set>
+#include <vector>
+#include <boost/thread/locks.hpp>
+
+#include <yaml-cpp/yaml.h>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+Scene::Scene(std::weak_ptr<Runtime> runtime) :
+ m_runtime(runtime)
+{
+ SURGSIM_ASSERT(!m_runtime.expired()) << "Can't create scene with empty runtime.";
+}
+
+Scene::~Scene()
+{
+
+}
+
+void Scene::addSceneElement(std::shared_ptr<SceneElement> element)
+{
+ SURGSIM_ASSERT(!m_runtime.expired()) << "Runtime pointer is expired, cannot add SceneElement to Scene.";
+
+ std::string name = element->getName();
+ element->setScene(getSharedPtr());
+
+ std::shared_ptr<Runtime> runtime = m_runtime.lock();
+ element->setRuntime(runtime);
+
+ if (element->initialize())
+ {
+ {
+ boost::lock_guard<boost::mutex> lock(m_sceneElementsMutex);
+ m_elements.push_back(element);
+ }
+ runtime->addSceneElement(element);
+ }
+
+}
+
+std::shared_ptr<Runtime> Scene::getRuntime()
+{
+ return m_runtime.lock();
+}
+
+const std::vector <std::shared_ptr<SceneElement>>& Scene::getSceneElements() const
+{
+ return m_elements;
+}
+
+const std::shared_ptr<SceneElement> Scene::getSceneElement(const std::string& name) const
+{
+ std::shared_ptr<SceneElement> result = nullptr;
+ for (auto it = std::begin(m_elements); it != std::end(m_elements); ++it)
+ {
+ if ((*it)->getName() == name)
+ {
+ result = *it;
+ break;
+ }
+ }
+
+ return result;
+}
+
+std::shared_ptr<Scene> Scene::getSharedPtr()
+{
+ std::shared_ptr<Scene> result;
+ try
+ {
+ result = shared_from_this();
+ }
+ catch (const std::exception&)
+ {
+ SURGSIM_FAILURE() << "Scene was not created as a shared_ptr";
+ }
+ return result;
+}
+
+YAML::Node Scene::encode() const
+{
+ YAML::Node result(YAML::NodeType::Map);
+ YAML::Node data(YAML::NodeType::Map);
+ for (auto sceneElement = m_elements.begin(); sceneElement != m_elements.end(); ++sceneElement)
+ {
+ data["SceneElements"].push_back(*(*sceneElement));
+ }
+
+ result["SurgSim::Framework::Scene"] = data;
+
+ return result;
+}
+
+bool Scene::decode(const YAML::Node& node)
+{
+ bool result = false;
+ if (node.IsMap())
+ {
+ YAML::Node data = node["SurgSim::Framework::Scene"];
+
+ if (data["SceneElements"].IsDefined())
+ {
+ auto sceneElements = data["SceneElements"].as<std::vector<std::shared_ptr<SceneElement>>>();
+
+ std::for_each(sceneElements.begin(), sceneElements.end(),
+ [&](std::shared_ptr<SceneElement> element)
+ {
+ addSceneElement(element);
+ });
+ }
+ result = true;
+ }
+ return result;
+}
+}; // namespace Framework
+}; // namespace SurgSim
+
diff --git a/SurgSim/Framework/Scene.h b/SurgSim/Framework/Scene.h
new file mode 100644
index 0000000..91f349d
--- /dev/null
+++ b/SurgSim/Framework/Scene.h
@@ -0,0 +1,92 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_SCENE_H
+#define SURGSIM_FRAMEWORK_SCENE_H
+
+#include <vector>
+#include <memory>
+#include <string>
+#include <boost/thread/mutex.hpp>
+
+#include "SurgSim/Framework/SceneElement.h"
+
+namespace YAML
+{
+class Node;
+}
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+class Runtime;
+
+/// Scene. Basic Container for SceneElements
+class Scene : public std::enable_shared_from_this<Scene>
+{
+public:
+
+ /// Constructor.
+ /// \param runtime The runtime to be used.
+ explicit Scene(std::weak_ptr<Runtime> runtime);
+
+ /// Destructor
+ ~Scene();
+
+ /// Adds a scene element to the Scene, the SceneElement will have its initialize() function called.
+ /// \param element The element.
+ void addSceneElement(std::shared_ptr<SceneElement> element);
+
+ /// Gets all the scene elements in the scene.
+ /// \return The scene elements.
+ const std::vector<std::shared_ptr<SceneElement>>& getSceneElements() const;
+
+ /// Retrieve a SceneElement for this scene with the given name.
+ /// \return A SceneElement with given name; Empty share_ptr<> will be returned if no such SceneElement found.
+ const std::shared_ptr<SceneElement> getSceneElement(const std::string& name) const;
+
+ /// Gets the runtime.
+ /// \return runtime The runtime for this scene.
+ std::shared_ptr<Runtime> getRuntime();
+
+ /// Convert to a YAML::Node
+ /// \return A node with all the public data of this instance
+ YAML::Node encode() const;
+
+ /// Pull data from a YAML::Node.
+ /// \param node the node to decode.
+ /// \return true if the decoding succeeded and the node was formatted correctly, false otherwise
+ bool decode(const YAML::Node& node);
+
+private:
+
+ /// Get a shared pointer to Scene.
+ /// \return The shared pointer.
+ std::shared_ptr<Scene> getSharedPtr();
+
+ std::weak_ptr<Runtime> m_runtime;
+
+ std::vector<std::shared_ptr<SceneElement>> m_elements;
+
+ // Used in a const function, need to declare mutable
+ mutable boost::mutex m_sceneElementsMutex;
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_SCENE_H
diff --git a/SurgSim/Framework/SceneElement-inl.h b/SurgSim/Framework/SceneElement-inl.h
new file mode 100644
index 0000000..078b570
--- /dev/null
+++ b/SurgSim/Framework/SceneElement-inl.h
@@ -0,0 +1,47 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_SCENEELEMENT_INL_H
+#define SURGSIM_FRAMEWORK_SCENEELEMENT_INL_H
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+
+/// Implementation of getComponents method to get all the components with type T
+/// \return The type T components
+template <class T>
+std::vector<std::shared_ptr<T>> SceneElement::getComponents() const
+{
+ std::vector<std::shared_ptr<T>> result;
+
+ for (auto componentIt = m_components.begin(); componentIt != m_components.end(); ++componentIt)
+ {
+ std::shared_ptr<T> typedElement = std::dynamic_pointer_cast<T>(componentIt->second);
+ if (typedElement)
+ {
+ result.emplace_back(std::move(typedElement));
+ }
+ }
+ return result;
+}
+
+}; // namespace Framework
+
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Framework/SceneElement.cpp b/SurgSim/Framework/SceneElement.cpp
new file mode 100644
index 0000000..c0524c2
--- /dev/null
+++ b/SurgSim/Framework/SceneElement.cpp
@@ -0,0 +1,295 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest LLC.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/SceneElement.h"
+
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Framework/Runtime.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+SceneElement::SceneElement(const std::string& name) :
+ m_name(name),
+ m_isInitialized(false),
+ m_isActive(true)
+{
+ m_pose = std::make_shared<SurgSim::Framework::PoseComponent>("Pose");
+ m_pose->setPose(SurgSim::Math::RigidTransform3d::Identity());
+ m_components[m_pose->getName()] = m_pose;
+}
+
+SceneElement::~SceneElement()
+{
+
+}
+
+bool SceneElement::addComponent(std::shared_ptr<Component> component)
+{
+ SURGSIM_ASSERT(nullptr != component) << "Cannot add a nullptr as a component";
+
+ bool result = false;
+ if (m_components.find(component->getName()) == m_components.end())
+ {
+ result = true;
+ if (isInitialized())
+ {
+ auto runtime = getRuntime();
+ SURGSIM_ASSERT(nullptr != runtime) << "Runtime cannot be expired when adding a component " << getName();
+ result = initializeComponent(component);
+ if (result)
+ {
+ runtime->addComponent(component);
+ }
+ }
+
+ if (result)
+ {
+ component->setSceneElement(getSharedPtr());
+ m_components[component->getName()] = component;
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(Logger::getLogger("runtime")) <<
+ "Component with name " << component->getName() <<
+ " already exists on SceneElement " << getName() <<
+ ", did not add component";
+ }
+
+ return result;
+}
+
+bool SceneElement::removeComponent(std::shared_ptr<Component> component)
+{
+ return removeComponent(component->getName());
+}
+
+bool SceneElement::removeComponent(const std::string& name)
+{
+ bool result = false;
+ if (m_components.find(name) != m_components.end())
+ {
+ size_t count = m_components.erase(name);
+ result = (count == 1);
+ }
+ return result;
+}
+
+std::shared_ptr<Component> SceneElement::getComponent(const std::string& name) const
+{
+ std::shared_ptr<Component> result;
+
+ auto findResult = m_components.find(name);
+ if (findResult != m_components.end())
+ {
+ result = findResult->second;
+ }
+ return result;
+}
+
+bool SceneElement::initialize()
+{
+ SURGSIM_ASSERT(!m_isInitialized) << "Double initialization calls on SceneElement " << m_name;
+ m_isInitialized = doInitialize();
+
+ // For completeness
+ m_pose->setSceneElement(getSharedPtr());
+
+ if (m_isInitialized)
+ {
+ // initialize all components
+ for (auto pair = std::begin(m_components); m_isInitialized && pair != std::end(m_components); ++pair)
+ {
+ m_isInitialized = initializeComponent(pair->second);
+ }
+ }
+
+ return m_isInitialized;
+}
+
+bool SceneElement::initializeComponent(std::shared_ptr<SurgSim::Framework::Component> component)
+{
+ component->setScene(m_scene);
+ return component->initialize(getRuntime());
+}
+
+std::string SceneElement::getName() const
+{
+ return m_name;
+}
+
+void SceneElement::setPose(const SurgSim::Math::RigidTransform3d& pose)
+{
+ m_pose->setPose(pose);
+}
+
+const SurgSim::Math::RigidTransform3d& SceneElement::getPose() const
+{
+ return m_pose->getPose();
+}
+
+std::shared_ptr<PoseComponent> SceneElement::getPoseComponent()
+{
+ return m_pose;
+}
+
+std::vector<std::shared_ptr<Component>> SceneElement::getComponents() const
+{
+ std::vector<std::shared_ptr<Component>> result(m_components.size());
+ auto componentIt = m_components.begin();
+ for (int i = 0; componentIt != m_components.end(); ++componentIt, ++i)
+ {
+ result[i] = componentIt->second;
+ }
+ return result;
+}
+
+void SceneElement::setScene(std::weak_ptr<Scene> scene)
+{
+ m_scene = scene;
+ for (auto it = std::begin(m_components); it != std::end(m_components); ++it)
+ {
+ (it->second)->setScene(scene);
+ }
+}
+
+std::shared_ptr<Scene> SceneElement::getScene()
+{
+ return m_scene.lock();
+}
+
+void SceneElement::setRuntime(std::weak_ptr<Runtime> runtime)
+{
+ m_runtime = runtime;
+}
+
+std::shared_ptr<Runtime> SceneElement::getRuntime()
+{
+ return m_runtime.lock();
+}
+
+bool SceneElement::isInitialized() const
+{
+ return m_isInitialized;
+}
+
+std::shared_ptr<SceneElement> SceneElement::getSharedPtr()
+{
+ std::shared_ptr<SceneElement> result;
+ try
+ {
+ result = shared_from_this();
+ }
+ catch (const std::exception&)
+ {
+ SURGSIM_FAILURE() << "SceneElement was not created as a shared_ptr.";
+ }
+ return result;
+}
+
+void SceneElement::setName(const std::string& name)
+{
+ m_name = name;
+}
+
+YAML::Node SceneElement::encode(bool standalone) const
+{
+ YAML::Node data(YAML::NodeType::Map);
+ data["Name"] = getName();
+ data["IsActive"] = isActive();
+
+ for (auto component = std::begin(m_components); component != std::end(m_components); ++component)
+ {
+ if (standalone)
+ {
+ data["Components"].push_back(*component->second);
+ }
+ else
+ {
+ data["Components"].push_back(component->second);
+ }
+ }
+ YAML::Node node(YAML::NodeType::Map);
+ node[getClassName()] = data;
+ return node;
+}
+
+bool SceneElement::decode(const YAML::Node& node)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Should not call decode on a SceneElement that has already been initialized.";
+ bool result = false;
+ if (node.IsMap())
+ {
+ std::string className = node.begin()->first.as<std::string>();
+
+ SURGSIM_ASSERT(className == getClassName()) << "Type in node does not match class, wanted <" <<
+ className << ">" << " but this is a <" << getClassName() << ">.";
+
+ YAML::Node data = node[getClassName()];
+
+ m_name = data["Name"].as<std::string>();
+
+ if (data["IsActive"].IsDefined())
+ {
+ m_isActive = data["IsActive"].as<bool>();
+ }
+
+ if (data["Components"].IsSequence())
+ {
+ for (auto nodeIt = data["Components"].begin(); nodeIt != data["Components"].end(); ++nodeIt)
+ {
+ if ("SurgSim::Framework::PoseComponent" == nodeIt->begin()->first.as<std::string>())
+ {
+ removeComponent(m_pose);
+ m_pose = nodeIt->as<std::shared_ptr<SurgSim::Framework::PoseComponent>>();
+ addComponent(m_pose);
+ }
+ else
+ {
+ addComponent(nodeIt->as<std::shared_ptr<SurgSim::Framework::Component>>());
+ }
+ }
+ result = true;
+ }
+ }
+ return result;
+}
+
+std::string SceneElement::getClassName() const
+{
+ // SURGSIM_FAILURE() << "SceneElement is abstract, this should not be called.";
+ return "SurgSim::Framework::SceneElement";
+}
+
+void SceneElement::setActive(bool val)
+{
+ m_isActive = val;
+}
+
+bool SceneElement::isActive() const
+{
+ return m_isActive;
+}
+
+}; // namespace Framework
+}; // namespace SurgSim
diff --git a/SurgSim/Framework/SceneElement.h b/SurgSim/Framework/SceneElement.h
new file mode 100644
index 0000000..37a227f
--- /dev/null
+++ b/SurgSim/Framework/SceneElement.h
@@ -0,0 +1,198 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_SCENEELEMENT_H
+#define SURGSIM_FRAMEWORK_SCENEELEMENT_H
+
+#include <string>
+#include <memory>
+#include <algorithm>
+#include <unordered_map>
+#include <vector>
+
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace YAML
+{
+class Node;
+}
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+class Component;
+class PoseComponent;
+class Scene;
+class Runtime;
+
+/// SceneElement is the basic part of a scene, it is a container of components. When a
+/// SceneElement is added to a Scene, the Scene will call initialize() on the SceneElement,
+/// which in turn will call initialize() on all its components.
+/// If a component is added to a SceneElement after the SceneElement was initialized, the component will
+/// be initialized immediately.
+/// \note A SceneElement needs to be created as a shared_ptr.
+class SceneElement : public std::enable_shared_from_this<SceneElement>
+{
+public:
+
+ /// Constructor
+ /// \param name Name of this SceneElement
+ explicit SceneElement(const std::string& name);
+
+ /// Destructor
+ virtual ~SceneElement();
+
+ /// \return the class name for this class
+ virtual std::string getClassName() const;
+
+ /// Adds a component, calls initialize() on the component, if SceneElement::isInitialized() is true
+ /// \param component The component.
+ /// \return true if it succeeds, false if it fails.
+ bool addComponent(std::shared_ptr<Component> component);
+
+ /// Removes a given component.
+ /// \param component The component.
+ /// \return true if it succeeds, false if it fails or the component cannot be found.
+ bool removeComponent(std::shared_ptr<Component> component);
+
+ /// Removes the component described by name.
+ /// \param name The name.
+ /// \return true if it succeeds, false if it fails or the component cannot be found.
+ bool removeComponent(const std::string& name);
+
+ /// Gets the component identified by name.
+ /// \param name The name.
+ /// \return The component or nullptr if the component cannot be found.
+ std::shared_ptr<Component> getComponent(const std::string& name) const;
+
+ /// Gets all the components of this SceneElement.
+ /// \return The components.
+ std::vector<std::shared_ptr<Component> > getComponents() const;
+
+ /// Template version of getComponents method to get all the components with type T
+ /// \return The type T components
+ template <class T>
+ std::vector<std::shared_ptr<T>> getComponents() const;
+
+ /// Executes the initialize operation.
+ /// \return true if it succeeds, false if it fails.
+ bool initialize();
+
+ /// Return the name of this SceneElement
+ /// \return The name.
+ std::string getName() const;
+
+ /// Set the pose of this SceneElement
+ /// \param pose The pose
+ void setPose(const SurgSim::Math::RigidTransform3d& pose);
+
+ /// Get the pose of this SceneElement
+ /// \return the pose
+ const SurgSim::Math::RigidTransform3d& getPose() const;
+
+ /// Get the PoseComponent that controls the pose all Representations
+ /// in this SceneElement
+ /// \return the PoseComponent
+ std::shared_ptr<PoseComponent> getPoseComponent();
+
+ /// Sets the Scene.
+ /// \param scene Pointer to the scene.
+ void setScene(std::weak_ptr<Scene> scene);
+
+ /// Gets the Scene.
+ /// \return The scene.
+ std::shared_ptr<Scene> getScene();
+
+ /// Sets the Runtime.
+ /// \param runtime Pointer to the runtime.
+ void setRuntime(std::weak_ptr<Runtime> runtime);
+ /// Gets the runtime.
+ /// \return The runtime.
+ std::shared_ptr<Runtime> getRuntime();
+
+ /// Return if this SceneElement is initialized.
+ /// \return True if this SceneElement is initialized; Otherwise, false.
+ bool isInitialized() const;
+
+ /// Set this SceneElement's status (active/inactive)
+ /// \note Set 'inactive' on an active SceneElement will cause all the components it contains to not be processed.
+ /// \note Set 'active' on an inactive SceneElement will cause all the components it contains which are active
+ /// \note to be processed again.
+ /// \note Inactive components will not be processed no matter the SceneElement is active or inactive.
+ /// \param val True to set the SceneElement active, false to set inactive.
+ void setActive(bool val);
+
+ /// \return True if this SceneElement is active; Otherwise, false.
+ bool isActive() const;
+
+ /// Gets a shared pointer to this SceneElement.
+ /// \return The shared pointer.
+ std::shared_ptr<SceneElement> getSharedPtr();
+
+ /// Convert to a YAML::Node
+ /// \param standalone when true, all the components will be represented as full component, when false
+ /// they will be represented as references
+ /// \return A node with all the public data of this instance
+ virtual YAML::Node encode(bool standalone) const;
+
+ /// Pull data from a YAML::Node.
+ /// \throws SurgSim::Framework::AssertionFailure if the SceneElement is already initialized
+ /// \param node the node to decode.
+ /// \return true if the decoding succeeded and the node was formatted correctly, false otherwise
+ virtual bool decode(const YAML::Node& node);
+
+private:
+ /// Initialize the given component
+ /// \param component The component to be initialized.
+ /// \return True if initialization succeeded, false otherwise.
+ bool initializeComponent(std::shared_ptr<SurgSim::Framework::Component> component);
+
+ /// Name of this SceneElement
+ std::string m_name;
+
+ std::shared_ptr<SurgSim::Framework::PoseComponent> m_pose;
+
+ /// A collection of Components
+ std::unordered_map<std::string, std::shared_ptr<Component>> m_components;
+
+ /// A (weak) back pointer to the Scene containing this SceneElement
+ std::weak_ptr<Scene> m_scene;
+
+ /// A (weak) back pointer to the Runtime containing this SceneElement
+ std::weak_ptr<Runtime> m_runtime;
+
+ /// Method to initialize this SceneElement. To be overridden by derived class(es).
+ /// \return True if initialization is successful; Otherwise, false.
+ virtual bool doInitialize() = 0;
+
+ /// Sets the name of this SceneElement.
+ /// \param name The new name.
+ void setName(const std::string& name);
+
+ /// Indicates if this SceneElement has been initialized or not.
+ bool m_isInitialized;
+
+ /// Indicates if this SceneElement is active or not.
+ bool m_isActive;
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#include "SurgSim/Framework/SceneElement-inl.h"
+
+#endif // SURGSIM_FRAMEWORK_SCENEELEMENT_H
diff --git a/SurgSim/Framework/SharedInstance-inl.h b/SurgSim/Framework/SharedInstance-inl.h
new file mode 100644
index 0000000..c34567e
--- /dev/null
+++ b/SurgSim/Framework/SharedInstance-inl.h
@@ -0,0 +1,81 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_SHAREDINSTANCE_INL_H
+#define SURGSIM_FRAMEWORK_SHAREDINSTANCE_INL_H
+
+#include <memory>
+#include <functional>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+ namespace Framework
+ {
+
+template <typename T>
+SharedInstance<T>::SharedInstance() :
+ m_instanceCreator(defaultInstanceCreator())
+{
+}
+
+template <typename T>
+SharedInstance<T>::SharedInstance(const InstanceCreator& instanceCreator) :
+ m_instanceCreator(instanceCreator)
+{
+}
+
+template <typename T>
+SharedInstance<T>::~SharedInstance()
+{
+}
+
+template <typename T>
+std::shared_ptr<T> SharedInstance<T>::get()
+{
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ std::shared_ptr<T> instance = m_weakInstance.lock();
+ if (! instance)
+ {
+ instance = createInstance();
+ m_weakInstance = instance;
+ }
+ return std::move(instance);
+}
+
+template <typename T>
+std::shared_ptr<T> SharedInstance<T>::createInstance()
+{
+ std::shared_ptr<T> instance = m_instanceCreator();
+ SURGSIM_ASSERT(instance);
+ return std::move(instance);
+}
+
+template <typename T>
+typename SharedInstance<T>::InstanceCreator SharedInstance<T>::defaultInstanceCreator()
+{
+ return []() { return std::make_shared<T>(); }; // NOLINT(readability/braces)
+}
+
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#endif // SURGSIM_FRAMEWORK_SHAREDINSTANCE_INL_H
diff --git a/SurgSim/Framework/SharedInstance.h b/SurgSim/Framework/SharedInstance.h
new file mode 100644
index 0000000..a59c064
--- /dev/null
+++ b/SurgSim/Framework/SharedInstance.h
@@ -0,0 +1,166 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_SHAREDINSTANCE_H
+#define SURGSIM_FRAMEWORK_SHAREDINSTANCE_H
+
+#include <memory>
+#include <functional>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// A utility class to manage a shared instance of an object.
+///
+
+/// This class behaves in a way that is superficially similar to the Singleton pattern, in that it manages a single
+/// shared instance of an object. However, there are some important differences to note:
+/// * The Singleton pattern applies to a <i>particular class,</i> so that a single instance is shared between any
+/// and all uses of that class. The SharedInstance case applies to a <i>specific use</i> of a class: the user
+/// creates a (shared) SharedInstance<T> object that manages the sharing of one instance of T. It is
+/// possible for another SharedInstance<T> that manages the same type T to be created elsewhere. This
+/// second SharedInstance manages a second, separate instance of T.
+/// * The object will not only be created as needed, but also destroyed when all of the shared_ptr references to it
+/// are released. If it is released and then needed again, a new instance will be created.
+/// * This life-cycle management based on reference counting makes it imperative for the calling code to <i>hold
+/// onto</i> their own shared_ptr for as long as they want to interact with the shared object instance.
+/// Releasing the shared_ptr indicates that the code using it is ready for the instance to be potentially
+/// destroyed.
+///
+/// Code using this class will normally use the get() method to initialize its own shared_ptr referencing the
+/// shared instance. Such code should hold onto the shared_ptr for as long as it needs to use the shared instance.
+/// As soon as all of the shared pointers are released, the shared object instance will be destroyed.
+///
+/// <b>A Simple Example</b>
+///
+/// To share an instance between many objects in the same class, you can declare a static SharedInstance member:
+/// ~~~~
+/// class ManyObjects
+/// {
+/// ManyObjects() : m_sharedInstance(m_sharedInstanceManager.get())
+/// {
+/// }
+///
+/// // ...
+///
+/// std::shared_ptr<SingleObject> m_sharedInstance;
+/// static SurgSim::Framework::SharedInstance<SingleObject> m_sharedInstanceManager;
+/// }
+///
+/// // Need to also define the static member somewhere:
+/// SurgSim::Framework::SharedInstance<SingleObject> ManyObjects::m_sharedInstanceManager;
+/// ~~~~
+/// The downside to this approach is that it requires the SharedInstance object itself to be created as a static
+/// member of a class. Since the order of construction (and destruction) of various static data members is not
+/// well defined by the C++ language standards, this can lead to objects getting used before they are initialized.
+///
+/// <b>A Better Example</b>
+///
+/// To avoid the initialization order problems, you can instead use the SharedInstance as a static variable
+/// inside a function or a (most likely also static) method, so it will not be initialized until that
+/// function/method is first called. (Note that this is only safe if your compiler follows the C++11
+/// requirement that the initialization of static variables inside functions must be atomic.)
+/// ~~~~
+/// class ManyObjects
+/// {
+/// ManyObjects() : m_sharedInstance(getSharedInstance())
+/// {
+/// }
+///
+/// // ...
+///
+/// static std::shared_ptr<SingleObject> getSharedInstance();
+/// std::shared_ptr<SingleObject> m_sharedInstance;
+/// }
+///
+/// std::shared_ptr<SingleObject> ManyObjects::getSharedInstance()
+/// {
+/// static SharedInstance<SingleObject> shared;
+/// return shared.get();
+/// }
+/// ~~~~
+///
+/// \tparam T Type of the data held by the SharedInstance.
+template <typename T>
+class SharedInstance
+{
+public:
+ /// A type that can hold a function object or lambda that takes no arguments and returns std::shared_ptr<T>.
+ typedef std::function<std::shared_ptr<T>()> InstanceCreator;
+
+ /// Create the SharedInstance object used to manage the shared instance.
+ /// Note that this <em>does not</em> immediately create the instance itself.
+ /// If and when the shared instance is created, it will be initialized using the default constructor via
+ /// std::make_shared.
+ SharedInstance();
+
+ /// Create the SharedInstance object used to manage the shared instance.
+ /// Note that this <em>does not</em> immediately create the instance itself.
+ /// If and when the shared instance is created, it will be initialized using the creator call.
+ explicit SharedInstance(const InstanceCreator& instanceCreator);
+
+ /// Destroy the container and the data it contains.
+ ~SharedInstance();
+
+ /// Gets the shared object instance.
+ /// If the instance has not been created previously, it will be created during the call.
+ ///
+ /// The calling code should generally copy the shared_ptr and hold onto it for as long as needed.
+ /// As soon as all of the shared pointers are released, the shared object instance will be destroyed.
+ ///
+ /// \return a shared_ptr holding the instance.
+ std::shared_ptr<T> get();
+
+private:
+ /// Prevent copying
+ SharedInstance(const SharedInstance&);
+ /// Prevent assignment
+ SharedInstance& operator=(const SharedInstance&);
+
+ /// Creates an object instance.
+ /// \return a shared_ptr containing a newly created object instance.
+ std::shared_ptr<T> createInstance();
+
+ /// Creates a function that can create an instance using std::make_shared<T>().
+ /// The function must be default-constuctible.
+ /// It was necessary to split this into a separate function because with VS2010, we can't just put a lambda in the
+ /// initializer for m_instanceCreator.
+ /// \return a function that creates a new object of type T using std::make_shared.
+ static InstanceCreator defaultInstanceCreator();
+
+ /// A creator function used to construct the shared instance.
+ InstanceCreator m_instanceCreator;
+
+ /// A weak reference to the shared instance, if any.
+ std::weak_ptr<T> m_weakInstance;
+
+ /// Mutex for synchronization of object creation.
+ boost::mutex m_mutex;
+};
+
+}; // namespace Framework
+}; // namespace SurgSim
+
+#include "SurgSim/Framework/SharedInstance-inl.h"
+
+#endif // SURGSIM_FRAMEWORK_SHAREDINSTANCE_H
diff --git a/SurgSim/Framework/Timer.cpp b/SurgSim/Framework/Timer.cpp
new file mode 100644
index 0000000..6b821c3
--- /dev/null
+++ b/SurgSim/Framework/Timer.cpp
@@ -0,0 +1,135 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <numeric>
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Timer.h"
+
+namespace SurgSim {
+namespace Framework {
+
+Timer::Timer() :
+ m_maxNumberOfFrames(100), m_clockFails(0)
+{
+ start();
+}
+
+void Timer::start()
+{
+ m_frameDurations.clear();
+ m_clockFails = 0;
+ beginFrame();
+}
+
+void Timer::beginFrame()
+{
+ m_lastTime = now();
+}
+
+void Timer::endFrame()
+{
+ TimerTimePoint currentTime = now();
+ m_frameDurations.push_back(currentTime - m_lastTime);
+ if (m_frameDurations.size() > m_maxNumberOfFrames)
+ {
+ m_frameDurations.pop_front();
+ }
+}
+
+void Timer::markFrame()
+{
+ endFrame();
+ beginFrame();
+}
+
+double Timer::getCumulativeTime() const
+{
+ SURGSIM_ASSERT(m_frameDurations.size() > 0) <<
+ "Attempted to access the frames for a Timer with no frames.";
+ TimerDuration cumulativeTime = std::accumulate(std::begin(m_frameDurations), std::end(m_frameDurations),
+ TimerDuration());
+ return cumulativeTime.count();
+}
+
+double Timer::getAverageFramePeriod() const
+{
+ return getCumulativeTime() / m_frameDurations.size();
+}
+
+double Timer::getAverageFrameRate() const
+{
+ return 1.0 / getAverageFramePeriod();
+}
+
+double Timer::getLastFramePeriod() const
+{
+ SURGSIM_ASSERT(m_frameDurations.size() > 0) <<
+ "Attempted to access the last frame period for a Timer with no frames.";
+ return m_frameDurations.back().count();
+}
+
+double Timer::getLastFrameRate() const
+{
+ return 1.0 / getLastFramePeriod();
+}
+
+void Timer::setMaxNumberOfFrames(size_t maxNumberOfFrames)
+{
+ m_maxNumberOfFrames = (maxNumberOfFrames > 0) ? maxNumberOfFrames : 1;
+ if (m_frameDurations.size() > m_maxNumberOfFrames)
+ {
+ m_frameDurations.erase(std::begin(m_frameDurations),
+ std::begin(m_frameDurations) + m_frameDurations.size() - m_maxNumberOfFrames);
+ }
+}
+
+size_t Timer::getCurrentNumberOfFrames() const
+{
+ return m_frameDurations.size();
+}
+
+size_t Timer::getNumberOfClockFails() const
+{
+ return m_clockFails;
+}
+
+Timer::TimerTimePoint Timer::now()
+{
+ boost::system::error_code ec;
+ TimerTimePoint currentTime = m_clock.now(ec);
+ if (ec.value() != 0)
+ {
+ ++ m_clockFails;
+ }
+ return currentTime;
+}
+
+double Timer::getMaxFramePeriod() const
+{
+ SURGSIM_ASSERT(m_frameDurations.size() > 0) <<
+ "Attempted to access the maximum frame period for a Timer with no frames.";
+ return std::max_element(m_frameDurations.cbegin(), m_frameDurations.cend())->count();
+}
+
+double Timer::getMinFramePeriod() const
+{
+ SURGSIM_ASSERT(m_frameDurations.size() > 0) <<
+ "Attempted to access the maximum frame period for a Timer with no frames.";
+ return std::min_element(m_frameDurations.cbegin(), m_frameDurations.cend())->count();
+}
+
+}; // namespace Framework
+}; // namespace SurgSim
diff --git a/SurgSim/Framework/Timer.h b/SurgSim/Framework/Timer.h
new file mode 100644
index 0000000..9ff56a3
--- /dev/null
+++ b/SurgSim/Framework/Timer.h
@@ -0,0 +1,119 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_TIMER_H
+#define SURGSIM_FRAMEWORK_TIMER_H
+
+#include <boost/chrono.hpp>
+#include <deque>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// Timer class, measures execution times. Multiple times can be stored as "frames" to provide an average rate or
+/// period.
+class Timer
+{
+public:
+ /// Instantiate a TimerClock and start a timing run.
+ Timer();
+
+ /// Begin a timing run by clearing out any stored frames and beginning a frame.
+ void start();
+
+ /// Begin a frame (storing the current time).
+ void beginFrame();
+
+ /// End this frame by storing the duration since the current frame was begun.
+ /// \note \c endFrame does not start a new frame, call \c beginFrame to do so.
+ /// \sa Timer::markFrame
+ void endFrame();
+
+ /// End the current frame and begin a new frame.
+ void markFrame();
+
+ /// Return the sum of the durations over all the stored frames. Asserts if there are no frames.
+ /// \return Sum of stored frame durations in seconds.
+ double getCumulativeTime() const;
+
+ /// Return the average duration across all stored frames. Asserts if there are no frames.
+ /// \return Average period in seconds.
+ double getAverageFramePeriod() const;
+
+ /// Return the inverse of the average duration across all stored frames. Asserts if there are no frames.
+ /// \return The average frequency in Hz.
+ double getAverageFrameRate() const;
+
+ /// Return the duration of the most-recent frame (time between last \c endFrame and the previous \c start,
+ /// \c beginFrame, or \c endFrame ). Asserts if there are no frames.
+ /// \return Most-recent period in seconds.
+ double getLastFramePeriod() const;
+
+ /// Return the inverse of the duration of the most-recent frame. Asserts if there are no frames.
+ /// \return Most-recent frequency in Hz.
+ double getLastFrameRate() const;
+
+ /// Set the maximum number of frames to store.
+ void setMaxNumberOfFrames(size_t numberOfFrames);
+
+ /// \return Number of frames currently stored (not the maximum number of frames).
+ size_t getCurrentNumberOfFrames() const;
+
+ /// \return Number of times the clock returned an error code since \c start. If this is non-zero, the frame
+ /// durations may be incorrect.
+ size_t getNumberOfClockFails() const;
+
+ /// \return The maximum duration across all the stored frames. Asserts if there are no frames.
+ double getMaxFramePeriod() const;
+
+ /// \return The minimum duration across all the stored frames. Asserts if there are no frames.
+ double getMinFramePeriod() const;
+
+private:
+ /// The Clock used by the Timer class.
+ typedef boost::chrono::steady_clock TimerClock;
+
+ /// Durations used by the Timer class.
+ typedef boost::chrono::duration<double> TimerDuration;
+
+ /// Time points used by the Timer class.
+ typedef boost::chrono::time_point<TimerClock, TimerDuration> TimerTimePoint;
+
+ /// Get the current time. Checks for any error code from the clock.
+ /// \return Current time.
+ TimerTimePoint now();
+
+ /// The clock used to get the time.
+ static const TimerClock m_clock;
+
+ /// The time at last \c start, \c beginFrame, or \c markFrame.
+ TimerTimePoint m_lastTime;
+
+ /// Maximum number of frames to store.
+ size_t m_maxNumberOfFrames;
+
+ /// Durations of the frames, i.e., the "stored frames".
+ std::deque<TimerDuration> m_frameDurations;
+
+ /// Number of clock errors since last \c start.
+ size_t m_clockFails;
+};
+
+} // Framework
+} // SurgSim
+
+#endif
diff --git a/SurgSim/Framework/TransferPropertiesBehavior.cpp b/SurgSim/Framework/TransferPropertiesBehavior.cpp
new file mode 100644
index 0000000..fa6b6cd
--- /dev/null
+++ b/SurgSim/Framework/TransferPropertiesBehavior.cpp
@@ -0,0 +1,127 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/TransferPropertiesBehavior.h"
+
+#include <boost/thread/lock_guard.hpp>
+#include <boost/any.hpp>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+TransferPropertiesBehavior::TransferPropertiesBehavior(const std::string& name) :
+ SurgSim::Framework::Behavior(name),
+ m_targetManager(MANAGER_TYPE_BEHAVIOR)
+{
+
+}
+
+TransferPropertiesBehavior::~TransferPropertiesBehavior()
+{
+
+}
+
+void TransferPropertiesBehavior::update(double dt)
+{
+ {
+ boost::lock_guard<boost::mutex> lock(m_incomingMutex);
+ m_connections.insert(m_connections.end(), m_incomingConnections.begin(), m_incomingConnections.end());
+ m_incomingConnections.clear();
+ }
+
+ for (auto it = std::begin(m_connections); it != std::end(m_connections); ++it)
+ {
+ if (!it->first.accessible.expired() && !it->second.accessible.expired())
+ {
+ auto source = it->first.accessible.lock();
+ auto target = it->second.accessible.lock();
+ target->setValue(it->second.name, source->getValue(it->first.name));
+ }
+ }
+}
+
+bool TransferPropertiesBehavior::connect(
+ std::shared_ptr<SurgSim::Framework::Accessible> sourceAccessible,
+ const std::string& sourcePropertyName,
+ std::shared_ptr<SurgSim::Framework::Accessible> targetAccessible,
+ const std::string& targetPropertyName)
+{
+ SURGSIM_ASSERT(sourceAccessible != nullptr && targetAccessible != nullptr) <<
+ "Accessibles cannot be nullptr";
+
+ Property source = {sourceAccessible, sourcePropertyName};
+ Property target = {targetAccessible, targetPropertyName};
+
+ return connect(source, target);
+}
+
+bool TransferPropertiesBehavior::connect(const Property& source, const Property& target)
+{
+ // Early outs
+ if (source.accessible.expired() || target.accessible.expired())
+ {
+ return false;
+ }
+
+ auto sharedSource = source.accessible.lock();
+ auto sharedTarget = target.accessible.lock();
+
+ SURGSIM_ASSERT(sharedSource != sharedTarget || source.name != target.name)
+ << "Cannot Read/Write with exactly the same property and object.";
+
+ SURGSIM_ASSERT(sharedSource->isReadable(source.name))
+ << "Source does not have a readable property called <" << source.name << ">.";
+
+ SURGSIM_ASSERT(sharedTarget->isWriteable(target.name))
+ << "Target does not have a writeable property called <" << target.name << ">.";
+
+ // \note HS-2013-nov-26 should also that the type of the output can be converted to the input
+
+ auto entry = std::make_pair(source, target);
+
+ boost::lock_guard<boost::mutex> lock(m_incomingMutex);
+ m_incomingConnections.push_back(std::move(entry));
+
+ return true;
+}
+
+bool TransferPropertiesBehavior::doInitialize()
+{
+ return true;
+}
+
+bool TransferPropertiesBehavior::doWakeUp()
+{
+ // Do an update step to initialize all the values
+ update(0.0);
+ return true;
+}
+
+void TransferPropertiesBehavior::setTargetManagerType(int managerType)
+{
+ SURGSIM_ASSERT(managerType > 0 && managerType < MANAGER_TYPE_COUNT) << "Invalid manager type.";
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot change the manager type after initialization.";
+ m_targetManager = managerType;
+}
+
+int TransferPropertiesBehavior::getTargetManagerType() const
+{
+ return m_targetManager;
+}
+
+}
+}
diff --git a/SurgSim/Framework/TransferPropertiesBehavior.h b/SurgSim/Framework/TransferPropertiesBehavior.h
new file mode 100644
index 0000000..05b5b92
--- /dev/null
+++ b/SurgSim/Framework/TransferPropertiesBehavior.h
@@ -0,0 +1,105 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_TRANSFERPROPERTIESBEHAVIOR_H
+#define SURGSIM_FRAMEWORK_TRANSFERPROPERTIESBEHAVIOR_H
+
+#include <string>
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/Accessible.h"
+
+#include <boost/thread/mutex.hpp>
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// Behavior to copy properties between instances of Accessible
+/// \note HS-2013-dec-12 does not support removal of connections yes
+class TransferPropertiesBehavior : public SurgSim::Framework::Behavior
+{
+public:
+
+ /// Constructor.
+ /// \param name The name of the behavior.
+ explicit TransferPropertiesBehavior(const std::string& name);
+
+ /// Destructor.
+ virtual ~TransferPropertiesBehavior();
+
+ /// Connect two properties of two instances of accessible, once connected the value of the property
+ /// will be copied from source to target at every update call.
+ /// \pre pointers cannot be nullptr, properties need to exist and the property
+ /// at the source needs to be readable and the property at the target needs
+ /// to be writeable
+ /// \param sourceAccessible Source Accessible instance.
+ /// \param sourcePropertyName The name of the source property.
+ /// \param tagetAccessible Target Accessible instance.
+ /// \param targetPropertyName The name of the target property.
+ /// \return true if the connection was created
+ bool connect(
+ std::shared_ptr<SurgSim::Framework::Accessible> sourceAccessible,
+ const std::string& sourcePropertyName,
+ std::shared_ptr<SurgSim::Framework::Accessible> tagetAccessible,
+ const std::string& targetPropertyName);
+
+ /// Connect two properties of two instances of accessible, once connected the value of the property
+ /// will be copied from source to target at every update call.
+ /// \pre pointers cannot be nullptr, properties need to exist and the property
+ /// at the source needs to be readable and the property at the target needs
+ /// to be writeable
+ /// \param source Source property.
+ /// \param target Target property.
+ /// \return true if the connection was created
+ bool connect(const Property& source, const Property& target);
+
+ /// Sets the type of manager that this behavior should use, this cannot be done after
+ /// initialization has occurred.
+ /// \param managerType Type of manager for this behavior
+ void setTargetManagerType(int managerType);
+
+ /// Overridden from Behavior
+ /// \param dt The time step.
+ virtual void update(double dt) override;
+
+private:
+
+ ///@{
+ /// Overridden from Behavior
+ virtual bool doInitialize() override;
+ virtual bool doWakeUp() override;
+ virtual int getTargetManagerType() const;
+ ///@}
+
+ /// Local typedefs
+ typedef std::pair<Property, Property> Connection;
+
+ /// List of connections in this object
+ std::vector<Connection> m_connections;
+
+ /// Lock for adding new connections
+ boost::mutex m_incomingMutex;
+
+ /// Queue for adding new connections
+ std::vector<Connection> m_incomingConnections;
+ int m_targetManager;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Framework/UnitTests/AccessibleTests.cpp b/SurgSim/Framework/UnitTests/AccessibleTests.cpp
new file mode 100644
index 0000000..f42181f
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/AccessibleTests.cpp
@@ -0,0 +1,442 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include "SurgSim/Framework/Accessible.h"
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <functional>
+#include <boost/any.hpp>
+
+#include "SurgSim/Math/Matrix.h"
+
+namespace
+{
+
+class TestClass : public SurgSim::Framework::Accessible
+{
+public:
+ TestClass() :
+ normal(100),
+ readWrite(100.0),
+ readOnly(100),
+ sharedPtr(std::make_shared<int>(4)),
+ overloadedValue(200.0),
+ virtualProperty(300),
+ privateProperty(100) // Don't forget to keep this last, otherwise there will be a gcc warning
+ {
+ setGetter("normal", std::bind(&TestClass::getNormal, this));
+ setSetter("normal", std::bind(&TestClass::setNormal, this, std::bind(SurgSim::Framework::convert<int>,
+ std::placeholders::_1)));
+
+ SURGSIM_ADD_RW_PROPERTY(TestClass, double, readWrite, getReadWrite, setReadWrite);
+ SURGSIM_ADD_RW_PROPERTY(TestClass, std::shared_ptr<int>, sharedPtr, getSharedPtr, setSharedPtr);
+
+ SURGSIM_ADD_RO_PROPERTY(TestClass, int, readOnly, getReadOnly);
+
+ SURGSIM_ADD_RW_PROPERTY(TestClass, double, privateProperty, getPrivateProperty, setPrivateProperty);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TestClass, float, serializableProperty,
+ getSerializableProperty, setSerializableProperty);
+
+ SURGSIM_ADD_RO_PROPERTY(TestClass, int, virtualProperty, getVirtualProperty);
+ SURGSIM_ADD_RO_PROPERTY(TestClass, int, overriddenProperty, getReadWrite);
+
+ }
+
+ int normal;
+ double readWrite;
+ int readOnly;
+
+
+ std::shared_ptr<int> sharedPtr;
+
+ float serializableProperty;
+
+ double overloadedValue;
+
+ int virtualProperty;
+
+ int getNormal()
+ {
+ return normal;
+ }
+ void setNormal(int val)
+ {
+ normal = val;
+ }
+
+ double getReadWrite()
+ {
+ return readWrite;
+ }
+ void setReadWrite(double val)
+ {
+ readWrite = val;
+ }
+
+ std::shared_ptr<int> getSharedPtr()
+ {
+ return sharedPtr;
+ }
+ void setSharedPtr(std::shared_ptr<int> val)
+ {
+ sharedPtr = val;
+ }
+
+ int getReadOnly()
+ {
+ return readOnly;
+ }
+
+ double getPrivateProperty() const
+ {
+ return privateProperty;
+ }
+ void setPrivateProperty(double val)
+ {
+ privateProperty = val;
+ }
+
+ float getSerializableProperty() const
+ {
+ return serializableProperty;
+ }
+ void setSerializableProperty(float val)
+ {
+ serializableProperty = val;
+ }
+
+ void getOverloadedFunction(double* x) const {}
+ double getOverloadedFunction() const
+ {
+ return overloadedValue;
+ }
+
+ void setOverloadedFunction(double x, double y) {}
+ void setOverloadedFunction(const double& x)
+ {
+ overloadedValue = x;
+ }
+
+ virtual int getVirtualProperty()
+ {
+ return virtualProperty;
+ }
+
+private:
+
+ double privateProperty;
+};
+
+class DerivedTestClass : public TestClass
+{
+
+public:
+
+ DerivedTestClass() :
+ TestClass(),
+ otherValue(400)
+ {
+ // Override the accessor from the base class by changing the entry in the function table
+ SURGSIM_ADD_RO_PROPERTY(DerivedTestClass, int, overriddenProperty, getOtherValue);
+ }
+
+ int otherValue;
+
+ int getOtherValue()
+ {
+ return otherValue;
+ }
+
+ // Overrides the accessor, this tests if the virtual function resolution works on the function pointer
+ virtual int getVirtualProperty() override
+ {
+ return otherValue;
+ }
+
+};
+}
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+TEST(AccessibleTest, GetterTest)
+{
+ TestClass t;
+ t.normal = 5;
+
+ int receiver = -1;
+
+ EXPECT_EQ(5, boost::any_cast<int>(t.getValue("normal")));
+ EXPECT_EQ(5, t.getValue<int>("normal"));
+ EXPECT_TRUE(t.getValue<int>("normal", &receiver));
+ EXPECT_EQ(5, receiver);
+
+ /// Response to fetching value that does not exist
+ EXPECT_ANY_THROW(t.getValue("xxx"));
+ EXPECT_ANY_THROW(t.getValue<int>("xxx"));
+ receiver = -1;
+ EXPECT_FALSE(t.getValue<int>("xxx", &receiver));
+ EXPECT_EQ(-1, receiver);
+
+ /// Response to trying to fetch an type that can't be converted
+ std::string string;
+ EXPECT_ANY_THROW(t.getValue<std::string>("normal"));
+ EXPECT_FALSE(t.getValue<std::string>("normal", &string));
+
+}
+
+TEST(AccessibleTest, SetterTest)
+{
+ TestClass t;
+ t.normal = 0;
+
+ t.setValue("normal", 4);
+ EXPECT_EQ(4, t.getNormal());
+ EXPECT_ANY_THROW(t.setValue("xxxx", 666.66));
+}
+
+TEST(AccessibleTest, TransferTest)
+{
+ TestClass a, b;
+ a.normal = 100;
+ b.normal = 0;
+
+ b.setValue("normal", a.getValue("normal"));
+
+ EXPECT_EQ(a.normal, b.normal);
+}
+
+TEST(AccessibleTest, ReadWriteMacroTest)
+{
+ TestClass a;
+ a.readWrite = 100.0;
+
+ EXPECT_EQ(a.readWrite, boost::any_cast<double>(a.getValue("readWrite")));
+ a.setValue("readWrite", 50.0);
+ EXPECT_EQ(50.0, a.readWrite);
+}
+
+TEST(AccessibleTest, ReadOnlyMacroTest)
+{
+ TestClass a;
+ a.readOnly = 200;
+
+ EXPECT_EQ(a.readOnly, boost::any_cast<int>(a.getValue("readOnly")));
+
+ EXPECT_ANY_THROW(a.setValue("readOnly", 100));
+}
+
+TEST(AccessibleTest, TemplateFunction)
+{
+ TestClass a;
+ a.normal = 10;
+ a.readWrite = 100.0;
+
+ // Parameter Deduction
+ int aDotNormal = 123;
+ double aDotReadWrite = 456;
+ EXPECT_TRUE(a.getValue("normal", &aDotNormal));
+ EXPECT_EQ(10, aDotNormal);
+ EXPECT_TRUE(a.getValue("readWrite", &aDotReadWrite));
+ EXPECT_EQ(100.0, aDotReadWrite);
+
+ EXPECT_FALSE(a.getValue("xxxx", &aDotNormal));
+
+ double* noValue = nullptr;
+
+ EXPECT_FALSE(a.getValue("normal", noValue));
+}
+
+TEST(AccessibleTest, Privates)
+{
+ TestClass a;
+
+ EXPECT_EQ(a.getPrivateProperty(), boost::any_cast<double>(a.getValue("privateProperty")));
+ EXPECT_NO_THROW(a.setValue("privateProperty", 123.456));
+ EXPECT_NEAR(123.456, boost::any_cast<double>(a.getValue("privateProperty")), 1e10);
+ EXPECT_NEAR(a.getPrivateProperty(), boost::any_cast<double>(a.getValue("privateProperty")), 1e10);
+}
+
+TEST(AccessibleTest, SharedPointer)
+{
+ TestClass a;
+ std::shared_ptr<int> x = std::make_shared<int>(5);
+ std::shared_ptr<int> y;
+
+ y = boost::any_cast<std::shared_ptr<int>>(a.getValue("sharedPtr"));
+ EXPECT_EQ(4, *y);
+
+ a.setValue("sharedPtr", x);
+ y = boost::any_cast<std::shared_ptr<int>>(a.getValue("sharedPtr"));
+ EXPECT_EQ(5, *y);
+}
+
+// We want to check whether we can forward a setter and a getter from one class to the other,
+// i.e. a.setValue("Forwarded") should actually change a specific value in b via Properties
+TEST(AccessibleTest, Forwarding)
+{
+ TestClass a;
+ TestClass b;
+ b.normal = 543;
+
+ ASSERT_NO_THROW(a.forwardProperty("forwarded", b, "normal"));
+
+ EXPECT_EQ(543, a.getValue<int>("forwarded"));
+ ASSERT_NO_THROW(a.setValue("forwarded", 345));
+ EXPECT_EQ(345, b.normal);
+}
+
+TEST(AccessibleTest, RemoveAccessors)
+{
+ TestClass a;
+
+ EXPECT_NO_THROW(a.getValue("readWrite"));
+ EXPECT_NO_THROW(a.setValue("readWrite", 2.0));
+
+ a.removeAccessors("readWrite");
+
+ EXPECT_ANY_THROW(a.getValue("readWrite"));
+ EXPECT_ANY_THROW(a.setValue("readWrite", 2.0));
+}
+
+TEST(AccessibleTests, VirtualFunctionTest)
+{
+ std::shared_ptr<DerivedTestClass> derived = std::make_shared<DerivedTestClass>();
+ std::shared_ptr<TestClass> base = std::dynamic_pointer_cast<TestClass>(derived);
+
+ EXPECT_EQ(derived->otherValue, derived->getValue<int>("virtualProperty"));
+ EXPECT_EQ(derived->otherValue, derived->getValue<int>("overriddenProperty"));
+
+ EXPECT_EQ(derived->otherValue, base->getValue<int>("virtualProperty"));
+ EXPECT_EQ(derived->otherValue, base->getValue<int>("overriddenProperty"));
+
+}
+
+TEST(AccessibleTest, ConvertDoubleToFloat)
+{
+ // Values don't matter only care for them to be filled
+ SurgSim::Math::Matrix44d sourceDouble;
+ sourceDouble <<
+ 1.0 / 2.0, 1.0 / 3.0, 1.0 / 4.0, 1.0 / 5.0,
+ 1.0 / 6.0, 1.0 / 7.0, 1.0 / 8.0, 1.0 / 9.0,
+ 1.0 / 10.0, 1.0 / 11.0, 1.0 / 12.0, 1.0 / 13.0,
+ 1.0 / 14.0, 1.0 / 15.0, 1.0 / 16.0, 1.0 / 17.0;
+
+ SurgSim::Math::Matrix44f sourceFloat;
+ sourceFloat <<
+ 1.0f / 2.0f, 1.0f / 3.0f, 1.0f / 4.0f, 1.0f / 5.0f,
+ 1.0f / 6.0f, 1.0f / 7.0f, 1.0f / 8.0f, 1.0f / 9.0f,
+ 1.0f / 10.0f, 1.0f / 11.0f, 1.0f / 12.0f, 1.0f / 13.0f,
+ 1.0f / 14.0f, 1.0f / 15.0f, 1.0f / 16.0f, 1.0f / 17.0f;
+
+ SurgSim::Math::Matrix44f target;
+
+ ASSERT_NO_THROW({convert<SurgSim::Math::Matrix44f>(sourceDouble);});
+ target = convert<SurgSim::Math::Matrix44f>(sourceDouble);
+ SurgSim::Math::Matrix44f doubleToFloat = sourceDouble.cast<float>();
+ EXPECT_TRUE(target.isApprox(doubleToFloat));
+
+ ASSERT_NO_THROW({convert<SurgSim::Math::Matrix44f>(sourceFloat);});
+ target = convert<SurgSim::Math::Matrix44f>(sourceFloat);
+ EXPECT_TRUE(target.isApprox(sourceFloat));
+}
+
+TEST(AccessibleTests, Serialize)
+{
+ TestClass a;
+ a.serializableProperty = 100;
+
+ YAML::Node node = a.encode();
+
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(100, node["serializableProperty"].as<int>());
+
+ node["serializableProperty"] = 50;
+ EXPECT_NO_THROW(a.decode(node));
+ EXPECT_EQ(50, a.serializableProperty);
+}
+
+class MultipleValuesClass : public Accessible
+{
+public:
+ MultipleValuesClass() : a("invalid"), b("invalid"), c("invalid")
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(MultipleValuesClass, std::string, a, getA, setA);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(MultipleValuesClass, std::string, b, getB, setB);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(MultipleValuesClass, std::string, c, getC, setC);
+ }
+
+ std::string a;
+ std::string getA() const
+ {
+ return a;
+ }
+ void setA(std::string val)
+ {
+ a = val;
+ }
+
+ std::string b;
+ std::string getB() const
+ {
+ return b;
+ }
+ void setB(std::string val)
+ {
+ b = val;
+ }
+
+ std::string c;
+ std::string getC() const
+ {
+ return c;
+ }
+ void setC(std::string val)
+ {
+ c = val;
+ }
+};
+
+TEST(AccessibleTests, MultipleValues)
+{
+ YAML::Node newValues;
+ newValues["xxx"] = "invalid";
+ newValues["a"] = "a";
+ newValues["b"] = "b";
+
+ MultipleValuesClass test;
+ test.decode(newValues);
+
+ EXPECT_EQ(test.a, "a");
+ EXPECT_EQ(test.b, "b");
+ EXPECT_EQ(test.c, "invalid");
+
+ YAML::Node encodedValues = test.encode();
+
+ EXPECT_EQ("a", encodedValues["a"].as<std::string>());
+ EXPECT_EQ("b", encodedValues["b"].as<std::string>());
+ EXPECT_EQ("invalid", encodedValues["c"].as<std::string>());
+}
+
+
+
+
+}; // namespace Framework
+}; // namespace SurgSim
diff --git a/SurgSim/Framework/UnitTests/AccessibleTypeTests.cpp b/SurgSim/Framework/UnitTests/AccessibleTypeTests.cpp
new file mode 100644
index 0000000..a509bf6
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/AccessibleTypeTests.cpp
@@ -0,0 +1,251 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+
+// We need ALL of the math types
+using namespace SurgSim::Math; //NOLINT
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+template <class T>
+class BaseTest : public testing::Test
+{
+public:
+ typedef T Scalar;
+};
+
+template <class T>
+class ScalarTest : public BaseTest<T>
+{
+};
+
+typedef ::testing::Types<int, size_t, double, float> ScalarTypes;
+TYPED_TEST_CASE(ScalarTest, ScalarTypes);
+
+template <class T>
+class VectorTest: public BaseTest<typename T::Scalar>
+{
+public:
+ typedef T Vector;
+};
+
+typedef ::testing::Types<SurgSim::Math::Vector2d,
+ SurgSim::Math::Vector2f,
+ SurgSim::Math::Vector3d,
+ SurgSim::Math::Vector3f,
+ SurgSim::Math::Vector4d,
+ SurgSim::Math::Vector4f> VectorTypes;
+
+TYPED_TEST_CASE(VectorTest, VectorTypes);
+
+template <class T>
+class QuaternionTest : public BaseTest<typename T::Scalar>
+{
+public:
+ typedef T Quaternion;
+};
+
+typedef ::testing::Types<SurgSim::Math::Quaterniond, SurgSim::Math::Quaternionf> QuaternionTypes;
+TYPED_TEST_CASE(QuaternionTest, QuaternionTypes);
+
+template <class T>
+class RigidTransformTest : public BaseTest<typename T::Scalar>
+{
+public:
+ typedef T RigidTransform;
+};
+typedef ::testing::Types <SurgSim::Math::RigidTransform3f, SurgSim::Math::RigidTransform3d> RigidTransformTypes;
+
+TYPED_TEST_CASE(RigidTransformTest, RigidTransformTypes);
+
+template <class T>
+class Testable : public Accessible
+{
+public:
+ Testable()
+ {
+ setAccessors("property",
+ std::bind(&Testable::getProperty, this),
+ std::bind(&Testable::setProperty, this,
+ std::bind(SurgSim::Framework::convert<T>, std::placeholders::_1)));
+ setSerializable("property",
+ std::bind(&YAML::convert<T>::encode, std::bind(&Testable::getProperty, this)),
+ std::bind(&Testable::setProperty, this, std::bind(&YAML::Node::as<T>, std::placeholders::_1)));
+ }
+
+ T property;
+
+ void setProperty(const T& value)
+ {
+ property = value;
+ }
+ T getProperty()
+ {
+ return property;
+ }
+};
+
+template <typename T>
+std::pair<T, T> testProperty(const T& a, const T& b)
+{
+ Testable<T> test;
+ std::pair<T, T> result;
+ test.property = a;
+
+ test.setValue("property", b);
+ result.first = test.property;
+
+ test.getValue("property", &result.second);
+
+ return result;
+}
+
+template <typename T>
+std::pair<T, T> testEncodeDecode(const T& a, const T& b)
+{
+ Testable<T> test;
+ std::pair<T, T> result;
+ test.property = a;
+
+ YAML::Node node;
+ node["property"] = b;
+ test.decode(node);
+
+ result.first = test.property;
+
+ YAML::Node resultNode = test.encode();
+ result.second = resultNode["property"].as<T>();
+
+ return result;
+}
+
+TYPED_TEST(ScalarTest, Accessible)
+{
+ typedef typename TestFixture::Scalar Scalar;
+
+ Scalar initialValue = static_cast<Scalar>(1);
+ Scalar newValue = static_cast<Scalar>(2);
+
+ std::pair<Scalar, Scalar> result = testProperty(initialValue, newValue);
+ EXPECT_NEAR(static_cast<double>(newValue), static_cast<double>(result.first), 1e-6);
+ EXPECT_NEAR(static_cast<double>(newValue), static_cast<double>(result.second), 1e-6);
+
+ result = testEncodeDecode(initialValue, newValue);
+ EXPECT_NEAR(static_cast<double>(newValue), static_cast<double>(result.first), 1e-6);
+ EXPECT_NEAR(static_cast<double>(newValue), static_cast<double>(result.second), 1e-6);
+}
+
+
+TYPED_TEST(VectorTest, Accessible)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ typedef typename TestFixture::Vector Vector;
+
+ Vector initialValue;
+ Vector newValue;
+
+ for (int i = 0; i < initialValue.size(); ++i)
+ {
+ initialValue[i] = static_cast<Scalar>(i);
+ newValue[i] = static_cast<Scalar>(i * 2);
+ }
+
+ std::pair<Vector, Vector> result = testProperty<TypeParam >(initialValue, newValue);
+ EXPECT_TRUE(newValue.isApprox(result.first));
+ EXPECT_TRUE(newValue.isApprox(result.second));
+
+ result = testEncodeDecode<TypeParam>(initialValue, newValue);
+ EXPECT_TRUE(newValue.isApprox(result.first));
+ EXPECT_TRUE(newValue.isApprox(result.second));
+}
+
+TYPED_TEST(QuaternionTest, Accessible)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ typedef typename TestFixture::Quaternion Quaternion;
+
+ Quaternion initialValue(static_cast<Scalar>(1.0),
+ static_cast<Scalar>(2.0),
+ static_cast<Scalar>(3.0),
+ static_cast<Scalar>(4.0));
+ Quaternion newValue(static_cast<Scalar>(5.0),
+ static_cast<Scalar>(6.0),
+ static_cast<Scalar>(7.0),
+ static_cast<Scalar>(8.0));
+
+ initialValue.normalize();
+ newValue.normalize();
+
+ std::pair<Quaternion, Quaternion> result = testProperty<TypeParam >(initialValue, newValue);
+ EXPECT_TRUE(newValue.isApprox(result.first));
+ EXPECT_TRUE(newValue.isApprox(result.second));
+
+ result = testEncodeDecode<Quaternion>(initialValue, newValue);
+ EXPECT_TRUE(newValue.isApprox(result.first));
+ EXPECT_TRUE(newValue.isApprox(result.second));
+}
+
+TYPED_TEST(RigidTransformTest, Accessible)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ typedef typename TestFixture::RigidTransform RigidTransform;
+ typedef Eigen::Quaternion<Scalar> Quaternion;
+ typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+
+ typename RigidTransform::MatrixType initialMatrix;
+ typename RigidTransform::MatrixType newMatrix;
+
+ RigidTransform initialValue;
+ {
+ Quaternion quaternion(1.0, 2.0, 3.0, 4.0);
+ quaternion.normalize();
+ Vector3 translation(1.0, 2.0, 3.0);
+
+ initialValue = SurgSim::Math::makeRigidTransform(quaternion, translation);
+ }
+
+ RigidTransform newValue;
+ {
+ Quaternion quaternion(4.0, 3.0, 2.0, 1.0);
+ quaternion.normalize();
+ Vector3 translation(3.0, 2.0, 1.0);
+
+ newValue = SurgSim::Math::makeRigidTransform(quaternion, translation);
+ }
+
+
+ std::pair<RigidTransform, RigidTransform> result = testProperty<RigidTransform >(initialValue, newValue);
+ EXPECT_TRUE(newValue.isApprox(result.first));
+ EXPECT_TRUE(newValue.isApprox(result.second));
+
+ result = testEncodeDecode<RigidTransform>(initialValue, newValue);
+ EXPECT_TRUE(newValue.isApprox(result.first));
+ EXPECT_TRUE(newValue.isApprox(result.second));
+}
+
+}
+}
diff --git a/SurgSim/Framework/UnitTests/ApplicationDataTest.cpp b/SurgSim/Framework/UnitTests/ApplicationDataTest.cpp
new file mode 100644
index 0000000..916af82
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/ApplicationDataTest.cpp
@@ -0,0 +1,254 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <vector>
+#include <algorithm>
+
+#include "SurgSim/Framework/ApplicationData.h"
+
+#include <boost/filesystem.hpp>
+
+using SurgSim::Framework::ApplicationData;
+using boost::filesystem::path;
+
+namespace std
+{
+
+::std::ostream& operator<<(::std::ostream& os, const vector<std::string>& content)
+{
+ os << "(";
+
+ for (size_t i = 0; i < content.size(); ++i)
+ {
+ if (i > 0)
+ {
+ os << ", ";
+ }
+ os << "\"" << content[i] << "\"";
+ }
+ os << ")";
+ return os;
+}
+
+}
+
+::testing::AssertionResult isContained(const std::string& expected,
+ const std::vector<std::string>& argument)
+{
+ if (std::find(argument.cbegin(), argument.cend(), expected) != argument.cend())
+ {
+ return ::testing::AssertionSuccess();
+ }
+ else
+ {
+ return ::testing::AssertionFailure() << "\"" << expected << "\"" << " not contained in " << argument;
+ }
+}
+
+::testing::AssertionResult fileIsFoundCorrectly(
+ const ApplicationData& data,
+ const std::string& searchFileName,
+ const std::string& expectedDirectoryName)
+{
+ // First file should be there and in directory1
+ std::string fileName = data.findFile(searchFileName);
+ boost::filesystem::path filePath(fileName);
+
+ if (! filePath.is_absolute())
+ {
+ return ::testing::AssertionFailure() << "Result not absolute path <" << fileName <<
+ "> expected " << boost::filesystem::canonical(filePath);
+ }
+ if (! boost::filesystem::exists(filePath))
+ {
+ return ::testing::AssertionFailure() << "Result does not exist <" << fileName << ">";
+ }
+ if (fileName != filePath.make_preferred().string())
+ {
+ return ::testing::AssertionFailure() << "Result not system format path " << fileName <<
+ " expected " << filePath.make_preferred().string();
+ }
+ if (boost::filesystem::path(searchFileName).filename() != filePath.filename())
+ {
+ return ::testing::AssertionFailure() << "Expected " << searchFileName <<
+ " Result " << filePath.filename();
+ }
+ if (fileName.find(expectedDirectoryName) == std::string::npos)
+ {
+ return ::testing::AssertionFailure() << "Expected the file to be in subdirectory " <<
+ expectedDirectoryName << " but is not " << fileName;
+ }
+ return ::testing::AssertionSuccess();
+}
+
+TEST(ApplicationDataTest, InitTest)
+{
+ std::vector<std::string> paths;
+ ASSERT_NO_THROW({ApplicationData appData(paths);});
+}
+
+TEST(ApplicationDataTest, OnlyValidPaths)
+{
+ // Expects to find "Data" correctly ...
+ ASSERT_TRUE(boost::filesystem::exists("Data")) <<
+ "WARNING: Data directory could not be found, possibly you are running the "
+ "test from the wrong directory, some or other following tests will fail. " << std::endl <<
+ "FIX THIS TEST FIRST!";
+
+ std::vector<std::string> paths;
+ paths.push_back("Data");
+ paths.push_back("Data");
+ paths.push_back("invalid");
+ paths.push_back("invalid");
+ paths.push_back("Data/ApplicationDataTest/Directory1");
+ // Path with backslashes should get ignored
+ paths.push_back("Data\\ApplicationDataTest\\Directory2");
+
+ ApplicationData data(paths);
+ ASSERT_EQ(2u, data.getPaths().size());
+}
+
+TEST(ApplicationDataTest, GetPathsTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ std::string path1 = "Data/ApplicationDataTest/Directory1";
+ std::string path2 = "Data/ApplicationDataTest/Directory2";
+ paths.push_back(path1);
+ paths.push_back(path2);
+ ApplicationData data(paths);
+
+ ASSERT_EQ(2u, data.getPaths().size());
+
+ paths = data.getPaths();
+
+ EXPECT_TRUE(boost::filesystem::equivalent(
+ path(path1),
+ path(paths[0]))) << path1 << " not considered equivalent to " << paths[0];
+ EXPECT_TRUE(boost::filesystem::equivalent(
+ path(path2),
+ path(paths[1]))) << path2 << " not considered equivalent to " << paths[1];
+}
+
+TEST(ApplicationDataTest, FindFileTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/ApplicationDataTest/Directory1");
+ paths.push_back("Data/ApplicationDataTest/Directory2");
+ ApplicationData data(paths);
+
+ boost::filesystem::path filePath;
+
+ ASSERT_EQ(2u, data.getPaths().size());
+ EXPECT_TRUE(fileIsFoundCorrectly(data, "uniqueFile1.txt", "Directory1"));
+ EXPECT_TRUE(fileIsFoundCorrectly(data, "uniqueFile2.txt", "Directory2"));
+ EXPECT_TRUE(fileIsFoundCorrectly(data, "duplicatedFile.txt", "Directory1"));
+
+ EXPECT_EQ("", data.findFile("missingFile.txt"));
+ EXPECT_EQ("", data.findFile(""));
+}
+
+TEST(ApplicationDataTest, TryFindFileTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/ApplicationDataTest/Directory1");
+ paths.push_back("Data/ApplicationDataTest/Directory2");
+ ApplicationData data(paths);
+
+ std::string fileName;
+ boost::filesystem::path filePath;
+
+ EXPECT_TRUE(data.tryFindFile("uniqueFile1.txt", &fileName));
+ EXPECT_EQ(data.findFile("uniqueFile1.txt"), fileName);
+
+ EXPECT_TRUE(data.tryFindFile("duplicatedFile.txt", &fileName));
+ EXPECT_EQ(data.findFile("duplicatedFile.txt"), fileName);
+
+ fileName = "Should Not Change";
+
+ EXPECT_FALSE(data.tryFindFile("missingFile.txt", &fileName));
+ EXPECT_EQ("Should Not Change", fileName);
+}
+
+
+TEST(ApplicationDataTest, DirectorywithSpaceTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::string path = "Data/ApplicationDataTest/Test Directory/uniqueFile.txt";
+ ApplicationData data(path);
+
+ EXPECT_TRUE(fileIsFoundCorrectly(data, "uniqueFile.txt", "Test Directory"));
+}
+
+// We don't accept anything that contains backslashes ...
+TEST(ApplicationDataTest, MessedUpSlashesTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data\\ApplicationDataTest/Directory1");
+ paths.push_back("Data/ApplicationDataTest\\Directory2");
+ ApplicationData data(paths);
+
+ ASSERT_EQ(0u, data.getPaths().size());
+}
+
+TEST(ApplicationDataTest, DeepPathTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/ApplicationDataTest");
+ ApplicationData data(paths);
+
+ EXPECT_TRUE(fileIsFoundCorrectly(data, "Directory1/uniqueFile1.txt", "Directory1"));
+ EXPECT_TRUE(fileIsFoundCorrectly(data, "Directory2/uniqueFile2.txt", "Directory2"));
+}
+
+TEST(ApplicationDataTest, InitFromFile)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ ASSERT_ANY_THROW({ApplicationData data("nonexistingFile.xxx");});
+ ASSERT_NO_THROW({ApplicationData data("Data/ApplicationDataTest/testFile1.txt");});
+
+ ApplicationData data("Data/ApplicationDataTest/testFile1.txt");
+
+ ASSERT_EQ(2u, data.getPaths().size());
+ EXPECT_TRUE(fileIsFoundCorrectly(data, "uniqueFile1.txt", "Directory1"));
+ EXPECT_TRUE(fileIsFoundCorrectly(data, "uniqueFile2.txt", "Directory2"));
+ EXPECT_TRUE(fileIsFoundCorrectly(data, "duplicatedFile.txt", "Directory1"));
+ EXPECT_EQ("", data.findFile("missingFile.txt"));
+}
+
+TEST(ApplicationDataTest, IsValidFilenameTest)
+{
+ ApplicationData data("Data/ApplicationDataTest/testFile1.txt");
+
+ std::string validFileName("123.txt");
+ std::string invalidFileName1("");
+ std::string invalidFileName2("\\invalid\\123.txt");
+
+ EXPECT_TRUE(data.isValidFilename(validFileName));
+ EXPECT_FALSE(data.isValidFilename(invalidFileName1));
+ EXPECT_FALSE(data.isValidFilename(invalidFileName2));
+}
diff --git a/SurgSim/Framework/UnitTests/AssertTest.cpp b/SurgSim/Framework/UnitTests/AssertTest.cpp
new file mode 100644
index 0000000..238f0e0
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/AssertTest.cpp
@@ -0,0 +1,202 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 SURGSIM_ASSERT() and SURGSIM_FAILURE().
+
+#include <gtest/gtest.h>
+#include "SurgSim/Framework/Assert.h"
+
+
+class MockOutput : public SurgSim::Framework::LogOutput
+{
+public:
+ MockOutput()
+ {
+ }
+
+ ~MockOutput()
+ {
+ }
+
+ bool writeMessage(const std::string& message)
+ {
+ logMessage = message;
+ return true;
+ }
+ void reset()
+ {
+ logMessage = "";
+ }
+
+ std::string logMessage;
+};
+
+class AssertTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ logOutput = std::make_shared<MockOutput>();
+ testLogger = SurgSim::Framework::Logger::getLogger("test");
+ testLogger->setOutput(logOutput);
+ // testLogger will be used for assertions in most tests, due to the definition of SURGSIM_ASSERT_LOGGER below.
+ savedCallback = SurgSim::Framework::AssertMessage::getFailureCallback();
+ }
+
+ void TearDown()
+ {
+ SurgSim::Framework::AssertMessage::setFailureCallback(savedCallback);
+ testLogger.reset();
+ logOutput.reset();
+ }
+
+ std::shared_ptr<MockOutput> logOutput;
+ std::shared_ptr<SurgSim::Framework::Logger> testLogger;
+ SurgSim::Framework::AssertMessage::DeathCallback savedCallback;
+};
+
+typedef AssertTest AssertDeathTest;
+
+
+TEST_F(AssertTest, DefaultAssertLogger)
+{
+ EXPECT_THROW(SURGSIM_ASSERT(1 == 2) << "extra information would go here", SurgSim::Framework::AssertionFailure);
+}
+
+#undef SURGSIM_ASSERT_LOGGER // override the default definition
+#define SURGSIM_ASSERT_LOGGER testLogger // defined in the test fixture, above
+
+inline bool stringContains(const std::string& string, const std::string& fragment)
+{
+ return string.find(fragment) != std::string::npos;
+}
+
+static int numIgnoredAssertions = 0;
+
+static void ignoreAssertionKeepGoing(const std::string& message)
+{
+ ++numIgnoredAssertions;
+}
+
+TEST_F(AssertTest, Assertion)
+{
+ logOutput->reset();
+ EXPECT_THROW(SURGSIM_ASSERT(1 == 2) << "extra information would go here", SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(stringContains(logOutput->logMessage, "1 == 2")) <<
+ "message: '" << logOutput->logMessage << "'";
+ EXPECT_TRUE(stringContains(logOutput->logMessage, "AssertTest.cpp")) <<
+ "message: '" << logOutput->logMessage << "'";
+ EXPECT_TRUE(stringContains(logOutput->logMessage, "extra information")) <<
+ "message: '" << logOutput->logMessage << "'";
+
+ logOutput->reset();
+ EXPECT_NO_THROW({SURGSIM_ASSERT(3 == 3) << "extra information would go here";});
+ EXPECT_EQ("", logOutput->logMessage) <<
+ "message: '" << logOutput->logMessage << "'";
+}
+
+TEST_F(AssertTest, Failure)
+{
+ logOutput->reset();
+ EXPECT_THROW(SURGSIM_FAILURE() << "extra information would go here", SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(stringContains(logOutput->logMessage, "AssertTest.cpp")) <<
+ "message: '" << logOutput->logMessage << "'";
+ EXPECT_TRUE(stringContains(logOutput->logMessage, "extra information")) <<
+ "message: '" << logOutput->logMessage << "'";
+}
+
+TEST_F(AssertTest, Manipulators)
+{
+ logOutput->reset();
+ EXPECT_THROW(SURGSIM_ASSERT(1 == 2) << "aAa" << std::endl << "bBb", SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(stringContains(logOutput->logMessage, "aAa\nbBb") ||
+ stringContains(logOutput->logMessage, "aAa\r\n\bBb")) <<
+ "message: '" << logOutput->logMessage << "'";
+
+ logOutput->reset();
+ EXPECT_THROW(SURGSIM_ASSERT(1 == 2) << "[" << std::hex << std::setw(5) << std::setfill('0') << 0x1234 << "]",
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(stringContains(logOutput->logMessage, "[01234]")) <<
+ "message: '" << logOutput->logMessage << "'";
+
+ logOutput->reset();
+ // The next message should not show any evidence of previous manipulators.
+ EXPECT_THROW(SURGSIM_ASSERT(1 == 2) << "[" << 987 << "]", SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(stringContains(logOutput->logMessage, "[987]")) <<
+ "message: '" << logOutput->logMessage << "'";
+}
+
+TEST_F(AssertTest, ExceptionBehavior)
+{
+ logOutput->reset();
+
+ SurgSim::Framework::AssertMessage::setFailureBehaviorToThrow();
+ EXPECT_THROW(SURGSIM_ASSERT(1 == 2) << "extra information would go here", SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW({SURGSIM_ASSERT(3 == 3) << "extra information would go here";});
+
+ EXPECT_THROW(SURGSIM_FAILURE() << "extra information would go here", SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(AssertTest, Callback)
+{
+ logOutput->reset();
+
+ typedef SurgSim::Framework::AssertMessage::DeathCallback CallbackType;
+ const CallbackType defaultCallback = SurgSim::Framework::AssertMessage::getFailureCallback();
+
+ SurgSim::Framework::AssertMessage::setFailureBehaviorToThrow();
+ const CallbackType throwCallback = SurgSim::Framework::AssertMessage::getFailureCallback();
+ EXPECT_EQ(throwCallback, defaultCallback);
+
+ SurgSim::Framework::AssertMessage::setFailureCallback(ignoreAssertionKeepGoing);
+ EXPECT_EQ(static_cast<CallbackType>(ignoreAssertionKeepGoing),
+ SurgSim::Framework::AssertMessage::getFailureCallback());
+ numIgnoredAssertions = 0;
+ EXPECT_NO_THROW(SURGSIM_ASSERT(1 == 2) << "extra information would go here");
+ EXPECT_EQ(1, numIgnoredAssertions);
+ EXPECT_NO_THROW({SURGSIM_ASSERT(3 == 3) << "extra information would go here";});
+ EXPECT_EQ(1, numIgnoredAssertions);
+ EXPECT_NO_THROW(SURGSIM_FAILURE() << "extra information would go here");
+ EXPECT_EQ(2, numIgnoredAssertions);
+
+ SurgSim::Framework::AssertMessage::setFailureBehaviorToThrow();
+ EXPECT_EQ(throwCallback, SurgSim::Framework::AssertMessage::getFailureCallback());
+ EXPECT_THROW(SURGSIM_ASSERT(1 == 2) << "extra information would go here", SurgSim::Framework::AssertionFailure);
+ EXPECT_EQ(2, numIgnoredAssertions);
+}
+
+TEST_F(AssertDeathTest, DebuggerBehaviorAssertFailed)
+{
+ logOutput->reset();
+ SurgSim::Framework::AssertMessage::setFailureBehaviorToDeath();
+ // The assertion should die with no output to stdout.
+ ASSERT_DEATH_IF_SUPPORTED({SURGSIM_ASSERT(1 == 2) << "extra information would go here";}, "^$");
+}
+
+TEST_F(AssertDeathTest, DebuggerBehaviorAssertSucceded)
+{
+ logOutput->reset();
+ SurgSim::Framework::AssertMessage::setFailureBehaviorToDeath();
+ EXPECT_NO_THROW({SURGSIM_ASSERT(3 == 3) << "extra information would go here";});
+}
+
+TEST_F(AssertDeathTest, DebuggerBehaviorFailure)
+{
+ logOutput->reset();
+ SurgSim::Framework::AssertMessage::setFailureBehaviorToDeath();
+ // The assertion should die with no output to stdout.
+ ASSERT_DEATH_IF_SUPPORTED({SURGSIM_FAILURE() << "extra information would go here";}, "^$");
+}
diff --git a/SurgSim/Framework/UnitTests/AssetTests.cpp b/SurgSim/Framework/UnitTests/AssetTests.cpp
new file mode 100644
index 0000000..3c1de38
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/AssetTests.cpp
@@ -0,0 +1,101 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 SURGSIM_ASSERT() and SURGSIM_FAILURE().
+
+#include <fstream>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Framework/Runtime.h"
+
+class MockAsset: public SurgSim::Framework::Asset
+{
+public:
+ MockAsset() {}
+ ~MockAsset() {}
+
+ virtual bool doLoad(const std::string& fileName)
+ {
+ bool result = false;
+ std::ifstream in(fileName);
+
+ if (in.is_open())
+ {
+ result = true;
+ }
+ return result;
+ }
+};
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+class AssetTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ }
+
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime;
+};
+
+TEST_F(AssetTest, InitTest)
+{
+ EXPECT_NO_THROW(MockAsset t);
+
+ MockAsset test;
+ EXPECT_EQ("", test.getFileName());
+}
+
+TEST_F(AssetTest, LoadAndFileNameTest)
+{
+ MockAsset test;
+
+ // HW-JULY-16, 2014
+ // Since Asset::load(const std::string&) simply delegates all calls to its overloading counterpart with
+ // ApplicationData from SurgSim::Framwork::Runtime,
+ // tests below actually test Asset::load(const std::string& fileName, const ApplicationData& data);
+ // No need to duplicate tests. Update when those two functions diverge.
+
+ // Call 'Asset::load()' with empty file name will fail.
+ EXPECT_ANY_THROW(test.load(""));
+ EXPECT_EQ("", test.getFileName());
+
+ // Loading nonexist file will fail, but the internal file name recorded by Asset will be updated.
+ std::string invalidFileName("Non-exist-file");
+ EXPECT_ANY_THROW(test.load(invalidFileName));
+ EXPECT_EQ(invalidFileName, test.getFileName());
+
+ // Loading existing file should success and internal file name recorded by Asset will be updated.
+ std::string validDummyFile("AssetTestData/DummyFile.txt");
+ EXPECT_NO_THROW(test.load(validDummyFile));
+ EXPECT_EQ(validDummyFile, test.getFileName());
+
+ // Loading same existing file again should success and internal file name will be the same.
+ EXPECT_NO_THROW(test.load(validDummyFile));
+ EXPECT_EQ(validDummyFile, test.getFileName());
+}
+
+}; // Framework
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Framework/UnitTests/BarrierTest.cpp b/SurgSim/Framework/UnitTests/BarrierTest.cpp
new file mode 100644
index 0000000..3e01c82
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/BarrierTest.cpp
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Original barrier.hpp
+// Copyright (C) 2002-2003
+// David Moore, William E. Kempf
+// Copyright (C) 2007-8 Anthony Williams
+//
+// Distributed under the Boost Software License, Version 1.0. (See accompanying
+// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
+
+#include <gtest/gtest.h>
+#include "SurgSim/Framework/Barrier.h"
+#include "MockObjects.h" //NOLINT
+
+#include <boost/thread.hpp>
+
+using SurgSim::Framework::Barrier;
+
+namespace {
+ std::shared_ptr<Barrier> barrier;
+ void threadSuccessFunc()
+ {
+ barrier->wait(true);
+ }
+
+ void threadFailureFunc()
+ {
+ barrier->wait(false);
+ }
+}
+
+
+TEST(BarrierTest, BasicTest)
+{
+ ASSERT_ANY_THROW({Barrier(0);});
+ EXPECT_NO_THROW({Barrier(2);});
+}
+
+TEST(BarrierTest, SuccessTest)
+{
+ barrier = std::make_shared<Barrier>(2);
+ boost::thread thread(threadSuccessFunc);
+ EXPECT_TRUE(barrier->wait(true));
+}
+
+TEST(BarrierTest, FailureTest)
+{
+ barrier = std::make_shared<Barrier>(2);
+ boost::thread thread(threadFailureFunc);
+ EXPECT_FALSE(barrier->wait(true));
+}
diff --git a/SurgSim/Framework/UnitTests/BasicSceneElementTests.cpp b/SurgSim/Framework/UnitTests/BasicSceneElementTests.cpp
new file mode 100644
index 0000000..ebbf90d
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/BasicSceneElementTests.cpp
@@ -0,0 +1,110 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 BasicSceneElement class.
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/UnitTests/MockObjects.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+using SurgSim::Framework::Behavior;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+
+namespace
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, MockRepresentation, MockRepresentation);
+}
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+
+TEST(BasicSceneElementTests, InitTest)
+{
+ std::shared_ptr<BasicSceneElement> sceneElement = std::make_shared<BasicSceneElement>("test name");
+
+ EXPECT_EQ("test name", sceneElement->getName());
+}
+
+TEST(BasicSceneElementTests, InitComponentTest)
+{
+ std::shared_ptr<SurgSim::Framework::SceneElement> sceneElement = std::make_shared<BasicSceneElement>(
+ "SceneElement");
+
+ /// Scene element needs a runtime to initialize
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ sceneElement->setRuntime(runtime);
+
+ std::shared_ptr<MockRepresentation> representation1 = std::make_shared<MockRepresentation>("TestRepresentation1");
+ std::shared_ptr<MockRepresentation> representation2 = std::make_shared<MockRepresentation>("TestRepresentation2");
+
+ sceneElement->addComponent(representation1);
+ sceneElement->addComponent(representation2);
+
+ EXPECT_FALSE(representation1->didInit());
+ EXPECT_FALSE(representation1->didWakeUp());
+ EXPECT_FALSE(representation2->didInit());
+ EXPECT_FALSE(representation2->didWakeUp());
+
+}
+
+TEST(BasicSceneElementTests, SerializationTest)
+{
+ std::shared_ptr<SceneElement> sceneElement = std::make_shared<BasicSceneElement>("SceneElement");
+
+ auto representation1 = std::make_shared<MockRepresentation>("TestRepresentation1");
+ auto representation2 = std::make_shared<MockRepresentation>("TestRepresentation2");
+
+ sceneElement->addComponent(representation1);
+ sceneElement->addComponent(representation2);
+ sceneElement->setActive(false);
+
+ RigidTransform3d pose(makeRigidTransform(Quaterniond(0.0, 1.0, 0.0, 0.0), Vector3d(1.0, 2.0, 3.0)));
+ sceneElement->setPose(pose);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(sceneElement->encode(false)) << "Failed to serialize a SceneElement";;
+ ASSERT_NO_THROW(node = sceneElement->encode(true)) << "Failed to serialize a SceneElement";
+
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ("SurgSim::Framework::BasicSceneElement", node.begin()->first.as<std::string>());
+
+ std::shared_ptr<SceneElement> result;
+
+ ASSERT_NO_THROW(result = node.as<std::shared_ptr<SceneElement>>()) << "Failed to restore SceneElement.";
+ EXPECT_EQ("SceneElement", result->getName());
+ EXPECT_EQ(3u, result->getComponents().size());
+ EXPECT_TRUE(pose.isApprox(result->getPose()));
+ EXPECT_FALSE(result->isActive());
+}
+
+}; // namespace Blocks
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Framework/UnitTests/BasicThreadTests.cpp b/SurgSim/Framework/UnitTests/BasicThreadTests.cpp
new file mode 100644
index 0000000..49d3375
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/BasicThreadTests.cpp
@@ -0,0 +1,304 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Original barrier.hpp
+// Copyright (C) 2002-2003
+// David Moore, William E. Kempf
+// Copyright (C) 2007-8 Anthony Williams
+//
+// Distributed under the Boost Software License, Version 1.0. (See accompanying
+// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
+
+#include <gtest/gtest.h>
+#include <boost/thread.hpp>
+
+
+#include "SurgSim/Framework/BasicThread.h"
+#include "MockObjects.h" //NOLINT
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+TEST(BasicThreadTest, Instantiation)
+{
+ MockThread m;
+ EXPECT_FALSE(m.isInitialized());
+ EXPECT_FALSE(m.isRunning());
+
+}
+
+TEST(BasicThreadTest, Running)
+{
+ MockThread m(10);
+ m.start(nullptr);
+
+ m.getThread().join();
+
+ EXPECT_EQ(0, m.count);
+}
+
+TEST(BasicThreadTest, Stop)
+{
+ MockThread m;
+ m.start(nullptr);
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+
+ EXPECT_TRUE(m.isRunning());
+ m.stop();
+
+ EXPECT_TRUE(m.didBeforeStop);
+ EXPECT_FALSE(m.isRunning());
+}
+
+TEST(BasicThreadTest, StopWithoutSleep)
+{
+ for (int i = 0; i < 10; ++i)
+ {
+ MockThread m;
+ m.count = 1000000;
+ m.start(nullptr);
+
+ // Stopping right away should not create a race condition.
+ m.stop();
+
+ EXPECT_TRUE(m.didBeforeStop);
+ EXPECT_FALSE(m.isRunning());
+ }
+}
+
+TEST(BasicThreadTest, RunTimeManagement)
+{
+ MockThread m;
+ EXPECT_EQ(-1, m.count);
+ std::shared_ptr<Barrier> barrier = std::make_shared<Barrier>(2);
+ EXPECT_FALSE(m.didInitialize);
+ EXPECT_FALSE(m.didStartUp);
+ m.start(barrier);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_TRUE(m.didInitialize);
+ EXPECT_FALSE(m.didStartUp);
+ barrier->wait(true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_TRUE(m.didInitialize);
+ EXPECT_TRUE(m.didStartUp);
+ EXPECT_FALSE(m.isRunning());
+ barrier->wait(true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_TRUE(m.isRunning());
+
+ m.stop();
+}
+
+TEST(BasicThreadTest, DestructStoppedThread)
+{
+ std::unique_ptr<MockThread> m;
+ m.reset(new MockThread());
+
+ m->start(nullptr);
+
+
+ m->stop();
+
+ EXPECT_NO_THROW(m.release());
+}
+
+TEST(BasicThreadTest, SynchronousThread)
+{
+ MockThread m(10);
+ EXPECT_EQ(10, m.count);
+ std::shared_ptr<Barrier> barrier = std::make_shared<Barrier>(2);
+ EXPECT_FALSE(m.didInitialize);
+ EXPECT_FALSE(m.didStartUp);
+ EXPECT_FALSE(m.isRunning());
+ EXPECT_FALSE(m.isSynchronous());
+
+ m.start(barrier, true);
+
+ // Run through the initialization
+ barrier->wait(true);
+ barrier->wait(true);
+ barrier->wait(true);
+
+ EXPECT_TRUE(m.isRunning());
+ EXPECT_TRUE(m.isSynchronous());
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ // It's running but waiting for a new wait call
+ EXPECT_EQ(9, m.count);
+ barrier->wait(true);
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_EQ(8, m.count);
+ barrier->wait(true);
+
+ barrier->wait(false);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ m.stop();
+}
+
+TEST(BasicThreadTest, PauseResumeUpdateTest)
+{
+ MockThread m(100000000);
+ int previousCount = m.count;
+ std::shared_ptr<Barrier> barrier = std::make_shared<Barrier>(2);
+
+ EXPECT_EQ(100000000, m.count);
+ EXPECT_FALSE(m.didInitialize);
+ EXPECT_FALSE(m.didStartUp);
+ EXPECT_FALSE(m.isRunning());
+ EXPECT_FALSE(m.isSynchronous());
+ EXPECT_FALSE(m.isIdle());
+
+ m.start(barrier, true);
+
+ // Run through the initialization
+ barrier->wait(true);
+ barrier->wait(true);
+ barrier->wait(true);
+
+ EXPECT_FALSE(m.isIdle());
+ EXPECT_TRUE(m.isRunning());
+ EXPECT_TRUE(m.isSynchronous());
+
+ for (int i = 0; i < 10; i++)
+ {
+ boost::this_thread::sleep(boost::posix_time::milliseconds(10));
+ EXPECT_LT(m.count, previousCount);
+ previousCount = m.count;
+ barrier->wait(true);
+ }
+
+ m.setIdle(true);
+
+ EXPECT_TRUE(m.isIdle());
+ EXPECT_TRUE(m.isRunning());
+
+ barrier->wait(true);
+ previousCount = m.count;
+
+ for (int i = 0; i < 10; i++)
+ {
+ boost::this_thread::sleep(boost::posix_time::milliseconds(10));
+ EXPECT_EQ(previousCount, m.count);
+ previousCount = m.count;
+ barrier->wait(true);
+ }
+
+ m.setIdle(false);
+
+ EXPECT_FALSE(m.isIdle());
+ EXPECT_TRUE(m.isRunning());
+
+ for (int i = 0; i < 10; i++)
+ {
+ boost::this_thread::sleep(boost::posix_time::milliseconds(10));
+ EXPECT_LT(m.count, previousCount);
+ previousCount = m.count;
+ barrier->wait(true);
+ }
+
+ barrier->wait(false);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(10));
+ m.stop();
+}
+
+TEST(BasicThreadTest, SwitchSyncOnThread)
+{
+ MockThread m(-1);
+ std::shared_ptr<Barrier> barrier = std::make_shared<Barrier>(2);
+ EXPECT_FALSE(m.didInitialize);
+ EXPECT_FALSE(m.didStartUp);
+ EXPECT_FALSE(m.isRunning());
+ EXPECT_FALSE(m.isSynchronous());
+
+ m.start(barrier, false);
+
+ // Run through the initialization
+ barrier->wait(true);
+ barrier->wait(true);
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_TRUE(m.isRunning());
+
+ // Thread is running, count should be less than count after waiting
+ int count = m.count;
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_GT(count, m.count);
+
+ m.setSynchronous(true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+
+ // Thread is stopped count should be equal to count after waiting
+ count = m.count;
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_EQ(count, m.count);
+
+ // Take one step, count should be just on less than last count
+ barrier->wait(true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_EQ(count-1, m.count);
+
+ count = count - 1;
+ m.setSynchronous(false);
+ // restart the thread ...
+ barrier->wait(true);
+
+ // Thread is running count should decrease and keep decreasing
+ count = m.count;
+ boost::this_thread::sleep(boost::posix_time::milliseconds(200));
+ EXPECT_GT(count, m.count);
+
+ // Thread is running count should decrease and keep decreasing
+ count = m.count;
+ boost::this_thread::sleep(boost::posix_time::milliseconds(200));
+ EXPECT_GT(count, m.count);
+
+ m.stop();
+}
+
+// HS-2013-jun-25 Can't figure out how to make this work or what is going wrong with the test
+class BasicThreadDeathTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ m = std::make_shared<MockThread>();
+ m->start(nullptr);
+ }
+
+ void TearDown()
+ {
+ m->stop();
+ }
+
+ std::shared_ptr<MockThread> m;
+};
+
+TEST_F(BasicThreadDeathTest, DestructLiveThread)
+{
+ ::testing::FLAGS_gtest_death_test_style = "threadsafe";
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+
+ ASSERT_DEATH_IF_SUPPORTED({
+ SurgSim::Framework::AssertMessage::setFailureBehaviorToDeath();
+ m.reset();
+ }, "");
+}
+
+}; // namespace Framework
+}; // namespace SurgSim
diff --git a/SurgSim/Framework/UnitTests/BehaviorManagerTest.cpp b/SurgSim/Framework/UnitTests/BehaviorManagerTest.cpp
new file mode 100644
index 0000000..4213b3e
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/BehaviorManagerTest.cpp
@@ -0,0 +1,94 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "boost/thread/thread.hpp"
+
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/UnitTests/MockObjects.h"
+
+using SurgSim::Framework::BehaviorManager;
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+using SurgSim::Framework::SceneElement;
+
+TEST(BehaviorManagerTest, BehaviorInitTest)
+{
+ std::shared_ptr<Runtime> runtime(new Runtime());
+ std::shared_ptr<BehaviorManager> behaviorManager(new BehaviorManager());
+
+ runtime->addManager(behaviorManager);
+ auto scene = runtime->getScene();
+ std::shared_ptr<SceneElement> element(new MockSceneElement());
+ std::shared_ptr<MockBehavior> behavior(new MockBehavior("MockBehavior"));
+ std::shared_ptr<MockComponent> component(new MockComponent("Test Component"));
+
+
+ element->addComponent(behavior);
+ element->addComponent(component);
+ scene->addSceneElement(element);
+ EXPECT_TRUE(element->isInitialized());
+ EXPECT_TRUE(behavior->isInitialized());
+ EXPECT_TRUE(component->isInitialized());
+
+ runtime->start();
+ EXPECT_TRUE(behaviorManager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ runtime->stop();
+
+ EXPECT_TRUE(behavior->isAwake());
+ EXPECT_FALSE(component->isAwake());
+ EXPECT_GT(behavior->updateCount, 0);
+
+}
+
+TEST(BehaviorManagerTest, UpdateTest)
+{
+ auto runtime = std::make_shared<Runtime>();
+ auto scene = runtime->getScene();
+ auto behaviorManager = std::make_shared<BehaviorManager>();
+ auto element = std::make_shared<MockSceneElement>();
+ auto behavior = std::make_shared<MockBehavior>("MockBehavior");
+
+ runtime->addManager(behaviorManager);
+ element->addComponent(behavior);
+ scene->addSceneElement(element);
+ EXPECT_TRUE(element->isInitialized());
+ EXPECT_TRUE(behavior->isInitialized());
+
+ behavior->setLocalActive(false);
+ behavior->updateCount = 0;
+
+ // BehaviorManager will not update inactive behaviors.
+ runtime->start();
+ EXPECT_TRUE(behaviorManager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_EQ(0, behavior->updateCount);
+
+ // Turn on the behavior, it will be updated.
+ behavior->setLocalActive(true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_GT(behavior->updateCount, 0);
+
+ // Turn off the behavior, it will not be updated any more.
+ behavior->setLocalActive(false);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ auto count = behavior->updateCount;
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_EQ(behavior->updateCount, count);
+
+ runtime->stop();
+}
diff --git a/SurgSim/Framework/UnitTests/CMakeLists.txt b/SurgSim/Framework/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..cced9df
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/CMakeLists.txt
@@ -0,0 +1,65 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories (
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ AccessibleTests.cpp
+ AccessibleTypeTests.cpp
+ ApplicationDataTest.cpp
+ AssertTest.cpp
+ AssetTests.cpp
+ BarrierTest.cpp
+ BasicSceneElementTests.cpp
+ BasicThreadTests.cpp
+ BehaviorManagerTest.cpp
+ ComponentManagerTests.cpp
+ ComponentTest.cpp
+ LockedContainerTest.cpp
+ LoggerManagerTest.cpp
+ LoggerTest.cpp
+ MockObjects.cpp
+ ObjectFactoryTests.cpp
+ ReuseFactoryTest.cpp
+ RuntimeTest.cpp
+ SceneElementTest.cpp
+ SceneTest.cpp
+ SharedInstanceTest.cpp
+ TimerTest.cpp
+ TransferPropertiesBehaviorTests.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ MockObjects.h
+)
+
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+set(LIBS
+ SurgSimFramework
+ SurgSimDataStructures
+)
+
+surgsim_add_unit_tests(SurgSimFrameworkTest)
+
+set_target_properties(SurgSimFrameworkTest PROPERTIES FOLDER "Framework")
diff --git a/SurgSim/Framework/UnitTests/ComponentManagerTests.cpp b/SurgSim/Framework/UnitTests/ComponentManagerTests.cpp
new file mode 100644
index 0000000..62e4bdf
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/ComponentManagerTests.cpp
@@ -0,0 +1,217 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 ComponentManagerTests.cpp test the basic functionality of the component manager
+/// mostly through a mock manager that exposes the private interface and implements
+/// the simplest version of the abstract interface.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ComponentManager.h"
+
+#include "SurgSim/Framework/UnitTests/MockObjects.h"
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Component;
+
+namespace SurgSim
+{
+namespace Framework
+{
+TEST(ComponentManagerTests, TestInternalAddRemove)
+{
+ std::shared_ptr<Component> mock1 = std::make_shared<MockComponent>("Component1");
+ std::shared_ptr<Component> mock2 = std::make_shared<MockComponent>("Component2");
+ std::shared_ptr<Component> invalid = std::make_shared<MockBehavior>("Behavior1");
+
+ MockManager manager;
+ EXPECT_EQ(0u, manager.getComponents().size());
+
+
+ // Basic case should be able to add mockcomponent
+ EXPECT_TRUE(manager.testTryAddComponent(mock1));
+ EXPECT_EQ(1u, manager.getComponents().size());
+ EXPECT_TRUE(manager.testTryAddComponent(mock2));
+ EXPECT_EQ(2u, manager.getComponents().size());
+
+ // Should not be able to add behavior
+ EXPECT_FALSE(manager.testTryAddComponent(invalid));
+ EXPECT_EQ(2u, manager.getComponents().size());
+
+ // Should not be able to add duplicate
+ EXPECT_FALSE(manager.testTryAddComponent(mock2));
+ EXPECT_EQ(2u, manager.getComponents().size());
+
+ // Test the removals
+ EXPECT_FALSE(manager.testTryRemoveComponent(invalid));
+ EXPECT_EQ(2u, manager.getComponents().size());
+
+ EXPECT_TRUE(manager.testTryRemoveComponent(mock1));
+ EXPECT_EQ(1u, manager.getComponents().size());
+
+ EXPECT_FALSE(manager.testTryRemoveComponent(mock1));
+ EXPECT_EQ(1u, manager.getComponents().size());
+
+ EXPECT_TRUE(manager.testTryRemoveComponent(mock2));
+ EXPECT_EQ(0u, manager.getComponents().size());
+
+ // Add after remove
+ EXPECT_TRUE(manager.testTryAddComponent(mock1));
+ EXPECT_EQ(1u, manager.getComponents().size());
+
+}
+
+TEST(ComponentManagerTests, SimpleAddRemoveComponentTest)
+{
+ std::shared_ptr<Component> mock1 = std::make_shared<MockComponent>("Component1");
+ std::shared_ptr<Component> mock2 = std::make_shared<MockComponent>("Component2");
+ std::shared_ptr<Component> invalid = std::make_shared<MockBehavior>("Behavior1");
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ MockManager manager;
+ manager.setRuntime(runtime);
+ manager.enqueueAddComponent(mock1);
+ manager.enqueueAddComponent(mock1);
+
+ manager.testProcessComponents();
+
+ EXPECT_EQ(1u, manager.getComponents().size());
+
+ manager.enqueueRemoveComponent(mock1);
+ manager.testProcessComponents();
+ EXPECT_EQ(0u, manager.getComponents().size());
+
+}
+
+TEST(ComponentManagerTests, CompoundAddRemoveComponentTest)
+{
+ std::shared_ptr<Component> mock1 = std::make_shared<MockComponent>("Component1");
+ std::shared_ptr<Component> mock2 = std::make_shared<MockComponent>("Component2");
+ std::shared_ptr<Component> invalid = std::make_shared<MockBehavior>("Behavior1");
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ MockManager manager;
+ manager.setRuntime(runtime);
+ manager.enqueueAddComponent(mock1);
+ manager.enqueueAddComponent(mock2);
+ manager.enqueueAddComponent(invalid);
+ manager.testProcessComponents();
+
+ EXPECT_EQ(2u, manager.getComponents().size());
+ manager.enqueueRemoveComponent(mock1);
+ manager.enqueueAddComponent(mock2);
+ manager.enqueueRemoveComponent(invalid);
+ manager.testProcessComponents();
+
+ EXPECT_EQ(1u, manager.getComponents().size());
+}
+
+// Bug: Components that were initialized by other threads, can be woken up by a thread
+// that does not have responsibility. Creating a race condition
+TEST(ComponentManagerTest, DoNotWakeupForeignComponents)
+{
+ std::shared_ptr<Component> mock1 = std::make_shared<MockComponent>("Component1");
+ std::shared_ptr<Component> mock2 = std::make_shared<MockComponent>("Component2");
+ std::shared_ptr<Component> invalid = std::make_shared<MockBehavior>("Behavior1");
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ MockManager manager;
+
+ manager.setRuntime(runtime);
+ manager.enqueueAddComponent(mock1);
+ manager.enqueueAddComponent(mock2);
+ manager.enqueueAddComponent(invalid);
+
+ // Simulate another thread initializing 'invalid'
+ invalid->initialize(runtime);
+
+ manager.testProcessComponents();
+
+ // invalid should not be awoken ...
+ EXPECT_FALSE(invalid->isAwake());
+}
+
+TEST(ComponentManagerTests, TypeTest)
+{
+ MockManager manager;
+ EXPECT_EQ(SurgSim::Framework::MANAGER_TYPE_NONE, manager.getType());
+}
+
+// Specific component manager to expose a bug where sceneelements added during initialization of
+// the ComponentManagers are not initialized themselves
+class InitializationBugManager : public ComponentManager
+{
+public:
+
+ InitializationBugManager()
+ {
+ m_sceneElementInitialize = std::make_shared<MockSceneElement>("Initialize");
+ auto mockComponent = std::make_shared<MockComponent>("Component");
+ m_sceneElementInitialize->addComponent(mockComponent);
+
+ m_sceneElementStartup = std::make_shared<MockSceneElement>("Startup");
+ mockComponent = std::make_shared<MockComponent>("Component");
+ m_sceneElementStartup->addComponent(mockComponent);
+ }
+
+ virtual int getType() const
+ {
+ return MANAGER_TYPE_NONE;
+ }
+
+ virtual bool executeAdditions(const std::shared_ptr<Component>& component)
+ {
+ return true;
+ }
+
+ virtual bool executeRemovals(const std::shared_ptr<Component>& component)
+ {
+ return true;
+ }
+
+ virtual bool doInitialize()
+ {
+ getRuntime()->getScene()->addSceneElement(m_sceneElementInitialize);
+ return true;
+ }
+
+ virtual bool doStartUp()
+ {
+ getRuntime()->getScene()->addSceneElement(m_sceneElementStartup);
+ return true;
+ }
+
+ std::shared_ptr<MockSceneElement> m_sceneElementInitialize;
+ std::shared_ptr<MockSceneElement> m_sceneElementStartup;
+};
+
+TEST(ComponentManagerTests, AdditionDuringInitializationTest)
+{
+ auto manager = std::make_shared<InitializationBugManager>();
+ auto runtime = std::make_shared<Runtime>();
+ auto scene = runtime->getScene();
+
+ runtime->addManager(manager);
+ runtime->start();
+
+ EXPECT_TRUE(manager->m_sceneElementInitialize->isInitialized());
+
+ EXPECT_TRUE(manager->m_sceneElementStartup->isInitialized());
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Framework/UnitTests/ComponentTest.cpp b/SurgSim/Framework/UnitTests/ComponentTest.cpp
new file mode 100644
index 0000000..61bd4ef
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/ComponentTest.cpp
@@ -0,0 +1,536 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 ComponentManagerTests.cpp test the basic functionality of the component manager
+/// mostly through a mock manager that exposes the private interface and implements
+/// the simplest version of the abstract interface.
+
+#include <boost/uuid/uuid_io.hpp>
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/UnitTests/MockObjects.h"
+#include "SurgSim/Testing/SerializationMockComponent.h"
+
+using SurgSim::Framework::Component;
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+
+/// Simple component class, no additional features
+class TestComponent1 : public Component
+{
+public:
+ explicit TestComponent1(const std::string& name) : Component(name)
+ {
+
+ }
+
+ virtual bool doInitialize()
+ {
+ return true;
+ }
+
+ virtual bool doWakeUp()
+ {
+ return true;
+ }
+
+ std::string getClassName() const override
+ {
+ return "TestComponent1";
+ }
+};
+
+/// TestComponent with properties, automatic registration in the factory
+class TestComponent2 : public Component
+{
+public:
+ explicit TestComponent2(const std::string& name) :
+ Component(name),
+ valueOne(999),
+ valueTwo(888)
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TestComponent2, int, ValueOne, getValueOne, setValueOne);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TestComponent2, int, ValueTwo, getValueTwo, setValueTwo);
+ }
+
+ virtual bool doInitialize()
+ {
+ return true;
+ }
+
+ virtual bool doWakeUp()
+ {
+ return true;
+ }
+
+ int getValueOne() const
+ {
+ return valueOne;
+ }
+ void setValueOne(int val)
+ {
+ valueOne = val;
+ }
+ int getValueTwo() const
+ {
+ return valueTwo;
+ }
+ void setValueTwo(int val)
+ {
+ valueTwo = val;
+ }
+
+ std::string getClassName() const override
+ {
+ return "TestComponent2";
+ }
+
+
+private:
+ int valueOne;
+ int valueTwo;
+};
+
+
+
+/// Testcomponent with references to other components
+class TestComponent3 : public Component
+{
+public:
+ explicit TestComponent3(const std::string& name) :
+ Component(name)
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(
+ TestComponent3,
+ std::shared_ptr<Component>,
+ ComponentOne,
+ getComponentOne,
+ setComponentOne);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(
+ TestComponent3,
+ std::shared_ptr<TestComponent2>,
+ ComponentTwo,
+ getComponentTwo,
+ setComponentTwo);
+ }
+
+ virtual bool doInitialize()
+ {
+ return true;
+ }
+
+ virtual bool doWakeUp()
+ {
+ return true;
+ }
+
+ std::shared_ptr<Component> getComponentOne() const
+ {
+ return m_componentOne;
+ }
+ void setComponentOne(std::shared_ptr<Component> val)
+ {
+ m_componentOne = val;
+ }
+ std::shared_ptr<TestComponent2> getComponentTwo() const
+ {
+ return m_componentTwo;
+ }
+ void setComponentTwo(std::shared_ptr<TestComponent2> val)
+ {
+ m_componentTwo = val;
+ }
+
+ std::string getClassName() const override
+ {
+ return "TestComponent3";
+ }
+
+private:
+
+ std::shared_ptr<Component> m_componentOne;
+ std::shared_ptr<TestComponent2> m_componentTwo;
+};
+
+namespace
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, TestComponent2, TestComponent2);
+SURGSIM_REGISTER(SurgSim::Framework::Component, TestComponent3, TestComponent3);
+}
+
+TEST(ComponentTests, Constructor)
+{
+ ASSERT_NO_THROW({MockComponent component("Component");});
+}
+
+TEST(ComponentTests, SetAndGetSceneElementTest)
+{
+ std::shared_ptr<Component> mock1 = std::make_shared<MockComponent>("Component");
+
+ std::shared_ptr<MockSceneElement> element1(new MockSceneElement("one"));
+
+ mock1->setSceneElement(element1);
+
+ EXPECT_EQ(element1, mock1->getSceneElement());
+
+}
+
+TEST(ComponentTests, SetAndGetSceneTest)
+{
+ std::shared_ptr<Component> mock1 = std::make_shared<MockComponent>("Component");
+
+ std::shared_ptr<Scene> scene = std::make_shared<Scene>(std::make_shared<Runtime>());
+
+ mock1->setScene(scene);
+
+ EXPECT_EQ(scene, mock1->getScene());
+
+}
+
+TEST(ComponentTests, PointerEncode)
+{
+ auto component = std::make_shared<TestComponent1>("TestComponent");
+ YAML::Node node = YAML::convert<std::shared_ptr<Component>>::encode(component);
+
+ EXPECT_FALSE(node.IsNull());
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+ std::string className = node.begin()->first.as<std::string>();
+ YAML::Node data = node[className];
+
+ EXPECT_EQ(2u, data.size());
+ EXPECT_EQ("TestComponent1", className);
+ EXPECT_EQ("TestComponent", data["Name"].as<std::string>());
+ EXPECT_EQ(to_string(component->getUuid()), data["Id"].as<std::string>());
+}
+
+TEST(ComponentTests, NullPointerSerialization)
+{
+ // Encode a nullptr
+ std::shared_ptr<Component> emptyComponent;
+ EXPECT_NO_THROW(YAML::convert<std::shared_ptr<Component>>::encode(emptyComponent));
+ YAML::Node node = YAML::convert<std::shared_ptr<Component>>::encode(emptyComponent);
+ EXPECT_FALSE(node.IsMap());
+
+ // Decode an empty node onto an empty receiver
+ std::shared_ptr<Component> emptyComponentReceiver;
+ EXPECT_NO_THROW(YAML::convert<std::shared_ptr<Component>>::decode(node, emptyComponentReceiver););
+ EXPECT_EQ(nullptr, emptyComponentReceiver);
+
+ // Decode an empty node onto a non-empty receiver.
+ std::shared_ptr<Component> component = std::make_shared<MockComponent>("TestMockComponent");
+ EXPECT_NO_THROW(YAML::convert<std::shared_ptr<Component>>::decode(node, component););
+ EXPECT_NE(nullptr, component);
+ EXPECT_EQ("TestMockComponent", component->getName());
+
+ // A derived class of component having empty components.
+ auto container = std::make_shared<TestComponent3>("TestComponent3");
+ YAML::Node containerNode = YAML::convert<Component>::encode(*container);
+ std::shared_ptr<TestComponent3> newContainer = std::dynamic_pointer_cast<TestComponent3>(
+ containerNode.as<std::shared_ptr<Component>>());
+ EXPECT_EQ(nullptr, newContainer->getComponentOne());
+ EXPECT_EQ(nullptr, newContainer->getComponentTwo());
+ EXPECT_EQ(container->getName(), newContainer->getName());
+}
+
+TEST(ComponentTests, ConvertFactoryTest)
+{
+ Component::getFactory().registerClass<TestComponent1>("TestComponent1");
+
+ YAML::Node node;
+ node["TestComponent1"]["Name"] = "ComponentName";
+ node["TestComponent1"]["Id"] = "ConvertFactoryTest_TestComponent1";
+
+ auto component = node.as<std::shared_ptr<Component>>();
+
+ auto testComponent = std::dynamic_pointer_cast<TestComponent1>(component);
+
+ EXPECT_NE(nullptr, testComponent);
+ EXPECT_EQ("ComponentName", testComponent->getName());
+ EXPECT_EQ("TestComponent1", testComponent->getClassName());
+
+ YAML::Node invalidNode;
+ node["Invalid"]["Name"] = "Other";
+ node["Invalid"]["Id"] = "ConvertFactoryTest_TestComponent2";
+
+ // Should not be able to convert this class
+ EXPECT_ANY_THROW({auto result = invalidNode.as<std::shared_ptr<Component>>();});
+}
+
+TEST(ComponentTests, MacroRegistrationTest)
+{
+ YAML::Node node;
+ node["TestComponent2"]["Name"] = "ComponentName";
+ node["TestComponent2"]["Id"] = "AutomaticRegistrationTest_ComponentName";
+
+
+ auto component = node.as<std::shared_ptr<Component>>();
+
+ auto testComponent = std::dynamic_pointer_cast<TestComponent2>(component);
+
+ EXPECT_NE(nullptr, testComponent);
+ EXPECT_EQ("ComponentName", testComponent->getName());
+ EXPECT_EQ("TestComponent2", testComponent->getClassName());
+ EXPECT_TRUE(testComponent->isActive());
+}
+
+TEST(ComponentTests, DecodeSharedReferences)
+{
+ YAML::Node node;
+ node["TestComponent2"]["Name"] = "OneComponentName";
+ node["TestComponent2"]["Id"] = "DecodeSharedReferences_OneComponentName";
+
+ auto component1 = node.as<std::shared_ptr<Component>>();
+ EXPECT_NE(nullptr, component1);
+ EXPECT_TRUE(component1->isActive());
+
+ auto component1copy = node.as<std::shared_ptr<Component>>();
+ EXPECT_NE(nullptr, component1copy);
+ EXPECT_EQ(component1, component1copy);
+ EXPECT_TRUE(component1copy->isActive());
+
+ node["TestComponent2"]["Id"] = "DecodeSharedReferences_TwoComponentName";
+
+ auto component2 = node.as<std::shared_ptr<Component>>();
+ EXPECT_NE(nullptr, component2);
+ EXPECT_NE(component2, component1);
+ EXPECT_NE(component2, component1copy);
+ EXPECT_TRUE(component2->isActive());
+
+ node["TestComponent2"]["IsLocalActive"] = false;
+ auto component3 = node.as<std::shared_ptr<Component>>();
+ EXPECT_FALSE(component3->isLocalActive());
+}
+
+TEST(ComponentTests, EncodeComponent)
+{
+ auto component = std::make_shared<TestComponent2>("TestComponent");
+ component->setValueOne(1);
+ component->setValueTwo(2);
+
+ YAML::Node node = YAML::convert<Component>::encode(*component);
+ std::string className = node.begin()->first.as<std::string>();
+ YAML::Node data = node[className];
+
+ EXPECT_EQ("TestComponent", (data["Name"].IsDefined() ? data["Name"].as<std::string>() : "undefined !"));
+ EXPECT_EQ("TestComponent2", className);
+ EXPECT_EQ(1, (data["ValueOne"].IsDefined() ? data["ValueOne"].as<int>() : 0xbad));
+ EXPECT_EQ(2, (data["ValueTwo"].IsDefined() ? data["ValueTwo"].as<int>() : 0xbad));
+ EXPECT_TRUE(data["IsLocalActive"].IsDefined());
+ EXPECT_TRUE(data["IsLocalActive"].as<bool>());
+}
+
+TEST(ComponentTests, DecodeComponent)
+{
+ YAML::Node node;
+ node["TestComponent2"]["Name"] = "TestComponentName";
+ node["TestComponent2"]["Id"] = "DecodeComponent_TestComponentName";
+ node["TestComponent2"]["ValueOne"] = 100;
+ node["TestComponent2"]["ValueTwo"] = 101;
+ node["TestComponent2"]["IsLocalActive"] = false;
+
+
+ auto component = node.as<std::shared_ptr<Component>>();
+
+ auto testComponent = std::dynamic_pointer_cast<TestComponent2>(component);
+
+ EXPECT_NE(nullptr, testComponent);
+ EXPECT_EQ("TestComponentName", testComponent->getName());
+ EXPECT_EQ(100, testComponent->getValueOne());
+ EXPECT_EQ(101, testComponent->getValueTwo());
+ EXPECT_FALSE(testComponent->isLocalActive());
+}
+
+TEST(ComponentTests, ComponentReferences)
+{
+ auto containerComponent = std::make_shared<TestComponent3>("Root");
+ auto componentOne = std::make_shared<TestComponent2>("Component1");
+ auto componentTwo = std::make_shared<TestComponent2>("Component2");
+
+ componentOne->setValueOne(100);
+ componentOne->setValueTwo(101);
+
+ componentTwo->setValueOne(200);
+ componentTwo->setValueTwo(201);
+
+ containerComponent->setComponentOne(componentOne);
+ containerComponent->setComponentTwo(componentTwo);
+
+ // Push the components onto the node, note that one component is serialized before the container
+ // and the other after the container, this is intentional to test referencing
+ YAML::Node node;
+
+ // because yaml internally does not know about our conversions we have to make them explicit
+ node.push_back(YAML::convert<Component>::encode(*componentOne));
+ node.push_back(YAML::convert<Component>::encode(*containerComponent));
+ node.push_back(YAML::convert<Component>::encode(*componentTwo));
+
+ // Convert from node to shared component
+ auto resultOne =
+ std::dynamic_pointer_cast<TestComponent2>(node[0].as<std::shared_ptr<Component>>());
+ auto resultContainer =
+ std::dynamic_pointer_cast<TestComponent3>(node[1].as<std::shared_ptr<Component>>());
+ auto resultTwo =
+ std::dynamic_pointer_cast<TestComponent2>(node[2].as<std::shared_ptr<Component>>());
+
+ // All of the components should have been de-serialized
+ ASSERT_NE(nullptr, resultContainer);
+ ASSERT_NE(nullptr, resultOne);
+ ASSERT_NE(nullptr, resultTwo);
+
+ // The references should have been resolved correctly
+ ASSERT_EQ(resultContainer->getComponentOne(), resultOne);
+ ASSERT_EQ(resultContainer->getComponentTwo(), resultTwo);
+
+
+ // All components should have the correct values ...
+ EXPECT_EQ(componentOne->getValueOne(),
+ boost::any_cast<int>(resultContainer->getComponentOne()->getValue("ValueOne")));
+
+ EXPECT_EQ(componentOne->getValueTwo(),
+ boost::any_cast<int>(resultContainer->getComponentOne()->getValue("ValueTwo")));
+
+ EXPECT_EQ(componentTwo->getValueOne(),
+ boost::any_cast<int>(resultContainer->getComponentTwo()->getValue("ValueOne")));
+
+ EXPECT_EQ(componentTwo->getValueTwo(),
+ boost::any_cast<int>(resultContainer->getComponentTwo()->getValue("ValueTwo")));
+}
+
+TEST(ComponentTests, MockComponent)
+{
+ auto component = SurgSim::Framework::Component::getFactory().create("MockComponent", "testcomponent");
+
+ ASSERT_NE(nullptr, component);
+
+ /// SerializationMockComponent does not have an explicit definition anywhere in the code
+ /// there is no SerializationMockComponent, but this should still succeed, this test protects
+ /// against linker optimization
+ auto nonDefinedComponent = SurgSim::Framework::Component::getFactory().create(
+ "SerializationMockComponent",
+ "othercomponent");
+
+ ASSERT_NE(nullptr, nonDefinedComponent) << "It looks like SerializationMockComponent was lost during linkage.";
+
+ YAML::Node node = YAML::convert<Component>::encode(*nonDefinedComponent);
+
+ auto roundtripComponent = node.as<std::shared_ptr<Component>>();
+
+ ASSERT_NE(nullptr, roundtripComponent);
+
+ EXPECT_EQ("SerializationMockComponent", roundtripComponent->getClassName());
+ EXPECT_EQ("othercomponent", roundtripComponent->getName());
+}
+
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+TEST(ComponentTests, PoseComponentTest)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component;
+ ASSERT_NO_THROW(component = SurgSim::Framework::Component::getFactory().create(
+ "SurgSim::Framework::PoseComponent",
+ "pose"));
+
+ EXPECT_EQ("SurgSim::Framework::PoseComponent", component->getClassName());
+
+ SurgSim::Math::RigidTransform3d pose(SurgSim::Math::RigidTransform3d::Identity());
+
+ component->setValue("Pose", pose);
+ YAML::Node node(YAML::convert<SurgSim::Framework::Component>::encode(*component));
+
+ auto decoded = std::dynamic_pointer_cast<SurgSim::Framework::PoseComponent>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>());
+
+ EXPECT_NE(nullptr, decoded);
+ EXPECT_TRUE(pose.isApprox(decoded->getValue<SurgSim::Math::RigidTransform3d>("Pose")));
+}
+
+
+TEST(ComponentTests, SetActiveTest)
+{
+ std::shared_ptr<Component> component = std::make_shared<MockComponent>("Component");
+ EXPECT_TRUE(component->isActive());
+ EXPECT_TRUE(component->isLocalActive());
+ EXPECT_NO_THROW(component->setLocalActive(false));
+ EXPECT_FALSE(component->isActive());
+ EXPECT_FALSE(component->isLocalActive());
+
+ // RW property test
+ component->setValue("IsLocalActive", true);
+ EXPECT_TRUE(component->isActive());
+ EXPECT_TRUE(component->isLocalActive());
+ EXPECT_TRUE(component->getValue<bool>("IsActive"));
+ component->setValue("IsLocalActive", false);
+ EXPECT_FALSE(component->isActive());
+ EXPECT_FALSE(component->isLocalActive());
+ EXPECT_FALSE(component->getValue<bool>("IsActive"));
+
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("SceneElement");
+ sceneElement->addComponent(component);
+ EXPECT_TRUE(sceneElement->isActive());
+
+ // An inactive component in an active SceneElement is 'inactive'.
+ EXPECT_FALSE(component->isActive());
+
+ // An active component in an active SceneElement is 'active'.
+ component->setLocalActive(true);
+ EXPECT_TRUE(component->isActive());
+
+ sceneElement->setActive(false);
+ // An active component in an inactive SceneElement is 'inactive'.
+ EXPECT_FALSE(component->isActive());
+
+ // An inactive component in an inactive SceneElement is 'inactive'.
+ component->setLocalActive(false);
+ EXPECT_FALSE(component->isActive());
+
+ // During serialization, it's Component::m_isActive being serialized, not Component::isActive().
+ component->setValue("IsLocalActive", true);
+ YAML::Node node = sceneElement->encode(true);
+ YAML::Node data = node["SurgSim::Framework::BasicSceneElement"];
+
+ // Decode the component only.
+ std::shared_ptr<SurgSim::Framework::Component> decodedComponent;
+ for (auto nodeIt = data["Components"].begin(); nodeIt != data["Components"].end(); ++nodeIt)
+ {
+ if ("MockComponent" == nodeIt->begin()->first.as<std::string>())
+ {
+ decodedComponent = nodeIt->as<std::shared_ptr<MockComponent>>();
+ break;
+ }
+ }
+ EXPECT_EQ(nullptr, decodedComponent->getSceneElement());
+ EXPECT_TRUE(decodedComponent->isActive());
+ EXPECT_TRUE(decodedComponent->isLocalActive());
+
+ // Decode the component with a SceneElement. The SceneElement's activity (active/inactive) will
+ // affect the return value of Component::isActive().
+ decodedComponent = nullptr;
+ auto decodedSceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Decoded");
+ decodedSceneElement->decode(node);
+ EXPECT_FALSE(decodedSceneElement->isActive());
+ decodedComponent = decodedSceneElement->getComponent("Component");
+ EXPECT_NE(nullptr, decodedComponent->getSceneElement());
+ EXPECT_FALSE(decodedComponent->isActive());
+ EXPECT_TRUE(decodedComponent->isLocalActive());
+}
diff --git a/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Directory1/duplicatedFile.txt b/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Directory1/duplicatedFile.txt
new file mode 100644
index 0000000..e69de29
diff --git a/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Directory1/uniqueFile1.txt b/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Directory1/uniqueFile1.txt
new file mode 100644
index 0000000..e69de29
diff --git a/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Directory2/duplicatedFile.txt b/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Directory2/duplicatedFile.txt
new file mode 100644
index 0000000..e69de29
diff --git a/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Directory2/uniqueFile2.txt b/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Directory2/uniqueFile2.txt
new file mode 100644
index 0000000..e69de29
diff --git a/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Test Directory/uniqueFile.txt b/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Test Directory/uniqueFile.txt
new file mode 100644
index 0000000..0a5c927
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/Test Directory/uniqueFile.txt
@@ -0,0 +1 @@
+Data/ApplicationDataTest/Test Directory
\ No newline at end of file
diff --git a/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/testFile1.txt b/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/testFile1.txt
new file mode 100644
index 0000000..d3d788f
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/ApplicationDataTest/testFile1.txt
@@ -0,0 +1,2 @@
+Data/ApplicationDataTest/Directory1
+Data/ApplicationDataTest/Directory2
diff --git a/SurgSim/Framework/UnitTests/Data/AssetTestData/DummyFile.txt b/SurgSim/Framework/UnitTests/Data/AssetTestData/DummyFile.txt
new file mode 100644
index 0000000..e69de29
diff --git a/SurgSim/Framework/UnitTests/LockedContainerTest.cpp b/SurgSim/Framework/UnitTests/LockedContainerTest.cpp
new file mode 100644
index 0000000..83c0165
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/LockedContainerTest.cpp
@@ -0,0 +1,657 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 LockedContainer class.
+
+#include <gtest/gtest.h>
+#include "SurgSim/Framework/LockedContainer.h"
+
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+
+using SurgSim::Framework::LockedContainer;
+
+
+// Define some helper data types for testing.
+
+/// A class that supports copy construction and copy assignment but not moving.
+class Copyable
+{
+public:
+ Copyable() : m_data(-1)
+ {
+ }
+
+ Copyable(const Copyable& o) : m_data(o.m_data)
+ {
+ }
+
+ void operator=(const Copyable& o)
+ {
+ m_data = o.m_data;
+ }
+
+ int getValue() const
+ {
+ return m_data;
+ }
+ void setValue(int d)
+ {
+ m_data = d;
+ }
+
+private:
+ // No need to disable move construction and move assignment-- the compiler does not provide those by default.
+
+ int m_data;
+};
+
+/// A class that supports neither copying nor moving.
+class NonCopyable
+{
+public:
+ NonCopyable() : m_data(-1)
+ {
+ }
+
+ int getValue() const
+ {
+ return m_data;
+ }
+ void setValue(int d)
+ {
+ m_data = d;
+ }
+
+private:
+ // Disable copy construction and copy assignment.
+ // No need to disable move construction and move assignment-- the compiler does not provide those by default.
+ NonCopyable(const NonCopyable&);
+ void operator=(const NonCopyable&);
+
+ int m_data;
+};
+
+/// A class that supports move construction and move assignment but not copying.
+class Movable
+{
+public:
+ Movable() : m_data(-1)
+ {
+ }
+
+ Movable(Movable&& o) : m_data(o.m_data)
+ {
+ o.m_data = -1;
+ }
+ void operator=(Movable&& o)
+ {
+ m_data = o.m_data;
+ o.m_data = -1;
+ }
+
+ int getValue() const
+ {
+ return m_data;
+ }
+ void setValue(int d)
+ {
+ m_data = d;
+ }
+
+private:
+ // Disable copy construction and copy assignment.
+ Movable(const Movable&);
+ void operator=(const Movable&);
+
+ int m_data;
+};
+
+/// A class with several pieces of data for checking consistency.
+class BigData
+{
+public:
+ BigData() : m_data1(-1), m_data2(-1)
+ {
+ }
+
+ BigData(const BigData& o) : m_data1(o.m_data1), m_data2(o.m_data2) {}
+ void operator=(const BigData& o)
+ {
+ m_data1 = o.m_data1;
+ m_data2 = o.m_data2;
+ }
+
+ BigData(BigData&& o) : m_data1(o.m_data1), m_data2(o.m_data2)
+ {
+ o.m_data1 = o.m_data2 = -1;
+ }
+ void operator=(BigData&& o)
+ {
+ m_data1 = o.m_data1;
+ m_data2 = o.m_data2;
+ o.m_data1 = o.m_data2 = -1;
+ }
+
+ int getValue1() const
+ {
+ return m_data1;
+ }
+ void setValue1(int d)
+ {
+ m_data1 = d;
+ }
+ int getValue2() const
+ {
+ return m_data2;
+ }
+ void setValue2(int d)
+ {
+ m_data2 = d;
+ }
+
+private:
+ int m_data1;
+ int padding[1023]; // make it less likely that the cache will be kind to us
+ int m_data2;
+};
+
+
+// Now we're ready to start testing...
+
+// ==================== SINGLE-THREADED TESTS ====================
+
+TEST(LockedContainerTest, Construct)
+{
+ // This should work:
+ EXPECT_NO_THROW( {LockedContainer<int> data;});
+ EXPECT_NO_THROW( {LockedContainer<Copyable> data;});
+ EXPECT_NO_THROW( {LockedContainer<Movable> data;});
+
+ // This should also work but it's useless (can't modify the data):
+ EXPECT_NO_THROW( {LockedContainer<NonCopyable> data;});
+}
+
+TEST(LockedContainerTest, InitializeAtConstruction)
+{
+ typedef Copyable DataType;
+ // With the following definition, the container construction should not compile:
+ //typedef NonCopyable DataType;
+
+ DataType initial;
+ initial.setValue(123);
+ EXPECT_EQ(123, initial.getValue());
+
+ LockedContainer<DataType> data(initial);
+
+ DataType contents;
+ data.get(&contents);
+ EXPECT_EQ(123, contents.getValue());
+ EXPECT_EQ(123, initial.getValue());
+}
+
+TEST(LockedContainerTest, MoveInitializeAtConstruction)
+{
+ typedef Movable DataType;
+
+ DataType initial;
+ initial.setValue(123);
+ EXPECT_EQ(123, initial.getValue());
+
+ LockedContainer<DataType> data(std::move(initial));
+
+ DataType contents;
+ data.take(&contents);
+ EXPECT_EQ(123, contents.getValue());
+ EXPECT_EQ(-1, initial.getValue()); // the old data has been moved out!
+}
+
+TEST(LockedContainerTest, Get)
+{
+ Copyable content;
+ content.setValue(987);
+
+ LockedContainer<Copyable> data(content);
+ EXPECT_EQ(987, content.getValue());
+
+ Copyable destination;
+ data.get(&destination);
+ EXPECT_EQ(987, destination.getValue());
+ data.get(&destination);
+ EXPECT_EQ(987, destination.getValue());
+ EXPECT_EQ(987, content.getValue());
+}
+
+TEST(LockedContainerTest, Take)
+{
+ Movable content;
+ content.setValue(987);
+
+ LockedContainer<Movable> data(std::move(content));
+ EXPECT_EQ(-1, content.getValue());
+
+ Movable destination;
+ data.take(&destination);
+ EXPECT_EQ(987, destination.getValue());
+ data.take(&destination);
+ EXPECT_EQ(-1, destination.getValue());
+ EXPECT_EQ(-1, content.getValue());
+}
+
+TEST(LockedContainerTest, SetAndGet)
+{
+ Copyable initial;
+ initial.setValue(1);
+
+ LockedContainer<Copyable> data(initial);
+
+ Copyable contents;
+ data.get(&contents);
+ EXPECT_EQ(1, contents.getValue());
+
+ // "writer":
+ initial.setValue(2);
+ // note: NOT writing to the container!
+
+ // "reader":
+ data.get(&contents);
+ EXPECT_EQ(1, contents.getValue());
+
+ // "writer":
+ initial.setValue(3);
+ data.set(initial);
+
+ // "reader":
+ data.get(&contents);
+ EXPECT_EQ(3, contents.getValue());
+}
+
+TEST(LockedContainerTest, SetAndTake)
+{
+ Movable initial;
+ initial.setValue(1);
+ EXPECT_EQ(1, initial.getValue());
+
+ LockedContainer<Movable> data(std::move(initial));
+
+ Movable contents;
+ data.take(&contents);
+ EXPECT_EQ(1, contents.getValue());
+ data.take(&contents);
+ EXPECT_EQ(-1, contents.getValue());
+ EXPECT_EQ(-1, initial.getValue());
+
+ // "writer":
+ initial.setValue(2);
+ // note: NOT writing to the container!
+
+ // "reader":
+ data.take(&contents);
+ EXPECT_EQ(-1, contents.getValue());
+
+ // "writer":
+ initial.setValue(3);
+ data.set(std::move(initial));
+
+ // "reader":
+ data.take(&contents);
+ EXPECT_EQ(3, contents.getValue());
+ data.take(&contents);
+ EXPECT_EQ(-1, contents.getValue());
+ EXPECT_EQ(-1, initial.getValue());
+}
+
+TEST(LockedContainerTest, TryGetChanged)
+{
+ {
+ LockedContainer<Copyable> data; // use default constructor for Copyable
+
+ Copyable destination;
+ destination.setValue(999);
+ {
+ bool changed = data.tryGetChanged(&destination);
+ EXPECT_FALSE(changed) << "no set(); tryGetChanged() should return false!";
+ if (! changed)
+ {
+ EXPECT_EQ(999, destination.getValue()) <<
+ "tryGetChanged() returned false; value shouldn't have changed!";
+ }
+ }
+
+ Copyable content;
+ content.setValue(101);
+ data.set(content);
+
+ {
+ bool changed = data.tryGetChanged(&destination);
+ EXPECT_TRUE(changed) << "set() called; tryGetChanged() should return true!";
+ if (changed)
+ {
+ EXPECT_EQ(101, destination.getValue()) <<
+ "tryGetChanged() returned true; value should have changed!";
+ }
+ }
+
+ destination.setValue(888);
+ {
+ bool changed = data.tryGetChanged(&destination);
+ EXPECT_FALSE(changed) << "no set() since get(); tryGetChanged() should return false!";
+ if (! changed)
+ {
+ EXPECT_EQ(888, destination.getValue()) <<
+ "tryGetChanged() returned false; value shouldn't have changed!";
+ }
+ }
+ }
+
+ {
+ Copyable content;
+ content.setValue(654);
+ LockedContainer<Copyable> data(content); // use initializing constructor for Copyable
+
+ Copyable destination;
+ destination.setValue(999);
+ {
+ bool changed = data.tryGetChanged(&destination);
+ EXPECT_FALSE(changed) << "no set(); tryGetChanged() should return false!";
+ if (! changed)
+ {
+ EXPECT_EQ(999, destination.getValue()) <<
+ "tryGetChanged() returned false; value shouldn't have changed!";
+ }
+ }
+
+ content.setValue(121);
+ data.set(content);
+
+ {
+ bool changed = data.tryGetChanged(&destination);
+ EXPECT_TRUE(changed) << "set() called; tryGetChanged() should return true!";
+ if (changed)
+ {
+ EXPECT_EQ(121, destination.getValue()) <<
+ "tryGetChanged() returned true; value should have changed!";
+ }
+ }
+
+ destination.setValue(888);
+ {
+ bool changed = data.tryGetChanged(&destination);
+ EXPECT_FALSE(changed) << "no set() since get(); tryGetChanged() should return false!";
+ if (! changed)
+ {
+ EXPECT_EQ(888, destination.getValue()) <<
+ "tryGetChanged() returned false; value shouldn't have changed!";
+ }
+ }
+ }
+}
+
+TEST(LockedContainerTest, TryTakeChanged)
+{
+ {
+ LockedContainer<Movable> data; // use default constructor for Movable
+
+ Movable destination;
+ destination.setValue(999);
+ {
+ bool changed = data.tryTakeChanged(&destination);
+ EXPECT_FALSE(changed) << "no set(); tryTakeChanged() should return false!";
+ if (! changed)
+ {
+ EXPECT_EQ(999, destination.getValue()) <<
+ "tryTakeChanged() returned false; value shouldn't have changed!";
+ }
+ }
+
+ Movable content;
+ content.setValue(101);
+ data.set(std::move(content));
+
+ {
+ bool changed = data.tryTakeChanged(&destination);
+ EXPECT_TRUE(changed) << "set() called; tryTakeChanged() should return true!";
+ if (changed)
+ {
+ EXPECT_EQ(101, destination.getValue()) <<
+ "tryTakeChanged() returned true; value should have changed!";
+ }
+ }
+
+ destination.setValue(888);
+ {
+ bool changed = data.tryTakeChanged(&destination);
+ EXPECT_FALSE(changed) << "no set() since get(); tryTakeChanged() should return false!";
+ if (! changed)
+ {
+ EXPECT_EQ(888, destination.getValue()) <<
+ "tryTakeChanged() returned false; value shouldn't have changed!";
+ }
+ }
+
+ destination.setValue(777);
+ {
+ data.take(&destination);
+ EXPECT_EQ(-1, destination.getValue()) <<
+ "an earlier tryTakeChanged() should have emptied the container!";
+ }
+ }
+
+ {
+ Movable content;
+ content.setValue(654);
+ LockedContainer<Movable> data(std::move(content)); // use initializing constructor for Movable
+
+ Movable destination;
+ destination.setValue(999);
+ {
+ bool changed = data.tryTakeChanged(&destination);
+ EXPECT_FALSE(changed) << "no set(); tryTakeChanged() should return false!";
+ if (! changed)
+ {
+ EXPECT_EQ(999, destination.getValue()) <<
+ "tryTakeChanged() returned false; value shouldn't have changed!";
+ }
+ }
+
+ content.setValue(121);
+ data.set(std::move(content));
+
+ {
+ bool changed = data.tryTakeChanged(&destination);
+ EXPECT_TRUE(changed) << "set() called; tryTakeChanged() should return true!";
+ if (changed)
+ {
+ EXPECT_EQ(121, destination.getValue()) <<
+ "tryTakeChanged() returned true; value should have changed!";
+ }
+ }
+
+ destination.setValue(888);
+ {
+ bool changed = data.tryTakeChanged(&destination);
+ EXPECT_FALSE(changed) << "no set() since get(); tryTakeChanged() should return false!";
+ if (! changed)
+ {
+ EXPECT_EQ(888, destination.getValue()) <<
+ "tryTakeChanged() returned false; value shouldn't have changed!";
+ }
+ }
+
+ destination.setValue(777);
+ {
+ data.take(&destination);
+ EXPECT_EQ(-1, destination.getValue()) <<
+ "an earlier tryTakeChanged() should have emptied the container!";
+ }
+ }
+}
+
+// ==================== MULTI-THREADED TESTS ====================
+
+typedef LockedContainer<BigData> SharedData;
+
+class DataWriter
+{
+public:
+ /// data parameter to be passed by non-const reference on purpose.
+ explicit DataWriter(SharedData& data, int start = 0, int step = 1, int loops = 100000) : //NOLINT
+ m_data(data),
+ m_start(start),
+ m_step(step),
+ m_loops(loops),
+ m_isDone(false)
+ {
+ }
+
+ bool isDone() const
+ {
+ // Direct access to the data member is a hack, since this is called from a different thread.
+ // If this were production code, we would really want to use a LockedContainer<bool> instead.
+ //
+ // But in this case, we really want to avoid locking around the flag in order to maximize the likelihood
+ // that adverse race conditions in *actual data* access may pop up, so we do this anyway.
+ return m_isDone;
+ }
+
+ void start()
+ {
+ m_thread = boost::thread(boost::ref(*this));
+ }
+ void join()
+ {
+ m_thread.join();
+ }
+
+ void operator()()
+ {
+ BigData current;
+
+ int value = m_start;
+ while (m_loops > 0)
+ {
+ --m_loops;
+ value += m_step;
+
+ current.setValue1(value);
+ current.setValue2(value);
+ m_data.set(current);
+ }
+
+ m_isDone = true;
+ }
+
+private:
+ DataWriter(const DataWriter&);
+ void operator=(const DataWriter&);
+
+ SharedData& m_data;
+ int m_start;
+ int m_step;
+ int m_loops;
+ bool m_isDone;
+
+ boost::thread m_thread;
+};
+
+void testReaderAndWriters(int numWriters)
+{
+ // The number of total writes performed by all writers.
+ //
+ // Note that this value shouldn't be too low: when you break the code by eliminating locking altogether, you
+ // don't get reliable failures below about 10000 writes (as tested on a MSI GE70 laptop), presumably because
+ // the threads finish too quickly and thus don't run concurrently.
+ const int NUM_TOTAL_WRITES = 1000000;
+
+ LockedContainer<BigData> data;
+
+ std::vector<DataWriter*> writers(numWriters);
+ for (size_t i = 0; i < writers.size(); ++i)
+ {
+ // The step has been chosen so two writers can't ever produce the same value
+ writers[i] = new DataWriter(data, static_cast<int>(i), numWriters, NUM_TOTAL_WRITES/numWriters);
+ }
+ {
+ BigData value;
+ data.get(&value);
+ EXPECT_EQ(-1, value.getValue1());
+ EXPECT_EQ(-1, value.getValue2());
+ }
+
+ for (size_t i = 0; i < writers.size(); ++i)
+ {
+ writers[i]->start();
+ }
+
+ BigData value;
+ while (1)
+ {
+ int z1 = value.getValue1();
+
+ bool wasUpdated = data.tryGetChanged(&value);
+ int a1 = value.getValue1();
+ int a2 = value.getValue2();
+ EXPECT_EQ(a1, a2);
+ if (wasUpdated)
+ {
+ EXPECT_NE(z1, a1);
+ }
+ else
+ {
+ EXPECT_EQ(z1, a1);
+ }
+
+ bool allDone = true;
+ for (size_t i = 0; i < writers.size(); ++i)
+ {
+ if (! writers[i]->isDone())
+ {
+ allDone = false;
+ break;
+ }
+ }
+ if (allDone)
+ {
+ break;
+ }
+ }
+
+ for (size_t i = 0; i < writers.size(); ++i)
+ {
+ writers[i]->join();
+ }
+}
+
+TEST(LockedContainerTest, DISABLED_OneWriterThread)
+{
+ testReaderAndWriters(1);
+}
+TEST(LockedContainerTest, TwoWriterThreads)
+{
+ testReaderAndWriters(2);
+}
+TEST(LockedContainerTest, DISABLED_FourWriterThreads)
+{
+ testReaderAndWriters(4);
+}
+TEST(LockedContainerTest, DISABLED_EightWriterThreads)
+{
+ testReaderAndWriters(8);
+}
+TEST(LockedContainerTest, SixteenWriterThreads)
+{
+ testReaderAndWriters(16);
+}
diff --git a/SurgSim/Framework/UnitTests/LoggerManagerTest.cpp b/SurgSim/Framework/UnitTests/LoggerManagerTest.cpp
new file mode 100644
index 0000000..56c81a8
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/LoggerManagerTest.cpp
@@ -0,0 +1,140 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 LoggerManager class.
+
+#include "SurgSim/Framework/Log.h"
+
+#include <string>
+#include <memory>
+
+#include <gtest/gtest.h>
+
+using SurgSim::Framework::Logger;
+using SurgSim::Framework::LoggerManager;
+using SurgSim::Framework::StreamOutput;
+
+class MockOutput : public SurgSim::Framework::LogOutput
+{
+public:
+ MockOutput()
+ {
+ }
+
+ ~MockOutput()
+ {
+ }
+
+ bool writeMessage(const std::string& message)
+ {
+ logMessage = message;
+ return true;
+ }
+ void reset()
+ {
+ logMessage.clear();
+ }
+
+ std::string logMessage;
+};
+
+
+TEST(LoggerManagerTest, Constructor)
+{
+ EXPECT_NO_THROW({std::shared_ptr<LoggerManager> loggerManager(new LoggerManager());});
+}
+
+TEST(LoggerManagerTest, DefaultOutput)
+{
+ auto loggerManager = std::make_shared<LoggerManager>();
+ auto output = std::make_shared<StreamOutput>(std::cerr);
+ loggerManager->setDefaultOutput(output);
+
+ EXPECT_EQ(output, loggerManager->getDefaultOutput());
+}
+
+TEST(LoggerManagerTest, DefaultLoggerTest)
+{
+ auto loggerManager = std::make_shared<LoggerManager>();
+ auto defaultLogger = loggerManager->getDefaultLogger();
+
+ EXPECT_TRUE(nullptr != defaultLogger);
+ EXPECT_EQ(loggerManager->getDefaultOutput(), defaultLogger->getOutput());
+ EXPECT_EQ(loggerManager->getThreshold(), defaultLogger->getThreshold());
+}
+
+
+TEST(LoggerManagerTest, GetLogger)
+{
+ auto loggerManager = std::make_shared<LoggerManager>();
+
+ /// getLogger() will create a new logger if a logger with given name is not found
+ auto retrieved = loggerManager->getLogger("test");
+ EXPECT_TRUE(nullptr != retrieved);
+
+ /// getLogger() guarantees to return a logger with given name
+ {
+ auto temp = loggerManager->getLogger("logger");
+ }
+ EXPECT_TRUE(nullptr != loggerManager->getLogger("logger"));
+}
+
+TEST(LoggerManagerTest, Threshold)
+{
+ auto loggerManager = std::make_shared<LoggerManager>();
+
+ /// Check default log level
+ auto logger0 = loggerManager->getLogger("logger0");
+ EXPECT_EQ(SurgSim::Framework::LOG_LEVEL_WARNING, logger0->getThreshold());
+
+ /// setThreshold(level) will change log level of existing loggers
+ /// and the default log level
+ loggerManager->setThreshold(SurgSim::Framework::LOG_LEVEL_CRITICAL);
+ EXPECT_EQ(SurgSim::Framework::LOG_LEVEL_CRITICAL, loggerManager->getThreshold());
+ EXPECT_EQ(SurgSim::Framework::LOG_LEVEL_CRITICAL, logger0->getThreshold());
+
+ /// setThreshold(level) will affect log level of newly created loggers
+ auto logger1 = loggerManager->getLogger("logger1");
+ EXPECT_EQ(SurgSim::Framework::LOG_LEVEL_CRITICAL, logger1->getThreshold());
+
+
+ auto logger2 = loggerManager->getLogger("logger2");
+ auto logger3 = loggerManager->getLogger("logger3");
+ auto testLogger = loggerManager->getLogger("testLogger");
+
+ /// setThreshold(pattern, level) will change log level of existing loggers
+ /// which match given name pattern
+ /// Logger with different pattern will not be changed
+ /// Default log level will not be affected
+ loggerManager->setThreshold("logger", SurgSim::Framework::LOG_LEVEL_WARNING);
+ EXPECT_EQ(SurgSim::Framework::LOG_LEVEL_WARNING, logger2->getThreshold());
+ EXPECT_EQ(SurgSim::Framework::LOG_LEVEL_WARNING, logger3->getThreshold());
+ EXPECT_NE(SurgSim::Framework::LOG_LEVEL_WARNING, testLogger->getThreshold());
+ EXPECT_NE(SurgSim::Framework::LOG_LEVEL_WARNING, loggerManager->getThreshold());
+
+ /// setThreshold(pattern, level) will not affect newly created loggers
+ auto testLogger2 = loggerManager->getLogger("testLogger2");
+ EXPECT_NE(SurgSim::Framework::LOG_LEVEL_WARNING, testLogger2->getThreshold());
+
+ /// setThreshold(pattern, level) will not affect newly created loggers
+ /// even the logger's name match the pattern
+ auto logger5 = loggerManager->getLogger("logger5");
+ EXPECT_NE(SurgSim::Framework::LOG_LEVEL_WARNING, logger5->getThreshold());
+
+ /// Logger manager should own the logger.
+ loggerManager->getLogger("xxx")->setThreshold(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ EXPECT_EQ(SurgSim::Framework::LOG_LEVEL_DEBUG, loggerManager->getLogger("xxx")->getThreshold());
+}
\ No newline at end of file
diff --git a/SurgSim/Framework/UnitTests/LoggerTest.cpp b/SurgSim/Framework/UnitTests/LoggerTest.cpp
new file mode 100644
index 0000000..aa3fa91
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/LoggerTest.cpp
@@ -0,0 +1,347 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 SURGSIM_LOG_*() macros and the related classes.
+
+#include <gtest/gtest.h>
+#include "SurgSim/Framework/Log.h"
+
+#include <boost/filesystem.hpp>
+
+#include <fstream>
+#include <string>
+#include <iomanip>
+
+using SurgSim::Framework::Logger;
+using SurgSim::Framework::FileOutput;
+
+::testing::AssertionResult isContained(const std::string& expected, const std::string& argument)
+{
+ if (std::string::npos != argument.find(expected))
+ {
+ return ::testing::AssertionSuccess();
+ }
+ else
+ {
+ return ::testing::AssertionFailure() << expected << " not contained in " << argument;
+ }
+}
+
+class MockOutput : public SurgSim::Framework::LogOutput
+{
+public:
+ MockOutput()
+ {
+ }
+
+ ~MockOutput()
+ {
+ }
+
+ bool writeMessage(const std::string& message)
+ {
+ logMessage = message;
+ return true;
+ }
+ void reset()
+ {
+ logMessage = "";
+ }
+
+ std::string logMessage;
+};
+
+
+TEST(LoggerTest, MessageTest)
+{
+ auto logger = Logger::getLogger("TestLogger");
+ auto output = std::make_shared<MockOutput>();
+ logger->setOutput(output);
+
+ logger->writeMessage("TestMessage");
+ EXPECT_EQ("TestMessage", output->logMessage);
+}
+
+TEST(LoggerTest, LogMacroTest)
+{
+ auto logger = Logger::getLogger("TestLogger");
+ auto output = std::make_shared<MockOutput>();
+ logger->setOutput(output);
+ logger->setThreshold(SurgSim::Framework::LOG_LEVEL_WARNING);
+
+ output->reset();
+ SURGSIM_LOG(logger, CRITICAL) << "Test Text";
+ EXPECT_TRUE(isContained("Test Text",output->logMessage));
+
+ output->reset();
+ SURGSIM_LOG(logger, DEBUG) << "Missing Text";
+ EXPECT_EQ("", output->logMessage);
+
+ output->reset();
+ SURGSIM_LOG(logger, WARNING) << "Exactly At Threshold";
+ EXPECT_TRUE(isContained("Exactly At Threshold", output->logMessage));
+}
+
+
+TEST(LoggerTest, SharedPtrTest)
+{
+ auto logger = Logger::getLogger("TestLogger");
+ auto output = std::make_shared<MockOutput>();
+ logger->setOutput(output);
+ logger->setThreshold(SurgSim::Framework::LOG_LEVEL_WARNING);
+
+ output->reset();
+ SURGSIM_LOG(logger, CRITICAL) << "Test Text";
+ EXPECT_TRUE(isContained("Test Text", output->logMessage));
+
+ output->reset();
+ SURGSIM_LOG(logger, DEBUG) << "Missing Text";
+ EXPECT_EQ("", output->logMessage);
+
+ output->reset();
+ SURGSIM_LOG(logger, WARNING) << "Exactly At Threshold";
+ EXPECT_TRUE(isContained("Exactly At Threshold", output->logMessage));
+}
+
+TEST(LoggerTest, LevelSpecificMacroTest)
+{
+ auto logger = Logger::getLogger("TestLogger");
+ auto output = std::make_shared<MockOutput>();
+
+ logger->setOutput(output);
+ logger->setThreshold(SurgSim::Framework::LOG_LEVEL_WARNING);
+
+ output->reset();
+ SURGSIM_LOG_CRITICAL(logger) << "Test Text";
+ EXPECT_TRUE(isContained("Test Text", output->logMessage));
+
+ output->reset();
+ SURGSIM_LOG_DEBUG(logger) << "Missing Text";
+ EXPECT_EQ("", output->logMessage);
+
+ output->reset();
+ SURGSIM_LOG_WARNING(logger) << "Exactly At Threshold";
+ EXPECT_TRUE(isContained("Exactly At Threshold", output->logMessage));
+}
+
+TEST(LoggerTest, IfMacroTest)
+{
+ auto logger = Logger::getLogger("TestLogger");
+ auto output = std::make_shared<MockOutput>();
+ logger->setOutput(output);
+ logger->setThreshold(SurgSim::Framework::LOG_LEVEL_WARNING);
+
+ output->reset();
+ SURGSIM_LOG_IF(true, logger, CRITICAL) << "Test Text";
+ EXPECT_TRUE(isContained("Test Text", output->logMessage));
+ output->reset();
+ SURGSIM_LOG_IF(false, logger, CRITICAL) << "Test Text";
+ EXPECT_EQ("", output->logMessage);
+
+ output->reset();
+ SURGSIM_LOG_IF(true, logger, DEBUG) << "Missing Text";
+ EXPECT_EQ("", output->logMessage);
+ output->reset();
+ SURGSIM_LOG_IF(false, logger, DEBUG) << "Missing Text";
+ EXPECT_EQ("", output->logMessage);
+
+ output->reset();
+ SURGSIM_LOG_IF(true, logger, WARNING) << "Exactly At Threshold";
+ EXPECT_TRUE(isContained("Exactly At Threshold", output->logMessage));
+ output->reset();
+ SURGSIM_LOG_IF(false, logger, WARNING) << "Exactly At Threshold";
+ EXPECT_EQ("", output->logMessage);
+}
+
+TEST(LoggerTest, OnceMacroTest)
+{
+ auto logger = Logger::getLogger("TestLogger");
+ auto output = std::make_shared<MockOutput>();
+ logger->setOutput(output);
+ logger->setThreshold(SurgSim::Framework::LOG_LEVEL_WARNING);
+
+ for (int i = 0; i < 3; ++i)
+ {
+ output->reset();
+ SURGSIM_LOG_ONCE(logger, CRITICAL) << "Test Text";
+ if (i == 0)
+ {
+ EXPECT_TRUE(isContained("Test Text", output->logMessage));
+ }
+ else
+ {
+ EXPECT_EQ("", output->logMessage);
+ }
+
+ output->reset();
+ SURGSIM_LOG_ONCE(logger, DEBUG) << "Missing Text";
+ EXPECT_EQ("", output->logMessage);
+
+ output->reset();
+ SURGSIM_LOG_ONCE(logger, CRITICAL) << "More Text";
+ if (i == 0)
+ {
+ EXPECT_TRUE(isContained("More Text", output->logMessage));
+ }
+ else
+ {
+ EXPECT_EQ("", output->logMessage);
+ }
+ }
+}
+
+TEST(LoggerTest, OnceIfMacroTest)
+{
+ auto logger = Logger::getLogger("TestLogger");
+ auto output = std::make_shared<MockOutput>();
+ logger->setOutput(output);
+ logger->setThreshold(SurgSim::Framework::LOG_LEVEL_WARNING);
+
+ for (int i = 0; i < 3; ++i)
+ {
+ output->reset();
+ SURGSIM_LOG_ONCE_IF(i >= 0, logger, CRITICAL) << "Test Text";
+ if (i == 0)
+ {
+ EXPECT_TRUE(isContained("Test Text", output->logMessage));
+ }
+ else
+ {
+ EXPECT_EQ("", output->logMessage);
+ }
+
+ output->reset();
+ SURGSIM_LOG_ONCE_IF(i >= 1, logger, DEBUG) << "Missing Text";
+ EXPECT_EQ("", output->logMessage);
+
+ output->reset();
+ SURGSIM_LOG_ONCE_IF(i >= 2, logger, CRITICAL) << "More Text";
+ if (i == 2)
+ {
+ EXPECT_TRUE(isContained("More Text", output->logMessage));
+ }
+ else
+ {
+ EXPECT_EQ("", output->logMessage);
+ }
+ }
+}
+
+TEST(LoggerTest, EndOfLineTest)
+{
+ auto logger = Logger::getLogger("TestLogger");
+ auto output = std::make_shared<MockOutput>();
+ logger->setOutput(output);
+ logger->setThreshold(SurgSim::Framework::LOG_LEVEL_WARNING);
+
+ output->reset();
+ SURGSIM_LOG_CRITICAL(logger) << "foo" << std::endl << "bar";
+ EXPECT_TRUE(isContained("foo\nbar", output->logMessage));
+}
+
+TEST(LoggerTest, LogLevelTest)
+{
+ auto logger = Logger::getLogger("TestLogger");
+ auto output = std::make_shared<MockOutput>();
+ logger->setOutput(output);
+ logger->setThreshold(SurgSim::Framework::LOG_LEVEL_DEBUG);
+
+ output->reset();
+ SURGSIM_LOG_DEBUG(logger) << "a message";
+ EXPECT_TRUE(isContained("DEBUG", output->logMessage));
+
+ output->reset();
+ SURGSIM_LOG_INFO(logger) << "a message";
+ EXPECT_TRUE(isContained("INFO", output->logMessage));
+
+ output->reset();
+ SURGSIM_LOG_WARNING(logger) << "a message";
+ EXPECT_TRUE(isContained("WARNING", output->logMessage));
+
+ output->reset();
+ SURGSIM_LOG_SEVERE(logger) << "a message";
+ EXPECT_TRUE(isContained("SEVERE", output->logMessage));
+
+ output->reset();
+ SURGSIM_LOG_CRITICAL(logger) << "a message";
+ EXPECT_TRUE(isContained("CRITICAL", output->logMessage));
+}
+
+TEST(LoggerTest, ManipulatorTest)
+{
+ auto logger = Logger::getLogger("TestLogger");
+ auto output = std::make_shared<MockOutput>();
+ logger->setOutput(output);
+
+ output->reset();
+ SURGSIM_LOG_WARNING(logger) << "aAa" << std::endl << "bBb";
+ EXPECT_TRUE(isContained("aAa\nbBb", output->logMessage) || isContained("aAa\r\n\bBb", output->logMessage)) <<
+ "message: '" << output->logMessage << "'";
+
+ output->reset();
+ SURGSIM_LOG_WARNING(logger) << "[" << std::hex << std::setw(5) << std::setfill('0') << 0x1234 << "]";
+ EXPECT_TRUE(isContained("[01234]", output->logMessage)) <<
+ "message: '" << output->logMessage << "'";
+
+ output->reset();
+ // The next message should not show any evidence of previous manipulators.
+ SURGSIM_LOG_WARNING(logger) << "[" << 987 << "]";
+ EXPECT_TRUE(isContained("[987]", output->logMessage)) <<
+ "message: '" << output->logMessage << "'";
+}
+
+class FileOutputTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ output = std::make_shared<FileOutput>("test.log");
+ }
+
+ void TearDown()
+ {
+ output = nullptr;
+ boost::filesystem::remove("test.log");
+ }
+
+ static std::shared_ptr<FileOutput> output;
+};
+
+std::shared_ptr<FileOutput> FileOutputTest::output;
+
+TEST_F(FileOutputTest, BasicTest)
+{
+ EXPECT_TRUE(boost::filesystem::exists("test.log"));
+}
+
+TEST_F(FileOutputTest, CreationFailure)
+{
+ ASSERT_ANY_THROW(FileOutput("asdfasdf/test.log"));
+}
+
+TEST_F(FileOutputTest, Write)
+{
+ output->writeMessage("TestMessage");
+ output = nullptr;
+
+ std::ifstream stream("test.log");
+ EXPECT_FALSE(stream.fail());
+
+ std::string message;
+ stream >> message;
+
+ EXPECT_EQ("TestMessage",message);
+}
diff --git a/SurgSim/Framework/UnitTests/MockObjects.cpp b/SurgSim/Framework/UnitTests/MockObjects.cpp
new file mode 100644
index 0000000..33cfcad
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/MockObjects.cpp
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/UnitTests/MockObjects.h"
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, MockComponent, MockComponent);
+
+MockComponent::MockComponent(const std::string& name, bool succeedInit, bool succeedWakeUp) :
+ Component(name),
+ succeedWithInit(succeedInit),
+ succeedWithWakeUp(succeedWakeUp),
+ didWakeUp(false),
+ didInit(false)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(
+ MockComponent, bool, SucceedWithInit, getSucceedWithInit, setSucceedWithInit);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(
+ MockComponent, bool, SucceedWithWakeUp, getSucceedWithWakeUp, setSucceedWithWakeUp);
+}
+
+MockComponent::~MockComponent()
+{
+
+}
+
+bool MockComponent::doInitialize()
+{
+ didInit = true;
+ return succeedWithInit;
+}
+
+bool MockComponent::doWakeUp()
+{
+ didWakeUp = true;
+ return succeedWithWakeUp;
+}
+
+bool MockComponent::getSucceedWithInit() const
+{
+ return succeedWithInit;
+}
+
+void MockComponent::setSucceedWithInit(bool val)
+{
+ succeedWithInit = val;
+}
+
+bool MockComponent::getSucceedWithWakeUp() const
+{
+ return succeedWithWakeUp;
+}
+
+void MockComponent::setSucceedWithWakeUp(bool val)
+{
+ succeedWithWakeUp = val;
+}
diff --git a/SurgSim/Framework/UnitTests/MockObjects.h b/SurgSim/Framework/UnitTests/MockObjects.h
new file mode 100644
index 0000000..3d0726d
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/MockObjects.h
@@ -0,0 +1,329 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest LLC.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_FRAMEWORK_UNITTESTS_MOCKOBJECTS_H
+#define SURGSIM_FRAMEWORK_UNITTESTS_MOCKOBJECTS_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/BasicThread.h"
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/ComponentManager.h"
+#include "SurgSim/Framework/Representation.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+
+/// Class to catch the calls made to the scene element, does nothing
+class MockSceneElement : public SurgSim::Framework::SceneElement
+{
+public:
+ explicit MockSceneElement(const std::string& name = "MockSceneElement") :
+ SceneElement(name),
+ didInit(false),
+ didUpdate(false),
+ didLateUpdate(false),
+ didFixedUpdate(false)
+ {
+ m_localRuntime = std::make_shared<SurgSim::Framework::Runtime>();
+ setRuntime(m_localRuntime);
+
+ m_localScene = m_localRuntime->getScene();
+ setScene(m_localScene);
+ }
+
+ virtual void update(double dt)
+ {
+ didUpdate = true;
+ }
+ virtual void lateUpdate(double dt)
+ {
+ didLateUpdate = true;
+ }
+ virtual void fixedRateUpdate(double dt)
+ {
+ didFixedUpdate = true;
+ }
+
+ virtual bool doInitialize()
+ {
+ didInit = true;
+ return didInit;
+ };
+
+ bool didInit;
+ bool didUpdate;
+ bool didLateUpdate;
+ bool didFixedUpdate;
+private:
+ std::shared_ptr<SurgSim::Framework::Runtime> m_localRuntime;
+ std::shared_ptr<SurgSim::Framework::Scene> m_localScene;
+
+};
+
+class MockThread : public SurgSim::Framework::BasicThread
+{
+public:
+ explicit MockThread(int runCount = -1) :
+ count(runCount),
+ totalTime(0.0),
+ didBeforeStop(false),
+ didInitialize(false),
+ didStartUp(false)
+ {
+ }
+
+ virtual ~MockThread()
+ {
+ }
+
+ int count;
+ double totalTime;
+
+ bool didBeforeStop;
+ bool runIndefinetly;
+ bool didInitialize;
+ bool didStartUp;
+
+private:
+ virtual bool doInitialize()
+ {
+ didInitialize = true;
+ return true;
+ };
+
+ virtual bool doStartUp()
+ {
+ didStartUp = true;
+ return true;
+ };
+
+ virtual bool doUpdate(double dt)
+ {
+ --count;
+ totalTime += dt;
+
+ return count != 0;
+ };
+
+ virtual void doBeforeStop()
+ {
+ didBeforeStop = true;
+ };
+};
+
+
+class MockComponent : public SurgSim::Framework::Component
+{
+public:
+ explicit MockComponent(const std::string& name, bool succeedInit = true, bool succeedWakeUp = true);
+
+ SURGSIM_CLASSNAME(MockComponent);
+
+ virtual ~MockComponent();
+
+ virtual bool doInitialize();
+ virtual bool doWakeUp();
+
+ bool getSucceedWithInit() const;
+ void setSucceedWithInit(bool val);
+
+ bool getSucceedWithWakeUp() const;
+ void setSucceedWithWakeUp(bool val);
+
+ bool succeedWithInit;
+ bool succeedWithWakeUp;
+ bool didWakeUp;
+ bool didInit;
+};
+
+class MockBehavior : public SurgSim::Framework::Behavior
+{
+public:
+ MockBehavior(const std::string& name, bool succeedInit = true, bool succeedWakeUp = true) :
+ Behavior(name),
+ succeedWithInit(succeedInit),
+ succeedWithWakeUp(succeedWakeUp),
+ updateCount(0)
+ {
+ }
+ virtual ~MockBehavior()
+ {
+ }
+
+ virtual bool doInitialize()
+ {
+ return succeedWithInit;
+ }
+
+ virtual bool doWakeUp()
+ {
+ return succeedWithWakeUp;
+ }
+
+ virtual void update(double dt)
+ {
+ updateCount++;
+ }
+
+ bool succeedWithInit;
+ bool succeedWithWakeUp;
+ int updateCount;
+};
+
+
+class MockManager : public SurgSim::Framework::ComponentManager
+{
+public:
+ MockManager(bool succeedInit = true, bool succeedStartup = true) :
+ succeedInit(succeedInit),
+ succeedStartup(succeedStartup),
+ didInitialize(false),
+ didStartUp(false),
+ didBeforeStop(false),
+ count(0)
+ {
+ }
+
+ virtual ~MockManager()
+ {
+ }
+
+ virtual int getType() const override
+ {
+ return SurgSim::Framework::MANAGER_TYPE_NONE;
+ }
+
+ const std::vector<std::shared_ptr<MockComponent>>& getComponents()
+ {
+ return m_components;
+ }
+
+ bool testTryAddComponent(const std::shared_ptr<SurgSim::Framework::Component>& component)
+ {
+ return executeAdditions(component);
+ }
+
+ bool testTryRemoveComponent(const std::shared_ptr<SurgSim::Framework::Component>& component)
+ {
+ return executeRemovals(component);
+ }
+
+ void testProcessComponents()
+ {
+ processComponents();
+ }
+
+ bool succeedInit;
+ bool succeedStartup;
+ bool didInitialize;
+ bool didStartUp;
+ bool didBeforeStop;
+ int count;
+
+private:
+ virtual bool doInitialize()
+ {
+ didInitialize = true;
+ return succeedInit;
+ };
+ virtual bool doStartUp()
+ {
+ didStartUp = true;
+ return succeedStartup;
+ };
+ virtual bool doUpdate(double dt)
+ {
+ ++count;
+ processComponents();
+ return true;
+ };
+
+ virtual void doBeforeStop()
+ {
+ didBeforeStop = true;
+ }
+
+ virtual bool executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component) override
+ {
+ return tryAddComponent(component, &m_components) != nullptr;
+ }
+
+ virtual bool executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component) override
+ {
+ return tryRemoveComponent(component, &m_components);
+ }
+
+
+ std::vector<std::shared_ptr<MockComponent>> m_components;
+
+
+};
+
+class MockRepresentation : public SurgSim::Framework::Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ /// \post m_pose is initialized to identity
+ /// \post m_didInit is initialized to false
+ /// \post m_didWakeUp is initialized to false
+ explicit MockRepresentation(const std::string& name) : SurgSim::Framework::Representation(name),
+ m_didInit(false),
+ m_didWakeUp(false)
+ {
+ }
+
+ SURGSIM_CLASSNAME(MockRepresentation);
+
+ /// Returns true if the representation has been initialized, otherwise false
+ bool didInit() const
+ {
+ return m_didInit;
+ }
+
+ /// Returns true if the representation has been woken up, otherwise false
+ bool didWakeUp() const
+ {
+ return m_didWakeUp;
+ }
+
+private:
+ /// Whether the representation has been initialized
+ bool m_didInit;
+ /// Whether the representation has been woken up
+ bool m_didWakeUp;
+
+ /// Initializes the representation
+ /// \return True if succeeds, otherwise false
+ virtual bool doInitialize()
+ {
+ m_didInit = true;
+ return true;
+ }
+
+ /// Wakes up the representation
+ /// \return True if succeeds, otherwise false
+ virtual bool doWakeUp()
+ {
+ m_didWakeUp = true;
+ return true;
+ }
+};
+
+
+#endif // SURGSIM_FRAMEWORK_UNITTESTS_MOCKOBJECTS_H
diff --git a/SurgSim/Framework/UnitTests/ObjectFactoryTests.cpp b/SurgSim/Framework/UnitTests/ObjectFactoryTests.cpp
new file mode 100644
index 0000000..1e803c3
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/ObjectFactoryTests.cpp
@@ -0,0 +1,122 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include "SurgSim/Framework/ObjectFactory.h"
+
+namespace
+{
+
+
+class TestClass
+{
+public:
+ TestClass() : stringValue("default"), className("TestClass")
+ {
+
+ }
+
+ ~TestClass()
+ {
+
+ }
+
+ explicit TestClass(std::string val) : stringValue(val)
+ {
+
+ }
+
+ std::string stringValue;
+ std::string className;
+};
+
+class TestClassA : public TestClass
+{
+public:
+ TestClassA() : TestClass("a")
+ {
+ className = "TestClassA";
+ }
+
+ explicit TestClassA(const std::string& val) : TestClass(val)
+ {
+ className = "TestClassA";
+ }
+
+};
+
+class TestClassB : public TestClass
+{
+public:
+ TestClassB() : TestClass("b")
+ {
+ className = "TestClassB";
+ }
+
+ explicit TestClassB(const std::string& val) : TestClass(val)
+ {
+ className = "TestClassB";
+ }
+};
+
+}
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+TEST(ObjectFactoryTests, Single)
+{
+ ObjectFactory<TestClass> factory;
+
+ factory.registerClass<TestClassA>("TestClassA");
+ factory.registerClass<TestClassB>("TestClassB");
+
+ auto a = factory.create("TestClassA");
+ EXPECT_NE(nullptr, a);
+ EXPECT_EQ("a", a->stringValue);
+
+ auto b = factory.create("TestClassB");
+ EXPECT_NE(nullptr, b);
+ EXPECT_EQ("b", b->stringValue);
+
+ EXPECT_ANY_THROW(factory.create("xxx"));
+}
+
+TEST(ObjectFactoryTests, OneParameter)
+{
+ ObjectFactory1<TestClass, std::string> factory;
+ factory.registerClass<TestClassA>("TestClassA");
+ factory.registerClass<TestClassB>("TestClassB");
+
+ auto a = factory.create("TestClassA", "abc");
+ EXPECT_NE(nullptr, a);
+ EXPECT_EQ("abc", a->stringValue);
+ EXPECT_EQ("TestClassA", a->className);
+
+ auto b = factory.create("TestClassB", "cde");
+ EXPECT_NE(nullptr, b);
+ EXPECT_EQ("cde", b->stringValue);
+ EXPECT_EQ("TestClassB", b->className);
+
+ EXPECT_ANY_THROW(factory.create("xxx","xyz"));
+
+}
+
+
+}; // Framework
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Framework/UnitTests/ReuseFactoryTest.cpp b/SurgSim/Framework/UnitTests/ReuseFactoryTest.cpp
new file mode 100644
index 0000000..97401dd
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/ReuseFactoryTest.cpp
@@ -0,0 +1,110 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/ReuseFactory.h"
+
+using SurgSim::Framework::ReuseFactory;
+
+class MockObject
+{
+public:
+ MockObject()
+ {
+ }
+
+ ~MockObject()
+ {
+ }
+};
+
+TEST(ReuseFactoryTest, InitTest)
+{
+ ASSERT_NO_THROW({ReuseFactory<MockObject>();});
+}
+
+TEST(ReuseFactoryTest, GetNewTest)
+{
+ ReuseFactory<MockObject> objectFactory;
+
+ std::shared_ptr<MockObject> object = objectFactory.getInstance();
+ ASSERT_NE(nullptr, object);
+}
+
+TEST(ReuseFactoryTest, DeleteTest)
+{
+ ReuseFactory<MockObject> objectFactory;
+
+ std::weak_ptr<MockObject> weakPointer;
+ {
+ std::shared_ptr<MockObject> sharedPointer;
+ {
+ // Get a new object.
+ std::shared_ptr<MockObject> object = objectFactory.getInstance();
+ ASSERT_NE(nullptr, object);
+
+ weakPointer = object;
+ ASSERT_NE(nullptr, weakPointer.lock());
+
+ // Make another shared pointer to the object.
+ sharedPointer = object;
+ }
+
+ // The object should not have been deleted yet, as there is still a shared pointer to it.
+ ASSERT_NE(nullptr, weakPointer.lock());
+ }
+
+ // It should be deleted now.
+ EXPECT_EQ(nullptr, weakPointer.lock()) << "The object should no longer exist.";
+}
+
+TEST(ReuseFactoryTest, ReuseTest)
+{
+ ReuseFactory<MockObject> objectFactory;
+
+ std::weak_ptr<MockObject> weakPointer;
+ MockObject* pointer;
+
+ // Get a new object
+ {
+ std::shared_ptr<MockObject> object = objectFactory.getInstance();
+ ASSERT_NE(nullptr, object);
+
+ weakPointer = object;
+ ASSERT_NE(nullptr, weakPointer.lock());
+
+ pointer = object.get();
+ }
+
+ // It should be deleted now.
+ EXPECT_EQ(nullptr, weakPointer.lock()) << "Weak pointer to object = " << weakPointer.lock();
+
+ // Reuse the object that has been deleted.
+ {
+ std::shared_ptr<MockObject> object = objectFactory.getInstance();
+ ASSERT_NE(nullptr, object);
+
+ EXPECT_EQ(pointer, object.get()) << "The retrieved object should be the object that was previously deleted.";
+
+ // Get another object, which should be new.
+ {
+ std::shared_ptr<MockObject> object = objectFactory.getInstance();
+ ASSERT_NE(nullptr, object);
+
+ EXPECT_NE(pointer, object.get()) << "We should have a completely new object, not the same previous object.";
+ }
+ }
+}
diff --git a/SurgSim/Framework/UnitTests/RuntimeTest.cpp b/SurgSim/Framework/UnitTests/RuntimeTest.cpp
new file mode 100644
index 0000000..1f34331
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/RuntimeTest.cpp
@@ -0,0 +1,240 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "MockObjects.h" //NOLINT
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+using SurgSim::Framework::Logger;
+
+TEST(RuntimeTest, Constructor)
+{
+ EXPECT_NO_THROW({std::shared_ptr<Runtime> runtime(new Runtime());});
+ std::shared_ptr<Runtime> runtime(new Runtime());
+ EXPECT_NE(nullptr, runtime->getScene());
+
+ EXPECT_NO_THROW(std::make_shared<Runtime>());
+ EXPECT_NO_THROW(std::make_shared<Runtime>("config.txt"));
+ EXPECT_THROW(std::make_shared<Runtime>("Non-exist-file"), SurgSim::Framework::AssertionFailure);
+}
+
+TEST(RuntimeTest, AddManager)
+{
+ std::shared_ptr<Runtime> runtime(new Runtime());
+ std::shared_ptr<MockManager> manager(new MockManager());
+
+ runtime->addManager(manager);
+
+ EXPECT_TRUE(runtime->start());
+
+ EXPECT_TRUE(manager->isInitialized());
+
+ EXPECT_TRUE(runtime->isRunning());
+
+ EXPECT_TRUE(runtime->stop());
+
+ EXPECT_FALSE(runtime->isRunning());
+
+ EXPECT_FALSE(manager->isRunning());
+}
+
+TEST(RuntimeTest, InitFailureDeathTest)
+{
+ std::shared_ptr<Runtime> runtime(new Runtime());
+ std::shared_ptr<MockManager> managerSucceeds(new MockManager());
+ std::shared_ptr<MockManager> managerFails(new MockManager(false,true));
+
+ runtime->addManager(managerSucceeds);
+ runtime->addManager(managerFails);
+
+ ASSERT_DEATH(runtime->start(), "");
+}
+
+TEST(RuntimeTest, StartupFailureDeathTest)
+{
+ std::shared_ptr<Runtime> runtime(new Runtime());
+ std::shared_ptr<MockManager> managerSucceeds(new MockManager());
+ std::shared_ptr<MockManager> managerFails(new MockManager(true,false));
+
+ runtime->addManager(managerSucceeds);
+ runtime->addManager(managerFails);
+
+ ASSERT_DEATH(runtime->start(), "");
+}
+
+TEST(RuntimeTest, SceneInitialization)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<MockManager> manager = std::make_shared<MockManager>();
+ runtime->addManager(manager);
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ std::vector<std::shared_ptr<MockSceneElement>> elements;
+ std::vector<std::shared_ptr<MockComponent>> components;
+
+ elements.push_back(std::make_shared<MockSceneElement>("one"));
+ elements.push_back(std::make_shared<MockSceneElement>("two"));
+ components.push_back(std::make_shared<MockComponent>("one"));
+ components.push_back(std::make_shared<MockComponent>("two"));
+ components.push_back(std::make_shared<MockComponent>("three"));
+ components.push_back(std::make_shared<MockComponent>("four"));
+
+ scene->addSceneElement(elements[0]);
+ scene->addSceneElement(elements[1]);
+ elements[0]->addComponent(components[0]);
+ elements[0]->addComponent(components[1]);
+ elements[1]->addComponent(components[2]);
+ elements[1]->addComponent(components[3]);
+
+ EXPECT_FALSE(manager->didInitialize);
+ EXPECT_FALSE(manager->didStartUp);
+ EXPECT_FALSE(manager->didBeforeStop);
+
+ runtime->start();
+
+ EXPECT_TRUE(manager->didInitialize);
+ EXPECT_TRUE(manager->didStartUp);
+ EXPECT_FALSE(manager->didBeforeStop);
+
+ for (int i = 0; i < 2; ++i)
+ {
+ EXPECT_NE(nullptr, elements[i]->getRuntime());
+ EXPECT_TRUE(elements[i]->didInit);
+ }
+
+ for (int i=0; i<4; i++)
+ {
+ EXPECT_TRUE(components[i]->didInit);
+ }
+
+ runtime->stop();
+ EXPECT_TRUE(manager->didBeforeStop);
+}
+
+TEST(RuntimeTest, PausedStep)
+{
+ std::shared_ptr<Runtime> runtime(new Runtime());
+ std::shared_ptr<MockManager> manager1(new MockManager());
+ std::shared_ptr<MockManager> manager2(new MockManager());
+
+ runtime->addManager(manager1);
+ runtime->addManager(manager2);
+
+ runtime->start(true);
+
+ EXPECT_TRUE(runtime->isPaused());
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+
+ EXPECT_TRUE(manager1->isSynchronous());
+ EXPECT_TRUE(manager2->isSynchronous());
+
+ int count = manager1->count;
+
+ runtime->step();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_EQ(count+1,manager1->count);
+
+ runtime->step();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_EQ(count+2,manager1->count);
+
+ runtime->stop();
+}
+
+
+TEST(RuntimeTest, PauseResume)
+{
+ std::shared_ptr<Runtime> runtime(new Runtime());
+ std::shared_ptr<MockManager> manager1(new MockManager());
+ std::shared_ptr<MockManager> manager2(new MockManager());
+
+ runtime->addManager(manager1);
+ runtime->addManager(manager2);
+
+ runtime->start(false);
+
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+
+ EXPECT_FALSE(manager1->isSynchronous());
+ EXPECT_FALSE(manager2->isSynchronous());
+
+ runtime->pause();
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_TRUE(manager1->isSynchronous());
+ EXPECT_TRUE(manager2->isSynchronous());
+
+ runtime->step();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ runtime->step();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ runtime->resume();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+
+ EXPECT_FALSE(manager1->isSynchronous());
+ EXPECT_FALSE(manager2->isSynchronous());
+
+ runtime->stop();
+}
+
+TEST(RuntimeTest, AddComponentAddDuringRuntime)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<MockManager> manager = std::make_shared<MockManager>();
+ runtime->addManager(manager);
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ std::vector<std::shared_ptr<MockComponent>> components;
+
+ auto element = std::make_shared<MockSceneElement>("one");
+ components.push_back(std::make_shared<MockComponent>("one"));
+ components.push_back(std::make_shared<MockComponent>("two"));
+
+ scene->addSceneElement(element);
+ element->addComponent(components[0]);
+
+ runtime->start(true);
+
+ EXPECT_TRUE(manager->didInitialize);
+ EXPECT_TRUE(manager->didStartUp);
+ EXPECT_FALSE(manager->didBeforeStop);
+
+ // Make sure we are out of initialization completely
+ runtime->step();
+
+ EXPECT_FALSE(components[1]->isInitialized());
+ EXPECT_FALSE(components[1]->isAwake());
+
+ EXPECT_TRUE(element->addComponent(components[1]));
+
+ EXPECT_TRUE(components[1]->isInitialized());
+ EXPECT_FALSE(components[1]->isAwake());
+
+ runtime->step();
+ runtime->step(); // Right now step is still non-blocking, make sure the thread has finished processing...
+
+ EXPECT_TRUE(components[1]->isInitialized());
+ EXPECT_TRUE(components[1]->isAwake());
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+
+ runtime->stop();
+}
diff --git a/SurgSim/Framework/UnitTests/SceneElementTest.cpp b/SurgSim/Framework/UnitTests/SceneElementTest.cpp
new file mode 100644
index 0000000..89586b2
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/SceneElementTest.cpp
@@ -0,0 +1,192 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+
+#include "MockObjects.h" //NOLINT
+
+using SurgSim::Framework::Component;
+using SurgSim::Framework::PoseComponent;
+using SurgSim::Framework::SceneElement;
+
+TEST(SceneElementTest, Constructor)
+{
+ ASSERT_NO_THROW(MockSceneElement element);
+}
+
+TEST(SceneElementTest, Pose)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::Quaterniond;
+ using SurgSim::Math::RigidTransform3d;
+ using SurgSim::Math::Vector3d;
+
+ MockSceneElement element;
+ EXPECT_TRUE(element.getPose().isApprox(RigidTransform3d::Identity()));
+
+ RigidTransform3d pose(makeRigidTransform(Quaterniond(0.0, 1.0, 0.0, 0.0), Vector3d(1.0, 2.0, 3.0)));
+ element.setPose(pose);
+ EXPECT_TRUE(element.getPose().isApprox(pose));
+ EXPECT_TRUE(element.getPoseComponent()->getPose().isApprox(pose));
+}
+
+TEST(SceneElementTest, UpdateFunctions)
+{
+ MockSceneElement element;
+
+ element.update(1.0);
+ EXPECT_TRUE(element.didUpdate);
+
+ element.lateUpdate(1.0);
+ EXPECT_TRUE(element.didLateUpdate);
+
+ element.fixedRateUpdate(1.0);
+ EXPECT_TRUE(element.didFixedUpdate);
+}
+
+TEST(SceneElementTest, AddAndTestComponents)
+{
+ std::shared_ptr<MockSceneElement> element = std::make_shared<MockSceneElement>();
+ std::shared_ptr<MockComponent> component = std::make_shared<MockComponent>("TestComponent");
+
+ EXPECT_TRUE(element->addComponent(component));
+
+ // SceneElement should be set after add
+ EXPECT_EQ(component->getSceneElement(), element);
+
+ // Scene in Component will not be set until initialization.
+ EXPECT_NE(component->getScene(), element->getScene() );
+}
+
+TEST(SceneElementTest, AddAndAccessComponents)
+{
+ std::shared_ptr<MockSceneElement> element(new MockSceneElement());
+
+ std::shared_ptr<MockComponent> component1(new MockComponent("TestComponent1"));
+ std::shared_ptr<MockComponent> component2(new MockComponent("TestComponent2"));
+
+ EXPECT_TRUE(element->addComponent(component1));
+ EXPECT_TRUE(element->addComponent(component2));
+
+ // Should not be able to add two with the same name
+ EXPECT_FALSE(element->addComponent(component1));
+
+ // Should not be able to add nullptr component
+ EXPECT_ANY_THROW(element->addComponent(nullptr));
+
+ std::shared_ptr<Component> fetched(element->getComponent("TestComponent1"));
+ ASSERT_NE(nullptr, fetched);
+ EXPECT_EQ("TestComponent1", fetched->getName());
+
+ fetched = element->getComponent("Random");
+ EXPECT_EQ(nullptr, fetched);
+}
+
+TEST(SceneElementTest, RemoveComponents)
+{
+ std::shared_ptr<MockSceneElement> element(new MockSceneElement());
+
+ std::shared_ptr<MockComponent> component1(new MockComponent("TestComponent1"));
+ std::shared_ptr<MockComponent> component2(new MockComponent("TestComponent2"));
+
+ EXPECT_TRUE(element->addComponent(component1));
+ EXPECT_TRUE(element->addComponent(component2));
+
+ EXPECT_TRUE(element->removeComponent("TestComponent2"));
+ EXPECT_EQ(nullptr, element->getComponent("TestComponent2"));
+
+ // Add back should work
+ EXPECT_TRUE(element->addComponent(component2));
+
+ EXPECT_TRUE(element->removeComponent(component1));
+ EXPECT_EQ(nullptr, element->getComponent("TestComponent1"));
+}
+
+TEST(SceneElementTest, GetComponentsTest)
+{
+ std::shared_ptr<MockSceneElement> element(new MockSceneElement());
+
+ std::shared_ptr<MockComponent> component1(new MockComponent("TestComponent1"));
+ std::shared_ptr<MockComponent> component2(new MockComponent("TestComponent2"));
+
+ element->addComponent(component1);
+ EXPECT_EQ(2u, element->getComponents().size());
+
+ element->addComponent(component2);
+ EXPECT_EQ(3u, element->getComponents().size());
+
+ std::vector<std::shared_ptr<Component>> components = element->getComponents();
+
+ EXPECT_NE(components.end(), std::find(components.cbegin(), components.cend(), component1));
+ EXPECT_NE(components.end(), std::find(components.cbegin(), components.cend(), component2));
+
+ element->removeComponent(component1);
+ components = element->getComponents();
+ EXPECT_EQ(2u, components.size());
+}
+
+TEST(SceneElementTest, GetTypedComponentsTests)
+{
+ std::shared_ptr<SceneElement> element(new MockSceneElement());
+ std::shared_ptr<MockBehavior> behavior(new MockBehavior("MockBehavior"));
+ std::shared_ptr<MockComponent> component1(new MockComponent("Test Component1"));
+ std::shared_ptr<MockComponent> component2(new MockComponent("Test Component2"));
+
+ element->addComponent(behavior);
+ element->addComponent(component1);
+ element->addComponent(component2);
+
+ EXPECT_EQ(1u, element->getComponents<MockBehavior>().size());
+ EXPECT_EQ(2u, element->getComponents<MockComponent>().size());
+
+ element->removeComponent(component1);
+ EXPECT_EQ(1u, element->getComponents<MockComponent>().size());
+
+ element->removeComponent(component2);
+ EXPECT_EQ(0u, element->getComponents<MockComponent>().size());
+}
+
+TEST(SceneElementTest, InitComponentTest)
+{
+ std::shared_ptr<MockSceneElement> element(new MockSceneElement());
+ std::shared_ptr<MockComponent> component1(new MockComponent("TestComponent1"));
+ std::shared_ptr<MockComponent> component2(new MockComponent("TestComponent2"));
+
+ element->addComponent(component1);
+ element->addComponent(component2);
+
+ element->initialize();
+
+ EXPECT_TRUE(element->didInit);
+}
+
+TEST(SceneElementTest, DoubleInitTest)
+{
+ std::shared_ptr<MockSceneElement> element(new MockSceneElement());
+
+ EXPECT_FALSE(element->didInit);
+
+ element->initialize();
+ EXPECT_TRUE(element->didInit);
+
+ ASSERT_ANY_THROW(element->initialize());
+}
diff --git a/SurgSim/Framework/UnitTests/SceneTest.cpp b/SurgSim/Framework/UnitTests/SceneTest.cpp
new file mode 100644
index 0000000..d331240
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/SceneTest.cpp
@@ -0,0 +1,124 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include "MockObjects.h" //NOLINT
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+TEST(SceneTest, ConstructorTest)
+{
+ ASSERT_NO_THROW({Scene scene(std::make_shared<Runtime>());});
+}
+
+TEST(SceneTest, ElementManagement)
+{
+ auto runtime(std::make_shared<Runtime>());
+ std::shared_ptr<Scene> scene = runtime->getScene();
+ std::shared_ptr<MockSceneElement> element1(new MockSceneElement("one"));
+ std::shared_ptr<MockSceneElement> element2(new MockSceneElement("two"));
+
+ EXPECT_EQ(0u, scene->getSceneElements().size());
+
+ scene->addSceneElement(element1);
+ EXPECT_EQ(1u, scene->getSceneElements().size());
+ scene->addSceneElement(element2);
+ EXPECT_EQ(2u, scene->getSceneElements().size());
+
+ EXPECT_ANY_THROW(scene->addSceneElement(element1));
+ EXPECT_EQ(2u, scene->getSceneElements().size());
+
+ EXPECT_EQ(element1, scene->getSceneElement("one"));
+ EXPECT_EQ(nullptr, scene->getSceneElement("three"));
+}
+
+TEST(SceneTest, AddAndTestScene)
+{
+ auto runtime(std::make_shared<Runtime>());
+ std::shared_ptr<Scene> scene = runtime->getScene();
+ std::shared_ptr<MockSceneElement> element = std::make_shared<MockSceneElement>("element");
+ std::shared_ptr<MockComponent> component = std::make_shared<MockComponent>("component");
+
+ EXPECT_TRUE(element->addComponent(component));
+ scene->addSceneElement(element);
+
+ EXPECT_EQ(scene, component->getScene());
+ EXPECT_EQ(element, component->getSceneElement());
+ EXPECT_EQ(scene, element->getScene());
+}
+
+TEST(SceneTest, CheckForExpiredRuntime)
+{
+ auto runtime = std::make_shared<Runtime>();
+ auto scene = std::make_shared<Scene>(runtime);
+
+ auto element0 = std::make_shared<MockSceneElement>("element0");
+ auto element1 = std::make_shared<MockSceneElement>("element1");
+
+ auto component0 = std::make_shared<MockComponent>("component0");
+ auto component1 = std::make_shared<MockComponent>("component1");
+
+ // This is the normal behavior
+ EXPECT_NO_THROW(scene->addSceneElement(element0));
+ EXPECT_NO_THROW(element0->addComponent(component0));
+
+ // invalidate runtime pointer, this will cause the weak_ptr inside of scene and sceneelement
+ // to expire
+ runtime.reset();
+
+ // We should not be able to do this with an expired runtime pointer
+ EXPECT_THROW(scene->addSceneElement(element1), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(element0->addComponent(component1), SurgSim::Framework::AssertionFailure);
+}
+
+TEST(SceneTest, YamlTest)
+{
+ auto runtime = std::make_shared<Runtime>();
+ auto scene = std::make_shared<Scene>(runtime);
+
+ auto element0 = std::make_shared<BasicSceneElement>("element0");
+ auto element1 = std::make_shared<BasicSceneElement>("element1");
+
+ auto component0 = std::make_shared<MockComponent>("component0");
+ auto component1 = std::make_shared<MockComponent>("component1");
+
+ element0->addComponent(component0);
+ element1->addComponent(component1);
+ scene->addSceneElement(element0);
+ scene->addSceneElement(element1);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = scene) << "Failed to serialize Scene.";
+
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ std::shared_ptr<Scene> newScene = std::make_shared<Scene>(runtime);
+
+ ASSERT_NO_THROW(newScene->decode(node));
+ EXPECT_EQ(2u, newScene->getSceneElements().size());
+
+}
+
+}
+}
diff --git a/SurgSim/Framework/UnitTests/SharedInstanceTest.cpp b/SurgSim/Framework/UnitTests/SharedInstanceTest.cpp
new file mode 100644
index 0000000..a7a8c32
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/SharedInstanceTest.cpp
@@ -0,0 +1,294 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 SharedInstance class.
+
+#include <type_traits>
+
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+
+#include <gtest/gtest.h>
+#include "SurgSim/Framework/SharedInstance.h"
+
+using SurgSim::Framework::SharedInstance;
+
+
+// Define some helper data types for testing.
+
+/// A helper base class.
+class ObjectCounts
+{
+public:
+ static int getNumInstancesCreated()
+ {
+ return m_constructionIndex;
+ }
+ static int getNumInstancesDestroyed()
+ {
+ return m_destructionIndex;
+ }
+
+ static void resetInstanceIndices()
+ {
+ m_constructionIndex = 0;
+ m_destructionIndex = 0;
+ }
+
+protected:
+ ObjectCounts()
+ {
+ }
+
+ static int getNextInstanceIndex()
+ {
+ // NB: not thread-safe!
+ ++m_constructionIndex;
+ return m_constructionIndex;
+ }
+
+ static void markDestruction()
+ {
+ // NB: not thread-safe!
+ ++m_destructionIndex;
+ }
+
+ static int m_constructionIndex;
+ static int m_destructionIndex;
+};
+
+int ObjectCounts::m_constructionIndex = 0;
+int ObjectCounts::m_destructionIndex = 0;
+
+
+/// A class that only supports construction with an argument, and doesn't support copying or moving.
+class ArgumentOnlyConstructible : public ObjectCounts
+{
+public:
+ explicit ArgumentOnlyConstructible(int data) : m_data(data), m_instanceIndex(getNextInstanceIndex())
+ {
+ }
+
+ ~ArgumentOnlyConstructible()
+ {
+ markDestruction();
+ }
+
+ int getValue() const
+ {
+ return m_data;
+ }
+ void setValue(int d)
+ {
+ m_data = d;
+ }
+
+ int getInstanceIndex() const
+ {
+ return m_instanceIndex;
+ }
+
+private:
+ // Disable copy construction and copy assignment.
+ // No need to disable move construction and move assignment-- the compiler does not provide those by default.
+ ArgumentOnlyConstructible(const ArgumentOnlyConstructible&);
+ void operator=(const ArgumentOnlyConstructible&);
+
+ int m_data;
+ int m_instanceIndex;
+};
+
+
+/// A class that supports default construction, but neither copying nor moving.
+class DefaultConstructible : public ArgumentOnlyConstructible
+{
+public:
+ DefaultConstructible() : ArgumentOnlyConstructible(-1)
+ {
+ }
+};
+
+
+// Now we're ready to start testing...
+
+// ==================== SINGLE-THREADED TESTS ====================
+
+TEST(SharedInstanceTest, Construct)
+{
+ ObjectCounts::resetInstanceIndices();
+
+ // This should work:
+ EXPECT_NO_THROW({SharedInstance<int> shared;});
+ EXPECT_NO_THROW({SharedInstance<DefaultConstructible> shared;});
+
+ // This should NOT work:
+// EXPECT_NO_THROW({SharedInstance<ArgumentOnlyConstructible> shared;});
+ // ...but this should:
+ SharedInstance<ArgumentOnlyConstructible>::InstanceCreator createArgumentOnlyConstructible =
+ []() { return std::make_shared<ArgumentOnlyConstructible>(1); };
+ EXPECT_NO_THROW({SharedInstance<ArgumentOnlyConstructible> shared(createArgumentOnlyConstructible);});
+
+ // No objects should have been constructed:
+ EXPECT_EQ(0, ObjectCounts::getNumInstancesCreated());
+}
+
+TEST(SharedInstanceTest, CreateInstance)
+{
+ typedef DefaultConstructible DataType;
+
+ ObjectCounts::resetInstanceIndices();
+
+ SharedInstance<DataType> shared;
+ EXPECT_EQ(0, ObjectCounts::getNumInstancesCreated());
+
+ {
+ std::shared_ptr<DataType> ref1 = shared.get();
+ ASSERT_NE(nullptr, ref1);
+ EXPECT_EQ(1, ref1->getInstanceIndex());
+ std::shared_ptr<DataType> ref2 = shared.get();
+ ASSERT_NE(nullptr, ref2);
+ EXPECT_EQ(1, ref2->getInstanceIndex());
+ EXPECT_EQ(ref1, ref2);
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(0, ObjectCounts::getNumInstancesDestroyed());
+ }
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesDestroyed());
+
+ {
+ std::shared_ptr<DataType> ref1 = shared.get();
+ std::shared_ptr<DataType> ref2 = shared.get();
+ EXPECT_EQ(ref1, ref2);
+ EXPECT_EQ(2, ref1->getInstanceIndex());
+ EXPECT_EQ(2, ref2->getInstanceIndex());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesDestroyed());
+
+ ref1.reset();
+ ASSERT_EQ(nullptr, ref1); // it's dead now!
+ ASSERT_NE(nullptr, ref2);
+ EXPECT_EQ(2, ref2->getInstanceIndex());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesDestroyed());
+ ref2.reset();
+ ASSERT_EQ(nullptr, ref2); // it too is dead now!
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesDestroyed());
+ }
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesDestroyed());
+}
+
+TEST(SharedInstanceTest, CreateInstanceUsingCreator)
+{
+ typedef DefaultConstructible DataType;
+
+ ObjectCounts::resetInstanceIndices();
+
+ SharedInstance<DataType>::InstanceCreator createDataType( []() { return std::make_shared<DataType>(); } );
+ SharedInstance<DataType> shared(createDataType);
+
+ EXPECT_EQ(0, ObjectCounts::getNumInstancesCreated());
+
+ {
+ std::shared_ptr<DataType> ref1 = shared.get();
+ ASSERT_NE(nullptr, ref1);
+ EXPECT_EQ(1, ref1->getInstanceIndex());
+ std::shared_ptr<DataType> ref2 = shared.get();
+ ASSERT_NE(nullptr, ref2);
+ EXPECT_EQ(1, ref2->getInstanceIndex());
+ EXPECT_EQ(ref1, ref2);
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(0, ObjectCounts::getNumInstancesDestroyed());
+ }
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesDestroyed());
+
+ {
+ std::shared_ptr<DataType> ref1 = shared.get();
+ std::shared_ptr<DataType> ref2 = shared.get();
+ EXPECT_EQ(ref1, ref2);
+ EXPECT_EQ(2, ref1->getInstanceIndex());
+ EXPECT_EQ(2, ref2->getInstanceIndex());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesDestroyed());
+
+ ref1.reset();
+ ASSERT_EQ(nullptr, ref1); // it's dead now!
+ ASSERT_NE(nullptr, ref2);
+ EXPECT_EQ(2, ref2->getInstanceIndex());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesDestroyed());
+ ref2.reset();
+ ASSERT_EQ(nullptr, ref2); // it too is dead now!
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesDestroyed());
+ }
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesDestroyed());
+}
+
+TEST(SharedInstanceTest, CreateNonDefaultConstructibleInstance)
+{
+ typedef ArgumentOnlyConstructible DataType;
+
+ ObjectCounts::resetInstanceIndices();
+
+ // This should NOT work for a type that's not default-constructible:
+// SharedInstance<DataType> shared;
+ // ...but this should:
+ SharedInstance<DataType>::InstanceCreator createDataType( []() { return std::make_shared<DataType>(2); } );
+ SharedInstance<DataType> shared(createDataType);
+
+ EXPECT_EQ(0, ObjectCounts::getNumInstancesCreated());
+
+ {
+ std::shared_ptr<DataType> ref1 = shared.get();
+ ASSERT_NE(nullptr, ref1);
+ EXPECT_EQ(1, ref1->getInstanceIndex());
+ std::shared_ptr<DataType> ref2 = shared.get();
+ ASSERT_NE(nullptr, ref2);
+ EXPECT_EQ(1, ref2->getInstanceIndex());
+ EXPECT_EQ(ref1, ref2);
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(0, ObjectCounts::getNumInstancesDestroyed());
+ }
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesDestroyed());
+
+ {
+ std::shared_ptr<DataType> ref1 = shared.get();
+ std::shared_ptr<DataType> ref2 = shared.get();
+ EXPECT_EQ(ref1, ref2);
+ EXPECT_EQ(2, ref1->getInstanceIndex());
+ EXPECT_EQ(2, ref2->getInstanceIndex());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesDestroyed());
+
+ ref1.reset();
+ ASSERT_EQ(nullptr, ref1); // it's dead now!
+ ASSERT_NE(nullptr, ref2);
+ EXPECT_EQ(2, ref2->getInstanceIndex());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(1, ObjectCounts::getNumInstancesDestroyed());
+ ref2.reset();
+ ASSERT_EQ(nullptr, ref2); // it too is dead now!
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesDestroyed());
+ }
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesCreated());
+ EXPECT_EQ(2, ObjectCounts::getNumInstancesDestroyed());
+}
diff --git a/SurgSim/Framework/UnitTests/TimerTest.cpp b/SurgSim/Framework/UnitTests/TimerTest.cpp
new file mode 100644
index 0000000..35866fe
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/TimerTest.cpp
@@ -0,0 +1,95 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include "SurgSim/Framework/Timer.h"
+#include "SurgSim/Framework/Assert.h"
+
+using SurgSim::Framework::Timer;
+
+TEST(TimerTest, Constructor)
+{
+ EXPECT_NO_THROW({std::shared_ptr<Timer> timer(new Timer());});
+}
+
+TEST(TimerTest, Starting)
+{
+ std::shared_ptr<Timer> timer = std::make_shared<Timer>();
+ EXPECT_EQ(timer->getCurrentNumberOfFrames(), 0);
+ EXPECT_EQ(timer->getNumberOfClockFails(), 0);
+ EXPECT_THROW(timer->getCumulativeTime(), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(timer->getAverageFramePeriod(), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(timer->getAverageFrameRate(), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(timer->getLastFramePeriod(), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(timer->getLastFrameRate(), SurgSim::Framework::AssertionFailure);
+}
+
+TEST(TimerTest, SettingFrames)
+{
+ std::shared_ptr<Timer> timer = std::make_shared<Timer>();
+ timer->endFrame();
+ EXPECT_EQ(timer->getCurrentNumberOfFrames(), 1);
+ EXPECT_EQ(timer->getAverageFrameRate(), timer->getLastFrameRate());
+ EXPECT_EQ(timer->getAverageFramePeriod(), timer->getLastFramePeriod());
+ EXPECT_EQ(timer->getLastFramePeriod(), timer->getCumulativeTime());
+
+ timer->setMaxNumberOfFrames(3);
+ timer->start();
+ for (auto i = 0; i < 5; ++i)
+ {
+ timer->markFrame();
+ }
+ EXPECT_EQ(timer->getCurrentNumberOfFrames(), 3);
+ timer->setMaxNumberOfFrames(2);
+ EXPECT_EQ(timer->getCurrentNumberOfFrames(), 2);
+}
+
+TEST(TimerTest, Comparison)
+{
+ std::shared_ptr<Timer> timer1 = std::make_shared<Timer>();
+ std::shared_ptr<Timer> timer2 = std::make_shared<Timer>();
+
+ for (auto i = 0; i < 100; ++i)
+ {
+ timer2->beginFrame();
+ timer2->endFrame();
+ timer1->markFrame();
+ }
+ // timer1's frames include timer2's frames and the for-loop operations, so timer1's frame period should be longer
+ // than timer2's frames (or at least no shorter).
+ if ((timer1->getNumberOfClockFails() == 0) && (timer2->getNumberOfClockFails() == 0))
+ {
+ EXPECT_GE(timer1->getAverageFramePeriod(), timer2->getAverageFramePeriod());
+ EXPECT_GT(timer1->getCumulativeTime(), timer1->getLastFramePeriod());
+
+ EXPECT_GE(timer1->getMaxFramePeriod(), timer2->getMaxFramePeriod());
+ EXPECT_GE(timer1->getMinFramePeriod(), timer2->getMinFramePeriod());
+ }
+}
+
+TEST(TimerTest, GetWithoutAnyFrames)
+{
+ std::shared_ptr<Timer> timer = std::make_shared<Timer>();
+
+ EXPECT_ANY_THROW(timer->getCumulativeTime());
+ EXPECT_ANY_THROW(timer->getAverageFramePeriod());
+ EXPECT_ANY_THROW(timer->getAverageFrameRate());
+ EXPECT_ANY_THROW(timer->getLastFramePeriod());
+ EXPECT_ANY_THROW(timer->getLastFrameRate());
+ EXPECT_ANY_THROW(timer->getMaxFramePeriod());
+ EXPECT_ANY_THROW(timer->getMinFramePeriod());
+}
+
+
diff --git a/SurgSim/Framework/UnitTests/TransferPropertiesBehaviorTests.cpp b/SurgSim/Framework/UnitTests/TransferPropertiesBehaviorTests.cpp
new file mode 100644
index 0000000..c452db7
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/TransferPropertiesBehaviorTests.cpp
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/TransferPropertiesBehavior.h"
+#include "SurgSim/Framework/Runtime.h"
+
+
+class A : public SurgSim::Framework::Accessible
+{
+public:
+ A(int initialA = 0, int initialB = 0) : a(initialA), b(initialB)
+ {
+ SURGSIM_ADD_RW_PROPERTY(A,int,a,getA,setA);
+ SURGSIM_ADD_RW_PROPERTY(A,int,b,getB,setB);
+ }
+
+ int a;
+ int getA() const { return a; }
+ void setA(int val) { a = val; }
+
+ int b;
+ int getB() const { return b; }
+ void setB(int val) { b = val; }
+};
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+TEST(TransferPropertiesBehaviorTest, InitTest)
+{
+ ASSERT_NO_THROW({TransferPropertiesBehavior b("TestName");});
+}
+
+TEST(TransferPropertiesBehaviorTest, ValidConnections)
+{
+ auto a = std::make_shared<A>();
+ auto b = std::make_shared<A>();
+
+ TransferPropertiesBehavior behavior("test");
+
+ EXPECT_ANY_THROW(behavior.connect(nullptr, "", a ,"a"));
+ EXPECT_ANY_THROW(behavior.connect(a, "a", nullptr, ""));
+
+ EXPECT_ANY_THROW(behavior.connect(a, "a", a, "a"));
+
+ EXPECT_ANY_THROW(behavior.connect(a, "xxx", b, "a"));
+ EXPECT_ANY_THROW(behavior.connect(a, "a", b, "xxx"));
+
+ EXPECT_TRUE(behavior.connect(a, "a", b, "b"));
+}
+
+TEST(TransferPropertiesBehaviorTest, ValidUpdates)
+{
+ auto runtime = std::make_shared<Runtime>();
+ auto behavior = std::make_shared<TransferPropertiesBehavior>("test");
+ auto a = std::make_shared<A>(1,2);
+ auto b = std::make_shared<A>(3,4);
+ auto c = std::make_shared<A>(5,6);
+
+ EXPECT_TRUE(behavior->initialize(runtime));
+ EXPECT_TRUE(behavior->wakeUp());
+
+ EXPECT_TRUE(behavior->connect(a, "a", b, "a"));
+ EXPECT_TRUE(behavior->connect(b, "b", c, "b"));
+ EXPECT_TRUE(behavior->connect(a, "a", c, "a"));
+
+ behavior->update(0.0);
+
+ EXPECT_EQ(1, a->a);
+ EXPECT_EQ(2, a->b);
+
+ EXPECT_EQ(1, b->a);
+ EXPECT_EQ(4, b->b);
+
+ EXPECT_EQ(1, c->a);
+ EXPECT_EQ(4, c->b);
+}
+
+}
+}
+
diff --git a/SurgSim/Framework/UnitTests/config.txt.in b/SurgSim/Framework/UnitTests/config.txt.in
new file mode 100644
index 0000000..9c1cf05
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/config.txt.in
@@ -0,0 +1 @@
+${CMAKE_CURRENT_BINARY_DIR}/Data/
\ No newline at end of file
diff --git a/SurgSim/Graphics/AxesRepresentation.h b/SurgSim/Graphics/AxesRepresentation.h
new file mode 100644
index 0000000..a235f89
--- /dev/null
+++ b/SurgSim/Graphics/AxesRepresentation.h
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_AXESREPRESENTATION_H
+#define SURGSIM_GRAPHICS_AXESREPRESENTATION_H
+
+#include "SurgSim/Graphics/Representation.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Displays the coordinate axes, as three lines from the origin
+/// default size is 1.0, the X/Y/Z axis are indicated by R/G/B respectively
+class AxesRepresentation : public virtual Representation
+{
+public:
+
+ /// Constructor
+ explicit AxesRepresentation(const std::string& name) : Representation(name)
+ {
+
+ };
+
+ /// Sets the size of the shown axes.
+ /// \param val The value.
+ virtual void setSize(double val) = 0;
+
+ /// Gets the current size.
+ /// \return The size.
+ virtual double getSize() = 0;
+private:
+
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/BoxRepresentation.h b/SurgSim/Graphics/BoxRepresentation.h
new file mode 100644
index 0000000..8ce7f61
--- /dev/null
+++ b/SurgSim/Graphics/BoxRepresentation.h
@@ -0,0 +1,89 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_BOXREPRESENTATION_H
+#define SURGSIM_GRAPHICS_BOXREPRESENTATION_H
+
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base graphics box representation class, which defines the basic interface for a box that can be visualized.
+/// The box center is at (0, 0, 0).
+class BoxRepresentation : public virtual Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ /// \post The box size is (1.0,1.0,1.0).
+ explicit BoxRepresentation(const std::string& name) : Representation(name)
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoxRepresentation, SurgSim::Math::Vector3d, Size, getSize, setSize);
+ SURGSIM_ADD_RW_PROPERTY(BoxRepresentation, double, SizeX, getSizeX, setSizeX);
+ SURGSIM_ADD_RW_PROPERTY(BoxRepresentation, double, SizeY, getSizeY, setSizeY);
+ SURGSIM_ADD_RW_PROPERTY(BoxRepresentation, double, SizeZ, getSizeZ, setSizeZ);
+ }
+
+ /// Sets the size along X-axis of the box
+ /// \param sizeX Size along X-axis of the box
+ virtual void setSizeX(double sizeX) = 0;
+ /// Returns the size along X-axis of the box
+ /// \return Size along X-axis of the box
+ virtual double getSizeX() const = 0;
+
+ /// Sets the size along Y-axis of the box
+ /// \param sizeY Size along Y-axis of the box
+ virtual void setSizeY(double sizeY) = 0;
+ /// Returns the size along Y-axis of the box
+ /// \return Size along Y-axis of the box
+ virtual double getSizeY() const = 0;
+
+ /// Sets the size along Z-axis of the box
+ /// \param sizeZ Size along Z-axis of the box
+ virtual void setSizeZ(double sizeZ) = 0;
+ /// Returns the size along Z-axis of the box
+ /// \return Size along Z-axis of the box
+ virtual double getSizeZ() const = 0;
+
+ /// Sets the size of the box
+ /// \param sizeX Size along X-axis of the box
+ /// \param sizeY Size along Y-axis of the box
+ /// \param sizeZ Size along Z-axis of the box
+ virtual void setSizeXYZ(double sizeX, double sizeY, double sizeZ) = 0;
+ /// Gets the size of the box
+ /// \param sizeX Reference to store the size along X-axis of the box
+ /// \param sizeY Reference to store the size along Y-axis of the box
+ /// \param sizeZ Reference to store the size along Z-axis of the box
+ virtual void getSizeXYZ(double* sizeX, double* sizeY, double* sizeZ) const = 0;
+
+ /// Sets the size of the box
+ /// \param size Size of the box
+ virtual void setSize(const SurgSim::Math::Vector3d& size) = 0;
+ /// Returns the radius of the sphere
+ /// \return Size of the box
+ virtual SurgSim::Math::Vector3d getSize() const = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_BOXREPRESENTATION_H
diff --git a/SurgSim/Graphics/CMakeLists.txt b/SurgSim/Graphics/CMakeLists.txt
new file mode 100644
index 0000000..19e0e5e
--- /dev/null
+++ b/SurgSim/Graphics/CMakeLists.txt
@@ -0,0 +1,185 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ ${OSG_INCLUDE_DIR}
+ ${OPENTHREADS_INCLUDE_DIR}
+)
+
+set(SURGSIM_GRAPHICS_SOURCES
+ Camera.cpp
+ Group.cpp
+ Manager.cpp
+ Mesh.cpp
+ MeshPlyReaderDelegate.cpp
+ MeshUtilities.cpp
+ OsgAxesRepresentation.cpp
+ OsgBoxRepresentation.cpp
+ OsgCamera.cpp
+ OsgCapsuleRepresentation.cpp
+ OsgCylinderRepresentation.cpp
+ OsgGroup.cpp
+ OsgLight.cpp
+ OsgLog.cpp
+ OsgManager.cpp
+ OsgMaterial.cpp
+ OsgMeshRepresentation.cpp
+ OsgOctreeRepresentation.cpp
+ OsgPlaneRepresentation.cpp
+ OsgPointCloudRepresentation.cpp
+ OsgRepresentation.cpp
+ OsgSceneryRepresentation.cpp
+ OsgScreenSpacePass.cpp
+ OsgScreenSpaceQuadRepresentation.cpp
+ OsgShader.cpp
+ OsgSphereRepresentation.cpp
+ OsgTexture.cpp
+ OsgTexture1d.cpp
+ OsgTexture2d.cpp
+ OsgTexture3d.cpp
+ OsgTextureCubeMap.cpp
+ OsgTextureRectangle.cpp
+ OsgTrackballZoomManipulator.cpp
+ OsgUniformBase.cpp
+ OsgUniformFactory.cpp
+ OsgUnitAxes.cpp
+ OsgVectorFieldRepresentation.cpp
+ OsgView.cpp
+ OsgViewElement.cpp
+ PointCloudRepresentation.cpp
+ RenderPass.cpp
+ Representation.cpp
+ TriangleNormalGenerator.cpp
+ View.cpp
+ ViewElement.cpp
+)
+
+set(SURGSIM_GRAPHICS_HEADERS
+ AxesRepresentation.h
+ BoxRepresentation.h
+ Camera.h
+ CapsuleRepresentation.h
+ CylinderRepresentation.h
+ Group.h
+ Light.h
+ Manager.h
+ Material.h
+ Mesh.h
+ Mesh-inl.h
+ MeshPlyReaderDelegate.h
+ MeshRepresentation.h
+ MeshUtilities.h
+ OctreeRepresentation.h
+ OsgAxesRepresentation.h
+ OsgBoxRepresentation.h
+ OsgCamera.h
+ OsgCapsuleRepresentation.h
+ OsgConversions.h
+ OsgCylinderRepresentation.h
+ OsgGroup.h
+ OsgLight.h
+ OsgLog.h
+ OsgManager.h
+ OsgMaterial.h
+ OsgMatrixConversions.h
+ OsgMeshRepresentation.h
+ OsgOctreeRepresentation.h
+ OsgPlane.h
+ OsgPlaneRepresentation.h
+ OsgPointCloudRepresentation.h
+ OsgQuaternionConversions.h
+ OsgRenderTarget.h
+ OsgRenderTarget-inl.h
+ OsgRepresentation.h
+ OsgRigidTransformConversions.h
+ OsgSceneryRepresentation.h
+ OsgScreenSpacePass.h
+ OsgScreenSpaceQuadRepresentation.h
+ OsgShader.h
+ OsgSphereRepresentation.h
+ OsgTexture.h
+ OsgTexture1d.h
+ OsgTexture2d.h
+ OsgTexture3d.h
+ OsgTextureCubeMap.h
+ OsgTextureRectangle.h
+ OsgTextureUniform.h
+ OsgTextureUniform-inl.h
+ OsgTrackballZoomManipulator.h
+ OsgUniform.h
+ OsgUniform-inl.h
+ OsgUniformBase.h
+ OsgUniformFactory.h
+ OsgUniformTypes.h
+ OsgUnitAxes.h
+ OsgUnitBox.h
+ OsgUnitCylinder.h
+ OsgUnitSphere.h
+ OsgVectorConversions.h
+ OsgVectorFieldRepresentation.h
+ OsgView.h
+ OsgViewElement.h
+ PlaneRepresentation.h
+ PointCloudRepresentation.h
+ RenderPass.h
+ RenderTarget.h
+ Representation.h
+ SceneryRepresentation.h
+ ScreenSpaceQuadRepresentation.h
+ Shader.h
+ SphereRepresentation.h
+ Texture.h
+ Texture1d.h
+ Texture2d.h
+ Texture3d.h
+ TextureCubeMap.h
+ TextureRectangle.h
+ TriangleNormalGenerator.h
+ Uniform.h
+ UniformBase.h
+ VectorField.h
+ VectorFieldRepresentation.h
+ View.h
+ ViewElement.h
+)
+
+surgsim_add_library(
+ SurgSimGraphics
+ "${SURGSIM_GRAPHICS_SOURCES}"
+ "${SURGSIM_GRAPHICS_HEADERS}"
+ "SurgSim/Graphics"
+)
+
+SET(LIBS
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ KeyboardDevice
+ MouseDevice
+ ${OPENSCENEGRAPH_LIBRARIES}
+)
+
+target_link_libraries(SurgSimGraphics ${LIBS}
+)
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+
+ if(BUILD_RENDER_TESTING)
+ add_subdirectory(RenderTests)
+ endif()
+endif()
+
+set_target_properties(SurgSimGraphics PROPERTIES FOLDER "Graphics")
diff --git a/SurgSim/Graphics/Camera.cpp b/SurgSim/Graphics/Camera.cpp
new file mode 100644
index 0000000..27ed882
--- /dev/null
+++ b/SurgSim/Graphics/Camera.cpp
@@ -0,0 +1,80 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Math/MathConvert.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+Camera::Camera(const std::string& name) : Representation(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Camera, SurgSim::Math::Matrix44d, ProjectionMatrix,
+ getProjectionMatrix, setProjectionMatrix);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Camera, std::string, RenderGroupReference,
+ getRenderGroupReference, setRenderGroupReference);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Camera, SurgSim::Math::Vector4d, AmbientColor,
+ getAmbientColor, setAmbientColor);
+
+ SURGSIM_ADD_RO_PROPERTY(Camera, SurgSim::Math::Matrix44d, ViewMatrix, getViewMatrix);
+ SURGSIM_ADD_RO_PROPERTY(Camera, SurgSim::Math::Matrix44f, FloatViewMatrix, getViewMatrix);
+ SURGSIM_ADD_RO_PROPERTY(Camera, SurgSim::Math::Matrix44f, FloatProjectionMatrix, getProjectionMatrix);
+ SURGSIM_ADD_RO_PROPERTY(Camera, SurgSim::Math::Matrix44f, FloatInverseViewMatrix, getInverseViewMatrix);
+}
+
+void Camera::setRenderGroupReference(const std::string& name)
+{
+ removeGroupReference(name);
+ m_renderGroupReference = name;
+}
+
+std::string Camera::getRenderGroupReference() const
+{
+ return m_renderGroupReference;
+}
+
+bool Camera::setRenderGroup(std::shared_ptr<Group> group)
+{
+ m_group = group;
+ return true;
+}
+
+std::shared_ptr<Group> Camera::getRenderGroup() const
+{
+ return m_group;
+}
+
+bool Camera::addGroupReference(const std::string& name)
+{
+ bool result = false;
+ if (name != m_renderGroupReference)
+ {
+ result = Representation::addGroupReference(name);
+ }
+ return result;
+}
+
+bool Camera::doInitialize()
+{
+ SURGSIM_ASSERT(m_renderGroupReference != "")
+ << "Can't have a camera without a RenderGroupReference.";
+ return true;
+}
+
+}
+}
+
diff --git a/SurgSim/Graphics/Camera.h b/SurgSim/Graphics/Camera.h
new file mode 100644
index 0000000..39758ab
--- /dev/null
+++ b/SurgSim/Graphics/Camera.h
@@ -0,0 +1,145 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_CAMERA_H
+#define SURGSIM_GRAPHICS_CAMERA_H
+
+#include "SurgSim/Graphics/Representation.h"
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+class Group;
+class Texture;
+class RenderTarget;
+
+/// Base graphics camera class, which defines the basic interface for all graphics cameras.
+///
+/// A Graphics::Camera provides the viewpoint to visualize the Graphics::Group assigned to it.
+///
+/// To disable a camera: setVisible(false). To re-enable, setVisible(true).
+/// A disabled (invisible) camera does not produce an image.
+///
+/// Graphics::Camera is used with Graphics::View to provide the visualization of the virtual scene to the user.
+/// Cameras refer to a group that contain all the elements that they render, they may also parts of other group that
+/// determine whether they are rendered.
+/// It should provide the following Uniforms:
+/// \code
+/// uniform mat4 viewMatrix;
+/// uniform mat4 inverseViewMatrix;
+/// \endcode
+class Camera : public virtual Representation
+{
+public:
+
+ enum RenderOrder
+ {
+ RENDER_ORDER_PRE_RENDER = 0,
+ RENDER_ORDER_IN_ORDER,
+ RENDER_ORDER_POST_RENDER,
+ RENDER_ORDER_COUNT
+ };
+
+ /// Constructor
+ /// \param name Name of the camera
+ explicit Camera(const std::string& name);
+
+ /// Set the group reference that this camera wants to use as the group for rendering. Objects that, reference
+ /// the same group will be rendered by this camera. The manager will do the actual creation of the group.
+ /// \param name Name of the group to be used for rendering
+ void setRenderGroupReference(const std::string& name);
+
+ /// Gets the name of the rendergroup used for rendering
+ /// \return The name of the group to be used for rendering
+ std::string getRenderGroupReference() const;
+
+ /// Sets the group of representations that will be seen by this camera.
+ /// Only the representations in this group will be rendered when this camera's view is rendered.
+ /// \note The camera can not be part of the group that it is rendering
+ /// \param group Group of representations
+ /// \return True if it succeeded, false if it failed
+ virtual bool setRenderGroup(std::shared_ptr<Group> group);
+
+ /// Gets the group of representations that will be seen by this camera.
+ /// Only the representations in this group will be rendered when this camera's view is rendered.
+ /// \return Group of representations
+ std::shared_ptr<Group> getRenderGroup() const;
+
+ /// Gets the view matrix of the camera
+ /// \return View matrix
+ virtual SurgSim::Math::Matrix44d getViewMatrix() const = 0;
+
+ /// Gets the inverse view matrix of the camera
+ /// \return Inverse view matrix
+ virtual SurgSim::Math::Matrix44d getInverseViewMatrix() const = 0;
+
+ /// Sets the projection matrix of the camera
+ /// \param matrix Projection matrix
+ virtual void setProjectionMatrix(const SurgSim::Math::Matrix44d& matrix) = 0;
+
+ /// Gets the projection matrix of the camera
+ /// \return Projection matrix
+ virtual const SurgSim::Math::Matrix44d& getProjectionMatrix() const = 0;
+
+ /// Sets RenderTarget for the current camera, enables the camera to render to off-screen textures.
+ /// \param renderTarget The render target.
+ virtual bool setRenderTarget(std::shared_ptr<RenderTarget> renderTarget) = 0;
+
+ /// Gets RenderTarget that is currently being used by the camera.
+ /// \return The RenderTarget.
+ virtual std::shared_ptr<RenderTarget> getRenderTarget() const = 0;
+
+ /// Determine when this camera will render. The main camera will render at (RENDER_ORDER_IN_ORDER,0)
+ /// In general all preprocessing should be done in RENDER_ORDER_PRE_ORDER, HUD Displaying usually
+ /// at RENDER_ORDER_POST_ORDER
+ /// \param order The phase of rendering.
+ /// \param value The index within the phase, the order between two cameras of the same phase and index is not
+ /// determined.
+ virtual void setRenderOrder(RenderOrder order, int value) = 0;
+
+ virtual bool addGroupReference(const std::string& name) override;
+
+ /// Sets a value for the ambient lighting term, this can add light to the scene when there is no lighting
+ /// \param color value for the light that should get added to the scene
+ virtual void setAmbientColor(const SurgSim::Math::Vector4d& color) = 0;
+
+ /// \return the ambient light that gets added to the scene
+ virtual SurgSim::Math::Vector4d getAmbientColor() = 0;
+
+
+private:
+
+ virtual bool doInitialize() override;
+
+ /// Group of representations that this camera sees
+ /// Only the representations in this group will be rendered when this camera's view is rendered.
+ std::shared_ptr<Group> m_group;
+
+ /// The name of the group that the camera wants to use for rendering, the graphics manager will actually assign
+ /// this group
+ std::string m_renderGroupReference;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_CAMERA_H
diff --git a/SurgSim/Graphics/CapsuleRepresentation.h b/SurgSim/Graphics/CapsuleRepresentation.h
new file mode 100644
index 0000000..a5a7e42
--- /dev/null
+++ b/SurgSim/Graphics/CapsuleRepresentation.h
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_CAPSULEREPRESENTATION_H
+#define SURGSIM_GRAPHICS_CAPSULEREPRESENTATION_H
+
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base graphics capsule representation class, which defines the basic interface for a capsule that can be visualized.
+/// The capsule center is at (0, 0, 0).
+class CapsuleRepresentation : public virtual Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ /// \post The capsule radius is 1.0.
+ explicit CapsuleRepresentation(const std::string& name) : Representation(name)
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CapsuleRepresentation, double, Radius, getRadius, setRadius);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CapsuleRepresentation, double, Height, getHeight, setHeight);
+ }
+
+ /// Sets the radius of the capsule
+ /// \param radius Radius of the capsule
+ virtual void setRadius(double radius) = 0;
+ /// Returns the radius of the capsule
+ /// \return Radius along X-axis and Z-axis of the capsule
+ virtual double getRadius() const = 0;
+
+ /// Sets the height of the capsulei
+ /// \param height Height of the capsule
+ virtual void setHeight(double height) = 0;
+ /// Returns the height of the capsule
+ /// \return Height along Y-axis of the capsule
+ virtual double getHeight() const = 0;
+
+ /// Sets the size of the capsule
+ /// \param radius Size along X-axis and Z-axis of the capsule
+ /// \param height Size along Y-axis of the capsule
+ virtual void setSize(double radius, double height) = 0;
+ /// Gets the size of the capsule
+ /// \param [out] radius Variable to receive the size along X-axis and Z-axis of the capsule
+ /// \param [out] height Variable to receive the size along Y-axis of the capsule
+ virtual void getSize(double* radius, double* height) = 0;
+
+ /// Sets the size of the capsule
+ /// \param size Size of the capsule
+ virtual void setSize(const SurgSim::Math::Vector2d& size) = 0;
+ /// Returns the radius of the capsule
+ /// \return Size of the capsule
+ virtual SurgSim::Math::Vector2d getSize() const = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_CAPSULEREPRESENTATION_H
diff --git a/SurgSim/Graphics/CylinderRepresentation.h b/SurgSim/Graphics/CylinderRepresentation.h
new file mode 100644
index 0000000..7f3af2f
--- /dev/null
+++ b/SurgSim/Graphics/CylinderRepresentation.h
@@ -0,0 +1,78 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_CYLINDERREPRESENTATION_H
+#define SURGSIM_GRAPHICS_CYLINDERREPRESENTATION_H
+
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base graphics cylinder representation class,
+/// which defines the basic interface for a cylinder that can be visualized.
+/// The cylinder center is at (0, 0, 0).
+class CylinderRepresentation : public virtual Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ /// \post The cylinder radius is 1.0.
+ explicit CylinderRepresentation(const std::string& name) : Representation(name)
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CylinderRepresentation, double, Radius, getRadius, setRadius);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CylinderRepresentation, double, Height, getHeight, setHeight);
+ }
+
+ /// Sets the radius of the cylinder
+ /// \param radius Radius along X-axis and Z-axis of the cylinder
+ virtual void setRadius(double radius) = 0;
+ /// Returns the radius of the cylinder
+ /// \return Radius along X-axis and Z-axis of cylinder
+ virtual double getRadius() const = 0;
+
+ /// Sets the height of the cylinder
+ /// \param height Height along Y-axis of the cylinder
+ virtual void setHeight(double height) = 0;
+ /// Returns the height of the cylinder
+ /// \return Height along Y-axis of the cylinder
+ virtual double getHeight() const = 0;
+
+ /// Sets the size of the cylinder
+ /// \param radius Size along X-axis and Z-axis of the cylinder
+ /// \param height Size along Y-axis of the cylinder
+ virtual void setSize(double radius, double height) = 0;
+ /// Gets the size of the cylinder
+ /// \param [out] radius Variable to receive the size along X-axis and Z-axis of the cylinder
+ /// \param [out] height Variable to receive the size along Y-axis of the cylinder
+ virtual void getSize(double* radius, double* height) = 0;
+
+ /// Sets the size of the cylinder
+ /// \param size Size of the cylinder
+ virtual void setSize(const SurgSim::Math::Vector2d& size) = 0;
+ /// Returns the size of the cylinder
+ /// \return Size of the cylinder
+ virtual SurgSim::Math::Vector2d getSize() const = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_CYLINDERREPRESENTATION_H
diff --git a/SurgSim/Graphics/Group.cpp b/SurgSim/Graphics/Group.cpp
new file mode 100644
index 0000000..72af95d
--- /dev/null
+++ b/SurgSim/Graphics/Group.cpp
@@ -0,0 +1,88 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/Group.h"
+
+#include "SurgSim/Graphics/Representation.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+Group::Group(const std::string& name) :
+ m_name(name)
+{
+}
+
+Group::~Group()
+{
+}
+
+bool Group::add(std::shared_ptr<Representation> representation)
+{
+ bool result = false;
+ if (std::find(m_representations.begin(), m_representations.end(), representation) == m_representations.end())
+ {
+ m_representations.push_back(representation);
+ result = true;
+ }
+ return result;
+}
+
+bool Group::append(std::shared_ptr<Group> group)
+{
+ bool result = true;
+ const std::vector<std::shared_ptr<Representation>>& members = group->getMembers();
+ for (auto it = members.begin(); it != members.end(); ++it)
+ {
+ if (! add(*it))
+ {
+ result = false;
+ }
+ }
+ return result;
+}
+
+bool Group::remove(std::shared_ptr<Representation> representation)
+{
+ bool result = false;
+ auto it = std::find(m_representations.begin(), m_representations.end(), representation);
+ if (it != m_representations.end())
+ {
+ m_representations.erase(it);
+ result = true;
+ }
+ return result;
+}
+
+const std::vector<std::shared_ptr<Representation>>& Group::getMembers() const
+{
+ return m_representations;
+}
+
+void Group::clear()
+{
+ m_representations.clear();
+}
+
+std::string Group::getName() const
+{
+ return m_name;
+}
+
+}
+}
+
diff --git a/SurgSim/Graphics/Group.h b/SurgSim/Graphics/Group.h
new file mode 100644
index 0000000..7cceea0
--- /dev/null
+++ b/SurgSim/Graphics/Group.h
@@ -0,0 +1,92 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_GROUP_H
+#define SURGSIM_GRAPHICS_GROUP_H
+
+#include "SurgSim/Framework/Component.h"
+
+#include <memory>
+#include <vector>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+class Representation;
+
+/// Base graphics group class, which defines the interface that all graphics groups must implement.
+///
+/// Graphics::Group allows the organization of Graphics::Representation objects so that different algorithms can
+/// operate on specific sub-sets rather than the entire scene.
+class Group
+{
+public:
+ /// Constructor. The group is initially empty.
+ /// \param name Name of the group, this has to be unique over the whole system, otherwise
+ /// adding the group will fail
+ explicit Group(const std::string& name);
+
+ /// Destructor
+ virtual ~Group();
+
+ /// Sets whether the group is currently visible
+ /// \param visible True for visible, false for invisible
+ virtual void setVisible(bool visible) = 0;
+
+ /// Gets whether the group is currently visible
+ /// \return visible True for visible, false for invisible
+ virtual bool isVisible() const = 0;
+
+ /// Adds an representation
+ /// \param representation Representation to add to this group
+ /// \return True if the representation is added successfully, false if failure
+ virtual bool add(std::shared_ptr<Representation> representation);
+
+ /// Adds all representations in another group to this group
+ /// \param group Group of representations to add
+ /// \return True if all representations are added successfully, false if failure
+ virtual bool append(std::shared_ptr<Group> group);
+
+ /// Removes an representation.
+ /// \param representation Representation to remove from this group
+ /// \return True if the representation is removed successfully, false if representation is not in this group or
+ /// other failure.
+ virtual bool remove(std::shared_ptr<Representation> representation);
+
+ /// \return a container with all the representations in this group.
+ const std::vector<std::shared_ptr<Representation>>& getMembers() const;
+
+ /// Removes all representations.
+ virtual void clear();
+
+ /// \return The name of this group.
+ std::string getName() const;
+
+private:
+
+ std::string m_name;
+
+ /// Representations in this group
+ std::vector<std::shared_ptr<Representation>> m_representations;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_GROUP_H
diff --git a/SurgSim/Graphics/Light.h b/SurgSim/Graphics/Light.h
new file mode 100644
index 0000000..2319783
--- /dev/null
+++ b/SurgSim/Graphics/Light.h
@@ -0,0 +1,121 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_LIGHT_H
+#define SURGSIM_GRAPHICS_LIGHT_H
+
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class Group;
+
+/// Abstract interface for a light, a light needs to be assigned to a group to be active, only the members of this
+/// group will be considered to be lit by this light. Currently this light implements a pointlight. It will have to
+/// be extended for a directional and spot lights. The class should provide the following uniform values. The position
+/// is set by using the representations setPose() call.
+/// \code
+/// struct LightSource {
+/// vec4 ambient;
+/// vec4 diffuse;
+/// vec4 specular;
+/// vec4 position;
+/// float constantAttenuation;
+/// float linearAttenuation;
+/// float quadraticAttenuation;
+/// };
+///
+/// uniform LightSource lightSource;
+/// \endcode
+///
+class Light : public virtual Representation
+{
+public:
+
+ /// Constructor
+ explicit Light(const std::string& name) : Representation(name)
+ {
+ }
+
+ virtual ~Light()
+ {
+ }
+
+ /// Sets the group for this light, setting nullptr here will remove the light from its current group
+ /// \param group The group.
+ /// \return true if it succeeds, false if the group is not an OsgGroup.
+ virtual bool setGroup(std::shared_ptr<SurgSim::Graphics::Group> group) = 0;
+
+ /// Gets the group that this light has been assigned to.
+ /// \return The group or nullptr if no group has been set.
+ virtual std::shared_ptr<SurgSim::Graphics::Group> getGroup() = 0;
+
+ /// Sets diffuse color of this light.
+ /// \param color The color.
+ virtual void setDiffuseColor(const SurgSim::Math::Vector4d& color) = 0;
+
+ /// Gets diffuse color.
+ /// \return The diffuse color.
+ virtual SurgSim::Math::Vector4d getDiffuseColor() = 0;
+
+ /// Sets specular color of this light.
+ /// \param color The color.
+ virtual void setSpecularColor(const SurgSim::Math::Vector4d& color) = 0;
+
+ /// Gets specular color.
+ /// \return The specular color.
+ virtual SurgSim::Math::Vector4d getSpecularColor() = 0;
+
+ /// Sets constant attenuation.
+ /// \param val The value.
+ virtual void setConstantAttenuation(double val) = 0;
+
+ /// Gets constant attenuation.
+ /// \return The constant attenuation.
+ virtual double getConstantAttenuation() = 0;
+
+ /// Sets linear attenuation.
+ /// \param val The value.
+ virtual void setLinearAttenuation(double val) = 0;
+
+ /// Gets linear attenuation.
+ /// \return The linear attenuation.
+ virtual double getLinearAttenuation() = 0;
+
+ /// Sets quadratic attenuation.
+ /// \param val The value.
+ virtual void setQuadraticAttenuation(double val) = 0;
+
+ /// Gets quadratic attenuation.
+ /// \return The quadratic attenuation.
+ virtual double getQuadraticAttenuation() = 0;
+
+ /// Sets the name of the group that this light should work on
+ /// \param name The name of the group to light
+ virtual void setLightGroupReference(const std::string& name) = 0;
+
+ /// Gets the name of the group this light should operate on
+ /// \return the name of the group for this light
+ virtual std::string getLightGroupReference() = 0;
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/Manager.cpp b/SurgSim/Graphics/Manager.cpp
new file mode 100644
index 0000000..96ffdfd
--- /dev/null
+++ b/SurgSim/Graphics/Manager.cpp
@@ -0,0 +1,214 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/Manager.h"
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Graphics/Light.h"
+#include "SurgSim/Graphics/Group.h"
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Graphics/View.h"
+
+using SurgSim::Graphics::Camera;
+using SurgSim::Graphics::Group;
+using SurgSim::Graphics::Manager;
+using SurgSim::Graphics::Representation;
+using SurgSim::Graphics::View;
+
+Manager::Manager() : ComponentManager("Graphics Manager")
+{
+}
+
+Manager::~Manager()
+{
+}
+
+bool Manager::executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component)
+{
+ bool result = false;
+ std::shared_ptr<Representation> representation = std::dynamic_pointer_cast<Representation>(component);
+ if (representation != nullptr)
+ {
+ result = removeRepresentation(representation);
+ }
+
+ std::shared_ptr<View> view = std::dynamic_pointer_cast<View>(component);
+ if (view != nullptr)
+ {
+ result = removeView(view);
+ }
+ return result;
+}
+
+bool Manager::executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component)
+{
+ bool result = false;
+ std::shared_ptr<Representation> representation = std::dynamic_pointer_cast<Representation>(component);
+ if (representation != nullptr)
+ {
+ result = addRepresentation(representation);
+ }
+
+ std::shared_ptr<View> view = std::dynamic_pointer_cast<View>(component);
+ if (view != nullptr)
+ {
+ result = addView(view);
+ }
+ return result;
+}
+
+bool Manager::addRepresentation(std::shared_ptr<Representation> representation)
+{
+ bool result = false;
+ if (std::find(m_representations.begin(), m_representations.end(), representation) == m_representations.end())
+ {
+ m_representations.push_back(representation);
+
+ // Check all the groups that are requested for this representation, fetch them and
+ // add this representation
+ std::vector<std::string> requestedGroups = representation->getGroupReferences();
+ for (auto groupName = std::begin(requestedGroups); groupName != std::end(requestedGroups); ++groupName)
+ {
+ auto group = getOrCreateGroup(*groupName);
+ group->add(representation);
+ }
+
+ // Additionally for a camera create or fetch the RenderGroup
+ auto camera = std::dynamic_pointer_cast<Camera>(representation);
+ if (camera != nullptr)
+ {
+ camera->setRenderGroup(getOrCreateGroup(camera->getRenderGroupReference()));
+ }
+
+ auto light = std::dynamic_pointer_cast<Light>(representation);
+ if (light != nullptr)
+ {
+ light->setGroup(getOrCreateGroup(light->getLightGroupReference()));
+ }
+
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added representation " << representation->getName();
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Duplicate representation " << representation->getName();
+ }
+ return result;
+}
+
+bool Manager::addView(std::shared_ptr<View> view)
+{
+ bool result = false;
+ if (std::find(m_views.begin(), m_views.end(), view) == m_views.end())
+ {
+ m_views.push_back(view);
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added view " << view->getName();
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Duplicate view " << view->getName();
+ }
+ return result;
+}
+
+bool Manager::removeRepresentation(std::shared_ptr<Representation> representation)
+{
+ bool result = false;
+
+ auto groupReferences = representation->getGroupReferences();
+ for (auto it = groupReferences.cbegin(); it != groupReferences.cend(); ++it)
+ {
+ m_groups[*it]->remove(representation);
+ }
+
+ auto it = std::find(m_representations.begin(), m_representations.end(), representation);
+ if (it != m_representations.end())
+ {
+ m_representations.erase(it);
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Removed representation " << representation->getName();
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Representation not found " << representation->getName();
+ }
+ return result;
+}
+
+bool Manager::removeView(std::shared_ptr<View> view)
+{
+ bool result = false;
+ auto it = std::find(m_views.begin(), m_views.end(), view);
+ if (it != m_views.end())
+ {
+ m_views.erase(it);
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Removed view " << view->getName();
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " View not found " << view->getName();
+ }
+ return result;
+}
+
+bool Manager::doInitialize()
+{
+ return true;
+}
+
+bool Manager::doStartUp()
+{
+ return true;
+}
+
+bool Manager::doUpdate(double dt)
+{
+ processComponents();
+ processBehaviors(dt);
+
+ for (auto it = m_representations.begin(); it != m_representations.end(); ++it)
+ {
+ if ((*it)->isActive())
+ {
+ (*it)->update(dt);
+ }
+ }
+
+ for (auto it = m_views.begin(); it != m_views.end(); ++it)
+ {
+ if ((*it)->isActive())
+ {
+ (*it)->update(dt);
+ }
+ }
+
+ return true;
+}
+
+int Manager::getType() const
+{
+ return SurgSim::Framework::MANAGER_TYPE_GRAPHICS;
+}
+
+void SurgSim::Graphics::Manager::addGroup(std::shared_ptr<Group> group)
+{
+ auto oldGroup = m_groups.find(group->getName());
+ SURGSIM_ASSERT(oldGroup == m_groups.end()) << "Tried to add a group that has already been added.";
+ m_groups[group->getName()] = group;
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added group " << group->getName();
+}
diff --git a/SurgSim/Graphics/Manager.h b/SurgSim/Graphics/Manager.h
new file mode 100644
index 0000000..a53e64d
--- /dev/null
+++ b/SurgSim/Graphics/Manager.h
@@ -0,0 +1,142 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_MANAGER_H
+#define SURGSIM_GRAPHICS_MANAGER_H
+
+#include "SurgSim/Framework/ComponentManager.h"
+
+#include <memory>
+#include <vector>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+class Group;
+class Representation;
+class View;
+
+/// Basic graphics manager class which manages graphics components to provide a visualization of the scene to the user.
+///
+/// Graphics::Manager manages Graphics::Representation, Graphics::Group, and Graphics::View components.
+class Manager : public SurgSim::Framework::ComponentManager
+{
+public:
+ /// Constructor
+ Manager();
+ /// Destructor
+ virtual ~Manager();
+
+ /// Returns the representations assigned to the manager
+ const std::vector<std::shared_ptr<Representation>>& getRepresentations() const
+ {
+ return m_representations;
+ }
+
+ /// Returns the groups assigned to the manager
+ const std::unordered_map<std::string, std::shared_ptr<Group>>& getGroups() const
+ {
+ return m_groups;
+ }
+
+ /// Returns the views assigned to the manager
+ const std::vector<std::shared_ptr<View>>& getViews() const
+ {
+ return m_views;
+ }
+
+ /// Generic unspecified debug handle, there are no requirements on this interface
+ /// the manager implementation can decide what to do
+ virtual void dumpDebugInfo() const = 0;
+
+protected:
+
+ /// Adds a component
+ /// \param component The component to be added.
+ /// \return True if it succeeds or the manager is not concerned with the component, false if it fails.
+ virtual bool executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component);
+
+ /// Removes a component
+ /// \param component The component to be removed.
+ /// \return True if it succeeds or the manager is not concerned with the component, false if it fails.
+ virtual bool executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component);
+
+ /// Adds an representation to the manager. This will also add the representation to all of the groups
+ /// contained in its groupRenferences.
+ /// \param representation The representation to be added.
+ /// \return True if the representation was not in this manager and has been successfully added, false if it fails.
+ virtual bool addRepresentation(std::shared_ptr<Representation> representation);
+
+ /// Adds a view to the manager
+ /// \param view The view to be added.
+ /// \return True if the view was not in this manager and has been successfully added, false if it fails.
+ virtual bool addView(std::shared_ptr<View> view);
+
+ /// Removes an representation from the manager
+ /// \param representation The representation to be removed.
+ /// \return True if the representation was in this manager and has been successfully removed, false if it fails.
+ virtual bool removeRepresentation(std::shared_ptr<Representation> representation);
+
+ /// Removes a view from the manager
+ /// \param view The view to be removed.
+ /// \return True if the view was in this manager and has been successfully removed, false if it fails.
+ virtual bool removeView(std::shared_ptr<View> view);
+
+ /// Performs an update for a single timestep
+ /// \param dt The time in seconds of the preceding timestep.
+ virtual bool doUpdate(double dt);
+
+ /// Overrides ComponentManager::getType()
+ virtual int getType() const override;
+
+ /// Fetch a group with a given name, if the group does not exist, create it.
+ /// \param name Name of the group to be fetched.
+ /// \return group with the given name.
+ virtual std::shared_ptr<Group> getOrCreateGroup(const std::string& name) = 0;
+
+protected:
+ /// Adds a group to the manager, override for manager specific behavior when adding
+ /// \param group The group to be added.
+ virtual void addGroup(std::shared_ptr<Group> group);
+
+
+private:
+
+
+
+ /// Initializes the manager
+ /// \return True if it succeeds, false if it fails
+ virtual bool doInitialize();
+
+ /// Starts up the manager after all threads have initialized
+ /// \return True if it succeeds, false if it fails
+ virtual bool doStartUp();
+
+ /// Representations assigned to the manager
+ std::vector<std::shared_ptr<Representation>> m_representations;
+ /// Groups assigned to the manager
+ std::unordered_map<std::string, std::shared_ptr<Group>> m_groups;
+ /// Views assigned to the manager
+ std::vector<std::shared_ptr<View>> m_views;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_MANAGER_H
diff --git a/SurgSim/Graphics/Material.h b/SurgSim/Graphics/Material.h
new file mode 100644
index 0000000..fb07217
--- /dev/null
+++ b/SurgSim/Graphics/Material.h
@@ -0,0 +1,108 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_MATERIAL_H
+#define SURGSIM_GRAPHICS_MATERIAL_H
+
+#include <memory>
+#include <set>
+
+#include "SurgSim/Framework/Component.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+class UniformBase;
+class Shader;
+
+/// Base class that defines the interface for graphics materials.
+///
+/// Graphics materials define the visual appearance of graphics representations and are composed of the uniforms and
+/// shaders applied used when rendering the geometry.
+/// \sa UniformBase
+/// \sa Shader
+class Material : public SurgSim::Framework::Component
+{
+public:
+
+ /// Constructor
+ explicit Material(const std::string& name) : Component(name) {}
+
+ /// Destructor.
+ // (Note that Visual Studio does not support "= default" yet.)
+ virtual ~Material()
+ {
+ }
+
+ /// Adds a uniform to this material.
+ /// \param uniform Uniform to add.
+ /// \return True if uniform was added successfully, otherwise false.
+ virtual bool addUniform(std::shared_ptr<UniformBase> uniform) = 0;
+
+ /// Adds a GLSL typed uniform to this material
+ /// \param type the type of the uniform
+ /// \param name the name that this uniform should have
+ virtual bool addUniform(const std::string& type, const std::string& name) = 0;
+
+ /// Removes a uniform from this material.
+ /// \param uniform Uniform to remove.
+ /// \return True if uniform was removed successfully, otherwise false.
+ virtual bool removeUniform(std::shared_ptr<UniformBase> uniform) = 0;
+
+ /// Removes a uniform from this material.
+ /// \param name Uniform to remove.
+ /// \return True if uniform was removed successfully, otherwise false.
+ virtual bool removeUniform(const std::string& name) = 0;
+
+ /// Gets a uniform in this material.
+ /// \param name The name of the Uniform to fetch.
+ /// \return The uniform, nullptr if the uniform does not exist.
+ virtual std::shared_ptr<UniformBase> getUniform(const std::string& name) const = 0;
+
+ /// Gets a uniform in this material
+ /// \param index Index of the uniform in the material's list of uniforms
+ /// \return Uniform at the index
+ virtual std::shared_ptr<UniformBase> getUniform(size_t index) const = 0;
+
+ /// Checks if this material has a uniform with the given name.
+ /// \param name The name of the Uniform to check.
+ /// \return true if the uniform is in the material, false otherwise.
+ virtual bool hasUniform(const std::string& name) const = 0;
+
+ /// Returns the number of uniforms in this material.
+ virtual size_t getNumUniforms() const = 0;
+
+
+ /// Sets the shader used by this material.
+ /// \param shader Shader program.
+ /// \return True if shader was set successfully, otherwise false.
+ virtual bool setShader(std::shared_ptr<Shader> shader) = 0;
+
+ /// Gets the shader used by this material.
+ /// \return Shader program.
+ virtual std::shared_ptr<Shader> getShader() const = 0;
+
+ /// Removes the shader from the material, falling back to fixed-function pipeline.
+ virtual void clearShader() = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_MATERIAL_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/Mesh-inl.h b/SurgSim/Graphics/Mesh-inl.h
new file mode 100644
index 0000000..bda629a
--- /dev/null
+++ b/SurgSim/Graphics/Mesh-inl.h
@@ -0,0 +1,36 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_MESH_INL_H
+#define SURGSIM_GRAPHICS_MESH_INL_H
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+template <class VertexDataSource, class EdgeDataSource, class TriangleDataSource>
+Mesh::Mesh(const TriangleMeshBase<VertexDataSource, EdgeDataSource, TriangleDataSource>& mesh)
+ : SurgSim::DataStructures::TriangleMeshBase<VertexData,
+ SurgSim::DataStructures::EmptyData,
+ SurgSim::DataStructures::EmptyData>(mesh)
+{
+}
+
+}; // Graphics
+}; // SurgSim
+
+#endif // SURGSIM_GRAPHICS_MESH_INL_H
diff --git a/SurgSim/Graphics/Mesh.cpp b/SurgSim/Graphics/Mesh.cpp
new file mode 100644
index 0000000..6b3c751
--- /dev/null
+++ b/SurgSim/Graphics/Mesh.cpp
@@ -0,0 +1,92 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Graphics/MeshPlyReaderDelegate.h"
+
+
+using SurgSim::DataStructures::EmptyData;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+Mesh::Mesh()
+{
+}
+
+void Mesh::initialize(
+ const std::vector<SurgSim::Math::Vector3d>& vertices,
+ const std::vector<SurgSim::Math::Vector4d>& colors,
+ const std::vector<SurgSim::Math::Vector2d>& textures,
+ const std::vector<size_t>& triangles)
+{
+ SURGSIM_ASSERT(textures.empty() || textures.size() >= vertices.size()) <<
+ "To make a mesh you need to either provide at least the same amount" <<
+ " of texture coordinates as vertices or none at all.";
+ SURGSIM_ASSERT(colors.empty() || colors.size() >= vertices.size()) <<
+ "To make a mesh you need to either provide at least the same amount" <<
+ " of colors as vertices or none at all.";
+
+ clear();
+
+ size_t i = 0;
+ for (auto it = std::begin(vertices); it != std::end(vertices); ++it, ++i)
+ {
+ VertexData data;
+ if (! colors.empty())
+ {
+ data.color.setValue(colors[i]);
+ }
+ if (! textures.empty())
+ {
+ data.texture.setValue(textures[i]);
+ }
+ addVertex(Mesh::VertexType(*it, data));
+ }
+
+ for (size_t i = 0; i < triangles.size(); i += 3)
+ {
+ TriangleType::IdType ids = {{triangles[i], triangles[i + 1], triangles[i + 2]}};
+
+ bool valid = true;
+ for (auto it = std::begin(ids); it != std::end(ids); ++it)
+ {
+ if (*it >= getNumVertices())
+ {
+ valid = false;
+ break;
+ }
+ }
+
+ if (valid)
+ {
+ Mesh::TriangleType triangle(ids);
+ addTriangle(triangle);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getLogger("Graphics")) <<
+ "When building a mesh a vertex was present in a triangle that was not in the list of vertices";
+ }
+ }
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/Mesh.h b/SurgSim/Graphics/Mesh.h
new file mode 100644
index 0000000..46f54b4
--- /dev/null
+++ b/SurgSim/Graphics/Mesh.h
@@ -0,0 +1,91 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_MESH_H
+#define SURGSIM_GRAPHICS_MESH_H
+
+#include <vector>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+struct VertexData
+{
+ SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector2d> texture;
+ SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector4d> color;
+
+ /// Equality operator.
+ /// \param rhs The right hand side.
+ /// \return true if the parameters are considered equivalent.
+ bool operator==(const SurgSim::Graphics::VertexData& rhs) const
+ {
+ return texture == rhs.texture &&
+ color == rhs.color;
+ }
+
+ /// Inequality operator.
+ /// \param rhs The right hand side.
+ /// \return true if the parameters are not considered equivalent.
+ bool operator!=(const SurgSim::Graphics::VertexData& rhs) const
+ {
+ return !((*this) == rhs);
+ }
+};
+
+class Mesh : public SurgSim::DataStructures::TriangleMeshBase<VertexData, SurgSim::DataStructures::EmptyData,
+ SurgSim::DataStructures::EmptyData>
+{
+public:
+ /// Default constructor
+ Mesh();
+
+ /// Copy constructor
+ /// \tparam VertexDataSource Type of extra data stored in each vertex
+ /// \tparam EdgeDataSource Type of extra data stored in each edge
+ /// \tparam TriangleDataSource Type of extra data stored in each triangle
+ /// \param mesh The mesh to be copied from. Vertex, edge and triangle data will be emptied.
+ /// \note: Data of the input mesh, i.e. VertexDataSource, EdgeDataSource and TrianleDataSource will not be copied.
+ template <class VertexDataSource, class EdgeDataSource, class TriangleDataSource>
+ explicit Mesh(const TriangleMeshBase<VertexDataSource, EdgeDataSource, TriangleDataSource>& mesh);
+
+ /// Utility function to initialize a mesh with plain data,
+ /// \param vertices An array of vertex coordinates.
+ /// \param colors The colors, the number of colors can be 0 or
+ /// there have to be at least as many colors as vertices.
+ /// \param textures The textures coordinates, the number of coordinates can be 0 or
+ /// there have to be at least as many texture coordinates as there are vertices.
+ /// \param triangles The triangles, a plain array of triplets of triangle indices, the indices should be
+ /// points in the vertices array.
+ void initialize(const std::vector<SurgSim::Math::Vector3d>& vertices,
+ const std::vector<SurgSim::Math::Vector4d>& colors,
+ const std::vector<SurgSim::Math::Vector2d>& textures,
+ const std::vector<size_t>& triangles);
+};
+
+
+}; // Graphics
+}; // SurgSim
+
+#include "SurgSim/Graphics/Mesh-inl.h"
+
+#endif // SURGSIM_GRAPHICS_MESH_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/MeshPlyReaderDelegate.cpp b/SurgSim/Graphics/MeshPlyReaderDelegate.cpp
new file mode 100644
index 0000000..b91b30d
--- /dev/null
+++ b/SurgSim/Graphics/MeshPlyReaderDelegate.cpp
@@ -0,0 +1,56 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/MeshPlyReaderDelegate.h"
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+
+using SurgSim::DataStructures::PlyReader;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+MeshPlyReaderDelegate::MeshPlyReaderDelegate() :
+ SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<Mesh>()
+{
+
+}
+
+
+MeshPlyReaderDelegate::MeshPlyReaderDelegate(std::shared_ptr<MeshType> mesh) :
+ SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<Mesh>(mesh)
+
+{
+
+}
+
+void MeshPlyReaderDelegate::processVertex(const std::string& elementName)
+{
+ MeshType::VertexType vertex(SurgSim::Math::Vector3d(m_vertexData.x, m_vertexData.y, m_vertexData.z));
+
+ if (hasTextureCoordinates())
+ {
+ vertex.data.texture.setValue(SurgSim::Math::Vector2d(m_vertexData.s, m_vertexData.t));
+ }
+
+ m_mesh->addVertex(vertex);
+}
+
+}
+}
+
diff --git a/SurgSim/Graphics/MeshPlyReaderDelegate.h b/SurgSim/Graphics/MeshPlyReaderDelegate.h
new file mode 100644
index 0000000..90112cc
--- /dev/null
+++ b/SurgSim/Graphics/MeshPlyReaderDelegate.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_MESHPLYREADERDELEGATE_H
+#define SURGSIM_GRAPHICS_MESHPLYREADERDELEGATE_H
+
+#include <array>
+#include <memory>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h"
+#include "SurgSim/Graphics/Mesh.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Implementation of PlyReaderDelegate for graphicsmeshes
+class MeshPlyReaderDelegate : public SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<Mesh>
+{
+public:
+
+ /// Default constructor.
+ MeshPlyReaderDelegate();
+
+ /// Constructor.
+ /// \param mesh The mesh to be used, it will be cleared by the constructor.
+ explicit MeshPlyReaderDelegate(std::shared_ptr<MeshType> mesh);
+
+ virtual void processVertex(const std::string& elementName) override;
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Graphics/MeshRepresentation.h b/SurgSim/Graphics/MeshRepresentation.h
new file mode 100644
index 0000000..a0af003
--- /dev/null
+++ b/SurgSim/Graphics/MeshRepresentation.h
@@ -0,0 +1,83 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_MESHREPRESENTATION_H
+#define SURGSIM_GRAPHICS_MESHREPRESENTATION_H
+
+#include "SurgSim/Graphics/Representation.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+class Mesh;
+
+/// Graphics representation of a mesh, can be initialized from a Mesh structure
+class MeshRepresentation : public virtual Representation
+{
+public:
+
+ enum UpdateOption
+ {
+ UPDATE_OPTION_NONE = 0,
+ UPDATE_OPTION_VERTICES = 0x01,
+ UPDATE_OPTION_COLORS = 0x02,
+ UPDATE_OPTION_TEXTURES = 0x04,
+ UPDATE_OPTION_TRIANGLES = 0x08,
+ UPDATE_OPTION_ALL = UPDATE_OPTION_VERTICES | UPDATE_OPTION_COLORS |
+ UPDATE_OPTION_TEXTURES | UPDATE_OPTION_TRIANGLES
+ };
+
+ /// Constructor.
+ /// \param name The name of the representation.
+ explicit MeshRepresentation(const std::string& name) : Representation(name)
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(MeshRepresentation, std::string, Filename, getFilename, setFilename);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(MeshRepresentation, int, UpdateOptions, getUpdateOptions, setUpdateOptions);
+ }
+
+ virtual ~MeshRepresentation() {}
+
+ /// Gets the mesh.
+ /// \return The mesh.
+ virtual std::shared_ptr<Mesh> getMesh() = 0;
+
+ /// Set loading filename
+ /// \param filename The filename to load
+ /// \note The mesh will be loaded right after the file name is set,
+ /// if 'filename' indicates a file containing a valid mesh.
+ /// \note If the valid file contains an empty mesh, i.e. no vertex is specified in that file,
+ /// an empty mesh will be held.
+ virtual void setFilename(std::string filename) = 0;
+
+ /// Get the file name of the external file which contains the triangle mesh.
+ /// \return File name of the external file which contains the triangle mesh.
+ virtual std::string getFilename() const = 0;
+
+ /// Sets the structures that are expected to change during the lifetime of the mesh, these will be updated
+ /// every frame, independent of a structural change in the mesh. UPDATE_OPTION_VERTICES is set in the constructor
+ /// as a default value.
+ /// \param val Boolean or expression of UpdateOption enum.
+ virtual void setUpdateOptions(int val) = 0;
+
+ /// Gets update options for this mesh.
+ /// \return The update options.
+ virtual int getUpdateOptions() const = 0;
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif // SURGSIM_GRAPHICS_MESHREPRESENTATION_H
diff --git a/SurgSim/Graphics/MeshUtilities.cpp b/SurgSim/Graphics/MeshUtilities.cpp
new file mode 100644
index 0000000..2fc1963
--- /dev/null
+++ b/SurgSim/Graphics/MeshUtilities.cpp
@@ -0,0 +1,28 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/MeshUtilities.h"
+
+template <>
+std::shared_ptr<SurgSim::Graphics::Mesh> SurgSim::DataStructures::loadTriangleMesh(const std::string& fileName)
+{
+ SurgSim::DataStructures::PlyReader reader(fileName);
+ auto delegate = std::make_shared<SurgSim::Graphics::MeshPlyReaderDelegate>();
+ SURGSIM_ASSERT(reader.setDelegate(delegate)) << "The input file " << fileName << " is malformed.";
+ reader.parseFile();
+
+ return delegate->getMesh();
+}
+
diff --git a/SurgSim/Graphics/MeshUtilities.h b/SurgSim/Graphics/MeshUtilities.h
new file mode 100644
index 0000000..2cdd0f8
--- /dev/null
+++ b/SurgSim/Graphics/MeshUtilities.h
@@ -0,0 +1,39 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_MESHUTILITIES_H
+#define SURGSIM_GRAPHICS_MESHUTILITIES_H
+
+#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+#include "SurgSim/Graphics/MeshPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// Specialization for Graphics::Mesh
+/// Helper function to load a mesh from a given filename, does NOT do path resolution.
+/// \throws SurgSim::Framework::AssertionFailure if the reader does not contain mesh information.
+/// \param filename Path to the file that is to be read.
+/// \return the filled mesh a filled mesh if the reading succeeds, nullptr otherwise
+template <>
+std::shared_ptr<SurgSim::Graphics::Mesh> loadTriangleMesh(const std::string& fileName);
+
+}
+}
+
+
+#endif
diff --git a/SurgSim/Graphics/OctreeRepresentation.h b/SurgSim/Graphics/OctreeRepresentation.h
new file mode 100644
index 0000000..ae98aa1
--- /dev/null
+++ b/SurgSim/Graphics/OctreeRepresentation.h
@@ -0,0 +1,63 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OCTREEREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OCTREEREPRESENTATION_H
+
+#include <string>
+
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/OctreeShape.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Graphic representation of an Octree
+class OctreeRepresentation : public virtual Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of OctreeRepresentation
+ explicit OctreeRepresentation(const std::string& name) : Representation(name)
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OctreeRepresentation, std::shared_ptr<SurgSim::Math::Shape>, OctreeShape,
+ getOctreeShape, setOctreeShape);
+ }
+
+ /// Destructor
+ virtual ~OctreeRepresentation()
+ {
+ }
+
+ /// Set the OctreeShape of this representation. The Octree is retrieved and visualized.
+ /// \param shape The OctreeShape from which the octree is retrieved and visualized.
+ virtual void setOctreeShape(const std::shared_ptr<SurgSim::Math::Shape>& shape) = 0;
+
+ /// \return The OctreeShape from which the Octree is retrieved.
+ virtual std::shared_ptr<SurgSim::Math::OctreeShape> getOctreeShape() const = 0;
+
+ /// Mark the OctreeNode visible/invisible in the given a OctreePath (typedef-ed in OctreeNode.h).
+ /// \param path An OctreePath, giving the path leads to the OctreeNode whose visibility to be changed.
+ /// \param visibility Whether or not the OctreeNode specified by 'path' is visible or not.
+ virtual void setNodeVisible(const SurgSim::DataStructures::OctreePath& path, bool visibility) = 0;
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif // SURGSIM_GRAPHICS_OCTREEREPRESENTATION_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgAxesRepresentation.cpp b/SurgSim/Graphics/OsgAxesRepresentation.cpp
new file mode 100644
index 0000000..9bc8ef7
--- /dev/null
+++ b/SurgSim/Graphics/OsgAxesRepresentation.cpp
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Framework/SharedInstance.h"
+
+#include <osg/PositionAttitudeTransform>
+
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+OsgAxesRepresentation::OsgAxesRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ AxesRepresentation(name),
+ m_sharedUnitAxes(getShareUnitAxes()),
+ m_size(1.0)
+{
+ m_transform->addChild(m_sharedUnitAxes->getNode());
+}
+
+OsgAxesRepresentation::~OsgAxesRepresentation()
+{
+
+}
+
+std::shared_ptr<OsgUnitAxes> OsgAxesRepresentation::getShareUnitAxes()
+{
+ static SurgSim::Framework::SharedInstance<OsgUnitAxes> shared;
+ return shared.get();
+}
+
+void OsgAxesRepresentation::setSize(double val)
+{
+ m_size = val;
+ m_transform->setScale(osg::Vec3d(val,val,val));
+}
+
+double OsgAxesRepresentation::getSize()
+{
+ return m_size;
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/OsgAxesRepresentation.h b/SurgSim/Graphics/OsgAxesRepresentation.h
new file mode 100644
index 0000000..230a1b3
--- /dev/null
+++ b/SurgSim/Graphics/OsgAxesRepresentation.h
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGAXESREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGAXESREPRESENTATION_H
+
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/AxesRepresentation.h"
+#include "SurgSim/Graphics/OsgUnitAxes.h"
+
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Osg axes representation implementation for the AxesRepresentation interface in graphics.
+class OsgAxesRepresentation : public OsgRepresentation, public AxesRepresentation
+{
+public:
+
+ /// Constructor
+ explicit OsgAxesRepresentation(const std::string& name);
+ ~OsgAxesRepresentation();
+
+ /// Sets the size of the shown axes.
+ /// \param val The value.
+ virtual void setSize(double val) override;
+
+ /// Gets the current size.
+ /// \return The size.
+ virtual double getSize() override;
+
+private:
+
+ /// Shared unit axes, so that the geometry can be instanced rather than having multiple copies.
+ std::shared_ptr<OsgUnitAxes> m_sharedUnitAxes;
+
+ /// Returns the shared unit axes
+ static std::shared_ptr<OsgUnitAxes> getShareUnitAxes();
+
+ /// Hold the size
+ double m_size;
+};
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+}; // Graphics
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgBoxRepresentation.cpp b/SurgSim/Graphics/OsgBoxRepresentation.cpp
new file mode 100644
index 0000000..e413eae
--- /dev/null
+++ b/SurgSim/Graphics/OsgBoxRepresentation.cpp
@@ -0,0 +1,104 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgRigidTransformConversions.h"
+#include "SurgSim/Graphics/OsgUnitBox.h"
+
+#include <osg/Geode>
+#include <osg/Shape>
+#include <osg/ShapeDrawable>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgBoxRepresentation, OsgBoxRepresentation);
+
+OsgBoxRepresentation::OsgBoxRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ BoxRepresentation(name),
+ m_scale(1.0, 1.0, 1.0),
+ m_sharedUnitBox(getSharedUnitBox())
+{
+ m_transform->addChild(m_sharedUnitBox->getNode());
+}
+
+
+void OsgBoxRepresentation::setSizeX(double sizeX)
+{
+ m_scale.x() = sizeX;
+ m_transform->setScale(m_scale);
+}
+double OsgBoxRepresentation::getSizeX() const
+{
+ return m_scale.x();
+}
+
+void OsgBoxRepresentation::setSizeY(double sizeY)
+{
+ m_scale.y() = sizeY;
+ m_transform->setScale(m_scale);
+}
+double OsgBoxRepresentation::getSizeY() const
+{
+ return m_scale.y();
+}
+
+void OsgBoxRepresentation::setSizeZ(double sizeZ)
+{
+ m_scale.z() = sizeZ;
+ m_transform->setScale(m_scale);
+}
+double OsgBoxRepresentation::getSizeZ() const
+{
+ return m_scale.z();
+}
+
+void OsgBoxRepresentation::setSizeXYZ(double sizeX, double sizeY, double sizeZ)
+{
+ m_scale.x() = sizeX;
+ m_scale.y() = sizeY;
+ m_scale.z() = sizeZ;
+ m_transform->setScale(m_scale);
+}
+void OsgBoxRepresentation::getSizeXYZ(double* sizeX, double* sizeY, double* sizeZ) const
+{
+ *sizeX = m_scale.x();
+ *sizeY = m_scale.y();
+ *sizeZ = m_scale.z();
+}
+
+void OsgBoxRepresentation::setSize(const SurgSim::Math::Vector3d& size)
+{
+ m_scale.set(size.x(), size.y(), size.z());
+ m_transform->setScale(m_scale);
+}
+SurgSim::Math::Vector3d OsgBoxRepresentation::getSize() const
+{
+ return SurgSim::Math::Vector3d(m_scale._v);
+}
+
+std::shared_ptr<OsgUnitBox> OsgBoxRepresentation::getSharedUnitBox()
+{
+ static SurgSim::Framework::SharedInstance<OsgUnitBox> shared;
+ return shared.get();
+}
+
+}; // Graphics
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgBoxRepresentation.h b/SurgSim/Graphics/OsgBoxRepresentation.h
new file mode 100644
index 0000000..189fbee
--- /dev/null
+++ b/SurgSim/Graphics/OsgBoxRepresentation.h
@@ -0,0 +1,113 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGBOXREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGBOXREPRESENTATION_H
+
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Graphics/BoxRepresentation.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+#include <osg/PositionAttitudeTransform>
+#include <osg/Switch>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+class OsgUnitBox;
+
+SURGSIM_STATIC_REGISTRATION(OsgBoxRepresentation);
+
+/// OSG implementation of a graphics box representation.
+class OsgBoxRepresentation : public OsgRepresentation, public BoxRepresentation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit OsgBoxRepresentation(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgBoxRepresentation);
+
+ /// Sets the size along X-axis of the box
+ /// \param sizeX Size along X-axis of the box
+ virtual void setSizeX(double sizeX) override;
+
+ /// Returns the size along X-axis of the box
+ /// \return Size along X-axis of the box
+ virtual double getSizeX() const override;
+
+ /// Sets the size along Y-axis of the box
+ /// \param sizeY Size along Y-axis of the box
+ virtual void setSizeY(double sizeY) override;
+
+ /// Returns the size along Y-axis of the box
+ /// \return Size along Y-axis of the box
+ virtual double getSizeY() const override;
+
+ /// Sets the size along Z-axis of the box
+ /// \param sizeZ Size along Z-axis of the box
+ virtual void setSizeZ(double sizeZ) override;
+
+ /// Returns the size along Z-axis of the box
+ /// \return Size along Z-axis of the box
+ virtual double getSizeZ() const override;
+
+ /// Sets the size of the box
+ /// \param sizeX Size along X-axis of the box
+ /// \param sizeY Size along Y-axis of the box
+ /// \param sizeZ Size along Z-axis of the box
+ virtual void setSizeXYZ(double sizeX, double sizeY, double sizeZ);
+ /// Gets the size of the box
+ /// \param sizeX Reference to store the size along X-axis of the box
+ /// \param sizeY Reference to store the size along Y-axis of the box
+ /// \param sizeZ Reference to store the size along Z-axis of the box
+ virtual void getSizeXYZ(double* sizeX, double* sizeY, double* sizeZ) const;
+
+ /// Sets the size of the box
+ /// \param size Size of the box
+ virtual void setSize(const SurgSim::Math::Vector3d& size) override;
+
+ /// Returns the extents of the box
+ /// \return Size of the box
+ virtual SurgSim::Math::Vector3d getSize() const override;
+
+private:
+ /// The OSG box shape is a unit box and this transform scales it to the size set.
+ osg::Vec3d m_scale;
+
+ /// Shared unit box, so that the geometry can be instanced rather than having multiple copies.
+ std::shared_ptr<OsgUnitBox> m_sharedUnitBox;
+ /// Returns the shared unit box
+ static std::shared_ptr<OsgUnitBox> getSharedUnitBox();
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGBOXREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgCamera.cpp b/SurgSim/Graphics/OsgCamera.cpp
new file mode 100644
index 0000000..c72405e
--- /dev/null
+++ b/SurgSim/Graphics/OsgCamera.cpp
@@ -0,0 +1,314 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgCamera.h"
+
+#include "SurgSim/Graphics/Manager.h"
+#include "SurgSim/Graphics/Material.h"
+#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgMatrixConversions.h"
+#include "SurgSim/Graphics/OsgQuaternionConversions.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgVectorConversions.h"
+#include "SurgSim/Math/Matrix.h"
+
+#include <osgUtil/CullVisitor>
+
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Matrix44f;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Vector4f;
+
+namespace
+{
+const osg::Camera::BufferComponent ColorBufferEnums[16] =
+{
+ osg::Camera::COLOR_BUFFER0,
+ osg::Camera::COLOR_BUFFER1,
+ osg::Camera::COLOR_BUFFER2,
+ osg::Camera::COLOR_BUFFER3,
+ osg::Camera::COLOR_BUFFER4,
+ osg::Camera::COLOR_BUFFER5,
+ osg::Camera::COLOR_BUFFER6,
+ osg::Camera::COLOR_BUFFER7,
+ osg::Camera::COLOR_BUFFER8,
+ osg::Camera::COLOR_BUFFER9,
+ osg::Camera::COLOR_BUFFER10,
+ osg::Camera::COLOR_BUFFER11,
+ osg::Camera::COLOR_BUFFER12,
+ osg::Camera::COLOR_BUFFER13,
+ osg::Camera::COLOR_BUFFER14,
+ osg::Camera::COLOR_BUFFER15
+};
+
+const osg::Camera::RenderOrder RenderOrderEnums[3] =
+{
+ osg::Camera::PRE_RENDER,
+ osg::Camera::NESTED_RENDER,
+ osg::Camera::POST_RENDER
+};
+
+
+};
+
+
+namespace SurgSim
+{
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgCamera, OsgCamera);
+
+OsgCamera::OsgCamera(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ Camera(name),
+ m_camera(new osg::Camera()),
+ m_materialProxy(new osg::Group()),
+ m_viewMatrixUniform(std::make_shared<OsgUniform<Matrix44f>>("viewMatrix")),
+ m_inverseViewMatrixUniform(std::make_shared<OsgUniform<Matrix44f>>("inverseViewMatrix")),
+ m_ambientColorUniform(std::make_shared<OsgUniform<Vector4f>>("ambientColor"))
+{
+ m_switch->removeChildren(0, m_switch->getNumChildren());
+ m_camera->setName(name + " Camera");
+
+ m_switch->addChild(m_camera);
+ m_camera->addChild(m_materialProxy);
+ m_materialProxy->setName(name + " MaterialProxy");
+
+ m_camera->setViewMatrix(toOsg(getLocalPose().inverse().matrix()));
+ m_camera->setProjectionMatrixAsPerspective(45.0, 1.0, 0.01, 10.0);
+
+ m_camera->setComputeNearFarMode(osgUtil::CullVisitor::DO_NOT_COMPUTE_NEAR_FAR);
+
+ /// Update storage of view and projection matrices
+ m_projectionMatrix = fromOsg(m_camera->getProjectionMatrix());
+
+ // Set up uniforms
+ osg::ref_ptr<osg::StateSet> state = m_camera->getOrCreateStateSet();
+ m_viewMatrixUniform->addToStateSet(state);
+ m_inverseViewMatrixUniform->addToStateSet(state);
+ m_ambientColorUniform->addToStateSet(state);
+
+ setAmbientColor(Vector4d(0.0, 0.0, 0.0, 0.0));
+}
+
+bool OsgCamera::setRenderGroup(std::shared_ptr<SurgSim::Graphics::Group> group)
+{
+
+ SURGSIM_ASSERT(group->getName() == Camera::getRenderGroupReference())
+ << "Trying to set the wrong group in the camera. getRenderGroupName() returns <"
+ << Camera::getRenderGroupReference() << "> group->getName() is <" << group->getName() << ">.";
+
+ std::shared_ptr<OsgGroup> osgGroup = std::dynamic_pointer_cast<OsgGroup>(group);
+ if (osgGroup && SurgSim::Graphics::Camera::setRenderGroup(group))
+ {
+ m_materialProxy->removeChildren(0, m_camera->getNumChildren()); /// Remove any previous group
+ m_materialProxy->addChild(osgGroup->getOsgGroup());
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+void OsgCamera::setVisible(bool visible)
+{
+ m_switch->setChildValue(m_camera, visible);
+}
+
+bool OsgCamera::isVisible() const
+{
+ return m_switch->getChildValue(m_camera);
+}
+
+SurgSim::Math::Matrix44d OsgCamera::getViewMatrix() const
+{
+ return getPose().matrix().inverse();
+}
+
+void OsgCamera::setProjectionMatrix(const SurgSim::Math::Matrix44d& matrix)
+{
+ m_projectionMatrix = matrix;
+ m_camera->setProjectionMatrix(toOsg(matrix));
+}
+
+const SurgSim::Math::Matrix44d& OsgCamera::getProjectionMatrix() const
+{
+ return m_projectionMatrix;
+}
+
+void OsgCamera::update(double dt)
+{
+ // HS-2014-may-05 There is an issue between setting the projection matrix in the constructor and the
+ // instantiation of the viewer with the view port that may change the matrix inbetween, for now ... update
+ // every frame
+ // #workaround
+ m_projectionMatrix = fromOsg(m_camera->getProjectionMatrix());
+
+ auto viewMatrix = getViewMatrix();
+ auto floatMatrix = viewMatrix.cast<float>();
+ m_camera->setViewMatrix(toOsg(viewMatrix));
+ m_viewMatrixUniform->set(floatMatrix);
+ m_inverseViewMatrixUniform->set(floatMatrix.inverse());
+}
+
+bool OsgCamera::setRenderTarget(std::shared_ptr<RenderTarget> renderTarget)
+{
+ bool result = false;
+
+ // Check for correct dynamic type
+ auto osg2dTarget = std::dynamic_pointer_cast<OsgRenderTarget2d>(renderTarget);
+ auto osgRectTarget = std::dynamic_pointer_cast<OsgRenderTargetRectangle>(renderTarget);
+
+ if (osg2dTarget != nullptr || osgRectTarget != nullptr)
+ {
+ if (m_renderTarget == nullptr)
+ {
+ detachCurrentRenderTarget();
+ }
+
+ int width, height;
+ renderTarget->getSize(&width, &height);
+ m_camera->setViewport(0, 0, width, height);
+
+ attachRenderTargetTexture(osg::Camera::DEPTH_BUFFER, renderTarget->getDepthTarget());
+
+ // OSG has 16 COLOR_BUFFER objects
+ for (int i = 0; i < 16; ++i)
+ {
+ attachRenderTargetTexture(ColorBufferEnums[i], renderTarget->getColorTarget(i));
+ }
+
+ m_camera->setRenderTargetImplementation(osg::Camera::FRAME_BUFFER_OBJECT, osg::Camera::PIXEL_BUFFER);
+ m_camera->setRenderOrder(osg::Camera::PRE_RENDER);
+ m_camera->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
+ m_camera->setClearColor(osg::Vec4f(0.0, 0.0, 0.0, 1.0));
+ m_renderTarget = renderTarget;
+ result = true;
+ }
+ else if (renderTarget == nullptr && m_renderTarget != nullptr)
+ {
+ detachCurrentRenderTarget();
+ result = true;
+ }
+
+ return result;
+}
+
+std::shared_ptr<RenderTarget> OsgCamera::getRenderTarget() const
+{
+ return m_renderTarget;
+}
+
+
+bool OsgCamera::setMaterial(std::shared_ptr<Material> material)
+{
+ std::shared_ptr<OsgMaterial> osgMaterial = std::dynamic_pointer_cast<OsgMaterial>(material);
+ bool result = false;
+ if (osgMaterial != nullptr)
+ {
+ m_materialProxy->setStateSet(osgMaterial->getOsgStateSet());
+ result = true;
+ m_material = osgMaterial;
+ }
+ return result;
+}
+
+std::shared_ptr<Material> OsgCamera::getMaterial() const
+{
+ return m_material;
+}
+
+void OsgCamera::clearMaterial()
+{
+ m_materialProxy->setStateSet(new osg::StateSet());
+}
+
+void OsgCamera::detachCurrentRenderTarget()
+{
+ if (m_renderTarget != nullptr)
+ {
+ if (m_renderTarget->doesUseDepthTarget())
+ {
+ m_camera->detach(osg::Camera::DEPTH_BUFFER);
+ }
+ for (int i = 0; i < m_renderTarget->getColorTargetCount(); ++i)
+ {
+ m_camera->detach(ColorBufferEnums[i]);
+ }
+ }
+ m_renderTarget = nullptr;
+}
+
+void OsgCamera::attachRenderTargetTexture(osg::Camera::BufferComponent buffer, std::shared_ptr<Texture> texture)
+{
+ if (texture == nullptr)
+ {
+ return;
+ }
+
+ std::shared_ptr<OsgTexture> osgTexture = std::dynamic_pointer_cast<OsgTexture>(texture);
+ SURGSIM_ASSERT(osgTexture != nullptr) <<
+ "RenderTarget used a texture that was not an OsgTexture subclass";
+
+ osg::Texture* actualTexture = osgTexture->getOsgTexture();
+ SURGSIM_ASSERT(actualTexture != nullptr) <<
+ "Could not find texture";
+
+ m_camera->attach(buffer, actualTexture, 0, 0);
+}
+
+void OsgCamera::setRenderOrder(RenderOrder order, int value)
+{
+ if (order < RENDER_ORDER_COUNT)
+ {
+ m_camera->setRenderOrder(RenderOrderEnums[order], value);
+ }
+}
+
+osg::ref_ptr<osg::Camera> OsgCamera::getOsgCamera() const
+{
+ return m_camera;
+}
+
+osg::ref_ptr<osg::Node> OsgCamera::getOsgNode() const
+{
+ return m_switch;
+}
+
+SurgSim::Math::Matrix44d OsgCamera::getInverseViewMatrix() const
+{
+ return getPose().matrix();
+}
+
+void OsgCamera::setAmbientColor(const SurgSim::Math::Vector4d& color)
+{
+ m_ambientColor = color;
+ m_ambientColorUniform->set(color.cast<float>());
+}
+
+SurgSim::Math::Vector4d OsgCamera::getAmbientColor()
+{
+ return m_ambientColor;
+}
+
+
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
diff --git a/SurgSim/Graphics/OsgCamera.h b/SurgSim/Graphics/OsgCamera.h
new file mode 100644
index 0000000..66f760a
--- /dev/null
+++ b/SurgSim/Graphics/OsgCamera.h
@@ -0,0 +1,145 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGCAMERA_H
+#define SURGSIM_GRAPHICS_OSGCAMERA_H
+
+#include <unordered_map>
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/Texture.h"
+
+
+#include <osg/Camera>
+#include <osg/Switch>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class Material;
+class Texture;
+class RenderTarget;
+
+template <class T>
+class OsgUniform;
+
+SURGSIM_STATIC_REGISTRATION(OsgCamera);
+
+/// OSG implementation of a graphics camera.
+///
+/// A Graphics::OsgCamera wraps a osg::Camera to provide camera functionality and a osg::Switch to allow enabling and
+/// disabling of the camera.
+class OsgCamera : public OsgRepresentation, public Camera
+{
+public:
+ /// Constructor
+ /// \param name Name of the camera
+ /// The view matrix is initialized with eye at (0, 0, 0), center at (0, 0, -1), and up (0, 1, 0).
+ /// The projection matrix is initialized to a perspective matrix with FOV Y of 45 deg, Aspect Ratio of 1.0,
+ /// Z Near of 0.01, and Z Far of 10.0.
+ explicit OsgCamera(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgCamera);
+
+ virtual bool setRenderGroup(std::shared_ptr<Group> group) override;
+
+ virtual void setVisible(bool visible) override;
+
+ virtual bool isVisible() const override;
+
+ virtual SurgSim::Math::Matrix44d getViewMatrix() const;
+
+ virtual SurgSim::Math::Matrix44d getInverseViewMatrix() const;
+
+ virtual void setProjectionMatrix(const SurgSim::Math::Matrix44d& matrix) override;
+
+ virtual const SurgSim::Math::Matrix44d& getProjectionMatrix() const override;
+
+ virtual void update(double dt) override;
+
+ /// \return the OSG camera node
+ osg::ref_ptr<osg::Camera> getOsgCamera() const;
+
+ /// \return the OSG parent node for this object
+ osg::ref_ptr<osg::Node> getOsgNode() const;
+
+ virtual bool setRenderTarget(std::shared_ptr<RenderTarget> renderTarget) override;
+
+ virtual std::shared_ptr<RenderTarget> getRenderTarget() const override;
+
+ virtual bool setMaterial(std::shared_ptr<Material> material) override;
+
+ virtual std::shared_ptr<Material> getMaterial() const override;
+
+ virtual void clearMaterial() override;
+
+ virtual void setRenderOrder(RenderOrder order, int value) override;
+
+ virtual void setAmbientColor(const SurgSim::Math::Vector4d& color) override;
+
+ virtual SurgSim::Math::Vector4d getAmbientColor() override;
+
+private:
+
+ osg::ref_ptr<osg::Camera> m_camera;
+ osg::ref_ptr<osg::Group> m_materialProxy;
+
+ /// Projection matrix of the camera
+ SurgSim::Math::Matrix44d m_projectionMatrix;
+
+ std::unordered_map<int, std::shared_ptr<Texture>> m_textureMap;
+ std::shared_ptr<RenderTarget> m_renderTarget;
+
+ /// Attach a specific texture to a specific BufferComponent, works for Depth and all the Colors.
+ /// \param buffer The BufferComponent enum.
+ /// \param texture The specific texture to attach.
+ void attachRenderTargetTexture(osg::Camera::BufferComponent buffer, std::shared_ptr<Texture> texture);
+
+ /// Detach the current render target from the camera.
+ void detachCurrentRenderTarget();
+
+ /// Uniform to carry the view matrix
+ std::shared_ptr<OsgUniform<SurgSim::Math::Matrix44f>> m_viewMatrixUniform;
+
+ /// Uniform to carry the inverse view matrix
+ std::shared_ptr<OsgUniform<SurgSim::Math::Matrix44f>> m_inverseViewMatrixUniform;
+
+ /// Uniform to carry the ambient color
+ std::shared_ptr<OsgUniform<SurgSim::Math::Vector4f>> m_ambientColorUniform;
+
+ /// Value for ambient color
+ SurgSim::Math::Vector4d m_ambientColor;
+
+
+};
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGCAMERA_H
diff --git a/SurgSim/Graphics/OsgCapsuleRepresentation.cpp b/SurgSim/Graphics/OsgCapsuleRepresentation.cpp
new file mode 100644
index 0000000..b2562eb
--- /dev/null
+++ b/SurgSim/Graphics/OsgCapsuleRepresentation.cpp
@@ -0,0 +1,122 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgCapsuleRepresentation.h"
+#include "SurgSim/Graphics/OsgUnitCylinder.h"
+#include "SurgSim/Graphics/OsgUnitSphere.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgCapsuleRepresentation, OsgCapsuleRepresentation);
+
+OsgCapsuleRepresentation::OsgCapsuleRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ CapsuleRepresentation(name),
+ m_scale(1.0, 1.0),
+ m_sharedUnitCylinder(getSharedUnitCylinder()),
+ m_sharedUnitSphere(getSharedUnitSphere()),
+ m_patCylinder(new osg::PositionAttitudeTransform()),
+ m_patSphere1(new osg::PositionAttitudeTransform()),
+ m_patSphere2(new osg::PositionAttitudeTransform())
+{
+ m_patCylinder->addChild(m_sharedUnitCylinder->getNode());
+ m_patSphere1->addChild(m_sharedUnitSphere->getNode());
+ m_patSphere2->addChild(m_sharedUnitSphere->getNode());
+
+ m_patCylinder->setAttitude(osg::Quat(osg::PI_2, osg::Vec3d(1.0, 0.0, 0.0)));
+ m_patSphere1->setPosition(osg::Vec3d(0.0, 0.5, 0.0));
+ m_patSphere2->setPosition(osg::Vec3d(0.0, -0.5, 0.0));
+
+ m_transform->addChild(m_patCylinder);
+ m_transform->addChild(m_patSphere1);
+ m_transform->addChild(m_patSphere2);
+}
+
+
+void OsgCapsuleRepresentation::setRadius(double radius)
+{
+ m_scale.x() = radius;
+ m_patCylinder->setScale(osg::Vec3d(radius, radius, m_scale.y()));
+ m_patSphere1->setScale(osg::Vec3d(radius, radius, radius));
+ m_patSphere2->setScale(osg::Vec3d(radius, radius, radius));
+}
+double OsgCapsuleRepresentation::getRadius() const
+{
+ return m_scale.x();
+}
+
+void OsgCapsuleRepresentation::setHeight(double height)
+{
+ m_scale.y() = height;
+ m_patCylinder->setScale(osg::Vec3d(m_scale.x(), m_scale.x(), height));
+ m_patSphere1->setPosition(osg::Vec3d(0.0, height / 2, 0.0));
+ m_patSphere2->setPosition(osg::Vec3d(0.0, -height / 2, 0.0));
+}
+double OsgCapsuleRepresentation::getHeight() const
+{
+ return m_scale.y();
+}
+
+void OsgCapsuleRepresentation::setSize(double radius, double height)
+{
+ m_scale.x() = radius;
+ m_scale.y() = height;
+
+ m_patCylinder->setScale(osg::Vec3d(radius, radius, height));
+ m_patSphere1->setScale(osg::Vec3d(radius, radius, radius));
+ m_patSphere2->setScale(osg::Vec3d(radius, radius, radius));
+
+ m_patSphere1->setPosition(osg::Vec3d(0.0, height / 2, 0.0));
+ m_patSphere2->setPosition(osg::Vec3d(0.0, -height / 2, 0.0));
+}
+void OsgCapsuleRepresentation::getSize(double* radius, double* height)
+{
+ *radius = m_scale.x();
+ *height = m_scale.y();
+}
+
+void OsgCapsuleRepresentation::setSize(const SurgSim::Math::Vector2d& size)
+{
+ m_scale.set(size.x(), size.y());
+
+ m_patCylinder->setScale(osg::Vec3d(size.x(), size.x(), size.y()));
+ m_patSphere1->setScale(osg::Vec3d(size.x(), size.x(), size.x()));
+ m_patSphere2->setScale(osg::Vec3d(size.x(), size.x(), size.x()));
+
+ m_patSphere1->setPosition(osg::Vec3d(0.0, size.y() / 2, 0.0));
+ m_patSphere2->setPosition(osg::Vec3d(0.0, -size.y() / 2, 0.0));
+}
+SurgSim::Math::Vector2d OsgCapsuleRepresentation::getSize() const
+{
+ return SurgSim::Math::Vector2d(m_scale._v);
+}
+
+std::shared_ptr<OsgUnitCylinder> OsgCapsuleRepresentation::getSharedUnitCylinder()
+{
+ static SurgSim::Framework::SharedInstance<OsgUnitCylinder> shared;
+ return shared.get();
+}
+
+std::shared_ptr<OsgUnitSphere> OsgCapsuleRepresentation::getSharedUnitSphere()
+{
+ static SurgSim::Framework::SharedInstance<OsgUnitSphere> sharedSphere;
+ return sharedSphere.get();
+}
+
+}; // Graphics
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgCapsuleRepresentation.h b/SurgSim/Graphics/OsgCapsuleRepresentation.h
new file mode 100644
index 0000000..e244264
--- /dev/null
+++ b/SurgSim/Graphics/OsgCapsuleRepresentation.h
@@ -0,0 +1,108 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGCAPSULEREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGCAPSULEREPRESENTATION_H
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Graphics/CapsuleRepresentation.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+
+#include "SurgSim/Framework/SharedInstance.h"
+
+#include <osg/PositionAttitudeTransform>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+class OsgUnitCylinder;
+class OsgUnitSphere;
+
+SURGSIM_STATIC_REGISTRATION(OsgCapsuleRepresentation);
+
+/// OSG implementation of a graphics capsule representation.
+class OsgCapsuleRepresentation : public OsgRepresentation, public CapsuleRepresentation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit OsgCapsuleRepresentation(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgCapsuleRepresentation);
+
+ /// Sets the radius of the capsule
+ /// \param radius Radius of the capsule
+ virtual void setRadius(double radius) override;
+ /// Returns the radius of the capsule
+ /// \return Radius along X-axis and Z-axis of the capsule
+ virtual double getRadius() const override;
+
+ /// Sets the height of the capsule
+ /// \param height Height of the capsule
+ virtual void setHeight(double height) override;
+ /// Returns the height of the capsule
+ /// \return Height along Y-axis of the capsule
+ virtual double getHeight() const override;
+
+ /// Sets the size of the capsule
+ /// \param radius Size along X-axis and Z-axis of the capsule
+ /// \param height Size along Y-axis of the capsule
+ virtual void setSize(double radius, double height) override;
+ /// Gets the size of the capsule
+ /// \param [out] radius Variable to receive the size along X-axis and Z-axis of the capsule
+ /// \param [out] height Variable to receive the size along Y-axis of the capsule
+ virtual void getSize(double* radius, double* height) override;
+
+ /// Sets the size of the capsule
+ /// \param size Size of the capsule
+ virtual void setSize(const SurgSim::Math::Vector2d& size) override;
+ /// Returns the radius of the capsule
+ /// \return Size of the capsule
+ virtual SurgSim::Math::Vector2d getSize() const override;
+
+private:
+ /// The OSG Capsule shape consist of one unit cylinder and two unit spheres
+ /// This transform scales it to the size set.
+ osg::Vec2d m_scale;
+
+ /// Shared capsule, so that the geometry can be instanced rather than having multiple copies.
+ std::shared_ptr<OsgUnitCylinder> m_sharedUnitCylinder;
+ std::shared_ptr<OsgUnitSphere> m_sharedUnitSphere;
+ /// Returns the shared geometry
+ static std::shared_ptr<OsgUnitCylinder> getSharedUnitCylinder();
+ static std::shared_ptr<OsgUnitSphere> getSharedUnitSphere();
+
+ osg::ref_ptr<osg::PositionAttitudeTransform> m_patCylinder;
+ osg::ref_ptr<osg::PositionAttitudeTransform> m_patSphere1;
+ osg::ref_ptr<osg::PositionAttitudeTransform> m_patSphere2;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGCAPSULEREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgConversions.h b/SurgSim/Graphics/OsgConversions.h
new file mode 100644
index 0000000..5fd1099
--- /dev/null
+++ b/SurgSim/Graphics/OsgConversions.h
@@ -0,0 +1,27 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Conversions to and from OSG types
+
+#ifndef SURGSIM_GRAPHICS_OSGCONVERSIONS_H
+#define SURGSIM_GRAPHICS_OSGCONVERSIONS_H
+
+#include "SurgSim/Graphics/OsgMatrixConversions.h"
+#include "SurgSim/Graphics/OsgQuaternionConversions.h"
+#include "SurgSim/Graphics/OsgRigidTransformConversions.h"
+#include "SurgSim/Graphics/OsgVectorConversions.h"
+
+#endif // SURGSIM_GRAPHICS_OSGCONVERSIONS_H
diff --git a/SurgSim/Graphics/OsgCylinderRepresentation.cpp b/SurgSim/Graphics/OsgCylinderRepresentation.cpp
new file mode 100644
index 0000000..130508c
--- /dev/null
+++ b/SurgSim/Graphics/OsgCylinderRepresentation.cpp
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgCylinderRepresentation.h"
+
+#include "SurgSim/Graphics/OsgRigidTransformConversions.h"
+#include "SurgSim/Graphics/OsgUnitCylinder.h"
+
+#include "SurgSim/Framework/SharedInstance.h"
+
+#include <osg/Geode>
+#include <osg/Shape>
+#include <osg/ShapeDrawable>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgCylinderRepresentation,
+ OsgCylinderRepresentation);
+
+OsgCylinderRepresentation::OsgCylinderRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ CylinderRepresentation(name),
+ m_scale(1.0, 1.0),
+ m_sharedUnitCylinder(getSharedUnitCylinder()),
+ m_patCylinder(new osg::PositionAttitudeTransform)
+{
+ m_patCylinder->addChild(m_sharedUnitCylinder->getNode());
+ m_patCylinder->setAttitude(osg::Quat(osg::PI_2, osg::Vec3d(1.0, 0.0, 0.0)));
+ m_transform->addChild(m_patCylinder);
+}
+
+
+void OsgCylinderRepresentation::setRadius(double radius)
+{
+ m_scale.x() = radius;
+ m_patCylinder->setScale(osg::Vec3d(radius, radius, m_scale.y()));
+}
+double OsgCylinderRepresentation::getRadius() const
+{
+ return m_scale.x();
+}
+
+void OsgCylinderRepresentation::setHeight(double height)
+{
+ m_scale.y() = height;
+ m_patCylinder->setScale(osg::Vec3d(m_scale.x(), m_scale.x(), height));
+}
+double OsgCylinderRepresentation::getHeight() const
+{
+ return m_scale.y();
+}
+
+void OsgCylinderRepresentation::setSize(double radius, double height)
+{
+ m_scale.x() = radius;
+ m_scale.y() = height;
+ m_patCylinder->setScale(osg::Vec3d(radius, radius, height));
+}
+void OsgCylinderRepresentation::getSize(double* radius, double* height)
+{
+ *radius = m_scale.x();
+ *height = m_scale.y();
+}
+
+void OsgCylinderRepresentation::setSize(const SurgSim::Math::Vector2d& size)
+{
+ m_scale.set(size.x(), size.y());
+ m_patCylinder->setScale(osg::Vec3d(size.x(), size.x(), size.y()));
+}
+SurgSim::Math::Vector2d OsgCylinderRepresentation::getSize() const
+{
+ return SurgSim::Math::Vector2d(m_scale._v);
+}
+
+std::shared_ptr<OsgUnitCylinder> OsgCylinderRepresentation::getSharedUnitCylinder()
+{
+ static SurgSim::Framework::SharedInstance<OsgUnitCylinder> shared;
+ return shared.get();
+}
+
+}; // Graphics
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgCylinderRepresentation.h b/SurgSim/Graphics/OsgCylinderRepresentation.h
new file mode 100644
index 0000000..c42b988
--- /dev/null
+++ b/SurgSim/Graphics/OsgCylinderRepresentation.h
@@ -0,0 +1,99 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGCYLINDERREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGCYLINDERREPRESENTATION_H
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Graphics/CylinderRepresentation.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+
+#include <osg/PositionAttitudeTransform>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+class OsgUnitCylinder;
+
+SURGSIM_STATIC_REGISTRATION(OsgCylinderRepresentation);
+
+/// OSG implementation of a graphics Cylinder representation.
+class OsgCylinderRepresentation : public OsgRepresentation, public CylinderRepresentation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit OsgCylinderRepresentation(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgCylinderRepresentation);
+
+ /// Sets the radius of the cylinder
+ /// \param radius Radius along X-axis and Z-axis of the cylinder
+ virtual void setRadius(double radius) override;
+ /// Returns the radius of the cylinder
+ /// \return Radius along X-axis and Z-axis of cylinder
+ virtual double getRadius() const override;
+
+ /// Sets the height of the cylinder
+ /// \param height Height along Y-axis of the cylinder
+ virtual void setHeight(double height) override;
+ /// Returns the height of the cylinder
+ /// \return Height along Y-axis of the cylinder
+ virtual double getHeight() const override;
+
+ /// Sets the size of the cylinder
+ /// \param radius Size along X-axis and Z-axis of the cylinder
+ /// \param height Size along Y-axis of the cylinder
+ virtual void setSize(double radius, double height) override;
+ /// Gets the size of the cylinder
+ /// \param [out] radius Variable to receive the size along X-axis and Z-axis of the cylinder
+ /// \param [out] height Variable to receive the size along Y-axis of the cylinder
+ virtual void getSize(double* radius, double* height) override;
+
+ /// Sets the size of the cylinder
+ /// \param size Size of the cylinder
+ virtual void setSize(const SurgSim::Math::Vector2d& size) override;
+ /// Returns the size of the cylinder
+ /// \return Size of the cylinder
+ virtual SurgSim::Math::Vector2d getSize() const override;
+
+private:
+ /// The OSG Cylinder shape is a unit Cylinder and this transform scales it to the size set.
+ osg::Vec2d m_scale;
+
+ /// Shared unit Cylinder, so that the geometry can be instanced rather than having multiple copies.
+ std::shared_ptr<OsgUnitCylinder> m_sharedUnitCylinder;
+ /// Returns the shared unit cylinder
+ static std::shared_ptr<OsgUnitCylinder> getSharedUnitCylinder();
+
+ osg::ref_ptr<osg::PositionAttitudeTransform> m_patCylinder;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGCYLINDERREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgGroup.cpp b/SurgSim/Graphics/OsgGroup.cpp
new file mode 100644
index 0000000..af34e93
--- /dev/null
+++ b/SurgSim/Graphics/OsgGroup.cpp
@@ -0,0 +1,109 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgGroup.h"
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+
+using SurgSim::Graphics::OsgRepresentation;
+using SurgSim::Graphics::OsgGroup;
+
+OsgGroup::OsgGroup(const std::string& name) : SurgSim::Graphics::Group(name),
+ m_isVisible(true),
+ m_switch(new osg::Switch())
+{
+ m_switch->getOrCreateStateSet()->setGlobalDefaults();
+ m_switch->setName(name + " Switch");
+};
+
+void OsgGroup::setVisible(bool visible)
+{
+ m_isVisible = visible;
+
+ if (m_isVisible)
+ {
+ m_switch->setAllChildrenOn();
+ }
+ else
+ {
+ m_switch->setAllChildrenOff();
+ }
+}
+
+bool OsgGroup::isVisible() const
+{
+ return m_isVisible;
+}
+
+bool OsgGroup::add(std::shared_ptr<SurgSim::Graphics::Representation> representation)
+{
+ std::shared_ptr<OsgRepresentation> osgRepresentation = std::dynamic_pointer_cast<OsgRepresentation>(representation);
+
+ if (osgRepresentation && Group::add(osgRepresentation))
+ {
+ m_switch->addChild(osgRepresentation->getOsgNode());
+ m_switch->setChildValue(osgRepresentation->getOsgNode(), m_isVisible);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+bool OsgGroup::append(std::shared_ptr<SurgSim::Graphics::Group> group)
+{
+ std::shared_ptr<OsgGroup> osgGroup = std::dynamic_pointer_cast<OsgGroup>(group);
+
+ if (osgGroup && Group::append(osgGroup))
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+bool OsgGroup::remove(std::shared_ptr<SurgSim::Graphics::Representation> representation)
+{
+ std::shared_ptr<OsgRepresentation> osgRepresentation = std::dynamic_pointer_cast<OsgRepresentation>(representation);
+
+ if (osgRepresentation && Group::remove(osgRepresentation))
+ {
+ m_switch->removeChild(osgRepresentation->getOsgNode());
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+void OsgGroup::clear()
+{
+ while (!getMembers().empty())
+ {
+ std::shared_ptr<Representation> representation = getMembers().front();
+ SURGSIM_ASSERT(remove(representation)) << "Removal of representation " << representation->getName() <<
+ " failed while attempting to clear group " << getName() << "!";
+ }
+}
+
+osg::ref_ptr<osg::Group> OsgGroup::getOsgGroup() const
+{
+ return m_switch;
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgGroup.h b/SurgSim/Graphics/OsgGroup.h
new file mode 100644
index 0000000..0a89e1e
--- /dev/null
+++ b/SurgSim/Graphics/OsgGroup.h
@@ -0,0 +1,86 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGGROUP_H
+#define SURGSIM_GRAPHICS_OSGGROUP_H
+
+#include "SurgSim/Graphics/Group.h"
+
+#include <osg/Group>
+#include <osg/Switch>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG implementation of a graphics group.
+///
+/// A Graphics::OsgGroup wraps a osg::Switch to provide group functionality.
+class OsgGroup : public Group
+{
+public:
+ /// Constructor
+ /// \param name Name of the group
+ explicit OsgGroup(const std::string& name);
+
+ /// Sets whether this group is currently visible
+ /// \param visible True for visible, false for invisible
+ virtual void setVisible(bool visible) override;
+
+ /// Gets whether this group is currently visible
+ /// \return True for visible, false for invisible
+ virtual bool isVisible() const override;
+
+ /// Adds an representation
+ /// \param representation Representation to add to this group
+ /// \return True if the representation is added successfully, false if failure
+ /// Only subclasses of OsgRepresentation will be added successfully.
+ virtual bool add(std::shared_ptr<Representation> representation) override;
+
+ /// Adds all representations in another group to this group
+ /// \param group Group of representations to add
+ /// \return True if all representations are added successfully, false if failure
+ /// Only subclasses of OsgGroup will be appended successfully.
+ virtual bool append(std::shared_ptr<Group> group) override;
+
+ /// Removes an representation
+ /// \param representation Representation to remove from this group
+ /// \return True if the representation is removed successfully, false if representation is not in this group or
+ /// other failure
+ virtual bool remove(std::shared_ptr<Representation> representation) override;
+
+ /// Removes all representations
+ virtual void clear() override;
+
+ /// Returns the root OSG group node
+ osg::ref_ptr<osg::Group> getOsgGroup() const;
+
+private:
+ /// Whether the group is currently visible or not
+ /// Newly added representations or groups will have this visibility.
+ bool m_isVisible;
+
+ /// OSG group node
+ /// A switch is used to provide visibility functionality.
+ osg::ref_ptr<osg::Switch> m_switch;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGGROUP_H
diff --git a/SurgSim/Graphics/OsgLight.cpp b/SurgSim/Graphics/OsgLight.cpp
new file mode 100644
index 0000000..99d2f10
--- /dev/null
+++ b/SurgSim/Graphics/OsgLight.cpp
@@ -0,0 +1,234 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+
+#include "SurgSim/Graphics/OsgLight.h"
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgConversions.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+
+#include <osg/Uniform>
+#include <osg/Light>
+#include <osg/LightSource>
+#include <osg/Node>
+
+using osg::Uniform;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// \note HS-2013-sep-09 Right now we are implementing all the shader uniforms as floats, this
+/// means that they all have to be downconverted from double, i don't know what the hit
+/// of going to double in the shaders would be
+OsgLight::OsgLight(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ Light(name),
+ m_diffuseColor(1.0, 1.0, 1.0, 1.0),
+ m_specularColor(1.0, 1.0, 1.0, 1.0),
+ m_constantAttenuation(1.0),
+ m_linearAttenuation(0.0),
+ m_quadraticAttenuation(0.0)
+{
+ std::string prefix = "lightSource.";
+
+ m_light = new osg::Light();
+ m_light->setName(name);
+ m_light->setLightNum(0);
+ m_lightSource = new osg::LightSource();
+ m_lightSource->setDataVariance(osg::Object::DYNAMIC);
+ m_lightSource->setLight(m_light);
+
+ m_switch->addChild(m_lightSource);
+
+ m_uniforms[POSITION] = new osg::Uniform(Uniform::FLOAT_VEC4, prefix + "position");
+
+ m_uniforms[DIFFUSE_COLOR] = new osg::Uniform(Uniform::FLOAT_VEC4, prefix + "diffuse");
+ setDiffuseColor(m_diffuseColor);
+
+ m_uniforms[SPECULAR_COLOR] = new osg::Uniform(Uniform::FLOAT_VEC4, prefix + "specular");
+ setSpecularColor(m_specularColor);
+
+ m_uniforms[CONSTANT_ATTENUATION] = new osg::Uniform(Uniform::FLOAT, prefix + "constantAttenuation");
+ setConstantAttenuation(m_constantAttenuation);
+
+ m_uniforms[LINEAR_ATTENUATION] = new osg::Uniform(Uniform::FLOAT, prefix + "linearAttenuation");
+ setLinearAttenuation(m_linearAttenuation);
+
+ m_uniforms[QUADRATIC_ATTENUATION] = new osg::Uniform(Uniform::FLOAT, prefix + "quadraticAttenuation");
+ setQuadraticAttenuation(m_quadraticAttenuation);
+
+ // By default light the main group
+ setLightGroupReference(SurgSim::Graphics::Representation::DefaultGroupName);
+
+}
+
+OsgLight::~OsgLight()
+{
+
+}
+
+
+
+bool OsgLight::setGroup(std::shared_ptr<SurgSim::Graphics::Group> group)
+{
+ std::shared_ptr<OsgGroup> newGroup = std::dynamic_pointer_cast<OsgGroup>(group);
+
+ if (group != nullptr && newGroup == nullptr)
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getLogger("Graphics"))
+ << "OsgLight::setGroup() called with a group that is not an OsgGroup.";
+ }
+
+ bool clearGroup = m_group != nullptr && (newGroup != nullptr || group == nullptr);
+ bool setGroup = (newGroup != nullptr);
+ bool success = (group != nullptr && newGroup != nullptr) || group == nullptr;
+
+ if (clearGroup)
+ {
+ osg::ref_ptr<osg::StateSet> stateSet = m_group->getOsgGroup()->getOrCreateStateSet();
+ remove(stateSet);
+ m_group = nullptr;
+ }
+
+ if (setGroup)
+ {
+ osg::ref_ptr<osg::StateSet> stateSet = newGroup->getOsgGroup()->getOrCreateStateSet();
+ apply(stateSet);
+ m_group = newGroup;
+ }
+
+ return success;
+}
+
+std::shared_ptr<SurgSim::Graphics::Group> OsgLight::getGroup()
+{
+ return m_group;
+}
+
+void OsgLight::setDiffuseColor(const SurgSim::Math::Vector4d& color)
+{
+ m_diffuseColor = color;
+ SurgSim::Math::Vector4f floatColor = color.cast<float>();
+ osg::Vec4f osgVec = toOsg(floatColor);
+ m_uniforms[DIFFUSE_COLOR]->set(osgVec);
+ m_light->setDiffuse(osgVec);
+}
+
+SurgSim::Math::Vector4d OsgLight::getDiffuseColor()
+{
+ return m_diffuseColor;
+}
+
+void OsgLight::setSpecularColor(const SurgSim::Math::Vector4d& color)
+{
+ m_specularColor = color;
+ SurgSim::Math::Vector4f floatColor = color.cast<float>();
+ osg::Vec4f osgVec = toOsg(floatColor);
+ m_uniforms[SPECULAR_COLOR]->set(osgVec);
+ m_light->setSpecular(osgVec);
+}
+
+SurgSim::Math::Vector4d OsgLight::getSpecularColor()
+{
+ return m_specularColor;
+}
+
+void OsgLight::setConstantAttenuation(double val)
+{
+ m_constantAttenuation = val;
+ m_uniforms[CONSTANT_ATTENUATION]->set(static_cast<float>(val));
+ m_light->setConstantAttenuation(val);
+}
+
+double OsgLight::getConstantAttenuation()
+{
+ return m_constantAttenuation;
+}
+
+void OsgLight::setLinearAttenuation(double val)
+{
+ m_linearAttenuation = val;
+ m_uniforms[LINEAR_ATTENUATION]->set(static_cast<float>(val));
+ m_light->setLinearAttenuation(val);
+}
+
+double OsgLight::getLinearAttenuation()
+{
+ return m_linearAttenuation;
+}
+
+void OsgLight::setQuadraticAttenuation(double val)
+{
+ m_quadraticAttenuation = val;
+ m_uniforms[QUADRATIC_ATTENUATION]->set(static_cast<float>(val));
+ m_light->setQuadraticAttenuation(val);
+}
+
+double OsgLight::getQuadraticAttenuation()
+{
+ return m_quadraticAttenuation;
+}
+
+void OsgLight::doUpdate(double dt)
+{
+ SurgSim::Math::Vector3f position = getPose().translation().cast<float>();
+ osg::Vec4f osgVec(osg::Vec4f(toOsg(position), 1.0));
+ m_uniforms[POSITION]->set(osgVec);
+ m_light->setPosition(osgVec);
+}
+
+void OsgLight::apply(osg::ref_ptr<osg::StateSet> stateSet)
+{
+ for (auto it = std::begin(m_uniforms); it != std::end(m_uniforms); ++it)
+ {
+ stateSet->addUniform(it->second);
+ }
+}
+
+void OsgLight::remove(osg::ref_ptr<osg::StateSet> stateSet)
+{
+ for (auto it = std::begin(m_uniforms); it != std::end(m_uniforms); ++it)
+ {
+ stateSet->removeUniform(it->second);
+ }
+}
+
+void OsgLight::setLightGroupReference(const std::string& name)
+{
+ m_groupReference = name;
+ removeGroupReference(name);
+}
+
+std::string OsgLight::getLightGroupReference()
+{
+ return m_groupReference;
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/OsgLight.h b/SurgSim/Graphics/OsgLight.h
new file mode 100644
index 0000000..1022edb
--- /dev/null
+++ b/SurgSim/Graphics/OsgLight.h
@@ -0,0 +1,141 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGLIGHT_H
+#define SURGSIM_GRAPHICS_OSGLIGHT_H
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/Light.h"
+
+#include <osg/ref_ptr>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace osg
+{
+
+class Uniform;
+class StateSet;
+class Light;
+class LightSource;
+
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class OsgGroup;
+
+/// OpenScenegraph implementation for the Light interface
+class OsgLight : public OsgRepresentation, public Light
+{
+public:
+
+ /// If we use the uniforms map, to check for all the uniforms that should be set in the stateset, then
+ /// we can reduce the breakability of the unittest
+ friend class OsgLightTests;
+
+ /// Constructor
+ explicit OsgLight(const std::string& name);
+ virtual ~OsgLight();
+
+ virtual bool setGroup(std::shared_ptr<SurgSim::Graphics::Group> group) override;
+
+ virtual void setLightGroupReference(const std::string& name) override;
+
+ virtual std::string getLightGroupReference() override;
+
+ virtual std::shared_ptr<SurgSim::Graphics::Group> getGroup() override;
+
+ virtual void setDiffuseColor(const SurgSim::Math::Vector4d& color) override;
+
+ virtual SurgSim::Math::Vector4d getDiffuseColor() override;
+
+ virtual void setSpecularColor(const SurgSim::Math::Vector4d& color) override;
+
+ virtual SurgSim::Math::Vector4d getSpecularColor() override;
+
+ virtual void setConstantAttenuation(double val) override;
+
+ virtual double getConstantAttenuation() override;
+
+ virtual void setLinearAttenuation(double val) override;
+
+ virtual double getLinearAttenuation() override;
+
+ virtual void setQuadraticAttenuation(double val) override;
+
+ virtual double getQuadraticAttenuation() override;
+
+
+private:
+ virtual void doUpdate(double dt) override;
+
+ /// Applies all the lights variables to the given StateSet
+ void apply(osg::ref_ptr<osg::StateSet> stateSet);
+
+ /// Removes all the lights variable from the given StateSet
+ void remove(osg::ref_ptr<osg::StateSet> stateSet);
+
+ /// Internal for managing uniforms
+ enum UniformType
+ {
+ POSITION = 0,
+ DIFFUSE_COLOR,
+ SPECULAR_COLOR,
+ CONSTANT_ATTENUATION,
+ LINEAR_ATTENUATION,
+ QUADRATIC_ATTENUATION
+ };
+
+ /// The group for this light
+ std::shared_ptr<OsgGroup> m_group;
+
+ /// Map for managing all uniforms that this object owns
+ std::unordered_map<int, osg::ref_ptr<osg::Uniform>> m_uniforms;
+
+ SurgSim::Math::Vector4d m_diffuseColor; ///< The actual diffuse color that was set
+ SurgSim::Math::Vector4d m_specularColor; ///< The actual specular color that was set
+
+ double m_constantAttenuation; ///< The actual constant attenuation value that was set
+ double m_linearAttenuation; ///< The actual linear attenuation value that was set
+ double m_quadraticAttenuation; ///< The actual quadratic attenuation value that was set
+
+ ///@{
+ /// Osg instances to manage the light
+ osg::ref_ptr<osg::Light> m_light;
+ osg::ref_ptr<osg::LightSource> m_lightSource;
+ ///@}
+
+ std::string m_groupReference; ///< Name of the group that this light should shine on...
+};
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+}; // Graphics
+}; // SurgSim
+
+#endif
diff --git a/SurgSim/Graphics/OsgLog.cpp b/SurgSim/Graphics/OsgLog.cpp
new file mode 100644
index 0000000..3c34eb9
--- /dev/null
+++ b/SurgSim/Graphics/OsgLog.cpp
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgLog.h"
+
+using SurgSim::Graphics::OsgLog;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+OsgLog::OsgLog() : m_logger(SurgSim::Framework::Logger::getLogger("Osg"))
+{
+#ifdef OSS_DEBUG
+ osg::setNotifyLevel(osg::DEBUG_FP);
+#endif
+}
+
+void OsgLog::notify(osg::NotifySeverity severity, const char *message)
+{
+ // Map osg logging levels into OSS logging levels
+ if (severity <= osg::FATAL)
+ {
+ SURGSIM_LOG(m_logger, CRITICAL) << message;
+ }
+ else if (osg::FATAL < severity && severity <= osg::WARN)
+ {
+ SURGSIM_LOG(m_logger, WARNING) << message;
+ }
+ else if (osg::WARN < severity && severity <= osg::INFO)
+ {
+ SURGSIM_LOG(m_logger, INFO) << message;
+ }
+ else if (osg::INFO < severity && severity <= osg::DEBUG_FP)
+ {
+ SURGSIM_LOG(m_logger, DEBUG) << message;
+ }
+ else
+ {
+ SURGSIM_LOG(m_logger, CRITICAL) << "Unknown severity in OsgLog::notify()";
+ }
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgLog.h b/SurgSim/Graphics/OsgLog.h
new file mode 100644
index 0000000..677a221
--- /dev/null
+++ b/SurgSim/Graphics/OsgLog.h
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGLOG_H
+#define SURGSIM_GRAPHICS_OSGLOG_H
+
+#include <osg/Notify>
+
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Enable logging of OSG through SurgSim Logging System
+/// To use this, an object of OsgLog class must be created.
+/// Then call osg::setNotifyHandler() to let OSG use OSS logging system.
+class OsgLog : public osg::NotifyHandler
+{
+public:
+ /// Constructor
+ /// If OSS_DEBUG is defined, set OSG's log level to the lowest (osg::DEBUG_FP).
+ /// So that all info can be logged.
+ /// Otherwise, keep OSG's default log level (osg::NOTICE).
+
+ /// Note that message can still be filtered out in user defined derived method notify().
+ OsgLog();
+
+
+
+ /// User defined derived log Method
+ /// Based on log level 'severity', this method decides whether to log 'message' with OSS logging system.
+
+ /// \param severity Log level of message to be logged.
+ /// \param message The actual message to be logged.
+ virtual void notify(osg::NotifySeverity severity, const char *message) override;
+
+private:
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGLOG_H
diff --git a/SurgSim/Graphics/OsgManager.cpp b/SurgSim/Graphics/OsgManager.cpp
new file mode 100644
index 0000000..f1ff717
--- /dev/null
+++ b/SurgSim/Graphics/OsgManager.cpp
@@ -0,0 +1,187 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgManager.h"
+
+#include <vector>
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/Runtime.h"
+
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgScreenSpacePass.h"
+
+#include <osgViewer/Scene>
+#include <osgDB/WriteFile>
+
+using SurgSim::Graphics::OsgRepresentation;
+using SurgSim::Graphics::OsgCamera;
+using SurgSim::Graphics::OsgGroup;
+using SurgSim::Graphics::OsgManager;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+OsgManager::OsgManager() : SurgSim::Graphics::Manager(),
+ m_viewer(new osgViewer::CompositeViewer())
+{
+}
+
+OsgManager::~OsgManager()
+{
+}
+
+std::shared_ptr<Group> OsgManager::getOrCreateGroup(const std::string& name)
+{
+ std::shared_ptr<Group> result;
+ auto groups = getGroups();
+
+ auto group = groups.find(name);
+
+ if (group == std::end(groups))
+ {
+ auto newGroup = std::make_shared<OsgGroup>(name);
+ addGroup(newGroup);
+ result = newGroup;
+ }
+ else
+ {
+ result = group->second;
+ }
+
+ return result;
+}
+
+bool OsgManager::addRepresentation(std::shared_ptr<SurgSim::Graphics::Representation> representation)
+{
+ std::shared_ptr<OsgRepresentation> osgRepresentation = std::dynamic_pointer_cast<OsgRepresentation>(representation);
+ bool result;
+ if (osgRepresentation)
+ {
+ result = Manager::addRepresentation(osgRepresentation);
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(getLogger())
+ << __FUNCTION__ << " Representation is not a subclass of OsgRepresentation "
+ << representation->getName();
+ result = false;
+ }
+ return result;
+}
+
+bool OsgManager::addView(std::shared_ptr<SurgSim::Graphics::View> view)
+{
+ std::shared_ptr<OsgView> osgView = std::dynamic_pointer_cast<OsgView>(view);
+
+ bool result = true;
+ if (osgView == nullptr)
+ {
+ SURGSIM_LOG_WARNING(getLogger()) << __FUNCTION__ << " View is not a subclass of OsgView " << view->getName();
+ result = false;
+ }
+ else
+ {
+ SURGSIM_ASSERT(view->getCamera() != nullptr) << "View should have a camera when added to the manager.";
+ if (Manager::addView(view))
+ {
+ m_viewer->addView(osgView->getOsgView());
+ }
+ }
+
+ return result;
+}
+
+bool OsgManager::removeView(std::shared_ptr<SurgSim::Graphics::View> view)
+{
+ std::shared_ptr<OsgView> osgView = std::dynamic_pointer_cast<OsgView>(view);
+ if (osgView)
+ {
+ m_viewer->removeView(osgView->getOsgView());
+ }
+
+ return Manager::removeView(view);
+}
+
+
+bool OsgManager::doInitialize()
+{
+ m_hudElement = std::make_shared<OsgScreenSpacePass>(Representation::DefaultHudGroupName);
+ return true;
+}
+
+bool OsgManager::doStartUp()
+{
+ return true;
+}
+
+bool OsgManager::doUpdate(double dt)
+{
+
+ // There is a bug in the scene initialisation where addSceneElement() will not be correctly executed if
+ // performed inside of doInitialize(), this needs to be fixed
+ // HS-2014-dec-12
+ // #workaround
+ if (!m_hudElement->isInitialized())
+ {
+ getRuntime()->getScene()->addSceneElement(m_hudElement);
+ }
+
+
+ if (Manager::doUpdate(dt))
+ {
+ m_viewer->frame();
+
+ // \note HS-2013-dec-12 This will work as long as we deal with one view, when we move to stereoscopic
+ // we might have to revise things. Or just assume that most views have the same size
+ if (m_viewer->getNumViews() > 0)
+ {
+ auto dimensions = getViews()[0]->getDimensions();
+ m_hudElement->setViewPort(dimensions[0], dimensions[1]);
+ }
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+void OsgManager::doBeforeStop()
+{
+ dumpDebugInfo();
+ // Delete the viewer so that the graphics context will be released in the manager's thread
+ m_viewer = nullptr;
+}
+
+osg::ref_ptr<osgViewer::CompositeViewer> OsgManager::getOsgCompositeViewer() const
+{
+ return m_viewer;
+}
+
+void SurgSim::Graphics::OsgManager::dumpDebugInfo() const
+{
+ osgDB::writeNodeFile(*m_viewer->getView(0)->getCamera(), "viewer_zero_camera.osgt");
+}
+
+}
+}
+
+
diff --git a/SurgSim/Graphics/OsgManager.h b/SurgSim/Graphics/OsgManager.h
new file mode 100644
index 0000000..f134b9d
--- /dev/null
+++ b/SurgSim/Graphics/OsgManager.h
@@ -0,0 +1,106 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGMANAGER_H
+#define SURGSIM_GRAPHICS_OSGMANAGER_H
+
+#include "SurgSim/Graphics/Manager.h"
+
+#include <memory>
+
+#include <osgViewer/CompositeViewer>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+class Representation;
+class Group;
+class View;
+class OsgCamera;
+class OsgGroup;
+class OsgScreenSpacePass;
+
+/// OSG-based implementation of graphics manager class.
+///
+/// A Graphics::OsgManager sets up an osgViewer::CompositeViewer to manage and render each Graphics::OsgView added to
+/// the Manager.
+class OsgManager : public Manager
+{
+public:
+ /// Constructor
+ ///
+ /// Sets up a default Camera which will be used if for any View that does not have a Camera assigned
+ /// Sets up a default Group which is assigned to the default Camera. All added representations and groups will be
+ /// added to this group.
+ OsgManager();
+ /// Destructor
+ virtual ~OsgManager();
+
+ friend class OsgManagerTest;
+
+ /// Returns the OSG CompositeViewer used to manage and render the views
+ osg::ref_ptr<osgViewer::CompositeViewer> getOsgCompositeViewer() const;
+
+ /// OsgManager will write out the scenegraph in the working directory
+ virtual void dumpDebugInfo() const override;
+
+protected:
+ virtual bool doUpdate(double dt) override;
+
+ virtual bool doInitialize() override;
+
+ virtual bool doStartUp() override;
+
+ /// Adds an representation to the manager
+ /// \param representation The representation to be added.
+ /// Only allows OsgRepresentation components, any other will not be set and it will return false.
+ /// \return True if the representation was not in this manager and has been successfully added, false if it fails.
+ virtual bool addRepresentation(std::shared_ptr<Representation> representation) override;
+
+ /// Adds a view to the manager
+ /// \param view The view to be added.
+ /// Only allows OsgView components, any other will not be set and it will return false.
+ /// \return True if the view was not in this manager and has been successfully added, false if it fails.
+ virtual bool addView(std::shared_ptr<View> view) override;
+
+ /// Removes a view from the manager
+ /// \param view The view to be removed.
+ /// \return True if the view was in this manager and has been successfully removed, false if it fails.
+ /// \post The view is removed from the manager and the osgViewer::CompositeViewer.
+ virtual bool removeView(std::shared_ptr<View> view) override;
+
+ virtual std::shared_ptr<Group> getOrCreateGroup(const std::string& name) override;
+
+private:
+
+ /// Prepares the manager for its execution to be stopped
+ /// \note Called from this thread before joined
+ void doBeforeStop();
+
+ /// OSG CompositeViewer to manage and render the individual views
+ osg::ref_ptr<osgViewer::CompositeViewer> m_viewer;
+
+ /// Builtin RenderPass that can be used for HUD functionality, uses Group "ossHud"
+ std::shared_ptr<OsgScreenSpacePass> m_hudElement;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGMANAGER_H
diff --git a/SurgSim/Graphics/OsgMaterial.cpp b/SurgSim/Graphics/OsgMaterial.cpp
new file mode 100644
index 0000000..86477f7
--- /dev/null
+++ b/SurgSim/Graphics/OsgMaterial.cpp
@@ -0,0 +1,217 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgMaterial.h"
+
+#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgUniformFactory.h"
+
+#include <algorithm>
+#include <functional>
+
+#include <boost/any.hpp>
+
+
+using SurgSim::Graphics::OsgMaterial;
+using SurgSim::Graphics::OsgUniformBase;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+OsgMaterial::OsgMaterial(const std::string& name) :
+ Material(name),
+ m_stateSet(new osg::StateSet())
+{
+
+}
+
+bool OsgMaterial::addUniform(std::shared_ptr<UniformBase> uniform)
+{
+ bool didSucceed = false;
+
+ std::shared_ptr<OsgUniformBase> osgUniform = std::dynamic_pointer_cast<OsgUniformBase>(uniform);
+ if (osgUniform != nullptr)
+ {
+ if (isInitialized())
+ {
+ osgUniform->addToStateSet(m_stateSet);
+ }
+ m_uniforms.push_back(osgUniform);
+
+ // add a property to Material, that carries the uniform name and forwards to the value of the uniform
+ // This exposes the non-shared pointer to the uniform in the function table, the entry in the function
+ // table will be removed when the uniform is removed from the material. The material holds a shared
+ // pointer to the uniform, keeping the uniform alive during the lifetime of the material.
+ forwardProperty(osgUniform->getName(), *osgUniform.get(), "Value");
+ didSucceed = true;
+ }
+ return didSucceed;
+}
+
+bool OsgMaterial::addUniform(const std::string& type, const std::string& name)
+{
+ OsgUniformFactory factory;
+
+ bool result = false;
+ if (factory.isRegistered(type))
+ {
+ result = addUniform(factory.create(type, name));
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Type " << type << " not supported.";
+ }
+ return result;
+}
+
+bool OsgMaterial::removeUniform(std::shared_ptr<UniformBase> uniform)
+{
+ std::shared_ptr<OsgUniformBase> osgUniform = std::dynamic_pointer_cast<OsgUniformBase>(uniform);
+
+ bool didSucceed = false;
+
+ if (osgUniform)
+ {
+ auto it = std::find(m_uniforms.begin(), m_uniforms.end(), osgUniform);
+
+ if (it != m_uniforms.end())
+ {
+ if (isInitialized())
+ {
+ osgUniform->removeFromStateSet(m_stateSet);
+ }
+ m_uniforms.erase(it);
+ didSucceed = true;
+ }
+
+ removeAccessors(osgUniform->getName());
+
+ }
+
+ return didSucceed;
+}
+
+size_t OsgMaterial::getNumUniforms() const
+{
+ return m_uniforms.size();
+}
+
+std::shared_ptr<UniformBase> OsgMaterial::getUniform(size_t index) const
+{
+ return m_uniforms[index];
+}
+
+
+
+std::shared_ptr<UniformBase>
+OsgMaterial::getUniform(const std::string& name) const
+{
+ std::shared_ptr<UniformBase> result;
+ auto it = std::find_if(
+ std::begin(m_uniforms),
+ std::end(m_uniforms),
+ [&name](const std::shared_ptr<OsgUniformBase>& uniform)
+ {
+ return uniform->getName() == name;
+ });
+ if (it != std::end(m_uniforms))
+ {
+ result = *it;
+ }
+ return result;
+}
+
+bool OsgMaterial::removeUniform(const std::string& name)
+{
+ bool result = false;
+ auto it = std::find_if(
+ std::begin(m_uniforms),
+ std::end(m_uniforms),
+ [&name](const std::shared_ptr<OsgUniformBase>& uniform)
+ {
+ return uniform->getName() == name;
+ });
+ if (it != std::end(m_uniforms))
+ {
+ result = removeUniform(*it);
+ }
+ return result;
+}
+
+bool OsgMaterial::hasUniform(const std::string& name) const
+{
+ return (getUniform(name) != nullptr);
+}
+
+bool OsgMaterial::setShader(std::shared_ptr<Shader> shader)
+{
+ bool didSucceed = false;
+
+ std::shared_ptr<OsgShader> osgShader = std::dynamic_pointer_cast<OsgShader>(shader);
+ if (osgShader)
+ {
+ if (m_shader)
+ {
+ m_shader->removeFromStateSet(m_stateSet.get());
+ }
+ osgShader->addToStateSet(m_stateSet);
+ m_shader = osgShader;
+ didSucceed = true;
+ }
+
+ return didSucceed;
+}
+
+std::shared_ptr<Shader> OsgMaterial::getShader() const
+{
+ return m_shader;
+}
+
+void OsgMaterial::clearShader()
+{
+ if (m_shader)
+ {
+ m_shader->removeFromStateSet(m_stateSet.get());
+ }
+ m_shader = nullptr;
+}
+
+bool OsgMaterial::doInitialize()
+{
+ for (auto it = m_uniforms.begin(); it != m_uniforms.end(); ++it)
+ {
+ (*it)->addToStateSet(m_stateSet);
+ }
+ return true;
+}
+
+bool OsgMaterial::doWakeUp()
+{
+ return true;
+}
+
+osg::ref_ptr<osg::StateSet> OsgMaterial::getOsgStateSet() const
+{
+ return m_stateSet;
+}
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/OsgMaterial.h b/SurgSim/Graphics/OsgMaterial.h
new file mode 100644
index 0000000..ddc8854
--- /dev/null
+++ b/SurgSim/Graphics/OsgMaterial.h
@@ -0,0 +1,107 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGMATERIAL_H
+#define SURGSIM_GRAPHICS_OSGMATERIAL_H
+
+#include "SurgSim/Graphics/Material.h"
+
+#include <osg/Material>
+#include <osg/StateSet>
+
+#include <set>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+class MaterialFace;
+class OsgShader;
+class OsgUniformBase;
+
+/// OSG-based implementation of a graphics material.
+///
+/// Wraps an osg::StateSet which is applied to the osg::Node of the Representation that is assigned this material.
+///
+/// \note
+/// Only uniforms that subclass OsgUniformBase and shaders that subclass OsgShader can be assigned to this material.
+/// \sa OsgUniformBase
+/// \sa OsgShader
+class OsgMaterial : public Material
+{
+public:
+ /// Constructor
+ /// \post The material has no uniforms and no shader.
+ explicit OsgMaterial(const std::string& name);
+
+ /// Adds a uniform to this material
+ /// \param uniform Uniform to add
+ /// \return True if uniform was added successfully, otherwise false
+ /// \note OsgMaterial only accepts subclasses of OsgUniformBase
+ virtual bool addUniform(std::shared_ptr<UniformBase> uniform) override;
+
+ virtual bool addUniform(const std::string& type, const std::string& name) override;
+
+ /// Removes a uniform from this material
+ /// \param uniform Uniform to remove
+ /// \return True if uniform was removed successfully, otherwise false
+ /// \note OsgMaterial only accepts subclasses of OsgUniformBase
+ virtual bool removeUniform(std::shared_ptr<UniformBase> uniform) override;
+
+ virtual bool removeUniform(const std::string& name) override;
+
+ virtual size_t getNumUniforms() const override;
+
+ virtual std::shared_ptr<UniformBase> getUniform(size_t index) const override;
+
+ virtual std::shared_ptr<UniformBase> getUniform(const std::string& name) const override;
+
+ virtual bool hasUniform(const std::string& name) const override;
+
+ /// Sets the shader used by this material
+ /// \param shader Shader program
+ /// \return True if shader was set successfully, otherwise false
+ /// \note OsgMaterial only accepts subclasses of OsgShader
+ virtual bool setShader(std::shared_ptr<Shader> shader) override;
+
+ virtual std::shared_ptr<Shader> getShader() const override;
+
+ virtual void clearShader() override;
+
+ /// \return the OSG state set with the material properties
+ osg::ref_ptr<osg::StateSet> getOsgStateSet() const;
+
+ virtual bool doInitialize() override;
+
+ virtual bool doWakeUp() override;
+
+private:
+ /// OSG state set which provides material properties in the scenegraph
+ osg::ref_ptr<osg::StateSet> m_stateSet;
+
+ /// Uniforms used by this material
+ std::vector<std::shared_ptr<OsgUniformBase>> m_uniforms;
+
+ /// Shader used by this material
+ std::shared_ptr<OsgShader> m_shader;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGMATERIAL_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgMatrixConversions.h b/SurgSim/Graphics/OsgMatrixConversions.h
new file mode 100644
index 0000000..7596c56
--- /dev/null
+++ b/SurgSim/Graphics/OsgMatrixConversions.h
@@ -0,0 +1,147 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Conversions to and from OSG matrix types
+///
+/// SurgSim's Eigen vectors are a single <b>column</b>, and matrix operations use <b>postfix</b> notation.
+/// OSG vectors are a single <b>row</b> and matrix operations use <b>prefix</b> notation,
+///
+/// For example, with Eigen, one might write:
+/// \code{.cpp}
+/// Vector3d columnVector;
+/// Matrix33d matrix;
+/// Vector3d result = matrix * columnVector;
+/// \endcode
+///
+/// However, with OSG, this should be written in the form:
+/// \code{.cpp}
+/// osg::Vec3d rowVector;
+/// osg::Matrix3d columnMajorMatrix;
+/// osg::Vec3d result = rowVector * columnMajorMatrix;
+/// \endcode
+///
+/// For the result to be the same, the OSG matrix data must be interpreted as column-major.
+/// These conversions handle that.
+
+#ifndef SURGSIM_GRAPHICS_OSGMATRIXCONVERSIONS_H
+#define SURGSIM_GRAPHICS_OSGMATRIXCONVERSIONS_H
+
+#include "SurgSim/Math/Matrix.h"
+
+#include <osg/Matrixf>
+#include <osg/Matrixd>
+#include <osg/Uniform>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Convert a fixed-size 2x2 matrix of floats to OSG.
+template <int MOpt> inline
+const osg::Matrix2 toOsg(const Eigen::Matrix<float, 2, 2, MOpt>& matrix)
+{
+ osg::Matrix2 osgMatrix;
+ Eigen::Map<Eigen::Matrix<float, 2, 2, Eigen::ColMajor>>(osgMatrix.ptr()) = matrix;
+ return osgMatrix;
+}
+
+/// Convert a fixed-size 2x2 matrix of doubles to OSG.
+template <int MOpt> inline
+const osg::Matrix2d toOsg(const Eigen::Matrix<double, 2, 2, MOpt>& matrix)
+{
+ osg::Matrix2d osgMatrix;
+ Eigen::Map<Eigen::Matrix<double, 2, 2, Eigen::ColMajor>>(osgMatrix.ptr()) = matrix;
+ return osgMatrix;
+}
+
+/// Convert from OSG to a 2x2 matrix of floats.
+inline const Eigen::Matrix<float, 2, 2, Eigen::RowMajor> fromOsg(const osg::Matrix2& matrix)
+{
+ return Eigen::Map<const Eigen::Matrix<float, 2, 2, Eigen::ColMajor>>(matrix.ptr());
+}
+/// Convert from OSG to a 2x2 matrix of doubles.
+inline const Eigen::Matrix<double, 2, 2, Eigen::RowMajor> fromOsg(const osg::Matrix2d& matrix)
+{
+ return Eigen::Map<const Eigen::Matrix<double, 2, 2, Eigen::ColMajor>>(matrix.ptr());
+}
+
+/// Convert a fixed-size 3x3 matrix of floats to OSG.
+template <int MOpt> inline
+const osg::Matrix3 toOsg(const Eigen::Matrix<float, 3, 3, MOpt>& matrix)
+{
+ osg::Matrix3 osgMatrix;
+ Eigen::Map<Eigen::Matrix<float, 3, 3, Eigen::ColMajor>>(osgMatrix.ptr()) = matrix;
+ return osgMatrix;
+}
+
+/// Convert a fixed-size 3x3 matrix of doubles to OSG.
+template <int MOpt> inline
+ const osg::Matrix3d toOsg(const Eigen::Matrix<double, 3, 3, MOpt>& matrix)
+{
+ osg::Matrix3d osgMatrix;
+ Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::ColMajor>>(osgMatrix.ptr()) = matrix;
+ return osgMatrix;
+}
+
+/// Convert from OSG to a 3x3 matrix of floats.
+inline const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> fromOsg(const osg::Matrix3& matrix)
+{
+ return Eigen::Map<const Eigen::Matrix<float, 3, 3, Eigen::ColMajor>>(matrix.ptr());
+}
+
+/// Convert from OSG to a 3x3 matrix of doubles.
+inline const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> fromOsg(const osg::Matrix3d& matrix)
+{
+ return Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::ColMajor>>(matrix.ptr());
+}
+
+/// Convert a fixed-size 4x4 matrix of floats to OSG.
+template <int MOpt> inline
+const osg::Matrixf toOsg(const Eigen::Matrix<float, 4, 4, MOpt>& matrix)
+{
+ osg::Matrixf osgMatrix;
+ Eigen::Map<Eigen::Matrix<float, 4, 4, Eigen::ColMajor>>(osgMatrix.ptr()) = matrix;
+ return osgMatrix;
+}
+
+/// Convert from OSG to a 4x4 matrix of floats.
+inline const Eigen::Matrix<float, 4, 4, Eigen::RowMajor> fromOsg(const osg::Matrixf& matrix)
+{
+ return Eigen::Map<const Eigen::Matrix<float, 4, 4, Eigen::ColMajor>>(matrix.ptr());
+}
+
+/// Convert a fixed-size 4x4 matrix of doubles to OSG.
+template <int MOpt> inline
+const osg::Matrixd toOsg(const Eigen::Matrix<double, 4, 4, MOpt>& matrix)
+{
+ osg::Matrixd osgMatrix;
+ Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::ColMajor>>(osgMatrix.ptr()) = matrix;
+ return osgMatrix;
+}
+
+/// Convert from OSG to a 4x4 matrix of doubles.
+inline const Eigen::Matrix<double, 4, 4, Eigen::RowMajor> fromOsg(const osg::Matrixd& matrix)
+{
+ return Eigen::Map<const Eigen::Matrix<double, 4, 4, Eigen::ColMajor>>(matrix.ptr());
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGMATRIXCONVERSIONS_H
diff --git a/SurgSim/Graphics/OsgMeshRepresentation.cpp b/SurgSim/Graphics/OsgMeshRepresentation.cpp
new file mode 100644
index 0000000..ceffa9e
--- /dev/null
+++ b/SurgSim/Graphics/OsgMeshRepresentation.cpp
@@ -0,0 +1,283 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+
+#include <osg/Array>
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osg/PositionAttitudeTransform>
+#include <osg/Vec3f>
+#include <osgUtil/SmoothingVisitor>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Graphics/MeshUtilities.h"
+#include "SurgSim/Graphics/OsgConversions.h"
+#include "SurgSim/Graphics/TriangleNormalGenerator.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgMeshRepresentation, OsgMeshRepresentation);
+
+OsgMeshRepresentation::OsgMeshRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ MeshRepresentation(name),
+ m_updateOptions(UPDATE_OPTION_VERTICES),
+ m_mesh(std::make_shared<Mesh>()),
+ m_filename()
+{
+ // The actual size of the mesh is not known at this time, just allocate the
+ // osg structures that are needed and add them to the geometry, and the node
+ m_geometry = new osg::Geometry();
+
+ // Set up vertices array
+ m_vertices = new osg::Vec3Array();
+ m_vertices->setDataVariance(osg::Object::DYNAMIC);
+ m_geometry->setVertexArray(m_vertices);
+ m_geometry->setUseDisplayList(false);
+
+ // Set up color array with default color
+ m_colors = new osg::Vec4Array(1);
+ (*m_colors)[0] = osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f);
+ m_geometry->setColorArray(m_colors, osg::Array::BIND_OVERALL);
+
+ // Set up textureCoordinates array, texture coords are optional, don't add them to the
+ // geometry yet
+ m_textureCoordinates = new osg::Vec2Array(0);
+ m_textureCoordinates->setDataVariance(osg::Object::DYNAMIC);
+
+ // Set up primitive set for triangles
+ m_triangles = new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES);
+ m_triangles->setDataVariance(osg::Object::DYNAMIC);
+ m_geometry->addPrimitiveSet(m_triangles);
+
+ // Create normals, currently per triangle
+ m_normals = new osg::Vec3Array();
+ m_normals->setDataVariance(osg::Object::DYNAMIC);
+ m_geometry->setNormalArray(m_normals, osg::Array::BIND_PER_VERTEX);
+
+ osg::ref_ptr<osg::Geode> geode = new osg::Geode();
+ geode->addDrawable(m_geometry);
+
+ m_transform->addChild(geode);
+}
+
+OsgMeshRepresentation::~OsgMeshRepresentation()
+{
+}
+
+std::shared_ptr<Mesh> OsgMeshRepresentation::getMesh()
+{
+ return m_mesh;
+}
+
+void OsgMeshRepresentation::doUpdate(double dt)
+{
+ SURGSIM_ASSERT(m_mesh->isValid()) << "The mesh in the OsgMeshRepresentation " << getName() << " is invalid.";
+
+ int updateOptions = updateOsgArrays();
+ updateOptions |= m_updateOptions;
+
+ if ((updateOptions & (UPDATE_OPTION_VERTICES | UPDATE_OPTION_TEXTURES | UPDATE_OPTION_COLORS)) != 0)
+ {
+ updateVertices(updateOptions);
+ m_geometry->dirtyDisplayList();
+ m_geometry->dirtyBound();
+ }
+
+ if ((updateOptions & UPDATE_OPTION_TRIANGLES) != 0)
+ {
+ updateTriangles();
+ m_triangles->dirty();
+ }
+}
+
+bool OsgMeshRepresentation::doInitialize()
+{
+ bool result = true;
+
+ if (!m_filename.empty())
+ {
+ std::string filePath = getRuntime()->getApplicationData()->findFile(m_filename);
+
+ if (filePath.empty())
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "OsgMeshRepresentation::doInitialize(): file " << m_filename << " can not be found.";
+ result = false;
+ }
+ else
+ {
+ m_mesh = SurgSim::DataStructures::loadTriangleMesh<Mesh>(filePath);
+ SURGSIM_ASSERT(nullptr != m_mesh && m_mesh->isValid())
+ << "OsgMeshRepresentation::doInitialize(): SurgSim::DataStructures::loadTriangleMesh() returned a "
+ << "null mesh or invalid mesh from file " << filePath;
+ }
+ }
+
+ return result;
+}
+
+void OsgMeshRepresentation::updateVertices(int updateOptions)
+{
+ static osg::Vec4d defaultColor(0.8, 0.2, 0.2, 1.0);
+ static osg::Vec2d defaultTextureCoord(0.0, 0.0);
+
+ bool updateColors = (updateOptions & UPDATE_OPTION_COLORS) != 0;
+ bool updateTextures = (updateOptions & UPDATE_OPTION_TEXTURES) != 0;
+ bool updateVertices = (updateOptions & UPDATE_OPTION_VERTICES) != 0;
+ size_t vertexCount = m_mesh->getNumVertices();
+
+ for (size_t i = 0; i < vertexCount; ++i)
+ {
+ Mesh::VertexType vertex = m_mesh->getVertex(i);
+ if (updateVertices)
+ {
+ (*m_vertices)[i].set(toOsg(vertex.position));
+ }
+ if (updateColors)
+ {
+ (*m_colors)[i] = (vertex.data.color.hasValue()) ? toOsg(vertex.data.color.getValue()) : defaultColor;
+ }
+ if (updateTextures)
+ {
+ (*m_textureCoordinates)[i] =
+ (vertex.data.texture.hasValue()) ? toOsg(vertex.data.texture.getValue()) : defaultTextureCoord;
+ }
+ }
+
+ if (updateVertices)
+ {
+ updateNormals();
+ }
+}
+
+void OsgMeshRepresentation::updateNormals()
+{
+ // Generate normals from geometry
+ auto normalGenerator = createNormalGenerator(m_vertices, m_normals);
+ m_geometry->accept(normalGenerator);
+ normalGenerator.normalize();
+}
+
+void OsgMeshRepresentation::updateTriangles()
+{
+ int i = 0;
+ m_triangles->resize(m_mesh->getNumTriangles() * 3);
+ for (auto const& triangle : m_mesh->getTriangles())
+ {
+ if (triangle.isValid)
+ {
+ (*m_triangles)[i++] = triangle.verticesId[0];
+ (*m_triangles)[i++] = triangle.verticesId[1];
+ (*m_triangles)[i++] = triangle.verticesId[2];
+ }
+ }
+}
+
+int OsgMeshRepresentation::updateOsgArrays()
+{
+ int result = 0;
+
+ size_t numVertices = m_mesh->getNumVertices();
+
+ if (numVertices > m_vertices->size())
+ {
+ m_vertices->resize(numVertices);
+ m_normals->resize(numVertices);
+
+ m_vertices->setDataVariance(getDataVariance(UPDATE_OPTION_VERTICES));
+ m_normals->setDataVariance(getDataVariance(UPDATE_OPTION_VERTICES));
+
+ result |= UPDATE_OPTION_VERTICES;
+ }
+
+ // The first vertex determines what values the mesh should have ...
+ Mesh::VertexType vertex = m_mesh->getVertex(0);
+
+ if (vertex.data.color.hasValue() && numVertices > m_colors->size())
+ {
+ if (m_colors->size() > 1)
+ {
+ m_colors->setDataVariance(getDataVariance(UPDATE_OPTION_COLORS));
+ m_geometry->setColorArray(m_colors, osg::Array::BIND_PER_VERTEX);
+ }
+ m_colors->resize(numVertices);
+ result |= UPDATE_OPTION_COLORS;
+ }
+
+ if (vertex.data.texture.hasValue() && numVertices > m_textureCoordinates->size())
+ {
+ bool setTextureArray = m_textureCoordinates->size() == 0;
+ m_textureCoordinates->resize(numVertices);
+ if (setTextureArray)
+ {
+ m_geometry->setTexCoordArray(0, m_textureCoordinates, osg::Array::BIND_PER_VERTEX);
+ m_textureCoordinates->setDataVariance(getDataVariance(UPDATE_OPTION_TEXTURES));
+ }
+ result |= UPDATE_OPTION_TEXTURES;
+ }
+
+ if (m_mesh->getNumTriangles() * 3 > m_triangles->size())
+ {
+ m_triangles->resize(m_mesh->getNumTriangles() * 3);
+ m_triangles->setDataVariance(getDataVariance(UPDATE_OPTION_TRIANGLES));
+ result |= UPDATE_OPTION_TRIANGLES;
+ }
+ return result;
+}
+
+void OsgMeshRepresentation::setUpdateOptions(int val)
+{
+ if (val <= UPDATE_OPTION_ALL && val >= UPDATE_OPTION_NONE)
+ {
+ m_updateOptions = val;
+ }
+}
+
+int OsgMeshRepresentation::getUpdateOptions() const
+{
+ return m_updateOptions;
+}
+
+osg::ref_ptr<osg::Geometry> OsgMeshRepresentation::getOsgGeometry()
+{
+ return m_geometry;
+}
+
+osg::Object::DataVariance OsgMeshRepresentation::getDataVariance(int updateOption)
+{
+ return ((m_updateOptions & updateOption) != 0) ? osg::Object::DYNAMIC : osg::Object::STATIC;
+}
+
+void OsgMeshRepresentation::setFilename(std::string filename)
+{
+ m_filename = filename;
+}
+
+std::string OsgMeshRepresentation::getFilename() const
+{
+ return m_filename;
+}
+
+}; // Graphics
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgMeshRepresentation.h b/SurgSim/Graphics/OsgMeshRepresentation.h
new file mode 100644
index 0000000..534dd8f
--- /dev/null
+++ b/SurgSim/Graphics/OsgMeshRepresentation.h
@@ -0,0 +1,126 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGMESHREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGMESHREPRESENTATION_H
+
+#include <memory>
+
+#include <osg/Array>
+#include <osg/ref_ptr>
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/MeshRepresentation.h"
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace osg
+{
+class Geometry;
+class DrawElementsUInt;
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+class Mesh;
+
+SURGSIM_STATIC_REGISTRATION(OsgMeshRepresentation);
+
+/// Implementation of a MeshRepresentation for rendering under osg.
+class OsgMeshRepresentation : public OsgRepresentation, public MeshRepresentation
+{
+public:
+ /// Constructor.
+ /// \param name The name.
+ explicit OsgMeshRepresentation(const std::string& name);
+
+ /// Destructor
+ ~OsgMeshRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgMeshRepresentation);
+
+ virtual std::shared_ptr<Mesh> getMesh() override;
+
+ virtual void setUpdateOptions(int val) override;
+ virtual int getUpdateOptions() const override;
+
+ osg::ref_ptr<osg::Geometry> getOsgGeometry();
+
+ virtual void setFilename(std::string filename) override;
+ virtual std::string getFilename() const override;
+
+protected:
+ virtual void doUpdate(double dt) override;
+
+ /// \note If m_filename is set, m_mesh will be overwritten with the mesh loaded from the external file.
+ virtual bool doInitialize() override;
+
+private:
+ /// Indicates which elements of the mesh should be updated on every frame
+ int m_updateOptions;
+
+ /// The mesh.
+ std::shared_ptr<Mesh> m_mesh;
+
+ /// File name of the external file which contains the mesh to be used by this class.
+ std::string m_filename;
+
+ ///@{
+ /// Osg structures
+ osg::ref_ptr<osg::Geometry> m_geometry;
+ osg::ref_ptr<osg::Vec3Array> m_vertices;
+ osg::ref_ptr<osg::Vec4Array> m_colors;
+ osg::ref_ptr<osg::Vec3Array> m_normals;
+ osg::ref_ptr<osg::Vec2Array> m_textureCoordinates;
+ osg::ref_ptr<osg::DrawElementsUInt> m_triangles;
+ ///@}
+
+ /// Updates the internal arrays in accordance to the sizes given in the mesh
+ /// \return updateOptions value that indicates which of the structures where updated in size and
+ /// will have to be updated independent of the value set in setUpdateOptions()
+ int updateOsgArrays();
+
+ /// Copies the attributes for each mesh vertex in the appropriate osg structure, this will only be done
+ /// for the data as is indicated by updateOptions
+ /// \param updateOptions Set of flags indicating whether a specific vertex attribute should be updated
+ void updateVertices(int updateOptions);
+
+ /// Updates the normals.
+ void updateNormals();
+
+ /// Updates the triangles.
+ void updateTriangles();
+
+ /// Gets data variance for a given update option.
+ /// \param updateOption The update option.
+ /// \return The data variance.
+ osg::Object::DataVariance getDataVariance(int updateOption);
+};
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+}; // Graphics
+}; // SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGMESHREPRESENTATION_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgOctreeRepresentation.cpp b/SurgSim/Graphics/OsgOctreeRepresentation.cpp
new file mode 100644
index 0000000..eb28d6f
--- /dev/null
+++ b/SurgSim/Graphics/OsgOctreeRepresentation.cpp
@@ -0,0 +1,127 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <osg/PositionAttitudeTransform>
+
+#include "SurgSim/Graphics/OsgOctreeRepresentation.h"
+
+#include "SurgSim/DataStructures/OctreeNode.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/Graphics/OsgConversions.h"
+#include "SurgSim/Graphics/OsgUnitBox.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgOctreeRepresentation, OsgOctreeRepresentation);
+
+using SurgSim::Math::Vector3d;
+
+OsgOctreeRepresentation::OsgOctreeRepresentation(const std::string& name) :
+ Representation(name),
+ OctreeRepresentation(name),
+ OsgRepresentation(name),
+ m_sharedUnitBox(getSharedUnitBox())
+{
+}
+
+OsgOctreeRepresentation::~OsgOctreeRepresentation()
+{
+}
+
+void OsgOctreeRepresentation::doUpdate(double dt)
+{
+}
+
+// An Octree(Node) is traversed in following order (the 2nd OctreeNode, i.e. OctreeNode with "1" is now shown):
+/*
+ ________
+ /3 / 7/|
+ /-------/ |
+ /2__/_6_/| |
+ | | | |/|
+ |___|___|/|5|
+ | | | |/
+ |0__|__4|/
+*/
+void OsgOctreeRepresentation::buildOctree(osg::ref_ptr<osg::PositionAttitudeTransform> transformNode,
+ std::shared_ptr<SurgSim::Math::OctreeShape::NodeType> octree)
+{
+ SURGSIM_ASSERT(!isAwake()) << "OsgOctreeRepresentation::buildOctree() should be called before wake up.";
+ osg::ref_ptr<osg::PositionAttitudeTransform> osgTransform = new osg::PositionAttitudeTransform();
+ transformNode->addChild(osgTransform);
+
+ if (octree->hasChildren())
+ {
+ auto octreeChildren = octree->getChildren();
+ for(int i = 0; i < 8; ++i)
+ {
+ buildOctree(osgTransform, octreeChildren[i]);
+ }
+ }
+ else
+ {
+ osgTransform->addChild(m_sharedUnitBox->getNode());
+ osgTransform->setPosition(toOsg(static_cast<Vector3d>(octree->getBoundingBox().center())));
+ osgTransform->setScale(toOsg(static_cast<Vector3d>(octree->getBoundingBox().sizes())));
+
+ int nodeMask = octree->isActive() ? 0xffffffff : 0;
+ osgTransform->setNodeMask(nodeMask);
+ }
+}
+
+void OsgOctreeRepresentation::setOctreeShape(const std::shared_ptr<SurgSim::Math::Shape>& shape)
+{
+ SURGSIM_ASSERT(!isAwake()) << "OsgOctreeRepresentation::setOctree() should be called before wake up.";
+
+ auto octreeShape = std::dynamic_pointer_cast<SurgSim::Math::OctreeShape>(shape);
+ SURGSIM_ASSERT(octreeShape != nullptr) << "OsgOctreeRepresentation can only accept an OctreeShape.";
+ m_octreeShape = octreeShape;
+
+ buildOctree(m_transform, m_octreeShape->getRootNode());
+}
+
+std::shared_ptr<SurgSim::Math::OctreeShape> OsgOctreeRepresentation::getOctreeShape() const
+{
+ return m_octreeShape;
+}
+
+void OsgOctreeRepresentation::setNodeVisible(const SurgSim::DataStructures::OctreePath& path, bool visibility)
+{
+ SURGSIM_ASSERT(0 != m_transform->getNumChildren()) << "No Octree held by OsgOctreeRepresentation";
+
+ osg::ref_ptr<osg::Group> result = m_transform->getChild(0)->asGroup();
+ for(auto index = std::begin(path); index != std::end(path); ++index)
+ {
+ SURGSIM_ASSERT(result->getNumChildren() > 1) <<
+ "OsgOctreeRepresentation::setNodeVisible(): Invalid OctreePath";
+
+ result = result->getChild(*index)->asGroup();
+ }
+ result->setNodeMask(visibility ? 0xffffffff : 0);
+}
+
+std::shared_ptr<SurgSim::Graphics::OsgUnitBox> OsgOctreeRepresentation::getSharedUnitBox()
+{
+ static SurgSim::Framework::SharedInstance<OsgUnitBox> shared;
+ return shared.get();
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/OsgOctreeRepresentation.h b/SurgSim/Graphics/OsgOctreeRepresentation.h
new file mode 100644
index 0000000..9a1e27e
--- /dev/null
+++ b/SurgSim/Graphics/OsgOctreeRepresentation.h
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGOCTREEREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGOCTREEREPRESENTATION_H
+
+#include <memory>
+#include <string>
+
+#include <osg/ref_ptr>
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Graphics/OctreeRepresentation.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Math/OctreeShape.h"
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class OsgUnitBox;
+
+SURGSIM_STATIC_REGISTRATION(OsgOctreeRepresentation);
+
+/// OSG octree representation, implements an OctreeRepresenation using OSG.
+/// Given a OctreeShape, this representation will copy the Octree instead of sharing the Octree (with the OctreeShape).
+/// Wake up call on this representation will fail if no octree is held.
+/// That is to say, setOctree() method MUST be called before WakeUp() to make this representation work properly.
+/// The OSG tree corresponds to the Octree will be built only once at wake up and can not be changed once awake.
+class OsgOctreeRepresentation : public OctreeRepresentation, public OsgRepresentation
+{
+public:
+ /// Constructor
+ /// \param name Name of OsgOctreeRepresentation
+ explicit OsgOctreeRepresentation(const std::string& name);
+
+ /// Destructor
+ ~OsgOctreeRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgOctreeRepresentation);
+
+ /// Executes the update operation
+ /// \param dt The time step
+ virtual void doUpdate(double dt) override;
+
+ virtual void setOctreeShape(const std::shared_ptr<SurgSim::Math::Shape>& shape) override;
+ virtual std::shared_ptr<SurgSim::Math::OctreeShape> getOctreeShape() const override;
+
+ /// Mark the OctreeNode visible/invisible in the given a OctreePath (typedef-ed in OctreeNode.h).
+ /// \param path An OctreePath, giving the path leads to the OctreeNode whose visibility to be changed.
+ /// \param visibility Whether or not the OctreeNode specified by 'path' is visible or not.
+ virtual void setNodeVisible(const SurgSim::DataStructures::OctreePath& path, bool visibility) override;
+
+private:
+ /// Draw the Octree associated with this OSG representation.
+ /// \param parentTransformNode The osg::PositionAttitudeTransform node under which either the octreeNode or its
+ /// children will be drawn.
+ /// \param octree The octree to be drawn.
+ void buildOctree(osg::ref_ptr<osg::PositionAttitudeTransform> parentTransformNode,
+ std::shared_ptr<SurgSim::Math::OctreeShape::NodeType> octree);
+
+ /// Shared unit box, so that the geometry can be instanced rather than having multiple copies.
+ std::shared_ptr<OsgUnitBox> m_sharedUnitBox;
+
+ /// The OctreeShape whose Octree will be visualized.
+ std::shared_ptr<SurgSim::Math::OctreeShape> m_octreeShape;
+
+ /// Returns the shared unit box
+ static std::shared_ptr<OsgUnitBox> getSharedUnitBox();
+};
+
+}; // Graphics
+}; // SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGOCTREEREPRESENTATION_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgPlane.h b/SurgSim/Graphics/OsgPlane.h
new file mode 100644
index 0000000..99296a5
--- /dev/null
+++ b/SurgSim/Graphics/OsgPlane.h
@@ -0,0 +1,65 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGPLANE_H
+#define SURGSIM_GRAPHICS_OSGPLANE_H
+
+#include <osg/Geode>
+#include <osg/Geometry>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG plane geode to be used as a primitive shape
+/// The plane is the XZ plane, with normal +Y.
+/// The plane is drawn with a Quad at (0.0, 0.0, 0.0) with length and width specified in the constructor (or default of
+/// 1000 for each).
+/// Add the plane geode to a transform node to position it.
+class OsgPlane
+{
+public:
+ /// Constructor
+ /// \param length Length of the plane in X (default is 1000)
+ /// \param width Width of the plane in Z (default is 1000)
+ OsgPlane(float length = 1000.0f, float width = 1000.0f) :
+ m_geode(new osg::Geode())
+ {
+ osg::ref_ptr<osg::Geometry> plane = osg::createTexturedQuadGeometry(
+ osg::Vec3(- length / 2.0f, 0.0f, width / 2.0f),
+ osg::Vec3(length, 0.0f, 0.0f),
+ osg::Vec3(0.0f, 0.0f, - width));
+ /// Normal is X^-Z = Y
+ m_geode->addDrawable(plane);
+ }
+
+ /// Returns the root OSG node for the plane to be inserted into the scene-graph
+ osg::ref_ptr<osg::Node> getNode() const
+ {
+ return m_geode;
+ }
+
+private:
+ /// Root OSG node of the plane
+ osg::ref_ptr<osg::Geode> m_geode;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGPLANE_H
diff --git a/SurgSim/Graphics/OsgPlaneRepresentation.cpp b/SurgSim/Graphics/OsgPlaneRepresentation.cpp
new file mode 100644
index 0000000..4d2e454
--- /dev/null
+++ b/SurgSim/Graphics/OsgPlaneRepresentation.cpp
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgPlaneRepresentation.h"
+
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgPlane.h"
+#include "SurgSim/Graphics/OsgRigidTransformConversions.h"
+
+#include <osg/Geode>
+#include <osg/Shape>
+#include <osg/ShapeDrawable>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgPlaneRepresentation, OsgPlaneRepresentation);
+
+OsgPlaneRepresentation::OsgPlaneRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ PlaneRepresentation(name),
+ m_sharedPlane(getSharedPlane())
+{
+ m_transform->addChild(m_sharedPlane->getNode());
+}
+
+std::shared_ptr<OsgPlane> OsgPlaneRepresentation::getSharedPlane()
+{
+ static SurgSim::Framework::SharedInstance<OsgPlane> shared;
+ return shared.get();
+}
+
+}; // Graphics
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgPlaneRepresentation.h b/SurgSim/Graphics/OsgPlaneRepresentation.h
new file mode 100644
index 0000000..5cb5359
--- /dev/null
+++ b/SurgSim/Graphics/OsgPlaneRepresentation.h
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGPLANEREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGPLANEREPRESENTATION_H
+
+#include "SurgSim/Graphics/PlaneRepresentation.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+#include <osg/PositionAttitudeTransform>
+#include <osg/Switch>
+
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+class OsgPlane;
+SURGSIM_STATIC_REGISTRATION(OsgPlaneRepresentation);
+
+/// OSG implementation of a graphics plane representation.
+class OsgPlaneRepresentation : public OsgRepresentation, public PlaneRepresentation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit OsgPlaneRepresentation(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgPlaneRepresentation);
+
+private:
+
+ /// Shared plane, so that the geometry can be instanced rather than having multiple copies.
+ std::shared_ptr<OsgPlane> m_sharedPlane;
+
+ /// Returns the shared plane
+ static std::shared_ptr<OsgPlane> getSharedPlane();
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGPLANEREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgPointCloudRepresentation.cpp b/SurgSim/Graphics/OsgPointCloudRepresentation.cpp
new file mode 100644
index 0000000..4f33107
--- /dev/null
+++ b/SurgSim/Graphics/OsgPointCloudRepresentation.cpp
@@ -0,0 +1,133 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <osg/Geode>
+#include <osg/PositionAttitudeTransform>
+#include <osg/StateAttribute>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Graphics/OsgConversions.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgPointCloudRepresentation,
+ OsgPointCloudRepresentation);
+
+OsgPointCloudRepresentation::OsgPointCloudRepresentation(const std::string& name) :
+ Representation(name),
+ PointCloudRepresentation(name),
+ OsgRepresentation(name),
+ m_color(1.0, 1.0, 1.0, 1.0)
+{
+ m_vertices = std::make_shared<PointCloud>();
+
+ osg::Geode* geode = new osg::Geode();
+ m_geometry = new osg::Geometry();
+ m_vertexData = new osg::Vec3Array;
+
+ m_geometry->setVertexArray(m_vertexData);
+
+ setColor(m_color);
+
+ // At this stage there are no vertices in there
+ m_drawArrays = new osg::DrawArrays(osg::PrimitiveSet::POINTS,0,m_vertexData->size());
+ m_geometry->addPrimitiveSet(m_drawArrays);
+ m_geometry->setUseDisplayList(false);
+ // m_geometry->setUseVertexBufferObjects(true);
+ m_geometry->setDataVariance(osg::Object::DYNAMIC);
+ m_geometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+
+ m_point = new osg::Point(1.0f);
+ m_geometry->getOrCreateStateSet()->setAttribute( m_point, osg::StateAttribute::ON );
+
+ geode->addDrawable(m_geometry);
+ m_transform->addChild(geode);
+}
+
+
+OsgPointCloudRepresentation::~OsgPointCloudRepresentation()
+{
+}
+
+void OsgPointCloudRepresentation::doUpdate(double dt)
+{
+ auto vertices = m_vertices->getVertices();
+ size_t count = vertices.size();
+
+ // Check for size change in number of vertices
+ if (count != static_cast<size_t>(m_drawArrays->getCount()))
+ {
+ m_drawArrays->setCount(count);
+ if (count > m_vertexData->size())
+ {
+ m_vertexData->resize(count);
+ }
+
+ m_drawArrays->set(osg::PrimitiveSet::POINTS,0,count);
+ m_drawArrays->dirty();
+ }
+
+ for (size_t i = 0; i < count; ++i)
+ {
+ (*m_vertexData)[i][0] = static_cast<float>(vertices[i].position[0]);
+ (*m_vertexData)[i][1] = static_cast<float>(vertices[i].position[1]);
+ (*m_vertexData)[i][2] = static_cast<float>(vertices[i].position[2]);
+ }
+
+ m_geometry->dirtyBound();
+ m_geometry->dirtyDisplayList();
+}
+
+std::shared_ptr<PointCloud> OsgPointCloudRepresentation::getVertices() const
+{
+ return m_vertices;
+}
+
+void OsgPointCloudRepresentation::setPointSize(double val)
+{
+ m_point->setSize(val);
+}
+
+double OsgPointCloudRepresentation::getPointSize() const
+{
+ return static_cast<double>(m_point->getSize());
+}
+
+void OsgPointCloudRepresentation::setColor(const SurgSim::Math::Vector4d& color)
+{
+ // Set the color of the particles to one single color by default
+ osg::Vec4Array* colors = dynamic_cast<osg::Vec4Array*>(m_geometry->getColorArray());
+ if (colors == nullptr)
+ {
+ colors = new osg::Vec4Array(1);
+ m_geometry->setColorArray(colors);
+ m_geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+ }
+ (*colors)[0] = SurgSim::Graphics::toOsg(color);
+ m_color = color;
+}
+
+SurgSim::Math::Vector4d OsgPointCloudRepresentation::getColor() const
+{
+ return m_color;
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/OsgPointCloudRepresentation.h b/SurgSim/Graphics/OsgPointCloudRepresentation.h
new file mode 100644
index 0000000..0f067a4
--- /dev/null
+++ b/SurgSim/Graphics/OsgPointCloudRepresentation.h
@@ -0,0 +1,111 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGPOINTCLOUDREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGPOINTCLOUDREPRESENTATION_H
+
+#include <osg/Array>
+#include <osg/Geometry>
+#include <osg/Point>
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Graphics/PointCloudRepresentation.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class EmptyData;
+
+template<class Data>
+class Vertices;
+}
+
+namespace Graphics
+{
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+SURGSIM_STATIC_REGISTRATION(OsgPointCloudRepresentation);
+
+/// Osg point cloud representation, implementation of a PointCloudRepresenation using OSG.
+class OsgPointCloudRepresentation : public PointCloudRepresentation, public OsgRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name of the Representation.
+ explicit OsgPointCloudRepresentation(const std::string& name);
+
+ /// Destructor
+ ~OsgPointCloudRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgPointCloudRepresentation);
+
+ /// Gets the vertices.
+ /// \return The vertices.
+ virtual std::shared_ptr<PointCloud> getVertices() const override;
+
+ /// Sets point size.
+ /// \param val The value.
+ virtual void setPointSize(double val) override;
+
+ /// Gets point size.
+ /// \return The point size.
+ virtual double getPointSize() const override;
+
+ /// Executes the update operation.
+ /// \param dt The dt.
+ virtual void doUpdate(double dt) override;
+
+ /// Sets a color.
+ /// \param color The color.
+ virtual void setColor(const SurgSim::Math::Vector4d& color) override;
+
+ /// Gets the color.
+ /// \return The current color.
+ virtual SurgSim::Math::Vector4d getColor() const override;
+
+private:
+ /// Local pointer to vertices with data
+ std::shared_ptr<PointCloud> m_vertices;
+
+ /// OSG vertex data for updating
+ osg::ref_ptr<osg::Vec3Array> m_vertexData;
+
+ /// OSG Geometry node holding the data
+ osg::ref_ptr<osg::Geometry> m_geometry;
+
+ /// OSG DrawArrays for local operations
+ osg::ref_ptr<osg::DrawArrays> m_drawArrays;
+
+ /// OSG::Point for local operations
+ osg::ref_ptr<osg::Point> m_point;
+
+ /// Color backing variable
+ SurgSim::Math::Vector4d m_color;
+};
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+}; // Graphics
+}; // SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGPOINTCLOUDREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgQuaternionConversions.h b/SurgSim/Graphics/OsgQuaternionConversions.h
new file mode 100644
index 0000000..f848296
--- /dev/null
+++ b/SurgSim/Graphics/OsgQuaternionConversions.h
@@ -0,0 +1,61 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Conversions to and from OSG quaternion types
+///
+/// Note that the Eigen quaternion constructor takes the W component first, while OSG stores it last.
+///
+/// Also note that OSG only has one Quat type, which uses double for the value type. Conversions are provided to and
+/// from this type for both SurgSim::Math::Quaternionf and SurgSim::Math::Quaterniond.
+
+#ifndef SURGSIM_GRAPHICS_OSGQUATERNIONCONVERSIONS_H
+#define SURGSIM_GRAPHICS_OSGQUATERNIONCONVERSIONS_H
+
+#include "SurgSim/Math/Quaternion.h"
+
+#include <osg/Quat>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Convert quaternion of floats to OSG
+inline osg::Quat toOsg(const SurgSim::Math::Quaternionf& quaternion)
+{
+ return osg::Quat(quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w());
+}
+
+/// Convert quaternion of doubles to OSG
+inline osg::Quat toOsg(const SurgSim::Math::Quaterniond& quaternion)
+{
+ return osg::Quat(quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w());
+}
+
+/// Convert from OSG to either quaternion of floats or doubles
+/// \tparam T Value type (float or double)
+template <typename T> inline
+Eigen::Quaternion<T> fromOsg(const osg::Quat& quaternion)
+{
+ return Eigen::Quaternion<T>(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGQUATERNIONCONVERSIONS_H
diff --git a/SurgSim/Graphics/OsgRenderTarget-inl.h b/SurgSim/Graphics/OsgRenderTarget-inl.h
new file mode 100644
index 0000000..67bb0be
--- /dev/null
+++ b/SurgSim/Graphics/OsgRenderTarget-inl.h
@@ -0,0 +1,182 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGRENDERTARGET_INL_H
+#define SURGSIM_GRAPHICS_OSGRENDERTARGET_INL_H
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+// Osg Supports 16 Color Attachments plus the depth texture
+const int OsgSupportedTextureCount = 16 + 1;
+
+template <class T>
+OsgRenderTarget<T>::OsgRenderTarget() :
+ m_width(0),
+ m_height(0),
+ m_colorTargetCount(0),
+ m_textures(OsgSupportedTextureCount)
+{
+}
+
+template <class T>
+OsgRenderTarget<T>::OsgRenderTarget(
+ int width,
+ int height,
+ double scale,
+ int colorCount,
+ bool useDepth) :
+ m_width(width * scale),
+ m_height(height * scale),
+ m_colorTargetCount(0),
+ m_textures(OsgSupportedTextureCount)
+{
+ setColorTargetCount(colorCount);
+ useDepthTarget(useDepth);
+}
+
+
+template <class T>
+OsgRenderTarget<T>::~OsgRenderTarget()
+{
+}
+
+template <class T>
+void OsgRenderTarget<T>::getSize(int* width, int* height) const
+{
+ *width = m_width;
+ *height = m_height;
+}
+
+template <class T>
+int OsgRenderTarget<T>::setColorTargetCount(int count)
+{
+ int result = (count < 16) ? count : 16;
+
+ // This does not check against graphics card capabilities, the max 16 provided
+ // by OSG might not be supported by the current graphics card
+ // Keep the other texture allocated when the count goes down
+ // Rendertargets are probably not going to change that much once set up
+ // #memory
+ for (int i = m_colorTargetCount; i<result; ++i)
+ {
+ setupTexture(TARGETTYPE_COLORBASE+i);
+ }
+ m_colorTargetCount = result;
+ return result;
+}
+
+template <class T>
+int OsgRenderTarget<T>::getColorTargetCount() const
+{
+ return m_colorTargetCount;
+}
+
+template <class T>
+std::shared_ptr<Texture> OsgRenderTarget<T>::getColorTarget(int index) const
+{
+ std::shared_ptr<Texture> result;
+
+ if (index < m_colorTargetCount)
+ {
+ result = m_textures[TARGETTYPE_COLORBASE + index];
+ }
+
+ return result;
+}
+
+template <class T>
+std::shared_ptr<OsgTexture> OsgRenderTarget<T>::getColorTargetOsg(int index) const
+{
+ std::shared_ptr<T> result;
+
+ if (index < m_colorTargetCount)
+ {
+ result = m_textures[TARGETTYPE_COLORBASE + index];
+ }
+
+ return result;
+}
+
+template <class T>
+void OsgRenderTarget<T>::useDepthTarget(bool val)
+{
+ if (val)
+ {
+ setupTexture(TARGETTYPE_DEPTH);
+ }
+ else
+ {
+ m_textures[TARGETTYPE_DEPTH] = nullptr;
+ }
+}
+
+template <class T>
+bool OsgRenderTarget<T>::doesUseDepthTarget() const
+{
+ return m_textures.at(TARGETTYPE_DEPTH) != nullptr;
+}
+
+template <class T>
+std::shared_ptr<Texture> OsgRenderTarget<T>::getDepthTarget() const
+{
+ return m_textures.at(TARGETTYPE_DEPTH);
+}
+
+template <class T>
+std::shared_ptr<OsgTexture> OsgRenderTarget<T>::getDepthTargetOsg() const
+{
+ return m_textures.at(TARGETTYPE_DEPTH);
+}
+
+template <class T>
+void OsgRenderTarget<T>::setupTexture(int type)
+{
+ if (m_textures[type] == nullptr)
+ {
+ m_textures[type] = std::make_shared<T>();
+ m_textures[type]->setSize(m_width, m_height);
+ osg::Texture* osgTexture = m_textures[type]->getOsgTexture();
+ // We are not dealing with mipmaps, fix up the filters to enable rendering to FBO
+ // see http://www.opengl.org/wiki/Common_Mistakes#Creating_a_complete_texture
+ osgTexture->setFilter(osg::Texture::MIN_FILTER,osg::Texture::LINEAR);
+ osgTexture->setFilter(osg::Texture::MAG_FILTER,osg::Texture::LINEAR);
+ osgTexture->setWrap(osg::Texture::WRAP_S, osg::Texture::CLAMP_TO_EDGE);
+ osgTexture->setWrap(osg::Texture::WRAP_T, osg::Texture::CLAMP_TO_EDGE);
+ if (type == TARGETTYPE_DEPTH)
+ {
+ osgTexture->setInternalFormat(GL_DEPTH_COMPONENT32F);
+ osgTexture->setSourceFormat(GL_DEPTH_COMPONENT);
+ osgTexture->setSourceType(GL_FLOAT);
+ osgTexture->setFilter(osg::Texture::MIN_FILTER,osg::Texture::NEAREST);
+ osgTexture->setFilter(osg::Texture::MAG_FILTER,osg::Texture::NEAREST);
+ }
+ if (type >= TARGETTYPE_COLORBASE)
+ {
+ osgTexture->setInternalFormat(GL_RGBA32F_ARB);
+ osgTexture->setSourceFormat(GL_RGBA);
+ osgTexture->setSourceType(GL_FLOAT);
+ }
+ }
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Graphics/OsgRenderTarget.h b/SurgSim/Graphics/OsgRenderTarget.h
new file mode 100644
index 0000000..db69e93
--- /dev/null
+++ b/SurgSim/Graphics/OsgRenderTarget.h
@@ -0,0 +1,146 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGRENDERTARGET_H
+#define SURGSIM_GRAPHICS_OSGRENDERTARGET_H
+
+#include <memory>
+#include <unordered_map>
+
+#include "SurgSim/Graphics/RenderTarget.h"
+#include "SurgSim/Graphics/OsgTexture.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/OsgTextureRectangle.h"
+
+#include <osg/FrameBufferObject>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+
+/// Osg abstract render target, this hides the type of the actual osg texture and lets us use
+/// OsgRenderTarget without the template type
+class OsgAbstractRenderTarget : public RenderTarget
+{
+ /// Accessor for the color target as an OsgTexture.
+ /// \param index Zero-based index of the color texture.
+ /// \return The color target as an osg specific class.
+ virtual std::shared_ptr<OsgTexture> getColorTargetOsg(int index) const = 0;
+
+ /// Accessor for the depth target as an OsgTexture.
+ /// \return The depth target as an osg specific class.
+ virtual std::shared_ptr<OsgTexture> getDepthTargetOsg() const = 0;
+};
+
+/// Specific implementation of the render target class. It is templated so different texture formats can be chosen.
+/// \tparam T Type of the texture that should be used as targets probably either OsgTexture2d or OsgTextureRectangle.
+template <class T>
+class OsgRenderTarget : public OsgAbstractRenderTarget
+{
+public:
+
+ /// The internal type of the texture, not exposed in the public interface
+ typedef T TextureType;
+
+ /// Default constructor
+ OsgRenderTarget();
+
+ /// Constructor set all the paramters for the render target
+ /// \param width, height The width and height of the target textures.
+ /// \param scale (Optional) the scale, scales width and height by this factor.
+ /// \param colorCount (Optional) number of color textures to use.
+ /// \param useDepth (Optional) whether to use a depth texture.
+ OsgRenderTarget(int width, int height, double scale = 1.0,
+ int colorCount = 0, bool useDepth = false);
+ /// Destructor
+ ~OsgRenderTarget();
+
+ /// Gets a size.
+ /// \param [out] width, height The width and height of the RenderTarget textures.
+ virtual void getSize(int* width, int* height) const override;
+
+ /// \return The number of color targets that are available.
+ virtual int getColorTargetCount() const override;
+
+ /// Generic accessor for a specific color target texture.
+ /// \param index Zero-based index of the texure.
+ /// \return The actual Texture.
+ virtual std::shared_ptr<Texture> getColorTarget(int index) const override;
+
+ /// Accessor for the color target as an OsgTexture.
+ /// \param index Zero-based index of the color texture.
+ /// \return The color target as an osg specific class.
+ std::shared_ptr<OsgTexture> getColorTargetOsg(int index) const;
+
+ /// Determines if RenderTarget does use a depth target.
+ /// \return true if it does.
+ virtual bool doesUseDepthTarget() const override;
+
+ /// Generic accessor for the depth Target.
+ /// \return The depth target.
+ virtual std::shared_ptr<Texture> getDepthTarget() const override;
+
+ /// Accessor for the depth target as an OsgTexture.
+ /// \return The depth target as an osg specific class.
+ std::shared_ptr<OsgTexture> getDepthTargetOsg() const;
+
+private:
+
+ /// Values that represent TargetTypes.
+ enum TargetTypes {
+ TARGETTYPE_DEPTH = 0,
+ TARGETTYPE_COLORBASE = 1
+ };
+
+ /// The width of this RenderTarget.
+ int m_width;
+
+ /// The height of this RenderTarget.
+ int m_height;
+
+ /// Number of color targets.
+ int m_colorTargetCount;
+
+ /// The textures that are being used as target, size of this is 16 (ColorTargets) + 1 (Depth).
+ std::vector<std::shared_ptr<TextureType>> m_textures;
+
+ /// Sets color target count.
+ /// \param count The number of color textures to use.
+ /// \return .
+ int setColorTargetCount(int count);
+
+ /// Use depth target.
+ /// \param val true to value.
+ void useDepthTarget(bool val);
+
+ /// Sets up the texture with a given target type (depth or color w/ index).
+ /// \param type The index of the texture to use.
+ void setupTexture(int type);
+};
+
+///@{
+/// Predefine specialized render targets
+typedef OsgRenderTarget<OsgTexture2d> OsgRenderTarget2d;
+typedef OsgRenderTarget<OsgTextureRectangle> OsgRenderTargetRectangle;
+///@}
+
+}; // Graphics
+}; // SurgSim
+
+#include "SurgSim/Graphics/OsgRenderTarget-inl.h"
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgRepresentation.cpp b/SurgSim/Graphics/OsgRepresentation.cpp
new file mode 100644
index 0000000..3fab621
--- /dev/null
+++ b/SurgSim/Graphics/OsgRepresentation.cpp
@@ -0,0 +1,139 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgRepresentation.h"
+
+#include <algorithm>
+
+#include <boost/thread/locks.hpp>
+
+#include <osg/Geode>
+#include <osg/Switch>
+#include <osg/PolygonMode>
+#include <osg/PositionAttitudeTransform>
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgRigidTransformConversions.h"
+#include "SurgSim/Graphics/OsgUnitBox.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+OsgRepresentation::OsgRepresentation(const std::string& name) :
+ Representation(name),
+ m_drawAsWireFrame(false)
+{
+ m_switch = new osg::Switch;
+ m_switch->setName(name + " Representation Switch");
+
+ m_transform = new osg::PositionAttitudeTransform();
+ m_transform->setName(name + " Transform");
+
+ m_switch->addChild(m_transform);
+
+ m_transform->setAttitude(osg::Quat(0.0, 0.0, 0.0, 1.0));
+ m_transform->setPosition(osg::Vec3d(0.0, 0.0, 0.0));
+}
+
+OsgRepresentation::~OsgRepresentation()
+{
+
+}
+
+void OsgRepresentation::setVisible(bool visible)
+{
+ m_switch->setChildValue(m_transform, isActive() && visible);
+ m_isVisible = visible;
+}
+
+bool OsgRepresentation::isVisible() const
+{
+ return m_switch->getChildValue(m_transform);
+}
+
+void OsgRepresentation::update(double dt)
+{
+ std::pair<osg::Quat, osg::Vec3d> pose = toOsg(getPose());
+ m_transform->setAttitude(pose.first);
+ m_transform->setPosition(pose.second);
+ doUpdate(dt);
+}
+
+bool OsgRepresentation::setMaterial(std::shared_ptr<SurgSim::Graphics::Material> material)
+{
+ bool didSucceed = false;
+
+ std::shared_ptr<OsgMaterial> osgMaterial = std::dynamic_pointer_cast<OsgMaterial>(material);
+ if (osgMaterial != nullptr)
+ {
+ m_transform->setStateSet(osgMaterial->getOsgStateSet());
+ didSucceed = true;
+ m_material = osgMaterial;
+ }
+ return didSucceed;
+}
+
+std::shared_ptr<Material> OsgRepresentation::getMaterial() const
+{
+ return m_material;
+}
+
+void OsgRepresentation::clearMaterial()
+{
+ m_transform->setStateSet(new osg::StateSet()); // Reset to empty state set
+ m_material = nullptr;
+}
+
+osg::ref_ptr<osg::Node> OsgRepresentation::getOsgNode() const
+{
+ return m_switch;
+}
+
+void OsgRepresentation::doUpdate(double dt)
+{
+
+}
+
+void OsgRepresentation::setDrawAsWireFrame(bool val)
+{
+ m_drawAsWireFrame = val;
+ osg::StateSet* state = m_switch->getOrCreateStateSet();
+
+ osg::ref_ptr<osg::PolygonMode> polygonMode;
+ if (val)
+ {
+ polygonMode = new osg::PolygonMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE);
+ }
+ else
+ {
+ polygonMode = new osg::PolygonMode(osg::PolygonMode::FRONT, osg::PolygonMode::FILL);
+ }
+
+ state->setAttributeAndModes(polygonMode, osg::StateAttribute::ON);
+}
+
+bool OsgRepresentation::getDrawAsWireFrame() const
+{
+ return m_drawAsWireFrame;
+}
+
+
+
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/OsgRepresentation.h b/SurgSim/Graphics/OsgRepresentation.h
new file mode 100644
index 0000000..e4506a9
--- /dev/null
+++ b/SurgSim/Graphics/OsgRepresentation.h
@@ -0,0 +1,100 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGREPRESENTATION_H
+
+#include <memory>
+
+#include <osg/ref_ptr>
+
+#include "SurgSim/Graphics/Representation.h"
+
+namespace osg
+{
+class Node;
+class PositionAttitudeTransform;
+class Switch;
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class OsgMaterial;
+
+/// Base OSG implementation of a graphics representation.
+///
+/// Wraps an osg::Node which serves as the root for the representation's portion of the scene graph.
+class OsgRepresentation : public virtual Representation
+{
+public:
+ /// Constructor
+ explicit OsgRepresentation(const std::string& name);
+ /// Destructor
+ virtual ~OsgRepresentation();
+
+ /// Returns the root OSG Node for this representations portion of the scene graph
+ osg::ref_ptr<osg::Node> getOsgNode() const;
+
+ /// Sets whether the representation is currently visible
+ /// \note If the representation is inactive, this method does not have any effect.
+ /// \param visible True for visible, false for invisible
+ virtual void setVisible(bool visible) override;
+
+ /// Gets whether the representation is currently visible
+ /// \return visible True for visible, false for invisible
+ virtual bool isVisible() const override;
+
+ /// Sets the material that defines the visual appearance of the representation
+ /// \param material Graphics material
+ /// \return True if set successfully, otherwise false
+ /// \note OsgPlaneRepresentation only accepts subclasses of OsgMaterial.
+ virtual bool setMaterial(std::shared_ptr<Material> material) override;
+
+ /// Gets the material that defines the visual appearance of the representation
+ /// \return Graphics material
+ virtual std::shared_ptr<Material> getMaterial() const override;
+
+ /// Removes the material from the representation
+ virtual void clearMaterial() override;
+
+ virtual void setDrawAsWireFrame(bool val) override;
+ virtual bool getDrawAsWireFrame() const override;
+
+ /// Updates the representation.
+ /// \param dt The time in seconds of the preceding timestep.
+ virtual void update(double dt) override;
+
+protected:
+ virtual void doUpdate(double dt);
+
+ /// Switch used to toggle the visibility of the representation
+ osg::ref_ptr<osg::Switch> m_switch;
+ /// Transform used to pose the representation
+ osg::ref_ptr<osg::PositionAttitudeTransform> m_transform;
+
+ /// Material defining the visual appearance of the representation
+ std::shared_ptr<OsgMaterial> m_material;
+
+ /// Indicates if the representation is rendered as a wireframe.
+ bool m_drawAsWireFrame;
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGREPRESENTATION_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgRigidTransformConversions.h b/SurgSim/Graphics/OsgRigidTransformConversions.h
new file mode 100644
index 0000000..1420d9e
--- /dev/null
+++ b/SurgSim/Graphics/OsgRigidTransformConversions.h
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Conversions to and from OSG rigid transform types
+///
+/// Note that OSG does not have a rigid transform type, so a quaternion, vector pair are used for the rotation and
+/// translation components of the rigid transform.
+
+#ifndef SURGSIM_GRAPHICS_OSGRIGIDTRANSFORMCONVERSIONS_H
+#define SURGSIM_GRAPHICS_OSGRIGIDTRANSFORMCONVERSIONS_H
+
+#include "SurgSim/Graphics/OsgQuaternionConversions.h"
+#include "SurgSim/Graphics/OsgVectorConversions.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <osg/Quat>
+#include <osg/Vec2>
+#include <osg/Vec3>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Convert 3D rigid body (isometric) transform, represented as floats, to OSG
+inline std::pair<osg::Quat, osg::Vec3f> toOsg(const SurgSim::Math::RigidTransform3f& transform)
+{
+ SurgSim::Math::Quaternionf normalizedQuaternion = SurgSim::Math::Quaternionf(transform.linear()).normalized();
+ return std::make_pair(toOsg(normalizedQuaternion), toOsg(SurgSim::Math::Vector3f(transform.translation())));
+}
+/// Convert 3D rigid body (isometric) transform, represented as doubles, to OSG
+inline std::pair<osg::Quat, osg::Vec3d> toOsg(const SurgSim::Math::RigidTransform3d& transform)
+{
+ SurgSim::Math::Quaterniond normalizedQuaternion = SurgSim::Math::Quaterniond(transform.linear()).normalized();
+ return std::make_pair(toOsg(normalizedQuaternion), toOsg(SurgSim::Math::Vector3d(transform.translation())));
+}
+
+/// Convert from OSG to 3D rigid body (isometric) transform, represented as floats
+inline SurgSim::Math::RigidTransform3f fromOsg(const osg::Quat& rotation, const osg::Vec3f& translation)
+{
+ return SurgSim::Math::makeRigidTransform(fromOsg<float>(rotation), fromOsg(translation));
+}
+/// Convert from OSG to 3D rigid body (isometric) transform, represented as floats
+inline SurgSim::Math::RigidTransform3f fromOsg(const std::pair<osg::Quat, osg::Vec3f>& transform)
+{
+ return fromOsg(transform.first, transform.second);
+}
+/// Convert from OSG to 3D rigid body (isometric) transform, represented as doubles
+inline SurgSim::Math::RigidTransform3d fromOsg(const osg::Quat& rotation, const osg::Vec3d& translation)
+{
+ return SurgSim::Math::makeRigidTransform(fromOsg<double>(rotation), fromOsg(translation));
+}
+/// Convert from OSG to 3D rigid body (isometric) transform, represented as doubles
+inline SurgSim::Math::RigidTransform3d fromOsg(const std::pair<osg::Quat, osg::Vec3d>& transform)
+{
+ return fromOsg(transform.first, transform.second);
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGRIGIDTRANSFORMCONVERSIONS_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgSceneryRepresentation.cpp b/SurgSim/Graphics/OsgSceneryRepresentation.cpp
new file mode 100644
index 0000000..f6d4ec1
--- /dev/null
+++ b/SurgSim/Graphics/OsgSceneryRepresentation.cpp
@@ -0,0 +1,67 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <osg/PositionAttitudeTransform>
+#include <osgDB/ReadFile>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgSceneryRepresentation, OsgSceneryRepresentation);
+
+OsgSceneryRepresentation::OsgSceneryRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ SceneryRepresentation(name),
+ m_sceneryRepresentation(nullptr),
+ m_fileName()
+{
+}
+
+bool OsgSceneryRepresentation::doInitialize()
+{
+ // HS-2013-dec-17 This should probably use a return value of false rather than assertions
+ SURGSIM_ASSERT(m_fileName != "") << "Filename can't be empty.";
+ std::shared_ptr<const SurgSim::Framework::ApplicationData> applicationData = getRuntime()->getApplicationData();
+
+ std::string objectPath = applicationData->findFile(m_fileName);
+ SURGSIM_ASSERT(!objectPath.empty()) << "Could not find file " << m_fileName << std::endl;
+
+ m_sceneryRepresentation = osgDB::readNodeFile(objectPath);
+ SURGSIM_ASSERT(m_sceneryRepresentation.valid()) << "Could not load file " << objectPath << std::endl;
+
+ m_transform->addChild(m_sceneryRepresentation);
+ return true;
+}
+
+std::string OsgSceneryRepresentation::getFileName() const
+{
+ return m_fileName;
+}
+
+void OsgSceneryRepresentation::setFileName(const std::string& fileName)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Can't set the filename after the object has been initialized.";
+ m_fileName = fileName;
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgSceneryRepresentation.h b/SurgSim/Graphics/OsgSceneryRepresentation.h
new file mode 100644
index 0000000..4add15b
--- /dev/null
+++ b/SurgSim/Graphics/OsgSceneryRepresentation.h
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGSCENERYREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGSCENERYREPRESENTATION_H
+
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/SceneryRepresentation.h"
+
+#include <osg/Node>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+namespace Graphics
+{
+SURGSIM_STATIC_REGISTRATION(OsgSceneryRepresentation);
+
+/// A OsgSceneryRepresentation is used to load osg object/node from file
+class OsgSceneryRepresentation: public OsgRepresentation, public SceneryRepresentation
+{
+public:
+ friend class OsgSceneryRepresentationTest;
+
+ /// Constructor
+ /// \param name Name of OsgSceneryRepresentation
+ explicit OsgSceneryRepresentation(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgSceneryRepresentation);
+
+ /// Return file name of the object
+ /// \return File name of the object
+ std::string getFileName() const override;
+ /// Set file name of the object to be loaded
+ /// \param fileName Name of the file to be loaded
+ void setFileName(const std::string& fileName) override;
+
+
+private:
+ virtual bool doInitialize() override;
+
+ /// A osg::Node to hold the objet loaded from file
+ osg::ref_ptr<osg::Node> m_sceneryRepresentation;
+
+ /// Name of the object file to be loaded
+ std::string m_fileName;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGSCENERYREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgScreenSpacePass.cpp b/SurgSim/Graphics/OsgScreenSpacePass.cpp
new file mode 100644
index 0000000..dd81b14
--- /dev/null
+++ b/SurgSim/Graphics/OsgScreenSpacePass.cpp
@@ -0,0 +1,85 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgScreenSpacePass.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+
+#include <memory>
+#include <osg/Camera>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+OsgScreenSpacePass::OsgScreenSpacePass(const std::string& name) :
+ RenderPass(name),
+ m_width(0),
+ m_height(0)
+{
+
+}
+
+OsgScreenSpacePass::~OsgScreenSpacePass()
+{
+
+}
+
+bool OsgScreenSpacePass::doInitialize()
+{
+ RenderPass::doInitialize();
+ auto camera = std::dynamic_pointer_cast<OsgCamera>(getCamera());
+
+ bool result = false;
+
+ if (camera != nullptr)
+ {
+ m_camera = camera->getOsgCamera();
+ m_camera->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
+ m_camera->setClearMask(GL_DEPTH_BUFFER_BIT);
+ setRenderOrder(Camera::RENDER_ORDER_POST_RENDER, 0);
+ updateViewport(m_width, m_height);
+ result = true;
+ }
+
+ return result;
+}
+
+
+void OsgScreenSpacePass::updateViewport(int width, int height)
+{
+ if (m_camera != nullptr)
+ {
+ m_camera->setProjectionMatrixAsOrtho2D(0, width, 0, height);
+ }
+}
+
+void OsgScreenSpacePass::setViewPort(int width, int height)
+{
+ SURGSIM_ASSERT(width >= 0 && height >= 0) << "Viewport width or height cannot be smaller than 0.";
+
+ if (m_width != width || m_height != height)
+ {
+ m_width = width;
+ m_height = height;
+
+ updateViewport(m_width, m_height);
+ }
+}
+
+
+}
+}
+
diff --git a/SurgSim/Graphics/OsgScreenSpacePass.h b/SurgSim/Graphics/OsgScreenSpacePass.h
new file mode 100644
index 0000000..d17200b
--- /dev/null
+++ b/SurgSim/Graphics/OsgScreenSpacePass.h
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGSCREENSPACEPASS_H
+#define SURGSIM_GRAPHICS_OSGSCREENSPACEPASS_H
+
+#include <string>
+
+#include "SurgSim/Graphics/RenderPass.h"
+
+#include <osg/ref_ptr>
+
+namespace osg
+{
+class Camera;
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Special RenderPass to draw items using a orthogonal projection, this is specific to the
+/// Osg implementation of the SurgSim rendering
+class OsgScreenSpacePass : public RenderPass
+{
+public:
+
+ /// Constructor.
+ /// \param name The name of the component
+ explicit OsgScreenSpacePass(const std::string& name);
+
+ /// Destructor
+ virtual ~OsgScreenSpacePass();
+
+ /// Set viewport dimensions
+ void setViewPort(int width, int height);
+
+ /// Initialize this Component
+ virtual bool doInitialize() override;
+
+
+private:
+
+ /// Update the projection matrix
+ void updateViewport(int width, int height);
+
+ /// The osg camera reference
+ osg::ref_ptr<osg::Camera> m_camera;
+
+ /// The width of the viewport
+ int m_width;
+
+ /// The height of the viewport
+ int m_height;
+};
+
+}
+}
+
+#endif // SURGSIM_GRAPHICS_OSGSCREENSPACEPASS_H
diff --git a/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.cpp b/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.cpp
new file mode 100644
index 0000000..5874f0d
--- /dev/null
+++ b/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.cpp
@@ -0,0 +1,288 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgRigidTransformConversions.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgUniformBase.h"
+#include "SurgSim/Graphics/Texture2d.h"
+#include "SurgSim/Graphics/TextureRectangle.h"
+#include "SurgSim/Graphics/View.h"
+
+
+#include <osg/Array>
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osg/MatrixTransform>
+#include <osg/PositionAttitudeTransform>
+#include <osg/Projection>
+#include <osg/StateAttribute>
+#include <osg/Switch>
+
+namespace
+{
+enum TextureType
+{
+ TEXTURE_TYPE_RECTANGLE,
+ TEXTURE_TYPE_POWER_OF_TWO
+};
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+
+OsgScreenSpaceQuadRepresentation::OsgScreenSpaceQuadRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ ScreenSpaceQuadRepresentation(name),
+ m_scale(1.0, 1.0, 1.0)
+{
+ m_switch = new osg::Switch;
+ m_switch->setName(name + " Switch");
+
+ m_transform = new osg::PositionAttitudeTransform();
+ m_transform->setName(name + " Transform");
+
+ m_geode = new osg::Geode;
+
+ // Make the quad
+ float depth = 0.0;
+ m_geometry = osg::createTexturedQuadGeometry(
+ osg::Vec3(0.0, 0.0, depth),
+ osg::Vec3(1.0, 0.0, depth),
+ osg::Vec3(0.0, 1.0, depth));
+
+ osg::Vec4Array* colors = new osg::Vec4Array;
+ colors->push_back(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
+ m_geometry->setColorArray(colors);
+ m_geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+ m_geometry->addPrimitiveSet(new osg::DrawArrays(GL_QUADS, 0, 4));
+
+ m_geode->addDrawable(m_geometry);
+
+ m_transform->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
+ m_transform->setCullingActive(false);
+ m_transform->addChild(m_geode);
+
+ m_switch->addChild(m_transform);
+
+ m_textureUniform = std::make_shared<OsgUniform<std::shared_ptr<OsgTexture2d>>>("texture");
+ m_rectangleTextureUniform = std::make_shared<OsgUniform<std::shared_ptr<OsgTextureRectangle>>>("texture");
+
+ removeGroupReference(Representation::DefaultGroupName);
+ addGroupReference(Representation::DefaultHudGroupName);
+}
+
+OsgScreenSpaceQuadRepresentation::~OsgScreenSpaceQuadRepresentation()
+{
+
+}
+
+void OsgScreenSpaceQuadRepresentation::setSize(double width, double height)
+{
+ m_scale.x() = width;
+ m_scale.y() = height;
+ m_transform->setScale(m_scale);
+}
+
+void OsgScreenSpaceQuadRepresentation::getSize(double* width, double* height) const
+{
+ SURGSIM_ASSERT(width != nullptr && height != nullptr) << "Cannot use a nullptr as an output parameter";
+ *width = m_scale.x();
+ *height = m_scale.y();
+}
+
+bool OsgScreenSpaceQuadRepresentation::setTexture(std::shared_ptr<Texture> texture)
+{
+ SURGSIM_ASSERT(texture != nullptr) << "Null texture passed to setTexture";
+ std::shared_ptr<OsgTexture2d> osgTexture2d = std::dynamic_pointer_cast<OsgTexture2d>(texture);
+ if (osgTexture2d != nullptr)
+ {
+ return setTexture(osgTexture2d);
+ }
+
+ std::shared_ptr<OsgTextureRectangle> osgTextureRectangle = std::dynamic_pointer_cast<OsgTextureRectangle>(texture);
+ if (osgTextureRectangle != nullptr)
+ {
+ return setTexture(osgTextureRectangle);
+ }
+
+ return false;
+
+}
+
+bool OsgScreenSpaceQuadRepresentation::setTexture(std::shared_ptr<OsgTexture2d> osgTexture)
+{
+ m_textureUniform->set(osgTexture);
+
+ if (m_texureType.hasValue() && m_texureType.getValue() == TEXTURE_TYPE_RECTANGLE)
+ {
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot change the type of texture once the quad has been initialized.";
+ m_rectangleTextureUniform->removeFromStateSet(m_switch->getOrCreateStateSet());
+ }
+ else
+ {
+ m_textureUniform->addToStateSet(m_switch->getOrCreateStateSet());
+ m_texureType.setValue(TEXTURE_TYPE_POWER_OF_TWO);
+ }
+
+ setTextureCoordinates(0.0, 0.0, 1.0, 1.0);
+
+ return true;
+}
+
+bool OsgScreenSpaceQuadRepresentation::setTexture(std::shared_ptr<OsgTextureRectangle> osgTexture)
+{
+ m_rectangleTextureUniform->set(osgTexture);
+
+ if (m_texureType.hasValue() && m_texureType.getValue() == TEXTURE_TYPE_POWER_OF_TWO)
+ {
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot change the type of texture once the quad has been initialized.";
+ m_textureUniform->removeFromStateSet(m_switch->getOrCreateStateSet());
+ }
+ else
+ {
+ m_rectangleTextureUniform->addToStateSet(m_switch->getOrCreateStateSet());
+ m_texureType.setValue(TEXTURE_TYPE_RECTANGLE);
+ }
+
+ int width, height;
+ osgTexture->getSize(&width, &height);
+ setTextureCoordinates(0.0, 0.0, static_cast<float>(width), static_cast<float>(height));
+
+ return true;
+}
+
+void OsgScreenSpaceQuadRepresentation::setTextureCoordinates(float left, float bottom, float right, float top)
+{
+ osg::Vec2Array* tcoords = new osg::Vec2Array(4);
+ (*tcoords)[0].set(left, top);
+ (*tcoords)[1].set(left, bottom);
+ (*tcoords)[2].set(right, bottom);
+ (*tcoords)[3].set(right, top);
+ m_geometry->setTexCoordArray(0, tcoords);
+}
+
+void OsgScreenSpaceQuadRepresentation::setLocation(double x, double y)
+{
+ SurgSim::Math::RigidTransform3d transform =
+ SurgSim::Math::makeRigidTransform(SurgSim::Math::Quaterniond::Identity(), SurgSim::Math::Vector3d(x, y, 0));
+ setLocalPose(transform);
+}
+
+void OsgScreenSpaceQuadRepresentation::getLocation(double* x, double* y)
+{
+ SURGSIM_ASSERT(x != nullptr && y != nullptr) << "Cannot use a nullptr as an output parameter.";
+ SurgSim::Math::Vector3d position = getLocalPose().translation();
+
+ *x = position.x();
+ *y = position.y();
+}
+
+
+void OsgScreenSpaceQuadRepresentation::doUpdate(double dt)
+{
+ m_transform->setAttitude(osg::Quat(0.0, 0.0, 0.0, 1.0));
+}
+
+
+bool OsgScreenSpaceQuadRepresentation::doInitialize()
+{
+ bool result = true;
+
+ // if the material was preassigned, don't create a default one
+ if (getMaterial() == nullptr && m_texureType.hasValue())
+ {
+ result = false;
+ std::shared_ptr<OsgMaterial> material;
+ switch (m_texureType.getValue())
+ {
+ case TEXTURE_TYPE_POWER_OF_TWO:
+ material = buildMaterial("Shaders/unlit_texture.vert", "Shaders/unlit_texture.frag");
+ break;
+ case TEXTURE_TYPE_RECTANGLE:
+ material = buildMaterial("Shaders/unlit_texture.vert", "Shaders/unlit_texture_rectangle.frag");
+ break;
+ default:
+ break;
+ }
+
+
+ if (material != nullptr)
+ {
+ setMaterial(material);
+ result = true;
+ }
+ }
+
+ return result;
+}
+
+std::shared_ptr<OsgMaterial> OsgScreenSpaceQuadRepresentation::buildMaterial(
+ const std::string& vertexShaderName,
+ const std::string& fragmentShaderName)
+{
+ bool result = true;
+
+ std::shared_ptr<OsgMaterial> material;
+
+ auto shader = std::make_shared<OsgShader>();
+ std::string fileName;
+ fileName = getRuntime()->getApplicationData()->findFile(vertexShaderName);
+ if (!shader->loadVertexShaderSource(fileName))
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getLogger("Graphics"))
+ << "Shader " << vertexShaderName << ", could not "
+ << ((fileName == "") ? "find shader file" : "compile " + fileName) << "."
+ << " The quad " << getName() << " might not show on the screen.";
+ result = false;
+ }
+
+ fileName = getRuntime()->getApplicationData()->findFile(fragmentShaderName);
+ if (!shader->loadFragmentShaderSource(fileName))
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getLogger("Graphics"))
+ << "Shader " << fragmentShaderName << " , could not "
+ << ((fileName == "") ? "find shader file" : "compile " + fileName) << "."
+ << " The quad " << getName() << " might not show on the screen.";
+ result = false;
+ }
+
+ if (result)
+ {
+ material = std::make_shared<OsgMaterial>("material");
+ material->setShader(shader);
+ }
+
+ return material;
+}
+
+
+
+
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h b/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h
new file mode 100644
index 0000000..4de96f5
--- /dev/null
+++ b/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h
@@ -0,0 +1,159 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGSCREENSPACEQUADREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGSCREENSPACEQUADREPRESENTATION_H
+
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Graphics/ScreenSpaceQuadRepresentation.h"
+
+#include <osg/Vec3>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace osg
+{
+class Projection;
+class Geode;
+class Geometry;
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class UniformBase;
+class Texture;
+class OsgTexture2d;
+class OsgTextureRectangle;
+
+
+/// Implements the ScreenSpaceQuadRepresentation, provides the uniform 'texture' for the texture that
+/// it carries.
+class OsgScreenSpaceQuadRepresentation : public OsgRepresentation, public ScreenSpaceQuadRepresentation
+{
+public:
+
+ /// Constructor
+ explicit OsgScreenSpaceQuadRepresentation(const std::string& name);
+ ~OsgScreenSpaceQuadRepresentation();
+
+ /// Sets the location in screen space.
+ /// \param x,y The x and y coordinates.
+ virtual void setLocation(double x, double y);
+
+ /// Gets the location in screen space.
+ /// \param [out] x,y If non-null the x and y coordinates. Throws exception otherwise.
+ virtual void getLocation(double* x, double* y);
+
+ /// Sets the size for the quad in screen coordinates.
+ /// \param width The width of the quad in screen coordinates.
+ /// \param height The height of the quad in screen coordinates.
+ virtual void setSize(double width, double height) override;
+
+ /// Gets the size of the quad.
+ /// \param [out] width If non-null, the width. Throws exception otherwise.
+ /// \param [out] height If non-null, the height. Throws exception otherwise.
+ virtual void getSize(double* width, double* height) const override;
+
+ /// Sets a Texture for this quad, this should replace a current texture, this is a convenience function and
+ /// this will use the uniform name "texture" for the uniform in this operation. This can be accomplished
+ /// from the outside as well by using the material.
+ /// \param texture The texture to be set on the quad.
+ /// \return true if it succeeds, false if it fails.
+ virtual bool setTexture(std::shared_ptr<Texture> texture) override;
+
+ /// Sets a Texture2d for this quad, this should replace a current texture, this is a convenience function and
+ /// this will use the uniform name "texture" for the uniform in this operation. This can be accomplished
+ /// from the outside as well by using the material.
+ /// \param texture The texture to be set on the quad.
+ /// \return true if it succeeds, false if it fails.
+ bool setTexture(std::shared_ptr<OsgTexture2d> texture);
+
+ /// Sets a rectangular texture for this quad, this should replace a current texture,
+ /// this is a convenience function and will use the uniform name "texture" for the uniform in this operation.
+ /// \throws SurgSim::Framework::AssertionFailure if the type of the texture is changed during runtime.
+ /// \param texture The texture to be set on the quad.
+ /// \return true if it succeeds, false if it fails.
+ bool setTexture(std::shared_ptr<OsgTextureRectangle> texture);
+
+protected:
+ virtual void doUpdate(double dt) override;
+
+ virtual bool doInitialize() override;
+
+private:
+
+ /// Local geode to contain geometry
+ osg::ref_ptr<osg::Geode> m_geode;
+
+ /// Local geometry pointer
+ osg::ref_ptr<osg::Geometry> m_geometry;
+
+ /// Projection matrix, needs to be updated when the view is changed
+ osg::ref_ptr<osg::Projection> m_projection;
+
+ /// Size of the quad
+ osg::Vec3 m_scale;
+
+ ///@{
+ /// Cached view extensions
+ int m_displayWidth;
+ int m_displayHeight;
+ ///@}
+
+ /// Sets texture coordinates for the quad.
+ /// \param left, bottom, right, top The extents for the texture coordinates for the corners of the quad.
+ void setTextureCoordinates(float left, float bottom, float right, float top);
+
+ /// Replace a uniform in the material, will create the material if necessary
+ /// \param name The name of the uniform to replace.
+ /// \param newUniform The new uniform.
+ /// \return true if it succeeds, false if it fails.
+ bool replaceUniform(const std::string& name, std::shared_ptr<SurgSim::Graphics::UniformBase> newUniform);
+
+ /// Uniform to carry the power of two texture, "texture"
+ std::shared_ptr<OsgUniform<std::shared_ptr<OsgTexture2d>>> m_textureUniform;
+
+ /// Uniform to carry the rectangle texture "texture"
+ std::shared_ptr<OsgUniform<std::shared_ptr<OsgTextureRectangle>>> m_rectangleTextureUniform;
+
+ /// Indicate which type of texture is currently being used
+ SurgSim::DataStructures::OptionalValue<int> m_texureType;
+
+ /// Utility function to build the material.
+ /// \param vertexShaderName name of the vertex shader to be used, needs to be available on the path.
+ /// \param fragmentShaderName name of the fragmen shader to be used, needs to be available on the path.
+ /// \return a valid material if all the shaders are found
+ std::shared_ptr<OsgMaterial> buildMaterial(
+ const std::string& vertexShaderName,
+ const std::string& fragmentShaderName);
+};
+
+}; // Graphics
+}; // SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgShader.cpp b/SurgSim/Graphics/OsgShader.cpp
new file mode 100644
index 0000000..45d098f
--- /dev/null
+++ b/SurgSim/Graphics/OsgShader.cpp
@@ -0,0 +1,259 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Log.h"
+
+using SurgSim::Graphics::OsgShader;
+
+OsgShader::OsgShader() : SurgSim::Graphics::Shader(),
+ m_program(new osg::Program()),
+ m_globalScope(false)
+{
+
+}
+
+void OsgShader::addToStateSet(osg::StateSet* stateSet)
+{
+ if (stateSet != nullptr)
+ {
+ int attribute = osg::StateAttribute::ON | ((m_globalScope) ? osg::StateAttribute::OVERRIDE : 0);
+ stateSet->setAttributeAndModes(m_program, attribute);
+ }
+}
+
+void OsgShader::removeFromStateSet(osg::StateSet* stateSet)
+{
+ if (stateSet != nullptr)
+ {
+ stateSet->removeAttribute(m_program);
+ }
+}
+
+bool OsgShader:: hasVertexShader() const
+{
+ return m_vertexShader.valid();
+}
+
+void OsgShader::clearVertexShader()
+{
+ if (m_vertexShader)
+ {
+ m_program->removeShader(m_vertexShader);
+ m_vertexShader = nullptr;
+ }
+}
+
+bool OsgShader::loadVertexShaderSource(const std::string& filePath)
+{
+ if (! m_vertexShader.valid())
+ {
+ m_vertexShader = new osg::Shader(osg::Shader::VERTEX);
+ m_vertexShader->setName(filePath);
+ m_program->addShader(m_vertexShader);
+ }
+ return m_vertexShader->loadShaderSourceFromFile(filePath);
+}
+
+void OsgShader::setVertexShaderSource(const std::string& source)
+{
+ if (! m_vertexShader.valid())
+ {
+ m_vertexShader = new osg::Shader(osg::Shader::VERTEX);
+ m_program->addShader(m_vertexShader);
+ }
+ m_vertexShader->setShaderSource(source);
+}
+
+bool OsgShader::getVertexShaderSource(std::string* source) const
+{
+ if (m_vertexShader)
+ {
+ *source = m_vertexShader->getShaderSource();
+ return true;
+ }
+ else
+ {
+ *source = "";
+ return false;
+ }
+}
+
+bool OsgShader::hasGeometryShader() const
+{
+ return m_geometryShader.valid();
+}
+
+void OsgShader::clearGeometryShader()
+{
+ if (m_geometryShader)
+ {
+ m_program->removeShader(m_geometryShader);
+ m_geometryShader = nullptr;
+ }
+}
+
+bool OsgShader::loadGeometryShaderSource(const std::string& filePath)
+{
+ if (! m_geometryShader.valid())
+ {
+ m_geometryShader = new osg::Shader(osg::Shader::GEOMETRY);
+ m_geometryShader->setName(filePath);
+ m_program->addShader(m_geometryShader);
+ }
+ return m_geometryShader->loadShaderSourceFromFile(filePath);
+}
+
+void OsgShader::setGeometryShaderSource(const std::string& source)
+{
+ if (! m_geometryShader.valid())
+ {
+ m_geometryShader = new osg::Shader(osg::Shader::GEOMETRY);
+ m_program->addShader(m_geometryShader);
+ }
+ m_geometryShader->setShaderSource(source);
+}
+
+bool OsgShader::getGeometryShaderSource(std::string* source) const
+{
+ if (m_geometryShader)
+ {
+ *source = m_geometryShader->getShaderSource();
+ return true;
+ }
+ else
+ {
+ *source = "";
+ return false;
+ }
+}
+
+bool OsgShader::hasFragmentShader() const
+{
+ return m_fragmentShader.valid();
+}
+
+void OsgShader::clearFragmentShader()
+{
+ if (m_fragmentShader)
+ {
+ m_program->removeShader(m_fragmentShader);
+ m_fragmentShader = nullptr;
+ }
+}
+
+bool OsgShader::loadFragmentShaderSource(const std::string& filePath)
+{
+ if (! m_fragmentShader.valid())
+ {
+ m_fragmentShader = new osg::Shader(osg::Shader::FRAGMENT);
+ m_fragmentShader->setName(filePath);
+ m_program->addShader(m_fragmentShader);
+ }
+ return m_fragmentShader->loadShaderSourceFromFile(filePath);
+}
+
+void OsgShader::setFragmentShaderSource(const std::string& source)
+{
+ if (! m_fragmentShader.valid())
+ {
+ m_fragmentShader = new osg::Shader(osg::Shader::FRAGMENT);
+ m_program->addShader(m_fragmentShader);
+ }
+ m_fragmentShader->setShaderSource(source);
+}
+
+bool OsgShader::getFragmentShaderSource(std::string* source) const
+{
+ if (m_fragmentShader)
+ {
+ *source = m_fragmentShader->getShaderSource();
+ return true;
+ }
+ else
+ {
+ *source = "";
+ return false;
+ }
+}
+
+osg::ref_ptr<osg::Program> SurgSim::Graphics::OsgShader::getOsgProgram() const
+{
+ return m_program;
+}
+
+void SurgSim::Graphics::OsgShader::setGlobalScope(bool val)
+{
+ m_globalScope = val;
+ osg::StateAttribute::ParentList parents = m_program->getParents();
+ for (auto it = std::begin(parents); it != std::end(parents); ++it)
+ {
+ addToStateSet(*it);
+ }
+}
+
+bool SurgSim::Graphics::OsgShader::isGlobalScope() const
+{
+ return m_globalScope;
+}
+
+std::shared_ptr<SurgSim::Graphics::OsgShader> SurgSim::Graphics::loadShader(
+ const SurgSim::Framework::ApplicationData& data,
+ const std::string& name)
+{
+ std::string vertexShaderName = name + ".vert";
+ std::string fragmentShaderName = name + ".frag";
+
+ std::string filename;
+
+ auto shader(std::make_shared<SurgSim::Graphics::OsgShader>());
+ bool success = true;
+ filename = data.findFile(vertexShaderName);
+ if (filename == "")
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Could not find vertex shader " << vertexShaderName;
+ success = false;
+ }
+ else if (! shader->loadVertexShaderSource(filename))
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Could not load vertex shader " << vertexShaderName;
+ success = false;
+ }
+
+
+ filename = data.findFile(fragmentShaderName);
+ if (filename == "")
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Could not find fragment shader " << fragmentShaderName;
+ success = false;
+ }
+ if (! shader->loadFragmentShaderSource(filename))
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Could not load fragment shader " << fragmentShaderName;
+ success = false;
+ }
+
+ if (!success)
+ {
+ shader = nullptr;
+ }
+
+ return shader;
+}
diff --git a/SurgSim/Graphics/OsgShader.h b/SurgSim/Graphics/OsgShader.h
new file mode 100644
index 0000000..87415d4
--- /dev/null
+++ b/SurgSim/Graphics/OsgShader.h
@@ -0,0 +1,150 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGSHADER_H
+#define SURGSIM_GRAPHICS_OSGSHADER_H
+
+#include "SurgSim/Graphics/Shader.h"
+
+#include <osg/Program>
+#include <osg/StateSet>
+
+#include <string>
+#include <memory>
+
+namespace SurgSim
+{
+namespace Framework
+{
+class ApplicationData;
+}
+
+namespace Graphics
+{
+
+/// OSG-based implementation of a graphics shader.
+///
+/// Wraps an osg::Program which manages the geometry, vertex, and fragment shaders.
+/// The osg::Program is added to the osg::StateSet of an osg::Node to use the shaders for the rendering of that
+/// node's geometry.
+class OsgShader : public Shader
+{
+public:
+ /// Constructor
+ /// \post No shader code is set, so the fixed-function pipeline is used.
+ OsgShader();
+
+ /// Adds this shader to the OSG state set
+ /// \param stateSet OSG state set
+ virtual void addToStateSet(osg::StateSet* stateSet);
+
+ /// Removes this uniform from the OSG state set
+ /// \param stateSet OSG state set
+ virtual void removeFromStateSet(osg::StateSet* stateSet);
+
+ /// Returns true if the vertex shader has been set, otherwise false.
+ virtual bool hasVertexShader() const;
+
+ /// Removes the vertex shader, returning that portion of the shader program to fixed-function.
+ virtual void clearVertexShader();
+
+ /// Loads the vertex shader source code from a file
+ /// \param filePath Path to file containing shader source code
+ /// \return True if the source is successfully loaded, otherwise false.
+ virtual bool loadVertexShaderSource(const std::string& filePath);
+
+ /// Set the vertex shader source code
+ /// \param source Shader source code
+ virtual void setVertexShaderSource(const std::string& source);
+
+ /// Gets the vertex shader source code
+ /// \return Shader source code
+ virtual bool getVertexShaderSource(std::string* source) const;
+
+ /// Returns true if the geometry shader has been set, otherwise false.
+ virtual bool hasGeometryShader() const;
+
+ /// Removes the geometry shader, returning that portion of the shader program to fixed-function.
+ virtual void clearGeometryShader();
+
+ /// Loads the geometry shader source code from a file
+ /// \param filePath Path to file containing shader source code
+ /// \return True if the source is successfully loaded, otherwise false.
+ virtual bool loadGeometryShaderSource(const std::string& filePath);
+
+ /// Set the geometry shader source code
+ /// \param source Shader source code
+ virtual void setGeometryShaderSource(const std::string& source);
+
+ /// Gets the geometry shader source code
+ /// \return Shader source code
+ virtual bool getGeometryShaderSource(std::string* source) const;
+
+
+ /// Returns true if the fragment shader has been set, otherwise false.
+ virtual bool hasFragmentShader() const;
+
+ /// Removes the fragment shader, returning that portion of the shader program to fixed-function.
+ virtual void clearFragmentShader();
+
+ /// Loads the fragment shader source code from a file
+ /// \param filePath Path to file containing shader source code
+ /// \return True if the source is successfully loaded, otherwise false.
+ virtual bool loadFragmentShaderSource(const std::string& filePath);
+
+ /// Set the fragment shader source code
+ /// \param source Shader source code
+ virtual void setFragmentShaderSource(const std::string& source);
+
+ /// Gets the fragment shader source code
+ /// \return Shader source code
+ virtual bool getFragmentShaderSource(std::string* source) const;
+
+ /// Returns the OSG program attribute
+ osg::ref_ptr<osg::Program> getOsgProgram() const;
+
+ /// Enables the shader to override other material shaders
+ /// \param val if true the shader will replace other shaders in a lower hierarchy.
+ virtual void setGlobalScope(bool val) override;
+
+ /// Query if this object is global scope and overrides other lower level shaders.
+ /// \return true if global scope, false if not.
+ virtual bool isGlobalScope() const override;
+
+
+private:
+ /// OSG program attribute
+ osg::ref_ptr<osg::Program> m_program;
+
+ /// OSG vertex shader
+ osg::ref_ptr<osg::Shader> m_vertexShader;
+ /// OSG geometry shader
+ osg::ref_ptr<osg::Shader> m_geometryShader;
+ /// OSG fragment shader
+ osg::ref_ptr<osg::Shader> m_fragmentShader;
+
+ /// Is the shader supposed to be used globally
+ bool m_globalScope;
+};
+
+std::shared_ptr<SurgSim::Graphics::OsgShader> loadShader(const SurgSim::Framework::ApplicationData& data,
+ const std::string& name);
+
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGSHADER_H
diff --git a/SurgSim/Graphics/OsgSphereRepresentation.cpp b/SurgSim/Graphics/OsgSphereRepresentation.cpp
new file mode 100644
index 0000000..f6ff120
--- /dev/null
+++ b/SurgSim/Graphics/OsgSphereRepresentation.cpp
@@ -0,0 +1,61 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgRigidTransformConversions.h"
+#include "SurgSim/Graphics/OsgUnitSphere.h"
+
+#include <osg/Geode>
+#include <osg/Shape>
+#include <osg/ShapeDrawable>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgSphereRepresentation, OsgSphereRepresentation);
+
+OsgSphereRepresentation::OsgSphereRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ SphereRepresentation(name),
+ m_sharedUnitSphere(getSharedUnitSphere())
+{
+ m_transform->addChild(m_sharedUnitSphere->getNode());
+}
+
+void OsgSphereRepresentation::setRadius(double radius)
+{
+ m_transform->setScale(osg::Vec3d(radius, radius, radius));
+}
+
+double OsgSphereRepresentation::getRadius() const
+{
+ SURGSIM_ASSERT(m_transform->getScale().x() == m_transform->getScale().y() &&
+ m_transform->getScale().x() == m_transform->getScale().z()) <<
+ "Sphere should be scaled equally in all directions!";
+ return m_transform->getScale().x();
+}
+
+std::shared_ptr<OsgUnitSphere> OsgSphereRepresentation::getSharedUnitSphere()
+{
+ static SurgSim::Framework::SharedInstance<OsgUnitSphere> shared;
+ return shared.get();
+}
+
+}; // Graphics
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgSphereRepresentation.h b/SurgSim/Graphics/OsgSphereRepresentation.h
new file mode 100644
index 0000000..7215894
--- /dev/null
+++ b/SurgSim/Graphics/OsgSphereRepresentation.h
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGSPHEREREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGSPHEREREPRESENTATION_H
+
+#include "SurgSim/Graphics/SphereRepresentation.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+#include <osg/PositionAttitudeTransform>
+#include <osg/Switch>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+namespace Graphics
+{
+class OsgUnitSphere;
+
+SURGSIM_STATIC_REGISTRATION(OsgSphereRepresentation);
+
+/// OSG implementation of a graphics sphere representation.
+class OsgSphereRepresentation : public OsgRepresentation, public SphereRepresentation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit OsgSphereRepresentation(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgSphereRepresentation);
+
+ /// Sets the radius of the sphere
+ /// \param radius Radius of the sphere
+ virtual void setRadius(double radius);
+
+ /// Returns the radius of the sphere
+ /// \return Radius of the sphere
+ virtual double getRadius() const;
+
+private:
+ /// Shared unit sphere, so that the geometry can be instanced rather than having multiple copies.
+ std::shared_ptr<OsgUnitSphere> m_sharedUnitSphere;
+
+ /// Returns the shared unit sphere
+ static std::shared_ptr<OsgUnitSphere> getSharedUnitSphere();
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGSPHEREREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgTexture.cpp b/SurgSim/Graphics/OsgTexture.cpp
new file mode 100644
index 0000000..079cdb1
--- /dev/null
+++ b/SurgSim/Graphics/OsgTexture.cpp
@@ -0,0 +1,45 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgTexture.h"
+
+#include <osgDB/ReadFile>
+
+using SurgSim::Graphics::OsgTexture;
+
+OsgTexture::OsgTexture(osg::Texture* texture) :
+ m_texture(texture)
+{
+}
+
+bool OsgTexture::loadImage(const std::string& filePath)
+{
+ osg::ref_ptr<osg::Image> imageNode = osgDB::readImageFile(filePath);
+
+ if (imageNode.valid())
+ {
+ m_texture->setImage(0u, imageNode);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+void OsgTexture::clearImage()
+{
+ m_texture->setImage(0, nullptr);
+}
diff --git a/SurgSim/Graphics/OsgTexture.h b/SurgSim/Graphics/OsgTexture.h
new file mode 100644
index 0000000..0b8cdf4
--- /dev/null
+++ b/SurgSim/Graphics/OsgTexture.h
@@ -0,0 +1,63 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGTEXTURE_H
+#define SURGSIM_GRAPHICS_OSGTEXTURE_H
+
+#include "SurgSim/Graphics/Texture.h"
+
+#include <osg/Texture>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base class for OSG implementations of Graphics Textures.
+///
+/// Wraps an osg::Texture.
+class OsgTexture : public virtual Texture
+{
+public:
+ /// Loads an image into the texture from a file
+ /// \param filePath Path to the image file
+ /// \return True if the image is successfully loaded, otherwise false
+ virtual bool loadImage(const std::string& filePath);
+
+ /// Removes the image from the texture
+ virtual void clearImage();
+
+ /// Returns the osg::Texture1D
+ osg::ref_ptr<osg::Texture> getOsgTexture() const
+ {
+ return m_texture;
+ }
+
+protected:
+ /// Constructor
+ /// \param texture OSG texture
+ explicit OsgTexture(osg::Texture* texture);
+
+private:
+ /// OSG texture
+ osg::ref_ptr<osg::Texture> m_texture;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGTEXTURE_H
diff --git a/SurgSim/Graphics/OsgTexture1d.cpp b/SurgSim/Graphics/OsgTexture1d.cpp
new file mode 100644
index 0000000..e0263cd
--- /dev/null
+++ b/SurgSim/Graphics/OsgTexture1d.cpp
@@ -0,0 +1,36 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgTexture1d.h"
+
+using SurgSim::Graphics::OsgTexture1d;
+
+OsgTexture1d::OsgTexture1d() : OsgTexture(new osg::Texture1D())
+{
+}
+
+void OsgTexture1d::setSize(int width)
+{
+ getOsgTexture1d()->setTextureWidth(width);
+}
+
+void OsgTexture1d::getSize(int* width) const
+{
+ *width = getOsgTexture1d()->getTextureWidth();
+ if (*width == 0 && getOsgTexture()->getNumImages() > 0)
+ {
+ *width = getOsgTexture()->getImage(0)->s();
+ }
+}
diff --git a/SurgSim/Graphics/OsgTexture1d.h b/SurgSim/Graphics/OsgTexture1d.h
new file mode 100644
index 0000000..67266f3
--- /dev/null
+++ b/SurgSim/Graphics/OsgTexture1d.h
@@ -0,0 +1,69 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGTEXTURE1D_H
+#define SURGSIM_GRAPHICS_OSGTEXTURE1D_H
+
+#include "SurgSim/Graphics/OsgTexture.h"
+#include "SurgSim/Graphics/Texture1d.h"
+
+#include <osg/Texture1D>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG implementation of a 1D Texture
+///
+/// Wraps an osg::Texture1d
+class OsgTexture1d : public OsgTexture, public Texture1d
+{
+public:
+ /// Constructor
+ /// \post No image is loaded in the texture.
+ OsgTexture1d();
+
+ /// Sets the size of the texture
+ /// \param width Width of the texture
+ /// \note Use this to setup a texture as a render target rather than loading from file.
+ virtual void setSize(int width);
+
+ /// Gets the size of the texture
+ /// \return width Width of the texture
+ virtual void getSize(int* width) const;
+
+ /// Returns the osg::Texture1D
+ osg::ref_ptr<osg::Texture1D> getOsgTexture1d() const
+ {
+ return static_cast<osg::Texture1D*>(getOsgTexture().get());
+ }
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGTEXTURE1D_H
diff --git a/SurgSim/Graphics/OsgTexture2d.cpp b/SurgSim/Graphics/OsgTexture2d.cpp
new file mode 100644
index 0000000..b570296
--- /dev/null
+++ b/SurgSim/Graphics/OsgTexture2d.cpp
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgTexture2d.h"
+
+using SurgSim::Graphics::OsgTexture2d;
+
+OsgTexture2d::OsgTexture2d() : OsgTexture(new osg::Texture2D())
+{
+}
+
+void OsgTexture2d::setSize(int width, int height)
+{
+ getOsgTexture2d()->setTextureSize(width, height);
+}
+
+void OsgTexture2d::getSize(int* width, int* height) const
+{
+ *width = getOsgTexture2d()->getTextureWidth();
+ if (*width == 0 && getOsgTexture()->getNumImages() > 0)
+ {
+ *width = getOsgTexture()->getImage(0)->s();
+ }
+ *height = getOsgTexture2d()->getTextureHeight();
+ if (*height == 0 && getOsgTexture()->getNumImages() > 0)
+ {
+ *height = getOsgTexture()->getImage(0)->t();
+ }
+}
+
+template <>
+std::shared_ptr<SurgSim::Graphics::OsgTexture2d>
+SurgSim::Framework::convert(boost::any val)
+{
+ std::shared_ptr<SurgSim::Graphics::OsgTexture2d> result;
+ try
+ {
+ std::shared_ptr<SurgSim::Graphics::Texture> tex =
+ boost::any_cast<std::shared_ptr<SurgSim::Graphics::Texture>>(val);
+ result = std::dynamic_pointer_cast<SurgSim::Graphics::OsgTexture2d>(tex);
+ }
+ catch (boost::bad_any_cast&)
+ {
+ result = boost::any_cast<std::shared_ptr<SurgSim::Graphics::OsgTexture2d>>(val);
+ }
+
+ SURGSIM_ASSERT(result != nullptr)
+ << "Conversion from the incoming type to OsgTexture2d failed, this probably means that the input value "
+ << "was a texture but not an OsgTexture2d.";
+
+ return result;
+}
diff --git a/SurgSim/Graphics/OsgTexture2d.h b/SurgSim/Graphics/OsgTexture2d.h
new file mode 100644
index 0000000..9246b0a
--- /dev/null
+++ b/SurgSim/Graphics/OsgTexture2d.h
@@ -0,0 +1,84 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGTEXTURE2D_H
+#define SURGSIM_GRAPHICS_OSGTEXTURE2D_H
+
+#include "SurgSim/Graphics/OsgTexture.h"
+#include "SurgSim/Graphics/Texture2d.h"
+
+#include <osg/Texture2D>
+
+#include "SurgSim/Framework/Accessible.h"
+#include <boost/any.hpp>
+#include <memory>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG implementation of a 2D Texture
+///
+/// Wraps an osg::Texture2d
+class OsgTexture2d : public OsgTexture, public Texture2d
+{
+public:
+ /// Constructor
+ /// \post No image is loaded in the texture.
+ OsgTexture2d();
+
+ /// Sets the size of the texture
+ /// \param width Width of the texture
+ /// \param height Height of the texture
+ /// \note Use this to setup a texture as a render target rather than loading from file.
+ virtual void setSize(int width, int height);
+
+ /// Gets the size of the texture
+ /// \param[out] width Width of the texture
+ /// \param[out] height Height of the texture
+ virtual void getSize(int* width, int* height) const;
+
+ /// Returns the osg::Texture2D
+ osg::ref_ptr<osg::Texture2D> getOsgTexture2d() const
+ {
+ return static_cast<osg::Texture2D*>(getOsgTexture().get());
+ }
+};
+
+}; // namespace Graphics
+
+
+namespace Framework
+{
+/// Template specialization for the convert<> function used in accessible, this one can convert an incoming
+/// SurgSim::Graphics::Texture to an outgoing OsgTexture2d
+template <>
+std::shared_ptr<SurgSim::Graphics::OsgTexture2d> convert(boost::any val);
+}
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGTEXTURE2D_H
diff --git a/SurgSim/Graphics/OsgTexture3d.cpp b/SurgSim/Graphics/OsgTexture3d.cpp
new file mode 100644
index 0000000..989f3d7
--- /dev/null
+++ b/SurgSim/Graphics/OsgTexture3d.cpp
@@ -0,0 +1,110 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgTexture3d.h"
+
+#include "SurgSim/Framework/Assert.h"
+
+#include <osgDB/ReadFile>
+
+using SurgSim::Graphics::OsgTexture3d;
+
+OsgTexture3d::OsgTexture3d() : OsgTexture(new osg::Texture3D())
+{
+}
+
+void OsgTexture3d::setSize(int width, int height, int depth)
+{
+ getOsgTexture3d()->setTextureSize(width, height, depth);
+}
+
+void OsgTexture3d::getSize(int* width, int* height, int* depth) const
+{
+ *width = getOsgTexture3d()->getTextureWidth();
+ if (*width == 0 && getOsgTexture()->getNumImages() > 0)
+ {
+ *width = getOsgTexture()->getImage(0)->s();
+ }
+ *height = getOsgTexture3d()->getTextureHeight();
+ if (*height == 0 && getOsgTexture()->getNumImages() > 0)
+ {
+ *height = getOsgTexture()->getImage(0)->t();
+ }
+ *depth = getOsgTexture3d()->getTextureDepth();
+ if (*depth == 0 && getOsgTexture()->getNumImages() > 0)
+ {
+ *depth = getOsgTexture()->getImage(0)->r();
+ }
+}
+
+bool OsgTexture3d::loadImageSlices(const std::vector<std::string>& filePaths)
+{
+ SURGSIM_ASSERT(! filePaths.empty()) << "No image file paths to load!";
+
+ std::vector<osg::ref_ptr<osg::Image>> slices;
+ slices.reserve(filePaths.size());
+
+ int width;
+ int height;
+ GLenum pixelFormat;
+ GLenum dataType;
+
+ /// Load first slice, all others should have same properties
+ {
+ osg::ref_ptr<osg::Image> firstSlice = osgDB::readImageFile(filePaths.front());
+ if (! firstSlice.valid())
+ {
+ return false;
+ }
+ slices.push_back(firstSlice);
+ width = firstSlice->s();
+ height = firstSlice->t();
+ pixelFormat = firstSlice->getPixelFormat();
+ dataType = firstSlice->getDataType();
+ }
+
+ /// Load the remaining slices
+ for (auto it = filePaths.begin() + 1; it != filePaths.end(); ++it)
+ {
+ osg::ref_ptr<osg::Image> slice = osgDB::readImageFile(*it);
+ if (! slice.valid())
+ {
+ return false;
+ }
+ SURGSIM_ASSERT(slice->s() == width) << "Slice has different width! File: " << *it <<
+ " Width: " << slice->s() << " Expected Width: " << width;
+ SURGSIM_ASSERT(slice->t() == height) << "Slice has different height! File: " << *it <<
+ " Height: " << slice->s() << " Expected Height: " << height;
+ SURGSIM_ASSERT(slice->getPixelFormat() == pixelFormat) << "Slice has different pixel format! File: " <<
+ *it << " Pixel Format: " << slice->getPixelFormat() << " Expected Pixel Format: " << pixelFormat;
+ SURGSIM_ASSERT(slice->getDataType() == dataType) << "Slice has different data type! File: " <<
+ *it << " Data type: " << slice->getDataType() << " Expected Data Type: " << dataType;
+ slices.push_back(slice);
+ }
+
+ /// Copy the slices into the 3D image, starting at depth 0
+ osg::ref_ptr<osg::Image> image = new osg::Image();
+ image->allocateImage(width, height, slices.size(), pixelFormat, dataType);
+
+ int depth = 0;
+ for (auto it = slices.begin(); it != slices.end(); ++it)
+ {
+ image->copySubImage(0, 0, depth, it->get());
+ }
+
+ getOsgTexture()->setImage(0u, image);
+
+ return true;
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgTexture3d.h b/SurgSim/Graphics/OsgTexture3d.h
new file mode 100644
index 0000000..eecc62b
--- /dev/null
+++ b/SurgSim/Graphics/OsgTexture3d.h
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGTEXTURE3D_H
+#define SURGSIM_GRAPHICS_OSGTEXTURE3D_H
+
+#include "SurgSim/Graphics/OsgTexture.h"
+#include "SurgSim/Graphics/Texture3d.h"
+
+#include <osg/Texture3D>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG implementation of a 3D Texture
+///
+/// Wraps an osg::Texture3d
+class OsgTexture3d : public OsgTexture, public Texture3d
+{
+public:
+ /// Constructor
+ /// \post No image is loaded in the texture.
+ OsgTexture3d();
+
+ /// Sets the size of the texture
+ /// \param width Width of the texture
+ /// \param height Height of the texture
+ /// \param depth Depth of the texture
+ /// \note Use this to setup a texture as a render target rather than loading from file.
+ virtual void setSize(int width, int height, int depth);
+
+ /// Gets the size of the texture
+ /// \param[out] width Width of the texture
+ /// \param[out] height Height of the texture
+ /// \param[out] depth Depth of the texture
+ virtual void getSize(int* width, int* height, int* depth) const;
+
+ /// Loads images slices from files into the 3D texture
+ /// \param filePaths Paths to the image files
+ /// \return True if the image is successfully loaded, otherwise false
+ /// \note The slices are stacked in the order provided to create the depth of the 3D texture.
+ virtual bool loadImageSlices(const std::vector<std::string>& filePaths);
+
+ /// Returns the osg::Texture3D
+ osg::ref_ptr<osg::Texture3D> getOsgTexture3d() const
+ {
+ return static_cast<osg::Texture3D*>(getOsgTexture().get());
+ }
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGTEXTURE3D_H
diff --git a/SurgSim/Graphics/OsgTextureCubeMap.cpp b/SurgSim/Graphics/OsgTextureCubeMap.cpp
new file mode 100644
index 0000000..102caeb
--- /dev/null
+++ b/SurgSim/Graphics/OsgTextureCubeMap.cpp
@@ -0,0 +1,143 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgTextureCubeMap.h"
+
+#include <osgDB/ReadFile>
+
+using SurgSim::Graphics::OsgTextureCubeMap;
+
+OsgTextureCubeMap::OsgTextureCubeMap() : OsgTexture(new osg::TextureCubeMap())
+{
+}
+
+void OsgTextureCubeMap::setSize(int width, int height)
+{
+ getOsgTextureCubeMap()->setTextureSize(width, height);
+}
+
+void OsgTextureCubeMap::getSize(int* width, int* height) const
+{
+ *width = getOsgTextureCubeMap()->getTextureWidth();
+ if (*width == 0 && getOsgTexture()->getNumImages() > 0)
+ {
+ *width = getOsgTexture()->getImage(0)->s();
+ }
+ *height = getOsgTextureCubeMap()->getTextureHeight();
+ if (*height == 0 && getOsgTexture()->getNumImages() > 0)
+ {
+ *height = getOsgTexture()->getImage(0)->t();
+ }
+}
+
+bool OsgTextureCubeMap::loadImage(const std::string& filePath)
+{
+ osg::ref_ptr<osg::Image> imageNode = osgDB::readImageFile(filePath);
+ if (! imageNode.valid())
+ {
+ return false;
+ }
+
+ // Split up image to the six sides of the cube
+ int width = imageNode->s() / 3;
+ int height = imageNode->t() / 4;
+
+ osg::ref_ptr<osg::Image> negativeZ = copyImageBlock(*imageNode, width, 0, width, height);
+ negativeZ->flipHorizontal();
+
+ osg::ref_ptr<osg::Image> negativeY = copyImageBlock(*imageNode, width, height, width, height);
+ negativeY->flipVertical();
+
+ osg::ref_ptr<osg::Image> negativeX = copyImageBlock(*imageNode, 0, height*2, width, height);
+ negativeX->flipVertical();
+
+ osg::ref_ptr<osg::Image> positiveZ = copyImageBlock(*imageNode, width, height*2, width, height);
+ positiveZ->flipVertical();
+
+ osg::ref_ptr<osg::Image> positiveX = copyImageBlock(*imageNode, width*2, height*2, width, height);
+ positiveX->flipVertical();
+
+ osg::ref_ptr<osg::Image> positiveY = copyImageBlock(*imageNode, width, height*3, width, height);
+ positiveY->flipVertical();
+
+ getOsgTexture()->setImage(osg::TextureCubeMap::POSITIVE_X, positiveX);
+ getOsgTexture()->setImage(osg::TextureCubeMap::NEGATIVE_X, negativeX);
+ getOsgTexture()->setImage(osg::TextureCubeMap::POSITIVE_Y, positiveY);
+ getOsgTexture()->setImage(osg::TextureCubeMap::NEGATIVE_Y, negativeY);
+ getOsgTexture()->setImage(osg::TextureCubeMap::POSITIVE_Z, positiveZ);
+ getOsgTexture()->setImage(osg::TextureCubeMap::NEGATIVE_Z, negativeZ);
+
+ return true;
+}
+
+bool OsgTextureCubeMap::loadImageFaces(const std::string& negativeX, const std::string& positiveX,
+ const std::string& negativeY, const std::string& positiveY,
+ const std::string& negativeZ, const std::string& positiveZ)
+{
+ osg::ref_ptr<osg::Image> negativeXImage = osgDB::readImageFile(negativeX);
+ osg::ref_ptr<osg::Image> positiveXImage = osgDB::readImageFile(positiveX);
+ osg::ref_ptr<osg::Image> negativeYImage = osgDB::readImageFile(negativeY);
+ osg::ref_ptr<osg::Image> positiveYImage = osgDB::readImageFile(positiveY);
+ osg::ref_ptr<osg::Image> negativeZImage = osgDB::readImageFile(negativeZ);
+ osg::ref_ptr<osg::Image> positiveZImage = osgDB::readImageFile(positiveZ);
+
+ if (negativeXImage.valid() && positiveXImage.valid() && negativeYImage.valid() && positiveYImage.valid() &&
+ negativeZImage.valid() && positiveZImage.valid())
+ {
+ getOsgTexture()->setImage(osg::TextureCubeMap::NEGATIVE_X, negativeXImage);
+ getOsgTexture()->setImage(osg::TextureCubeMap::POSITIVE_X, positiveXImage);
+ getOsgTexture()->setImage(osg::TextureCubeMap::NEGATIVE_Y, negativeYImage);
+ getOsgTexture()->setImage(osg::TextureCubeMap::POSITIVE_Y, positiveYImage);
+ getOsgTexture()->setImage(osg::TextureCubeMap::NEGATIVE_Z, negativeZImage);
+ getOsgTexture()->setImage(osg::TextureCubeMap::POSITIVE_Z, positiveZImage);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+osg::ref_ptr<osg::Image> OsgTextureCubeMap::copyImageBlock(const osg::Image& source, int column, int row,
+ int width, int height)
+{
+ int pixelSize = source.getPixelSizeInBits() / 8;
+
+ unsigned char* buffer = new unsigned char[(width*height) * pixelSize];
+
+ int index = 0;
+
+ for(int i = row; i < row + height; ++i)
+ {
+ for(int j = column; j < column + width; ++j)
+ {
+ const unsigned char* pixel = source.data(column, row, 0);
+
+ for (int p = 0; p < pixelSize; ++p)
+ {
+ buffer[index] = pixel[p];
+ index++;
+ }
+ }
+ }
+
+ osg::ref_ptr<osg::Image> subImage = new osg::Image();
+
+ subImage->allocateImage(width, height, 1, source.getPixelFormat(), GL_UNSIGNED_BYTE, 1);
+ subImage->setImage(width, height, 1, source.getInternalTextureFormat(), source.getPixelFormat(),
+ GL_UNSIGNED_BYTE, buffer, osg::Image::NO_DELETE, 1);
+
+ return subImage;
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgTextureCubeMap.h b/SurgSim/Graphics/OsgTextureCubeMap.h
new file mode 100644
index 0000000..8847a95
--- /dev/null
+++ b/SurgSim/Graphics/OsgTextureCubeMap.h
@@ -0,0 +1,107 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGTEXTURECUBEMAP_H
+#define SURGSIM_GRAPHICS_OSGTEXTURECUBEMAP_H
+
+#include "SurgSim/Graphics/OsgTexture.h"
+
+#include <osg/TextureCubeMap>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG implementation of a Cube Map Texture
+///
+/// Wraps an osg::TextureCubeMap
+class OsgTextureCubeMap : public OsgTexture
+{
+public:
+ /// Constructor
+ /// \post No image is loaded in the texture.
+ OsgTextureCubeMap();
+
+ /// Sets the size of the texture
+ /// \param width Width of the texture
+ /// \param height Height of the texture
+ /// \note Use this to setup a texture as a render target rather than loading from file.
+ virtual void setSize(int width, int height);
+
+ /// Gets the size of the texture
+ /// \param[out] width Width of the texture
+ /// \param[out] height Height of the texture
+ virtual void getSize(int* width, int* height) const;
+
+ /// Loads an image into the texture from a file
+ /// \param filePath Path to the image file
+ /// \return True if the image is successfully loaded, otherwise false
+ /// \note
+ /// The image should contain the cube map unwrapped such that each face is width/3 x height/4 and the corners
+ /// for the faces are arranged as follows, with (0, 0) as the bottom-left corner and (width, height) as the
+ /// top-right corner:
+ /// (-Z): (width * 1/3, 0 ) to (width * 2/3, height * 1/4)
+ /// (-Y): (width * 1/3, height * 1/4) to (width * 2/3, height * 1/2)
+ /// (-X): (0, height * 1/2) to (width * 1/3, height * 3/4)
+ /// (+Z): (width * 1/3, height * 1/2) to (width * 2/3, height * 3/4)
+ /// (+X): (width * 2/3, height * 1/2) to (width, height * 3/4)
+ /// (+Y): (width * 1/3, height * 3/4) to (width * 2/3, height )
+ virtual bool loadImage(const std::string& filePath);
+
+ /// Loads images from files into the faces of the cube map
+ /// \param negativeX Path to the image for the (-X) face
+ /// \param positiveX Path to the image for the (+X) face
+ /// \param negativeY Path to the image for the (-Y) face
+ /// \param positiveY Path to the image for the (+Y) face
+ /// \param negativeZ Path to the image for the (-Z) face
+ /// \param positiveZ Path to the image for the (+Z) face
+ /// \return True if the image is successfully loaded, otherwise false
+ virtual bool loadImageFaces(const std::string& negativeX, const std::string& positiveX,
+ const std::string& negativeY, const std::string& positiveY,
+ const std::string& negativeZ, const std::string& positiveZ);
+
+ /// Returns the osg::TextureCubeMap
+ osg::ref_ptr<osg::TextureCubeMap> getOsgTextureCubeMap() const
+ {
+ return static_cast<osg::TextureCubeMap*>(getOsgTexture().get());
+ }
+
+protected:
+ /// Makes a copy of an image block
+ /// \param source Source image to copy from
+ /// \param column First column of block in the source image
+ /// \param row First row of block in the source image
+ /// \param width Width of the block
+ /// \param height Height of the block
+ /// \return Copy of the image block
+ osg::ref_ptr<osg::Image> copyImageBlock(const osg::Image& source, int column, int row, int width, int height);
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGTEXTURECUBEMAP_H
diff --git a/SurgSim/Graphics/OsgTextureRectangle.cpp b/SurgSim/Graphics/OsgTextureRectangle.cpp
new file mode 100644
index 0000000..7d1af56
--- /dev/null
+++ b/SurgSim/Graphics/OsgTextureRectangle.cpp
@@ -0,0 +1,41 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgTextureRectangle.h"
+
+using SurgSim::Graphics::OsgTextureRectangle;
+
+OsgTextureRectangle::OsgTextureRectangle() : OsgTexture(new osg::TextureRectangle())
+{
+}
+
+void OsgTextureRectangle::setSize(int width, int height)
+{
+ getOsgTextureRectangle()->setTextureSize(width, height);
+}
+
+void OsgTextureRectangle::getSize(int* width, int* height) const
+{
+ *width = getOsgTextureRectangle()->getTextureWidth();
+ if (*width == 0 && getOsgTexture()->getNumImages() > 0)
+ {
+ *width = getOsgTexture()->getImage(0)->s();
+ }
+ *height = getOsgTextureRectangle()->getTextureHeight();
+ if (*height == 0 && getOsgTexture()->getNumImages() > 0)
+ {
+ *height = getOsgTexture()->getImage(0)->t();
+ }
+}
diff --git a/SurgSim/Graphics/OsgTextureRectangle.h b/SurgSim/Graphics/OsgTextureRectangle.h
new file mode 100644
index 0000000..00389a4
--- /dev/null
+++ b/SurgSim/Graphics/OsgTextureRectangle.h
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGTEXTURERECTANGLE_H
+#define SURGSIM_GRAPHICS_OSGTEXTURERECTANGLE_H
+
+#include "SurgSim/Graphics/OsgTexture.h"
+#include "SurgSim/Graphics/TextureRectangle.h"
+
+#include <osg/TextureRectangle>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG implementation of a Rectangle Texture
+///
+/// Wraps an osg::TextureRectangle
+///
+/// \note Texel coordinates are used to access this texture.
+class OsgTextureRectangle : public OsgTexture, public TextureRectangle
+{
+public:
+ /// Constructor
+ /// \post No image is loaded in the texture.
+ OsgTextureRectangle();
+
+ /// Sets the size of the texture
+ /// \param width Width of the texture
+ /// \param height Height of the texture
+ /// \note Use this to setup a texture as a render target rather than loading from file.
+ virtual void setSize(int width, int height);
+
+ /// Gets the size of the texture
+ /// \param[out] width Width of the texture
+ /// \param[out] height Height of the texture
+ virtual void getSize(int* width, int* height) const;
+
+ /// Returns the osg::TextureRectangle
+ osg::ref_ptr<osg::TextureRectangle> getOsgTextureRectangle() const
+ {
+ return static_cast<osg::TextureRectangle*>(getOsgTexture().get());
+ }
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGTEXTURERECTANGLE_H
diff --git a/SurgSim/Graphics/OsgTextureUniform-inl.h b/SurgSim/Graphics/OsgTextureUniform-inl.h
new file mode 100644
index 0000000..6db3571
--- /dev/null
+++ b/SurgSim/Graphics/OsgTextureUniform-inl.h
@@ -0,0 +1,121 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGTEXTUREUNIFORM_INL_H
+#define SURGSIM_GRAPHICS_OSGTEXTUREUNIFORM_INL_H
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Graphics/OsgTexture.h"
+#include "SurgSim/Graphics/OsgTexture1d.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/OsgTexture3d.h"
+#include "SurgSim/Graphics/OsgTextureCubeMap.h"
+#include "SurgSim/Graphics/OsgTextureRectangle.h"
+#include "SurgSim/Graphics/OsgUniformTypes.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+template <class T>
+OsgTextureUniform<T>::OsgTextureUniform(const std::string& name) :
+ UniformBase(), Uniform<std::shared_ptr<T>>(), OsgUniformBase(name), m_unit(-1), m_minimumTextureUnit(0)
+{
+ SURGSIM_ADD_RW_PROPERTY(OsgTextureUniform<T>, size_t,
+ MinimumTextureUnit, getMinimumTextureUnit, setMinimumTextureUnit);
+
+ osg::Uniform::Type osgUniformType = getOsgUniformType<std::shared_ptr<T>>();
+ SURGSIM_ASSERT(osgUniformType != osg::Uniform::UNDEFINED) << "Failed to get OSG uniform type!";
+ SURGSIM_ASSERT(m_uniform->setType(osgUniformType)) << "Failed to set OSG uniform type!";
+ m_uniform->setNumElements(1);
+}
+
+template <class T>
+void OsgTextureUniform<T>::set(const std::shared_ptr<T>& value)
+{
+ m_texture = value;
+ if (m_stateset != nullptr)
+ {
+ m_stateset->setTextureAttributeAndModes(m_unit, m_texture->getOsgTexture(), osg::StateAttribute::ON);
+ }
+}
+
+template <class T>
+const std::shared_ptr<T>& OsgTextureUniform<T>::get() const
+{
+ return m_texture;
+}
+
+template <class T>
+void OsgTextureUniform<T>::addToStateSet(osg::StateSet* stateSet)
+{
+ SURGSIM_ASSERT(m_stateset == nullptr) << "Unexpected addToStateSet for OsgTextureUniform.";
+
+ const osg::StateSet::TextureAttributeList& textures = stateSet->getTextureAttributeList();
+
+ // Grab the smallest unit that is equal or higher than m_minimumTextureUnit
+ // and search through allocated units for free ones
+ int availableUnit = m_minimumTextureUnit;
+ if (textures.size() > m_minimumTextureUnit)
+ {
+ for (auto it = textures.begin() + m_minimumTextureUnit; it != textures.end(); ++it)
+ {
+ if (it->empty())
+ {
+ break;
+ }
+ availableUnit++;
+ }
+ }
+
+ m_unit = availableUnit;
+
+ SURGSIM_ASSERT(m_texture != nullptr) << "Tried to add this uniform without a valid Texture";
+ stateSet->setTextureAttributeAndModes(m_unit, m_texture->getOsgTexture(),
+ osg::StateAttribute::ON);
+ SURGSIM_ASSERT(m_uniform->set(static_cast<int>(m_unit))) << "Failed to set OSG texture uniform unit!" <<
+ " Uniform: " << getName() << " unit: " << m_unit;
+ stateSet->addUniform(m_uniform);
+ m_stateset = stateSet;
+}
+
+template <class T>
+void OsgTextureUniform<T>::removeFromStateSet(osg::StateSet* stateSet)
+{
+ SURGSIM_ASSERT(m_stateset != stateSet) << "Unexpected Remove for OsgTextureUniform";
+ stateSet->removeTextureAttribute(m_unit, m_texture->getOsgTexture());
+ stateSet->removeUniform(m_uniform);
+ m_stateset = nullptr;
+}
+
+template <class T>
+void OsgTextureUniform<T>::setMinimumTextureUnit(size_t unit)
+{
+ SURGSIM_ASSERT(m_unit == -1) << "Can't set minimumTextureUnit after the unit has been assigned.";
+ m_minimumTextureUnit = unit;
+}
+
+template <class T>
+size_t OsgTextureUniform<T>::getMinimumTextureUnit() const
+{
+ return m_minimumTextureUnit;
+}
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGTEXTUREUNIFORM_INL_H
diff --git a/SurgSim/Graphics/OsgTextureUniform.h b/SurgSim/Graphics/OsgTextureUniform.h
new file mode 100644
index 0000000..df632e3
--- /dev/null
+++ b/SurgSim/Graphics/OsgTextureUniform.h
@@ -0,0 +1,137 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGTEXTUREUNIFORM_H
+#define SURGSIM_GRAPHICS_OSGTEXTUREUNIFORM_H
+
+/// \note HS-2013-jul-07 This file is included by OsgUniform.h, it is not meant to be used on its own
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG implementation of graphics uniform with a texture value
+/// \tparam T Texture type
+template <class T>
+class OsgTextureUniform : public Uniform<std::shared_ptr<T>>, public OsgUniformBase
+{
+public:
+ /// Constructor
+ /// \param name Name used in shader code to access this uniform
+ explicit OsgTextureUniform(const std::string& name);
+
+ /// This is the texture unit from where the search for a free texture unit will start
+ /// \param unit lowest texture unit to use, default is 0
+ void setMinimumTextureUnit(size_t unit);
+
+ /// \return the value of the lowest texture unit to use for this uniform
+ size_t getMinimumTextureUnit() const;
+
+ /// Sets the value of the uniform
+ /// \note using this directly might make the state of the uniform incosistent with
+ /// the texture that was used to create this uniform
+ virtual void set(const std::shared_ptr<T>& value);
+
+ /// Returns the value of the uniform
+ virtual const std::shared_ptr<T>& get() const;
+
+ /// Adds this uniform to the OSG state set
+ /// \param stateSet OSG state set
+ virtual void addToStateSet(osg::StateSet* stateSet);
+
+ /// Removes this uniform from the OSG state set
+ /// \param stateSet OSG state set
+ virtual void removeFromStateSet(osg::StateSet* stateSet);
+
+private:
+ /// Texture
+ std::shared_ptr<T> m_texture;
+
+ osg::ref_ptr<osg::StateSet> m_stateset;
+
+ /// Texture unit
+ ptrdiff_t m_unit;
+
+ /// The smallest unit to be used
+ size_t m_minimumTextureUnit;
+};
+
+/// Specialization of OsgUniform for OsgTexture1d
+template <>
+class OsgUniform<std::shared_ptr<OsgTexture1d>> : public OsgTextureUniform<OsgTexture1d>
+{
+public:
+ /// Constructor
+ /// \param name Name used in shader code to access this uniform
+ explicit OsgUniform(const std::string& name) : OsgTextureUniform<OsgTexture1d>(name)
+ {
+ }
+};
+
+/// Specialization of OsgUniform for OsgTexture2d
+template <>
+class OsgUniform<std::shared_ptr<OsgTexture2d>> : public OsgTextureUniform<OsgTexture2d>
+{
+public:
+ /// Constructor
+ /// \param name Name used in shader code to access this uniform
+ explicit OsgUniform(const std::string& name) : OsgTextureUniform<OsgTexture2d>(name)
+ {
+ }
+};
+
+/// Specialization of OsgUniform for OsgTexture3d
+template <>
+class OsgUniform<std::shared_ptr<OsgTexture3d>> : public OsgTextureUniform<OsgTexture3d>
+{
+public:
+ /// Constructor
+ /// \param name Name used in shader code to access this uniform
+ explicit OsgUniform(const std::string& name) : OsgTextureUniform<OsgTexture3d>(name)
+ {
+ }
+};
+
+/// Specialization of OsgUniform for OsgTextureCubeMap
+template <>
+class OsgUniform<std::shared_ptr<OsgTextureCubeMap>> : public OsgTextureUniform<OsgTextureCubeMap>
+{
+public:
+ /// Constructor
+ /// \param name Name used in shader code to access this uniform
+ explicit OsgUniform(const std::string& name) : OsgTextureUniform<OsgTextureCubeMap>(name)
+ {
+ }
+};
+
+/// Specialization of OsgUniform for OsgTextureRectangle
+template <>
+class OsgUniform<std::shared_ptr<OsgTextureRectangle>> : public OsgTextureUniform<OsgTextureRectangle>
+{
+public:
+ /// Constructor
+ /// \param name Name used in shader code to access this uniform
+ explicit OsgUniform(const std::string& name) : OsgTextureUniform<OsgTextureRectangle>(name)
+ {
+ }
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGTEXTUREUNIFORM_H
diff --git a/SurgSim/Graphics/OsgTrackballZoomManipulator.cpp b/SurgSim/Graphics/OsgTrackballZoomManipulator.cpp
new file mode 100644
index 0000000..8c76b21
--- /dev/null
+++ b/SurgSim/Graphics/OsgTrackballZoomManipulator.cpp
@@ -0,0 +1,214 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgTrackballZoomManipulator.h"
+#include <osgUtil/UpdateVisitor>
+
+/// Calculate the key code value of Ctrl-{character}, given the uppercase character.
+/// If a key is pressed while holding Ctrl, OSG "helpfully" gives you the key code of the control character
+/// (i.e. ^A == 1) instead of the key itself ('A' == 65).
+/// To cope with this, you can use CONTROL_CHAR_FROM_UPPERCASE('A') which is easier to read than
+/// strange character ('\001') or integral (1) constants.
+#define CONTROL_CHAR_FROM_UPPERCASE(uppercaseCharacter) ((uppercaseCharacter) - ('A' - 1))
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+OsgTrackballZoomManipulator::OsgTrackballZoomManipulator() :
+ osgGA::TrackballManipulator(),
+ m_minZoomFactor(0.05),
+ m_maxZoomFactor(1.0),
+ m_minZoomAmount(0.01),
+ m_maxZoomAmount(1.0),
+ m_zoomFactor(1.0),
+ m_zoomFactorScale(1.0)
+{
+}
+
+void OsgTrackballZoomManipulator::setMinZoomFactor(double factor)
+{
+ m_minZoomFactor = factor;
+}
+double OsgTrackballZoomManipulator::getMinZoomFactor() const
+{
+ return m_minZoomFactor;
+}
+
+void OsgTrackballZoomManipulator::setMaxZoomFactor(double factor)
+{
+ m_maxZoomFactor = factor;
+}
+double OsgTrackballZoomManipulator::getMaxZoomFactor() const
+{
+ return m_maxZoomFactor;
+}
+
+void OsgTrackballZoomManipulator::setMinZoomAmount(double amount)
+{
+ m_minZoomAmount = amount;
+}
+double OsgTrackballZoomManipulator::getMinZoomAmount() const
+{
+ return m_minZoomAmount;
+}
+
+void OsgTrackballZoomManipulator::setMaxZoomAmount(double amount)
+{
+ m_maxZoomAmount = amount;
+}
+double OsgTrackballZoomManipulator::getMaxZoomAmount() const
+{
+ return m_maxZoomAmount;
+}
+
+void OsgTrackballZoomManipulator::setZoomFactor(double factor)
+{
+ m_zoomFactor = factor;
+}
+double OsgTrackballZoomManipulator::getZoomFactor() const
+{
+ return m_zoomFactor;
+}
+
+void OsgTrackballZoomManipulator::setZoomFactorScale(double factor)
+{
+ m_zoomFactorScale = factor;
+}
+double OsgTrackballZoomManipulator::getZoomFactorScale() const
+{
+ return m_zoomFactorScale;
+}
+
+void OsgTrackballZoomManipulator::zoom(double zoomPercent)
+{
+ double difference = m_zoomFactor - m_minZoomFactor;
+
+ if (difference < m_minZoomAmount)
+ {
+ difference = m_minZoomAmount;
+ }
+ else if (difference > m_maxZoomAmount)
+ {
+ difference = m_maxZoomAmount;
+ }
+
+ m_zoomFactor += difference * zoomPercent;
+
+ if (m_zoomFactor < m_minZoomFactor)
+ {
+ m_zoomFactor = m_minZoomFactor;
+ }
+ else if (m_zoomFactor > m_maxZoomFactor)
+ {
+ m_zoomFactor = m_maxZoomFactor;
+ }
+}
+
+void OsgTrackballZoomManipulator::makeUpright()
+{
+ osg::Matrixd rotationMatrix(getRotation());
+
+ // Remove any roll from the camera pose.
+ osg::Vec3d backwards(rotationMatrix(2, 0), rotationMatrix(2, 1), rotationMatrix(2, 2));
+
+ osg::Vec3d right = osg::Vec3d(0.0, 1.0, 0.0) ^ backwards;
+ if (right.length2() < 1e-12)
+ {
+ right.set(0.0, 0.0, 1.0); // arbitrary...
+ }
+ right.normalize();
+
+ osg::Vec3d up = backwards ^ right; // shouldn't need to be normalized
+
+ for (int c = 0; c < 3; ++c)
+ {
+ rotationMatrix(0, c) = right[c];
+ }
+ for (int c = 0; c < 3; ++c)
+ {
+ rotationMatrix(1, c) = up[c];
+ }
+
+ setRotation(rotationMatrix.getRotate());
+}
+
+bool OsgTrackballZoomManipulator::handle(
+ const osgGA::GUIEventAdapter& eventAdapter,
+ osgGA::GUIActionAdapter& actionAdapter)
+{
+ unsigned int mask = eventAdapter.getModKeyMask();
+
+ switch (eventAdapter.getEventType())
+ {
+ case (osgGA::GUIEventAdapter::KEYDOWN):
+ {
+ int key = eventAdapter.getKey();
+ switch (key)
+ {
+ case 'u':
+ case 'U':
+ case CONTROL_CHAR_FROM_UPPERCASE('U'):
+ {
+ // ctrl-U makes the camera upright, so that the up axis points up
+ if ((mask & osgGA::GUIEventAdapter::MODKEY_CTRL) &&
+ !(mask & osgGA::GUIEventAdapter::MODKEY_ALT) &&
+ !(mask & osgGA::GUIEventAdapter::MODKEY_SHIFT))
+ {
+ makeUpright();
+ }
+ }
+ break;
+ }
+ }
+ default:
+ break;
+ }
+
+ return osgGA::TrackballManipulator::handle(eventAdapter, actionAdapter);
+}
+
+bool OsgTrackballZoomManipulator::handleMouseWheel(
+ const osgGA::GUIEventAdapter& eventAdapter,
+ osgGA::GUIActionAdapter& actionAdapter)
+{
+ osgGA::GUIEventAdapter::ScrollingMotion scrollingMotion = eventAdapter.getScrollingMotion();
+
+ switch (scrollingMotion)
+ {
+ // Scroll up to zoom in
+ case osgGA::GUIEventAdapter::SCROLL_UP:
+ {
+ zoom(-_wheelZoomFactor);
+ actionAdapter.requestRedraw();
+ actionAdapter.requestContinuousUpdate(false);
+ return true;
+ }
+ // Scroll down to zoom out
+ case osgGA::GUIEventAdapter::SCROLL_DOWN:
+ {
+ zoom(_wheelZoomFactor);
+ actionAdapter.requestRedraw();
+ actionAdapter.requestContinuousUpdate(false);
+ return true;
+ }
+ default:
+ return false;
+ }
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/OsgTrackballZoomManipulator.h b/SurgSim/Graphics/OsgTrackballZoomManipulator.h
new file mode 100644
index 0000000..c7bb993
--- /dev/null
+++ b/SurgSim/Graphics/OsgTrackballZoomManipulator.h
@@ -0,0 +1,131 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGTRACKBALLZOOMMANIPULATOR_H
+#define SURGSIM_GRAPHICS_OSGTRACKBALLZOOMMANIPULATOR_H
+
+#include <osgGA/TrackballManipulator>
+
+
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Trackball manipulator that uses the mouse wheel to control zoom amount.
+/// The output matrices are view matrices and do not include the zoom.
+/// To apply the zoom, get the value from getZoomFactor() and multiply
+/// it by some base FOV to calculate the FOV for the current zoom level.
+class OsgTrackballZoomManipulator : public osgGA::TrackballManipulator
+{
+public:
+ /// Initializes the zoom parameters to default values
+ OsgTrackballZoomManipulator();
+
+ /// Sets the minimum zoom factor (zoomed out)
+ /// \param factor Minimum zoom factor
+ virtual void setMinZoomFactor(double factor);
+
+ /// Gets the minimum zoom factor
+ /// \return Minimum zoom factor
+ double getMinZoomFactor() const;
+
+ /// Sets the maximum zoom factor (zoomed out)
+ /// \param factor Maximum zoom factor
+ virtual void setMaxZoomFactor(double factor);
+
+ /// Gets the maximum zoom factor
+ /// \return Maximum zoom factor
+ double getMaxZoomFactor() const;
+
+ /// Sets the minimum amount to change the zoom factor in one step
+ /// \param amount Minimum zoom amount
+ virtual void setMinZoomAmount(double amount);
+
+ /// Gets the minimum amount to change the zoom factor in one step
+ /// \return Minimum zoom factor
+ double getMinZoomAmount() const;//
+
+ /// Sets the maximum amount to change the zoom factor in one step
+ /// \param amount Maximum zoom amount
+ virtual void setMaxZoomAmount(double amount);
+
+ /// Gets the maximum amount to change the zoom factor in one step
+ /// \return Maximum zoom factor
+ double getMaxZoomAmount() const;
+
+ /// Sets the current zoom factor
+ /// \param factor Zoom factor
+ virtual void setZoomFactor(double factor);
+
+ /// Gets the current zoom factor
+ /// \return Zoom factor
+ double getZoomFactor() const;
+
+ /// Sets the scale applied to the zoom factor before it is applied to the FOV
+ /// \param factor Scale applied to the zoom factor
+ virtual void setZoomFactorScale(double factor);
+
+ /// Gets the current zoom factor
+ /// \return Scale applied to the zoom factor
+ double getZoomFactorScale() const;
+
+ /// Zoom by a percent of the difference between the current zoom amount and minimum zoom factor
+ /// \param zoomPercent Percent to zoom by: positive values zoom out, negative values zoom in
+ virtual void zoom(double zoomPercent);
+
+ /// Removes roll of the camera, so that the top of the view is towards the Y direction.
+ virtual void makeUpright();
+
+protected:
+
+ /// Minimum zoom factor value (zoomed in)
+ double m_minZoomFactor;
+ /// Maximum zoom factor value (zoomed out)
+ double m_maxZoomFactor;
+
+ /// Minimum amount to change the zoom factor in one step
+ /// This minimum prevents zooming by infinitely smaller amounts.
+ double m_minZoomAmount;
+
+ /// Maximum amount to change the zoom factor in one step
+ double m_maxZoomAmount;
+
+ /// Current zoom factor
+ /// Larger values are zoomed out, smaller values are zoomed in.
+ double m_zoomFactor;
+
+ /// Scaling factor applied to the zoom factor before it is applied to the FOV
+ double m_zoomFactorScale;
+
+ /// Handle keyboard CTRL-U events to make the view upright
+ /// \param eventAdapter Event adapter
+ /// \param actionAdapter Action adapter
+ /// \return true if the event was handled, false otherwise
+ virtual bool handle(const osgGA::GUIEventAdapter& eventAdapter, osgGA::GUIActionAdapter& actionAdapter);
+
+ /// Handle mouse wheel scrolling to zoom in or out
+ /// \param eventAdapter Event adapter
+ /// \param actionAdapter Action adapter
+ /// \return true if the mouse wheel was handled, false otherwise
+ virtual bool handleMouseWheel(const osgGA::GUIEventAdapter& eventAdapter, osgGA::GUIActionAdapter& actionAdapter);
+};
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgUniform-inl.h b/SurgSim/Graphics/OsgUniform-inl.h
new file mode 100644
index 0000000..2b5408b
--- /dev/null
+++ b/SurgSim/Graphics/OsgUniform-inl.h
@@ -0,0 +1,117 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGUNIFORM_INL_H
+#define SURGSIM_GRAPHICS_OSGUNIFORM_INL_H
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Graphics/OsgConversions.h"
+#include "SurgSim/Graphics/OsgUniformTypes.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Default type conversion to OSG
+/// \tparam T Type
+/// \note
+/// The input value is just returned. This allows this conversion method to be used with scalar types such as int and
+/// float, which are the same in Eigen and OSG.
+template <typename T>
+const T& toOsg(const T& value)
+{
+ return value;
+}
+
+template <class T>
+OsgUniform<T>::OsgUniform(const std::string& name) :
+ UniformBase(), Uniform<T>(), OsgUniformBase(name)
+{
+ osg::Uniform::Type osgUniformType = getOsgUniformType<T>();
+ SURGSIM_ASSERT(osgUniformType != osg::Uniform::UNDEFINED) << "Failed to get OSG uniform type!";
+ SURGSIM_ASSERT(m_uniform->setType(osgUniformType)) << "Failed to set OSG uniform type!";
+ m_uniform->setNumElements(1);
+}
+
+template <class T>
+void OsgUniform<T>::set(const T& value)
+{
+ SURGSIM_ASSERT(m_uniform->set(toOsg(value))) << "Failed to set OSG uniform value!" <<
+ " Uniform: " << getName() << " value: " << value;
+ m_value = value;
+}
+
+template <class T>
+const T& OsgUniform<T>::get() const
+{
+ return m_value;
+}
+
+template <class T>
+OsgUniform<std::vector<T>>::OsgUniform(const std::string& name, size_t numElements) :
+ UniformBase(), Uniform<std::vector<T>>(), OsgUniformBase(name)
+{
+ osg::Uniform::Type osgUniformType = getOsgUniformType<T>();
+ SURGSIM_ASSERT(osgUniformType != osg::Uniform::UNDEFINED) << "Failed to get OSG uniform type!";
+ SURGSIM_ASSERT(m_uniform->setType(osgUniformType)) << "Failed to set OSG uniform type!";
+ m_value.resize(numElements);
+ m_uniform->setNumElements(numElements);
+}
+
+template <class T>
+size_t OsgUniform<std::vector<T>>::getNumElements() const
+{
+ return m_uniform->getNumElements();
+}
+
+template <class T>
+void OsgUniform<std::vector<T>>::setElement(size_t index, const T& value)
+{
+ SURGSIM_ASSERT(m_uniform->setElement(index, toOsg(value))) << "Failed to set OSG uniform value!" <<
+ " Uniform: " << getName() << " index: " << index << " value: " << value;
+ m_value[index] = value;
+}
+
+template <class T>
+void OsgUniform<std::vector<T>>::set(const std::vector<T>& value)
+{
+ SURGSIM_ASSERT(value.size() == m_uniform->getNumElements()) <<
+ "Number of elements (" << value.size() << ") must match uniform's number of elements (" <<
+ m_uniform->getNumElements() << ")! Uniform: " << getName();
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setElement(i, value[i]);
+ }
+}
+
+template <class T>
+typename std::vector<T>::const_reference OsgUniform<std::vector<T>>::getElement(size_t index) const
+{
+ return m_value[index];
+}
+
+template <class T>
+const std::vector<T>& OsgUniform<std::vector<T>>::get() const
+{
+ return m_value;
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGUNIFORM_INL_H
diff --git a/SurgSim/Graphics/OsgUniform.h b/SurgSim/Graphics/OsgUniform.h
new file mode 100644
index 0000000..fad307e
--- /dev/null
+++ b/SurgSim/Graphics/OsgUniform.h
@@ -0,0 +1,99 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGUNIFORM_H
+#define SURGSIM_GRAPHICS_OSGUNIFORM_H
+
+#include "SurgSim/Graphics/OsgUniformBase.h"
+#include "SurgSim/Graphics/Uniform.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG implementation of graphics uniform with a value of type T.
+/// \tparam Value type
+template <class T>
+class OsgUniform : public Uniform<T>, public OsgUniformBase
+{
+public:
+ /// Constructor
+ /// \param name Name used in shader code to access this uniform
+ explicit OsgUniform(const std::string& name);
+
+ /// Sets the value of the uniform
+ virtual void set(const T& value);
+
+ /// Returns the value of the uniform
+ virtual const T& get() const;
+
+private:
+ /// Value of the uniform
+ T m_value;
+};
+
+/// Specialization of OsgUniform for vectors of values.
+/// \tparam Value type stored in the vector
+template <class T>
+class OsgUniform<std::vector<T>> : public Uniform<std::vector<T>>, public OsgUniformBase
+{
+public:
+ /// Constructor
+ /// \param name Name used in shader code to access this uniform
+ /// \param numElements Number of elements
+ OsgUniform(const std::string& name, size_t numElements);
+
+ /// Returns the number of elements
+ virtual size_t getNumElements() const;
+
+ /// Sets the value of one of the uniform's elements
+ /// \param index Index of the element
+ /// \param value Value to set
+ virtual void setElement(size_t index, const T& value);
+
+ /// Sets the value of all of the uniform's elements
+ /// \param value Array of values
+ virtual void set(const std::vector<T>& value);
+
+ /// Gets the value of one of the uniform's elements
+ /// \param index Index of the element
+ /// \return Value of the element
+ virtual typename std::vector<T>::const_reference getElement(size_t index) const;
+
+ /// Gets the value of all of the uniform's elements
+ /// \return Vector of values
+ virtual const std::vector<T>& get() const;
+
+private:
+ /// Vector containing the values of the uniform's elements
+ std::vector<T> m_value;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#include "SurgSim/Graphics/OsgUniform-inl.h"
+
+/// \note HS-2013-jul-30 If OsgUniform.h is included by itself without OsgTextureUniform.h we have a
+/// state where the specializations that are implemented in OsgTextureUniform.h will not be found
+/// by the compiler, the code will compile but not work correctly, if you remove this include
+/// make sure that OsgUniform.h AND OsgTextureUniform.h are included at the same time.
+#include "SurgSim/Graphics/OsgTextureUniform.h"
+#include "SurgSim/Graphics/OsgTextureUniform-inl.h"
+
+#endif // SURGSIM_GRAPHICS_OSGUNIFORM_H
diff --git a/SurgSim/Graphics/OsgUniformBase.cpp b/SurgSim/Graphics/OsgUniformBase.cpp
new file mode 100644
index 0000000..de01127
--- /dev/null
+++ b/SurgSim/Graphics/OsgUniformBase.cpp
@@ -0,0 +1,36 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgUniformBase.h"
+#include "SurgSim/Framework/Log.h"
+
+using SurgSim::Graphics::OsgUniformBase;
+
+OsgUniformBase::OsgUniformBase(const std::string& name) : UniformBase(),
+ m_uniform(new osg::Uniform())
+{
+ m_uniform->setName(name);
+}
+
+void OsgUniformBase::addToStateSet(osg::StateSet* stateSet)
+{
+ SURGSIM_LOG_DEBUG(SurgSim::Framework::Logger::getDefaultLogger()) << "Base Add To Texture StateSet called";
+ stateSet->addUniform(m_uniform);
+}
+
+void OsgUniformBase::removeFromStateSet(osg::StateSet* stateSet)
+{
+ stateSet->removeUniform(m_uniform);
+}
diff --git a/SurgSim/Graphics/OsgUniformBase.h b/SurgSim/Graphics/OsgUniformBase.h
new file mode 100644
index 0000000..fb9fa93
--- /dev/null
+++ b/SurgSim/Graphics/OsgUniformBase.h
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGUNIFORMBASE_H
+#define SURGSIM_GRAPHICS_OSGUNIFORMBASE_H
+
+#include "SurgSim/Graphics/Uniform.h"
+
+#include <osg/StateSet>
+#include <osg/Uniform>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base OSG implementation of graphics uniforms.
+///
+/// Wraps an osg::Uniform.
+/// \note
+/// SurgSim::Graphics::OsgUniform is templated on the type of value, so this base class allows a pointer to any type of
+/// OSG Uniform.
+class OsgUniformBase : public virtual UniformBase
+{
+public:
+ /// Returns the name used in shader code to access this uniform
+ const std::string& getName() const
+ {
+ return m_uniform->getName();
+ }
+
+ /// Adds this uniform to the OSG state set
+ /// \param stateSet OSG state set
+ virtual void addToStateSet(osg::StateSet* stateSet);
+
+ /// Removes this uniform from the OSG state set
+ /// \param stateSet OSG state set
+ virtual void removeFromStateSet(osg::StateSet* stateSet);
+
+ /// Returns the OSG uniform node
+ osg::ref_ptr<osg::Uniform> getOsgUniform() const
+ {
+ return m_uniform;
+ }
+
+protected:
+ /// Constructor
+ /// \param name Name used in shader code to access this uniform
+ explicit OsgUniformBase(const std::string& name);
+
+ /// OSG uniform node
+ osg::ref_ptr<osg::Uniform> m_uniform;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGUNIFORMBASE_H
diff --git a/SurgSim/Graphics/OsgUniformFactory.cpp b/SurgSim/Graphics/OsgUniformFactory.cpp
new file mode 100644
index 0000000..65bc2c5
--- /dev/null
+++ b/SurgSim/Graphics/OsgUniformFactory.cpp
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgUniformFactory.h"
+
+
+
+#include "SurgSim/Graphics/OsgTexture1d.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/OsgTexture3d.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+OsgUniformFactory::OsgUniformFactory()
+{
+ // Scalar Types
+ registerClass<OsgUniform<bool>>("bool");
+ registerClass<OsgUniform<unsigned int>>("uint");
+ registerClass<OsgUniform<int>>("int");
+ registerClass<OsgUniform<float>>("float");
+ registerClass<OsgUniform<double>>("double");
+
+ // Vector Types
+ registerClass<OsgUniform<SurgSim::Math::Vector2f>>("vec2");
+ registerClass<OsgUniform<SurgSim::Math::Vector3f>>("vec3");
+ registerClass<OsgUniform<SurgSim::Math::Vector4f>>("vec4");
+
+ registerClass<OsgUniform<SurgSim::Math::Vector2d>>("dvec2");
+ registerClass<OsgUniform<SurgSim::Math::Vector3d>>("dvec3");
+ registerClass<OsgUniform<SurgSim::Math::Vector4d>>("dvec4");
+
+ // Matrix Types
+ registerClass<OsgUniform<SurgSim::Math::Matrix22f>>("mat2");
+ registerClass<OsgUniform<SurgSim::Math::Matrix33f>>("mat3");
+ registerClass<OsgUniform<SurgSim::Math::Matrix44f>>("mat4");
+
+ registerClass<OsgUniform<SurgSim::Math::Matrix22d>>("dmat2");
+ registerClass<OsgUniform<SurgSim::Math::Matrix33d>>("dmat3");
+ registerClass<OsgUniform<SurgSim::Math::Matrix44d>>("dmat4");
+
+ // Sampler Types
+ registerClass<OsgTextureUniform<OsgTexture1d>>("sampler1D");
+ registerClass<OsgTextureUniform<OsgTexture2d>>("sampler2D");
+ registerClass<OsgTextureUniform<OsgTexture3d>>("sampler3D");
+}
+
+OsgUniformFactory::~OsgUniformFactory()
+{
+
+}
+
+}
+}
+
diff --git a/SurgSim/Graphics/OsgUniformFactory.h b/SurgSim/Graphics/OsgUniformFactory.h
new file mode 100644
index 0000000..7e81441
--- /dev/null
+++ b/SurgSim/Graphics/OsgUniformFactory.h
@@ -0,0 +1,42 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGUNIFORMFACTORY_H
+#define SURGSIM_GRAPHICS_OSGUNIFORMFACTORY_H
+
+#include <string>
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Graphics/UniformBase.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// This class can create the appropriate OsgUniform from an OpenGl glsl type, use the appropriate name
+/// from glsl in the create() function to recieve the correctly typed uniform
+class OsgUniformFactory : public SurgSim::Framework::ObjectFactory1<UniformBase, std::string>
+{
+public:
+ OsgUniformFactory();
+
+ virtual ~OsgUniformFactory();
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Graphics/OsgUniformTypes.h b/SurgSim/Graphics/OsgUniformTypes.h
new file mode 100644
index 0000000..6888071
--- /dev/null
+++ b/SurgSim/Graphics/OsgUniformTypes.h
@@ -0,0 +1,176 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Functions to get the OSG uniform type enum for a given type
+
+#ifndef SURGSIM_GRAPHICS_OSGUNIFORMTYPES_H
+#define SURGSIM_GRAPHICS_OSGUNIFORMTYPES_H
+
+#include<memory>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <osg/Uniform>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+class OsgTexture1d;
+class OsgTexture2d;
+class OsgTexture3d;
+class OsgTextureCubeMap;
+class OsgTextureRectangle;
+
+/// Returns the OSG uniform type enum value for the template parameter type.
+/// This template function is specialized to match each type with an enum value.
+/// Any types for which the function is not specialized will return osg::Uniform::UNDEFINED.
+/// \tparam T Data type
+/// \return OSG uniform type enum value
+template <class T>
+inline osg::Uniform::Type getOsgUniformType()
+{
+ return osg::Uniform::UNDEFINED;
+}
+
+template <>
+inline osg::Uniform::Type getOsgUniformType<float>()
+{
+ return osg::Uniform::FLOAT;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<double>()
+{
+ return osg::Uniform::DOUBLE;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<int>()
+{
+ return osg::Uniform::INT;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<unsigned int>()
+{
+ return osg::Uniform::UNSIGNED_INT;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<bool>()
+{
+ return osg::Uniform::BOOL;
+}
+
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Vector2f>()
+{
+ return osg::Uniform::FLOAT_VEC2;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Vector3f>()
+{
+ return osg::Uniform::FLOAT_VEC3;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Vector4f>()
+{
+ return osg::Uniform::FLOAT_VEC4;
+}
+
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Vector2d>()
+{
+ return osg::Uniform::DOUBLE_VEC2;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Vector3d>()
+{
+ return osg::Uniform::DOUBLE_VEC3;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Vector4d>()
+{
+ return osg::Uniform::DOUBLE_VEC4;
+}
+
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Matrix22f>()
+{
+ return osg::Uniform::FLOAT_MAT2;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Matrix33f>()
+{
+ return osg::Uniform::FLOAT_MAT3;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Matrix44f>()
+{
+ return osg::Uniform::FLOAT_MAT4;
+}
+
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Matrix22d>()
+{
+ return osg::Uniform::DOUBLE_MAT2;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Matrix33d>()
+{
+ return osg::Uniform::DOUBLE_MAT3;
+}
+template <>
+inline osg::Uniform::Type getOsgUniformType<SurgSim::Math::Matrix44d>()
+{
+ return osg::Uniform::DOUBLE_MAT4;
+}
+
+template <>
+inline osg::Uniform::Type getOsgUniformType<std::shared_ptr<OsgTexture1d>>()
+{
+ return osg::Uniform::SAMPLER_1D;
+}
+
+template <>
+inline osg::Uniform::Type getOsgUniformType<std::shared_ptr<OsgTexture2d>>()
+{
+ return osg::Uniform::SAMPLER_2D;
+}
+
+template <>
+inline osg::Uniform::Type getOsgUniformType<std::shared_ptr<OsgTexture3d>>()
+{
+ return osg::Uniform::SAMPLER_3D;
+}
+
+template <>
+inline osg::Uniform::Type getOsgUniformType<std::shared_ptr<OsgTextureCubeMap>>()
+{
+ return osg::Uniform::SAMPLER_CUBE;
+}
+
+template <>
+inline osg::Uniform::Type getOsgUniformType<std::shared_ptr<OsgTextureRectangle>>()
+{
+ return osg::Uniform::SAMPLER_2D_RECT;
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGUNIFORMTYPES_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgUnitAxes.cpp b/SurgSim/Graphics/OsgUnitAxes.cpp
new file mode 100644
index 0000000..43b6167
--- /dev/null
+++ b/SurgSim/Graphics/OsgUnitAxes.cpp
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgUnitAxes.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+OsgUnitAxes::OsgUnitAxes()
+{
+ m_geode = new osg::Geode();
+ const int numVertices = 6;
+ osg::Geometry* geometry = new osg::Geometry();
+
+ osg::Vec3Array* vertices = new osg::Vec3Array(numVertices);
+ (*vertices)[0] = osg::Vec3f(0.0f, 0.0f, 0.0f);
+ (*vertices)[1] = osg::Vec3f(0.0f, 0.0f, 0.0f);
+ (*vertices)[2] = osg::Vec3f(0.0f, 0.0f, 0.0f);
+ (*vertices)[3] = osg::Vec3f(1.0f, 0.0f, 0.0f);
+ (*vertices)[4] = osg::Vec3f(0.0f, 1.0f, 0.0f);
+ (*vertices)[5] = osg::Vec3f(0.0f, 0.0f, 1.0f);
+ geometry->setVertexArray(vertices);
+
+ osg::Vec4Array* colors = new osg::Vec4Array(numVertices);
+ (*colors)[0] = osg::Vec4f(0.8f, 0.0f, 0.0f, 1.0f);
+ (*colors)[1] = osg::Vec4f(0.0f, 0.8f, 0.0f, 1.0f);
+ (*colors)[2] = osg::Vec4f(0.0f, 0.0f, 0.8f, 1.0f);
+ (*colors)[3] = osg::Vec4f(0.8f, 0.0f, 0.0f, 1.0f);
+ (*colors)[4] = osg::Vec4f(0.0f, 0.8f, 0.0f, 1.0f);
+ (*colors)[5] = osg::Vec4f(0.0f, 0.0f, 0.8f, 1.0f);
+ geometry->setColorArray(colors, osg::Array::BIND_PER_VERTEX);
+
+ // Copy triangles from mesh
+ unsigned int lines[] = {0,3,1,4,2,5};
+ geometry->addPrimitiveSet(new osg::DrawElementsUInt(osg::PrimitiveSet::LINES,6,lines));
+
+ m_geode->addDrawable(geometry);
+
+ osg::StateSet* state = m_geode->getOrCreateStateSet();
+ state->setAttribute(new osg::LineWidth(2.0));
+
+ // These lines should never be lit, always render full color
+ state->setMode(GL_LIGHTING, osg::StateAttribute::OFF | osg::StateAttribute::PROTECTED);
+
+}
+
+OsgUnitAxes::~OsgUnitAxes()
+{
+
+}
+
+osg::ref_ptr<osg::Node> OsgUnitAxes::getNode()
+{
+ return m_geode;
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/OsgUnitAxes.h b/SurgSim/Graphics/OsgUnitAxes.h
new file mode 100644
index 0000000..4f12acd
--- /dev/null
+++ b/SurgSim/Graphics/OsgUnitAxes.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGUNITAXES_H
+#define SURGSIM_GRAPHICS_OSGUNITAXES_H
+
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osg/Array>
+#include <osg/Vec3f>
+#include <osg/LineWidth>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class OsgUnitAxes
+{
+public:
+
+ /// Constructor
+ OsgUnitAxes();
+ ~OsgUnitAxes();
+
+ /// Gets the node.
+ /// \return The node.
+ osg::ref_ptr<osg::Node> getNode();
+
+private:
+ osg::ref_ptr<osg::Geode> m_geode;
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgUnitBox.h b/SurgSim/Graphics/OsgUnitBox.h
new file mode 100644
index 0000000..675b491
--- /dev/null
+++ b/SurgSim/Graphics/OsgUnitBox.h
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGUNITBOX_H
+#define SURGSIM_GRAPHICS_OSGUNITBOX_H
+
+#include <osg/Geode>
+#include <osg/Shape>
+#include <osg/ShapeDrawable>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG unit box geode to be used as a primitive shape
+/// The box is located at (0, 0, 0) and has a size of 1 on all three axes.
+/// Add the box geode to a transform node to position and scale it.
+class OsgUnitBox
+{
+public:
+ /// Constructor
+ OsgUnitBox() :
+ m_geode(new osg::Geode())
+ {
+ osg::ref_ptr<osg::Box> unitBox = new osg::Box(osg::Vec3(0.0, 0.0, 0.0), 1.0);
+ osg::ref_ptr<osg::ShapeDrawable> drawable = new osg::ShapeDrawable(unitBox);
+ m_geode->addDrawable(drawable);
+ m_geode->getOrCreateStateSet()->setMode(GL_NORMALIZE, osg::StateAttribute::ON);
+ }
+
+ /// Returns the root OSG node for the plane to be inserted into the scene-graph
+ osg::ref_ptr<osg::Node> getNode() const
+ {
+ return m_geode;
+ }
+
+private:
+ /// Root OSG node of the box
+ osg::ref_ptr<osg::Geode> m_geode;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGUNITBOX_H
diff --git a/SurgSim/Graphics/OsgUnitCylinder.h b/SurgSim/Graphics/OsgUnitCylinder.h
new file mode 100644
index 0000000..eb262ff
--- /dev/null
+++ b/SurgSim/Graphics/OsgUnitCylinder.h
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGUNITCYLINDER_H
+#define SURGSIM_GRAPHICS_OSGUNITCYLINDER_H
+
+#include <osg/Geode>
+#include <osg/Shape>
+#include <osg/ShapeDrawable>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG unit cylinder geode to be used as a primitive shape
+/// The cylinder is located at (0, 0, 0) and has a radius of 1 and height of 1.
+/// Add the cylinder geode to a transform node to position and scale it.
+class OsgUnitCylinder
+{
+public:
+ /// Constructor
+ OsgUnitCylinder() :
+ m_geode(new osg::Geode())
+ {
+ osg::ref_ptr<osg::Cylinder> unitCylinder = new osg::Cylinder(osg::Vec3(0.0, 0.0, 0.0), 1.0, 1.0);
+ osg::ref_ptr<osg::ShapeDrawable> drawable = new osg::ShapeDrawable(unitCylinder);
+ m_geode->addDrawable(drawable);
+ m_geode->getOrCreateStateSet()->setMode(GL_NORMALIZE, osg::StateAttribute::ON);
+ }
+
+ /// Returns the root OSG node for the plane to be inserted into the scene-graph
+ osg::ref_ptr<osg::Node> getNode() const
+ {
+ return m_geode;
+ }
+
+private:
+ /// Root OSG node of the cylinder
+ osg::ref_ptr<osg::Geode> m_geode;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGUNITCYLINDER_H
diff --git a/SurgSim/Graphics/OsgUnitSphere.h b/SurgSim/Graphics/OsgUnitSphere.h
new file mode 100644
index 0000000..8756274
--- /dev/null
+++ b/SurgSim/Graphics/OsgUnitSphere.h
@@ -0,0 +1,67 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGUNITSPHERE_H
+#define SURGSIM_GRAPHICS_OSGUNITSPHERE_H
+
+#include <osg/Geode>
+#include <osg/PositionAttitudeTransform>
+#include <osg/Quat>
+#include <osg/Shape>
+#include <osg/ShapeDrawable>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// OSG unit sphere geode to be used as a primitive shape
+/// The sphere is located at (0, 0, 0) and has a radius of 1.
+/// Add the sphere geode to a transform node to position and scale it.
+class OsgUnitSphere
+{
+public:
+ /// Constructor
+ OsgUnitSphere() :
+ m_transform(new osg::PositionAttitudeTransform())
+ {
+ osg::ref_ptr<osg::Sphere> unitSphere = new osg::Sphere(osg::Vec3(0.0, 0.0, 0.0), 1.0);
+ osg::ref_ptr<osg::ShapeDrawable> drawable = new osg::ShapeDrawable(unitSphere);
+ osg::ref_ptr<osg::Geode> geode = new osg::Geode();
+ geode->addDrawable(drawable);
+ geode->getOrCreateStateSet()->setMode(GL_NORMALIZE, osg::StateAttribute::ON);
+ m_transform->addChild(geode);
+ osg::Quat rotation;
+ rotation.makeRotate(osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(0.0, 1.0, 0.0));
+ m_transform->setAttitude(rotation);
+ }
+
+ /// Returns the root OSG node for the plane to be inserted into the scene-graph
+ osg::ref_ptr<osg::Node> getNode() const
+ {
+ return m_transform;
+ }
+
+private:
+ /// Root OSG node of the sphere
+ osg::ref_ptr<osg::PositionAttitudeTransform> m_transform;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGUNITSPHERE_H
diff --git a/SurgSim/Graphics/OsgVectorConversions.h b/SurgSim/Graphics/OsgVectorConversions.h
new file mode 100644
index 0000000..8cf0c7f
--- /dev/null
+++ b/SurgSim/Graphics/OsgVectorConversions.h
@@ -0,0 +1,120 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Conversions to and from OSG vector types
+
+#ifndef SURGSIM_GRAPHICS_OSGVECTORCONVERSIONS_H
+#define SURGSIM_GRAPHICS_OSGVECTORCONVERSIONS_H
+
+#include "SurgSim/Math/Vector.h"
+
+#include <osg/Vec2f>
+#include <osg/Vec2d>
+#include <osg/Vec3f>
+#include <osg/Vec3d>
+#include <osg/Vec4f>
+#include <osg/Vec4d>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Convert 2D vector of floats to OSG
+inline osg::Vec2f toOsg(const SurgSim::Math::Vector2f& vector)
+{
+ osg::Vec2f osgVector;
+ Eigen::Map<SurgSim::Math::Vector2f>(osgVector.ptr()) = vector;
+ return osgVector;
+}
+/// Convert from OSG to 2D vector of floats
+inline SurgSim::Math::Vector2f fromOsg(const osg::Vec2f& vector)
+{
+ return SurgSim::Math::Vector2f(vector.ptr());
+}
+
+/// Convert 2D vector of doubles to OSG
+inline osg::Vec2d toOsg(const SurgSim::Math::Vector2d& vector)
+{
+ osg::Vec2d osgVector;
+ Eigen::Map<SurgSim::Math::Vector2d>(osgVector.ptr()) = vector;
+ return osgVector;
+}
+/// Convert from OSG to 2D vector of doubles
+inline SurgSim::Math::Vector2d fromOsg(const osg::Vec2d& vector)
+{
+ return SurgSim::Math::Vector2d(vector.ptr());
+}
+
+/// Convert 3D vector of floats to OSG
+inline osg::Vec3f toOsg(const SurgSim::Math::Vector3f& vector)
+{
+ osg::Vec3f osgVector;
+ Eigen::Map<SurgSim::Math::Vector3f>(osgVector.ptr()) = vector;
+ return osgVector;
+}
+/// Convert from OSG to 3D vector of floats
+inline SurgSim::Math::Vector3f fromOsg(const osg::Vec3f& vector)
+{
+ return SurgSim::Math::Vector3f(vector.ptr());
+}
+
+/// Convert 3D vector of doubles to OSG
+inline osg::Vec3d toOsg(SurgSim::Math::Vector3d vector)
+{
+ osg::Vec3d osgVector;
+ Eigen::Map<SurgSim::Math::Vector3d>(osgVector.ptr()) = vector;
+ return osgVector;
+}
+/// Convert from OSG to 3D vector of doubles
+inline SurgSim::Math::Vector3d fromOsg(const osg::Vec3d& vector)
+{
+ return SurgSim::Math::Vector3d(vector.ptr());
+}
+
+
+/// Convert 4D vector of floats to OSG
+inline osg::Vec4f toOsg(const SurgSim::Math::Vector4f& vector)
+{
+ osg::Vec4f osgVector;
+ Eigen::Map<SurgSim::Math::Vector4f>(osgVector.ptr()) = vector;
+ return osgVector;
+}
+/// Convert from OSG to 4D vector of floats
+inline SurgSim::Math::Vector4f fromOsg(const osg::Vec4f& vector)
+{
+ return SurgSim::Math::Vector4f(vector.ptr());
+}
+
+/// Convert 4D vector of doubles to OSG
+inline osg::Vec4d toOsg(const SurgSim::Math::Vector4d& vector)
+{
+ osg::Vec4d osgVector;
+ Eigen::Map<SurgSim::Math::Vector4d>(osgVector.ptr()) = vector;
+ return osgVector;
+}
+/// Convert from OSG to 4D vector of doubles
+inline SurgSim::Math::Vector4d fromOsg(const osg::Vec4d& vector)
+{
+ return SurgSim::Math::Vector4d(vector.ptr());
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGVECTORCONVERSIONS_H
diff --git a/SurgSim/Graphics/OsgVectorFieldRepresentation.cpp b/SurgSim/Graphics/OsgVectorFieldRepresentation.cpp
new file mode 100644
index 0000000..c9cff07
--- /dev/null
+++ b/SurgSim/Graphics/OsgVectorFieldRepresentation.cpp
@@ -0,0 +1,173 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgVectorFieldRepresentation.h"
+
+#include <osg/Geode>
+#include <osg/PositionAttitudeTransform>
+#include <osg/PrimitiveSet>
+#include <osg/StateAttribute>
+
+#include "SurgSim/Graphics/OsgConversions.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+using SurgSim::DataStructures::Vertex;
+
+OsgVectorFieldRepresentation::OsgVectorFieldRepresentation(const std::string& name) :
+ Representation(name),
+ VectorFieldRepresentation(name),
+ OsgRepresentation(name),
+ m_scale(0.1)
+{
+ m_vectorField = std::make_shared<SurgSim::Graphics::VectorField>();
+ m_vertexData = new osg::Vec3Array;
+ m_lineGeometry = new osg::Geometry;
+ m_pointGeometry = new osg::Geometry;
+ m_drawArrays = new osg::DrawArrays(osg::PrimitiveSet::LINES);
+ m_drawPoints = new osg::DrawElementsUInt(osg::PrimitiveSet::POINTS);
+ m_line = new osg::LineWidth(1.0f);
+ m_point = new osg::Point(4.0f);
+ m_colors = new osg::Vec4Array;
+
+ m_lineGeometry->setVertexArray(m_vertexData);
+ m_lineGeometry->addPrimitiveSet(m_drawArrays);
+ m_lineGeometry->setUseDisplayList(false);
+ m_lineGeometry->setDataVariance(osg::Object::DYNAMIC);
+ m_lineGeometry->getOrCreateStateSet()->setAttribute(m_line, osg::StateAttribute::ON);
+ m_lineGeometry->setColorArray(m_colors);
+
+ m_pointGeometry->setVertexArray(m_vertexData);
+ m_pointGeometry->setUseDisplayList(false);
+ m_pointGeometry->setDataVariance(osg::Object::DYNAMIC);
+ m_pointGeometry->getOrCreateStateSet()->setAttribute(m_point, osg::StateAttribute::ON);
+ m_pointGeometry->setColorArray(m_colors);
+
+ osg::ref_ptr<osg::Geode> geode = new osg::Geode();
+ geode->addDrawable(m_lineGeometry);
+ geode->addDrawable(m_pointGeometry);
+ geode->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF | osg::StateAttribute::PROTECTED);
+ m_transform->addChild(geode);
+}
+
+
+OsgVectorFieldRepresentation::~OsgVectorFieldRepresentation()
+{
+}
+
+void OsgVectorFieldRepresentation::doUpdate(double dt)
+{
+ std::vector<Vertex<VectorFieldData>> vertices = m_vectorField->getVertices();
+ size_t count = vertices.size();
+
+ if (0 != count)
+ {
+ // osg::DrawElementsUInt can NOT work properly when there is no data in m_vertexData
+ // Thus, only use osg::DrawElementsUInt when there is something in m_vertexData
+ if (0 == m_pointGeometry->getNumPrimitiveSets())
+ {
+ m_pointGeometry->addPrimitiveSet(m_drawPoints);
+ }
+
+ SURGSIM_ASSERT(2 * m_drawPoints->size() == static_cast<std::size_t>(m_drawArrays->getCount()));
+ SURGSIM_ASSERT(m_vertexData->size() == m_colors->size());
+ SURGSIM_ASSERT(2 * m_drawPoints->size() == m_colors->size());
+ // Check for size change in number of vertices
+ if (count != m_drawPoints->size())
+ {
+ m_drawArrays->setCount(count * 2);
+ m_drawPoints->resize(count);
+ m_vertexData->resize(count * 2);
+ m_colors->resize(count * 2);
+
+ m_drawArrays->dirty();
+ m_drawPoints->dirty();
+ }
+
+ for (size_t i = 0; i < count; ++i)
+ {
+ // Starting location of vector
+ (*m_vertexData)[2 * i] = SurgSim::Graphics::toOsg(vertices[i].position);
+ // Ending location of vector
+ (*m_vertexData)[2 * i + 1] = SurgSim::Graphics::toOsg(vertices[i].position) +
+ SurgSim::Graphics::toOsg(vertices[i].data.direction) * m_scale;
+
+ m_drawPoints->at(i) = (2 * i);
+ }
+
+ // Update color information
+ if (vertices[0].data.color.hasValue())
+ {
+ for (size_t i = 0; i < count; ++i)
+ {
+ osg::Vec4d color = SurgSim::Graphics::toOsg(vertices[i].data.color.getValue());
+ (*m_colors)[2 * i] = color;
+ (*m_colors)[2 * i + 1] = color;
+ }
+ m_lineGeometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
+ m_pointGeometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
+ }
+ else
+ {
+ (*m_colors)[0]= osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f);
+ m_lineGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+ m_pointGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+ }
+
+ m_lineGeometry->dirtyBound();
+ m_pointGeometry->dirtyBound();
+ }
+}
+
+std::shared_ptr< SurgSim::Graphics::VectorField > OsgVectorFieldRepresentation::getVectorField() const
+{
+ return m_vectorField;
+}
+
+void OsgVectorFieldRepresentation::setLineWidth(double width)
+{
+ m_line->setWidth(width);
+}
+
+double OsgVectorFieldRepresentation::getLineWidth() const
+{
+ return static_cast<double>(m_line->getWidth());
+}
+
+void OsgVectorFieldRepresentation::setScale(double scale)
+{
+ m_scale = scale;
+}
+
+double OsgVectorFieldRepresentation::getScale() const
+{
+ return m_scale;
+}
+
+void OsgVectorFieldRepresentation::setPointSize(double size)
+{
+ m_point->setSize(size);
+}
+
+double OsgVectorFieldRepresentation::getPointSize() const
+{
+ return static_cast<double>(m_point->getSize());
+}
+
+}; // Graphics
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgVectorFieldRepresentation.h b/SurgSim/Graphics/OsgVectorFieldRepresentation.h
new file mode 100644
index 0000000..f8e599b
--- /dev/null
+++ b/SurgSim/Graphics/OsgVectorFieldRepresentation.h
@@ -0,0 +1,113 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGVECTORFIELDREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGVECTORFIELDREPRESENTATION_H
+
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/VectorFieldRepresentation.h"
+
+#include <osg/Array>
+#include <osg/Geometry>
+#include <osg/LineWidth>
+#include <osg/Point>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+/// OSG vector field representation, implements a VectorFieldRepresenation using OSG.
+class OsgVectorFieldRepresentation : public VectorFieldRepresentation, public OsgRepresentation
+{
+public:
+ /// Constructor
+ /// \param name Name of OsgVectorFieldRepresentation
+ explicit OsgVectorFieldRepresentation(const std::string& name);
+ /// Destructor
+ ~OsgVectorFieldRepresentation();
+
+ /// Gets the vector field
+ /// \return The vector field
+ virtual std::shared_ptr< SurgSim::Graphics::VectorField > getVectorField() const override;
+
+ /// Sets vector line width
+ /// \param width Width of vector line
+ virtual void setLineWidth(double width) override;
+ /// Gets line width
+ /// \return The line width
+ virtual double getLineWidth() const override;
+
+ /// Sets the scale to be applied to all vectors
+ /// \param scale The scale
+ virtual void setScale(double scale) override;
+ /// Gets the scale applied to all vectors
+ /// \return The scale
+ virtual double getScale() const override;
+
+ /// Sets the size of point indicating the starting of vector
+ /// \param size Size of starting point of a vector
+ virtual void setPointSize(double size);
+ /// Gets the size of starting point of a vector
+ /// \return The size of starting point of a vector
+ virtual double getPointSize() const;
+
+ /// Executes the update operation
+ /// \param dt The time step
+ virtual void doUpdate(double dt) override;
+
+private:
+ /// Vector Field holds a list of vertices/points (X,Y,Z) in 3D space
+ /// Each point is associated with a vector and an optional color
+ std::shared_ptr<SurgSim::Graphics::VectorField> m_vectorField;
+
+ /// OSG vertex data structure
+ osg::ref_ptr<osg::Vec3Array> m_vertexData;
+
+ /// OSG::Geometry node holding OSG representation of vectors
+ osg::ref_ptr<osg::Geometry> m_lineGeometry;
+ /// OSG::Geometry node holding OSG representation of vector starting points
+ osg::ref_ptr<osg::Geometry> m_pointGeometry;
+
+ /// An OSG::DrawArrays object specifying how vectors will be drawn
+ osg::ref_ptr<osg::DrawArrays> m_drawArrays;
+ /// An OSG::DrawElementUInt object specifying how vector starting points will be drawn
+ osg::ref_ptr<osg::DrawElementsUInt> m_drawPoints;
+
+ /// OSG::LineWidth for representing vector
+ osg::ref_ptr<osg::LineWidth> m_line;
+ /// OSG::Point for representing vector starting point
+ osg::ref_ptr<osg::Point> m_point;
+
+ /// OSG::Vec4Array to hold color for each vector
+ osg::ref_ptr<osg::Vec4Array> m_colors;
+
+ /// A scale to scale the length of vector
+ double m_scale;
+};
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+}; // Graphics
+}; // SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGVECTORFIELDREPRESENTATION_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgView.cpp b/SurgSim/Graphics/OsgView.cpp
new file mode 100644
index 0000000..cc2f802
--- /dev/null
+++ b/SurgSim/Graphics/OsgView.cpp
@@ -0,0 +1,454 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgView.h"
+
+
+#include "SurgSim/Devices/Keyboard/KeyboardDevice.h"
+#include "SurgSim/Devices/Keyboard/OsgKeyboardHandler.h"
+#include "SurgSim/Devices/Mouse/MouseDevice.h"
+#include "SurgSim/Devices/Mouse/OsgMouseHandler.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgConversions.h"
+#include "SurgSim/Graphics/OsgTrackballZoomManipulator.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/MathConvert.h"
+
+#include <osgViewer/ViewerEventHandlers>
+#include <osg/DisplaySettings>
+
+using SurgSim::Graphics::OsgCamera;
+using SurgSim::Graphics::OsgView;
+
+namespace
+{
+
+// Mapping from OSS values to OSG values, the order here needs to match the numerical value in
+// SurgSim::Graphics::View::StereoMode
+const osg::DisplaySettings::StereoMode StereoModeEnums[SurgSim::Graphics::View::STEREO_MODE_COUNT] =
+{
+ osg::DisplaySettings::QUAD_BUFFER,
+ osg::DisplaySettings::ANAGLYPHIC,
+ osg::DisplaySettings::HORIZONTAL_SPLIT,
+ osg::DisplaySettings::VERTICAL_SPLIT,
+ osg::DisplaySettings::LEFT_EYE,
+ osg::DisplaySettings::RIGHT_EYE,
+ osg::DisplaySettings::HORIZONTAL_INTERLACE,
+ osg::DisplaySettings::VERTICAL_INTERLACE,
+ osg::DisplaySettings::CHECKERBOARD
+};
+
+// Mapping from OSS values to OSG values, the order here needs to match the numerical value in
+// SurgSim::Graphics::View::DisplayType
+const osg::DisplaySettings::DisplayType DisplayTypeEnums[SurgSim::Graphics::View::DISPLAY_TYPE_COUNT] =
+{
+ osg::DisplaySettings::MONITOR,
+ osg::DisplaySettings::HEAD_MOUNTED_DISPLAY
+};
+
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgView, OsgView);
+
+OsgView::OsgView(const std::string& name) : View(name),
+ m_isWindowBorderEnabled(true),
+ m_isFirstUpdate(true),
+ m_areWindowSettingsDirty(false),
+ m_view(new osgViewer::View()),
+ m_osgMapUniforms(false),
+ m_manipulatorPosition(SurgSim::Math::Vector3d(0.0, 0.0, -3.0)),
+ m_manipulatorLookat(SurgSim::Math::Vector3d::Zero()),
+ m_keyboardEnabled(false),
+ m_mouseEnabled(false)
+{
+ m_position[0] = 0;
+ m_position[1] = 0;
+ m_dimensions[0] = 1024;
+ m_dimensions[1] = 768;
+
+ /// Clear the OSG default camera, let that be handled at a higher level.
+ m_view->setCamera(nullptr);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, bool, CameraManipulatorEnabled,
+ isManipulatorEnabled, enableManipulator);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, SurgSim::Math::Vector3d, CameraPosition,
+ getManipulatorPosition, setManipulatorPosition);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, SurgSim::Math::Vector3d, CameraLookAt,
+ getManipulatorLookAt, setManipulatorLookAt);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, bool, OsgMapUniforms, getOsgMapsUniforms, setOsgMapsUniforms);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, bool, KeyboardDeviceEnabled,
+ isKeyboardDeviceEnabled, enableKeyboardDevice);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, bool, MouseDeviceEnabled,
+ isMouseDeviceEnabled, enableMouseDevice);
+}
+
+OsgView::~OsgView()
+{
+ // Clean up the handler callbacks
+ enableKeyboardDevice(false);
+ enableMouseDevice(false);
+}
+
+void OsgView::setPosition(const std::array<int, 2>& position)
+{
+ if (position != m_position)
+ {
+ m_position = position;
+ m_areWindowSettingsDirty = true;
+ }
+}
+
+std::array<int, 2> OsgView::getPosition() const
+{
+ return m_position;
+}
+
+void OsgView::setDimensions(const std::array<int, 2>& dimensions)
+{
+ if (m_dimensions != dimensions)
+ {
+ m_areWindowSettingsDirty = true;
+ m_dimensions = dimensions;
+ }
+}
+
+std::array<int, 2> OsgView::getDimensions() const
+{
+ return m_dimensions;
+}
+
+void OsgView::setWindowBorderEnabled(bool enabled)
+{
+ m_isWindowBorderEnabled = enabled;
+ m_areWindowSettingsDirty = true;
+}
+
+bool OsgView::isWindowBorderEnabled() const
+{
+ return m_isWindowBorderEnabled;
+}
+
+void OsgView::setCamera(std::shared_ptr<SurgSim::Framework::Component> camera)
+{
+ std::shared_ptr<OsgCamera> osgCamera = std::dynamic_pointer_cast<OsgCamera>(camera);
+ SURGSIM_ASSERT(osgCamera != nullptr) << "OsgView can only take an OsgCamera.";
+
+ View::setCamera(camera);
+ m_view->setCamera(osgCamera->getOsgCamera());
+}
+
+void OsgView::update(double dt)
+{
+ if (m_areWindowSettingsDirty)
+ {
+ osg::Camera* viewCamera = m_view->getCamera();
+ if (viewCamera)
+ {
+ osgViewer::GraphicsWindow* window =
+ dynamic_cast<osgViewer::GraphicsWindow*>(viewCamera->getGraphicsContext());
+ if (window)
+ {
+ window->setWindowDecoration(m_isWindowBorderEnabled);
+ window->setWindowRectangle(m_position[0], m_position[1], m_dimensions[0], m_dimensions[1]);
+ m_areWindowSettingsDirty = false;
+ }
+ }
+ }
+}
+
+bool OsgView::doInitialize()
+{
+ return true;
+}
+
+bool OsgView::doWakeUp()
+{
+ osg::ref_ptr<osg::DisplaySettings> displaySettings = new osg::DisplaySettings;
+ displaySettings->setDefaults();
+
+ if (isStereo())
+ {
+ displaySettings->setStereo(isStereo());
+ displaySettings->setStereoMode(StereoModeEnums[getStereoMode()]);
+ displaySettings->setDisplayType(DisplayTypeEnums[getDisplayType()]);
+ displaySettings->setEyeSeparation(static_cast<float>(getEyeSeparation()));
+ displaySettings->setScreenDistance(static_cast<float>(getScreenDistance()));
+ displaySettings->setScreenWidth(static_cast<float>(getScreenWidth()));
+ displaySettings->setScreenHeight(static_cast<float>(getScreenHeight()));
+ }
+
+
+ m_view->setDisplaySettings(displaySettings);
+
+ osg::ref_ptr<osgViewer::ViewConfig> viewConfig;
+
+// #refactor
+// HS-2014-may-14 Linux is stuck at OSG 3.2.0-rc1, this is not implemented there, they are waiting for
+// 3.2.1 to move forward, implement this once linux has caught up
+// if (isFullScreen())
+// {
+// viewConfig = new osgViewer::SingleScreen(getTargetScreen());
+// }
+// else
+// {
+// viewConfig = new osgViewer::SingleWindow(m_x, m_y, m_width, m_height, getTargetScreen());
+// }
+// m_view->apply(viewConfig);
+
+ if (isFullScreen())
+ {
+ m_view->setUpViewOnSingleScreen(getTargetScreen());
+ }
+ else
+ {
+ m_view->setUpViewInWindow(m_position[0], m_position[1], m_dimensions[0], m_dimensions[1], getTargetScreen());
+ }
+
+
+ auto statsHandler = new osgViewer::StatsHandler;
+ m_view->addEventHandler(statsHandler);
+
+ if (m_osgMapUniforms)
+ {
+ fixupStatsHandler(statsHandler);
+
+ m_view->getCamera()->getGraphicsContext()->getState()->setUseModelViewAndProjectionUniforms(true);
+ m_view->getCamera()->getGraphicsContext()->getState()->setUseVertexAttributeAliasing(true);
+ }
+
+
+ return true;
+}
+
+void OsgView::fixupStatsHandler(osgViewer::StatsHandler* statsHandler)
+{
+ // use ref_ptr in case loading fails we don't have to clean up
+ osg::ref_ptr<osg::Shader> vertexShader = new osg::Shader(osg::Shader::VERTEX);
+ osg::ref_ptr<osg::Shader> fragmentShader = new osg::Shader(osg::Shader::FRAGMENT);
+
+ bool success = true;
+ std::string fileName;
+ if (getRuntime()->getApplicationData()->tryFindFile("Shaders/osg_statshandler.vert", &fileName))
+ {
+ success = vertexShader->loadShaderSourceFromFile(fileName);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Could not find Shaders/osg_statshandler.vert, the osg stats "
+ << "display will probably not work correctly.";
+ success = false;
+ }
+
+ if (getRuntime()->getApplicationData()->tryFindFile("Shaders/osg_statshandler.frag", &fileName))
+ {
+ success = fragmentShader->loadShaderSourceFromFile(fileName);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Could not find Shaders/osg_statshandler.frag, the osg stats "
+ << "display will probably not work correctly.";
+ success = false;
+ }
+
+ if (success)
+ {
+ osg::ref_ptr<osg::Program> program = new osg::Program;
+ program->addShader(fragmentShader);
+ program->addShader(vertexShader);
+
+ auto state = statsHandler->getCamera()->getOrCreateStateSet();
+
+ auto texture = new osg::Texture2D();
+ texture->setTextureSize(256, 256);
+
+ state->setAttributeAndModes(program);
+ state->setTextureAttributeAndModes(0, texture);
+ state->addUniform(new osg::Uniform("osg_TextTexture", static_cast<int>(0)));
+ }
+}
+
+void SurgSim::Graphics::OsgView::setOsgMapsUniforms(bool val)
+{
+ SURGSIM_ASSERT(!isAwake()) << "Can't change mapping mode after waking up.";
+ m_osgMapUniforms = val;
+}
+
+bool SurgSim::Graphics::OsgView::getOsgMapsUniforms()
+{
+ return m_osgMapUniforms;
+}
+
+osg::ref_ptr<osgViewer::View> SurgSim::Graphics::OsgView::getOsgView() const
+{
+ return m_view;
+}
+
+void SurgSim::Graphics::OsgView::enableManipulator(bool val)
+{
+ if (m_manipulator == nullptr)
+ {
+ m_manipulator = new SurgSim::Graphics::OsgTrackballZoomManipulator();
+ // Set a default position
+ m_manipulator->setTransformation(
+ SurgSim::Graphics::toOsg(m_manipulatorPosition),
+ SurgSim::Graphics::toOsg(m_manipulatorLookat),
+ osg::Vec3d(0.0f, 1.0f, 0.0f));
+ }
+
+ if (val)
+ {
+ getOsgView()->setCameraManipulator(m_manipulator);
+ }
+ else
+ {
+ getOsgView()->setCameraManipulator(nullptr);
+ }
+}
+
+bool SurgSim::Graphics::OsgView::isManipulatorEnabled()
+{
+ return getOsgView()->getCameraManipulator() != nullptr;
+}
+
+void SurgSim::Graphics::OsgView::enableKeyboardDevice(bool val)
+{
+ // Early return if device is already turned on/off.
+ if (val == m_keyboardEnabled)
+ {
+ return;
+ }
+
+ std::shared_ptr<SurgSim::Input::CommonDevice> keyboardDevice = getKeyboardDevice();
+ osg::ref_ptr<osgGA::GUIEventHandler> keyboardHandle =
+ std::static_pointer_cast<SurgSim::Device::KeyboardDevice>(keyboardDevice)->getKeyboardHandler();
+ if (val)
+ {
+ getOsgView()->addEventHandler(keyboardHandle);
+ m_keyboardEnabled = true;
+ }
+ else
+ {
+ getOsgView()->removeEventHandler(keyboardHandle);
+ m_keyboardEnabled = false;
+ }
+}
+
+bool SurgSim::Graphics::OsgView::isKeyboardDeviceEnabled()
+{
+ return m_keyboardEnabled;
+}
+
+std::shared_ptr<SurgSim::Input::CommonDevice> SurgSim::Graphics::OsgView::getKeyboardDevice()
+{
+ if (m_keyboardDevice == nullptr)
+ {
+ m_keyboardDevice = std::make_shared<SurgSim::Device::KeyboardDevice>("Keyboard");
+ if (!m_keyboardDevice->isInitialized())
+ {
+ m_keyboardDevice->initialize();
+ }
+ }
+ return m_keyboardDevice;
+}
+
+void SurgSim::Graphics::OsgView::enableMouseDevice(bool val)
+{
+ // Early return if device is already turned on/off.
+ if (val == m_mouseEnabled)
+ {
+ return;
+ }
+
+ std::shared_ptr<SurgSim::Input::CommonDevice> mouseDevice = getMouseDevice();
+ osg::ref_ptr<osgGA::GUIEventHandler> mouseHandler =
+ std::static_pointer_cast<SurgSim::Device::MouseDevice>(mouseDevice)->getMouseHandler();
+ if (val)
+ {
+ getOsgView()->addEventHandler(mouseHandler);
+ m_mouseEnabled = true;
+ }
+ else
+ {
+ getOsgView()->removeEventHandler(mouseHandler);
+ m_mouseEnabled = false;
+ }
+}
+
+bool SurgSim::Graphics::OsgView::isMouseDeviceEnabled()
+{
+ return m_mouseEnabled;
+}
+
+std::shared_ptr<SurgSim::Input::CommonDevice> SurgSim::Graphics::OsgView::getMouseDevice()
+{
+ if (m_mouseDevice == nullptr)
+ {
+ m_mouseDevice = std::make_shared<SurgSim::Device::MouseDevice>("Mouse");
+ if (!m_mouseDevice->isInitialized())
+ {
+ m_mouseDevice->initialize();
+ }
+ }
+ return m_mouseDevice;
+}
+
+
+void SurgSim::Graphics::OsgView::setManipulatorParameters(const SurgSim::Math::Vector3d& position,
+ const SurgSim::Math::Vector3d& lookat)
+{
+ m_manipulatorPosition = position;
+ m_manipulatorLookat = lookat;
+
+ if (m_manipulator != nullptr)
+ {
+ m_manipulator->setTransformation(
+ SurgSim::Graphics::toOsg(m_manipulatorPosition),
+ SurgSim::Graphics::toOsg(m_manipulatorLookat),
+ osg::Vec3d(0.0f, 1.0f, 0.0f));
+ }
+}
+
+void SurgSim::Graphics::OsgView::setManipulatorPosition(const SurgSim::Math::Vector3d& position)
+{
+ setManipulatorParameters(position, m_manipulatorLookat);
+}
+
+SurgSim::Math::Vector3d SurgSim::Graphics::OsgView::getManipulatorPosition()
+{
+ return m_manipulatorPosition;
+}
+
+void SurgSim::Graphics::OsgView::setManipulatorLookAt(const SurgSim::Math::Vector3d& lookAt)
+{
+ setManipulatorParameters(m_manipulatorPosition, lookAt);
+}
+
+SurgSim::Math::Vector3d SurgSim::Graphics::OsgView::getManipulatorLookAt()
+{
+ return m_manipulatorLookat;
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgView.h b/SurgSim/Graphics/OsgView.h
new file mode 100644
index 0000000..f53448e
--- /dev/null
+++ b/SurgSim/Graphics/OsgView.h
@@ -0,0 +1,202 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGVIEW_H
+#define SURGSIM_GRAPHICS_OSGVIEW_H
+
+#include <osgViewer/Viewer>
+
+#include "SurgSim/Graphics/View.h"
+
+namespace osgViewer
+{
+class StatsHandler;
+class DisplaySettings;
+}
+
+namespace SurgSim
+{
+
+namespace Input
+{
+class CommonDevice;
+}
+
+namespace Device
+{
+class KeyboardDevice;
+class MouseDevice;
+}
+
+namespace Graphics
+{
+
+class OsgCamera;
+class OsgTrackballZoomManipulator;
+
+SURGSIM_STATIC_REGISTRATION(OsgView);
+
+/// OSG-based implementation of graphics view class.
+///
+/// A Graphics::OsgView wraps a osgViewer::View to provide a visualization of the scene to the user.
+///
+/// A Graphics::OsgCamera controls the viewpoint of this View.
+class OsgView : public View
+{
+public:
+ /// Constructor
+ /// \post The view has no camera.
+ /// \post The position of the view is (0, 0).
+ /// \post The dimensions of the view are 800 x 600.
+ /// \post The window border is enabled.
+ /// \param name Name of the view
+ explicit OsgView(const std::string& name);
+
+ /// Destructor
+ ~OsgView();
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgView);
+
+ virtual void setPosition(const std::array<int, 2>& position) override;
+
+ virtual std::array<int, 2> getPosition() const override;
+
+ virtual void setDimensions(const std::array<int, 2>& dimensions) override;
+
+ virtual std::array<int, 2> getDimensions() const override;
+
+ virtual void setWindowBorderEnabled(bool enabled) override;
+
+ virtual bool isWindowBorderEnabled() const override;
+
+ /// Sets the camera which provides the viewpoint in the scene
+ /// Only allows OsgCamera components, any other will not be set and it will return false.
+ /// \param camera Camera whose image will be shown in this view
+ /// \return True if it succeeded, false if it failed
+ virtual void setCamera(std::shared_ptr<SurgSim::Framework::Component> camera) override;
+
+ /// Enables a camera manipulator, implemented via a trackball, this is a temporary solution as it uses
+ /// the OSG input events rather than reading from the OpenSurgSim input.
+ /// \param val whether to enable the manipulator or not.
+ void enableManipulator(bool val);
+
+ /// \return whether the manipulator is enabled or not.
+ bool isManipulatorEnabled();
+
+ /// As the camera is not accessible from here and as it cannot be controlled from the outside
+ /// any more we let the user set the parameters from here.
+ /// \param position The position of the camera.
+ /// \param lookat The location the camera looks at.
+ void setManipulatorParameters(const SurgSim::Math::Vector3d& position, const SurgSim::Math::Vector3d& lookat);
+
+ /// Set the camera manipulator position.
+ /// \param position The position of the camera.
+ void setManipulatorPosition(const SurgSim::Math::Vector3d& position);
+
+ /// \return The position of the camera.
+ SurgSim::Math::Vector3d getManipulatorPosition();
+
+ /// Set the camera manipulator lookAt.
+ /// \param lookAt The location the camera looks at.
+ void setManipulatorLookAt(const SurgSim::Math::Vector3d& lookAt);
+
+ /// \return The location the camera looks at.
+ SurgSim::Math::Vector3d getManipulatorLookAt();
+
+ /// Enable osg modelview uniforms mapping, in this mode osg replaces the gl builtins with osg_* names, for
+ /// uniforms and vertex attributes
+ /// \param val Whether to enable osg uniform mapping, default false
+ void setOsgMapsUniforms(bool val);
+
+ /// \return the state of the osg modelview mapping mode.
+ bool getOsgMapsUniforms();
+
+ /// Return the keyboard to be used with this view.
+ /// \return A keyboard device
+ std::shared_ptr<SurgSim::Input::CommonDevice> getKeyboardDevice();
+
+ /// Turn on/off the keyboard device to be used.
+ /// \param val Indicate whether or not to use keyboard device
+ void enableKeyboardDevice(bool val);
+
+ /// \return Whether the keyboard device is enabled.
+ bool isKeyboardDeviceEnabled();
+
+ /// Return the mouse to be used with this view.
+ /// \return A mouse device
+ std::shared_ptr<SurgSim::Input::CommonDevice> getMouseDevice();
+
+ /// Turn on/off the mouse device to be used.
+ /// \param val Indicate whether or not to use mouse device
+ void enableMouseDevice(bool val);
+
+ /// \return Whether the mouse device is enabled.
+ bool isMouseDeviceEnabled();
+
+ virtual void update(double dt) override;
+
+ /// \return the OSG view which performs the actual work involved in setting up and rendering to a window
+ osg::ref_ptr<osgViewer::View> getOsgView() const;
+
+protected:
+ /// Initialize the view
+ /// \post The view's window is setup.
+ virtual bool doInitialize() override;
+
+ /// Wake up the view
+ virtual bool doWakeUp() override;
+private:
+
+ /// Patch the StatsHandler rendering
+ /// \param statsHandler The statshandler that will be patched.
+ void fixupStatsHandler(osgViewer::StatsHandler* statsHandler);
+
+ /// Position of the view on the screen (in pixels)
+ std::array<int, 2> m_position;
+ /// Dimensions of the view on the screen (in pixels)
+ std::array<int, 2> m_dimensions;
+ /// Whether the view window has a border
+ bool m_isWindowBorderEnabled;
+
+ /// Whether the next update will be the first time the view has been updated
+ /// On the first update, the view window is setup.
+ bool m_isFirstUpdate;
+ /// Whether the settings have been changed and the window needs to be updated
+ bool m_areWindowSettingsDirty;
+
+ /// OSG view which performs the actual work involved in setting up and rendering to a window
+ osg::ref_ptr<osgViewer::View> m_view;
+
+ /// Wether to enable osg uniform mapping
+ bool m_osgMapUniforms;
+
+ osg::ref_ptr<OsgTrackballZoomManipulator> m_manipulator;
+ SurgSim::Math::Vector3d m_manipulatorPosition;
+ SurgSim::Math::Vector3d m_manipulatorLookat;
+
+ /// Indicate if a keyboard device is enabled
+ bool m_keyboardEnabled;
+ std::shared_ptr<SurgSim::Device::KeyboardDevice> m_keyboardDevice;
+
+ /// Indicate if a mouse device is enabled
+ bool m_mouseEnabled;
+ std::shared_ptr<SurgSim::Device::MouseDevice> m_mouseDevice;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGVIEW_H
diff --git a/SurgSim/Graphics/OsgViewElement.cpp b/SurgSim/Graphics/OsgViewElement.cpp
new file mode 100644
index 0000000..ee806ee
--- /dev/null
+++ b/SurgSim/Graphics/OsgViewElement.cpp
@@ -0,0 +1,104 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgViewElement.h"
+
+#include "SurgSim/Devices/Keyboard/KeyboardDevice.h"
+#include "SurgSim/Devices/Keyboard/OsgKeyboardHandler.h"
+#include "SurgSim/Devices/Mouse/MouseDevice.h"
+#include "SurgSim/Devices/Mouse/OsgMouseHandler.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgConversions.h"
+#include "SurgSim/Graphics/OsgTrackballZoomManipulator.h"
+#include "SurgSim/Graphics/OsgView.h"
+
+using SurgSim::Graphics::OsgCamera;
+using SurgSim::Graphics::OsgView;
+using SurgSim::Graphics::OsgViewElement;
+
+OsgViewElement::OsgViewElement(const std::string& name) :
+ SurgSim::Graphics::ViewElement(name),
+ m_keyboardEnabled(false),
+ m_mouseEnabled(false)
+{
+ setView(std::make_shared<OsgView>(name + " View"));
+ setCamera(std::make_shared<OsgCamera>(name + " Camera"));
+ getCamera()->setRenderGroupReference(Representation::DefaultGroupName);
+}
+
+OsgViewElement::~OsgViewElement()
+{
+}
+
+bool OsgViewElement::setView(std::shared_ptr<SurgSim::Graphics::View> view)
+{
+ bool result = false;
+ if (getView() == view)
+ {
+ result = true;
+ }
+ else
+ {
+ std::shared_ptr<OsgView> osgView = std::dynamic_pointer_cast<OsgView>(view);
+ if (osgView)
+ {
+ result = ViewElement::setView(view);
+ }
+ }
+
+ return result;
+}
+
+void SurgSim::Graphics::OsgViewElement::enableManipulator(bool val)
+{
+
+ std::shared_ptr<OsgView> osgView = std::static_pointer_cast<OsgView>(getView());
+ osgView->enableManipulator(val);
+
+}
+
+void SurgSim::Graphics::OsgViewElement::enableKeyboardDevice(bool val)
+{
+ std::shared_ptr<OsgView> osgView = std::static_pointer_cast<OsgView>(getView());
+ osgView->enableKeyboardDevice(val);
+}
+
+
+std::shared_ptr<SurgSim::Input::CommonDevice> SurgSim::Graphics::OsgViewElement::getKeyboardDevice()
+{
+ std::shared_ptr<OsgView> osgView = std::static_pointer_cast<OsgView>(getView());
+ return osgView->getKeyboardDevice();
+}
+
+void SurgSim::Graphics::OsgViewElement::enableMouseDevice(bool val)
+{
+ std::shared_ptr<OsgView> osgView = std::static_pointer_cast<OsgView>(getView());
+ osgView->enableMouseDevice(val);
+}
+
+
+std::shared_ptr<SurgSim::Input::CommonDevice> SurgSim::Graphics::OsgViewElement::getMouseDevice()
+{
+ std::shared_ptr<OsgView> osgView = std::static_pointer_cast<OsgView>(getView());
+ return osgView->getMouseDevice();
+}
+
+
+void SurgSim::Graphics::OsgViewElement::setManipulatorParameters(const SurgSim::Math::Vector3d& position,
+ const SurgSim::Math::Vector3d& lookat)
+{
+ std::shared_ptr<OsgView> osgView = std::static_pointer_cast<OsgView>(getView());
+ osgView->setManipulatorParameters(position, lookat);
+}
diff --git a/SurgSim/Graphics/OsgViewElement.h b/SurgSim/Graphics/OsgViewElement.h
new file mode 100644
index 0000000..b7a6c99
--- /dev/null
+++ b/SurgSim/Graphics/OsgViewElement.h
@@ -0,0 +1,93 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_OSGVIEWELEMENT_H
+#define SURGSIM_GRAPHICS_OSGVIEWELEMENT_H
+
+#include <osg/ref_ptr>
+
+#include "SurgSim/Graphics/ViewElement.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Input
+{
+class CommonDevice;
+}
+
+namespace Graphics
+{
+
+/// OSG-based implementation of graphics view element.
+///
+/// A Graphics::OsgViewElement creates and wraps a Graphics::OsgView so that it can be added to the Scene.
+///
+/// A Scene needs at least one Graphics::View component for any visualization of Graphics:Representation objects
+/// to be shown.
+class OsgViewElement : public Graphics::ViewElement
+{
+public:
+ /// Constructor
+ /// \param name Name of the scene element
+ explicit OsgViewElement(const std::string& name);
+
+ /// Destructor
+ virtual ~OsgViewElement();
+
+ /// Sets the view component that provides the visualization of the graphics representations
+ /// Only allows OsgView components, any other will not be set and it will return false.
+ /// \param view The view that should be used.
+ /// \return True if it succeeds, false if it fails.
+ virtual bool setView(std::shared_ptr<View> view) override;
+
+ /// Enables a camera manipulator, implemented via a trackball, this is a temporary solution as it uses
+ /// the OSG input events rather than reading from the OpenSurgSim input.
+ /// \param val whether to enable the manipulator or not.
+ void enableManipulator(bool val);
+
+ /// As the camera is not accessible from here and as it cannot be controlled from the outside
+ /// any more we let the user set the parameters from here.
+ /// \param position The position of the camera.
+ /// \param lookat The location the camera looks at.
+ void setManipulatorParameters(const SurgSim::Math::Vector3d& position, const SurgSim::Math::Vector3d& lookat);
+
+ /// Return the keyboard to be used with this view.
+ /// \return A keyboard device
+ virtual std::shared_ptr<SurgSim::Input::CommonDevice> getKeyboardDevice() override;
+ /// Turn on/off the keyboard device to be used.
+ /// \param val Indicate whether or not to use keyboard device
+ virtual void enableKeyboardDevice(bool val) override;
+
+ /// Return the mouse to be used with this view.
+ /// \return A mouse device
+ virtual std::shared_ptr<SurgSim::Input::CommonDevice> getMouseDevice() override;
+ /// Turn on/off the mouse device to be used.
+ /// \param val Indicate whether or not to use mouse device
+ virtual void enableMouseDevice(bool val) override;
+
+private:
+ /// Indicate if a keyboard device is enabled
+ bool m_keyboardEnabled;
+ /// Indicate if a mouse device is enabled
+ bool m_mouseEnabled;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGVIEWELEMENT_H
diff --git a/SurgSim/Graphics/PlaneRepresentation.h b/SurgSim/Graphics/PlaneRepresentation.h
new file mode 100644
index 0000000..0b87214
--- /dev/null
+++ b/SurgSim/Graphics/PlaneRepresentation.h
@@ -0,0 +1,43 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_PLANEREPRESENTATION_H
+#define SURGSIM_GRAPHICS_PLANEREPRESENTATION_H
+
+#include "SurgSim/Graphics/Representation.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base graphics plane representation class, which defines the basic interface for a plane that can be visualized.
+/// The plane is the XZ plane, with normal +Y.
+class PlaneRepresentation : public virtual Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit PlaneRepresentation(const std::string& name) : Representation(name)
+ {
+ }
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_PLANEREPRESENTATION_H
diff --git a/SurgSim/Graphics/PointCloudRepresentation.cpp b/SurgSim/Graphics/PointCloudRepresentation.cpp
new file mode 100644
index 0000000..40f1cd2
--- /dev/null
+++ b/SurgSim/Graphics/PointCloudRepresentation.cpp
@@ -0,0 +1,36 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Graphics/PointCloudRepresentation.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+PointCloudRepresentation::PointCloudRepresentation(const std::string& name) : Representation(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(PointCloudRepresentation, double, PointSize, getPointSize, setPointSize);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(PointCloudRepresentation, SurgSim::Math::Vector4d, Color, getColor, setColor);
+}
+
+PointCloudRepresentation::~PointCloudRepresentation()
+{
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/PointCloudRepresentation.h b/SurgSim/Graphics/PointCloudRepresentation.h
new file mode 100644
index 0000000..d930276
--- /dev/null
+++ b/SurgSim/Graphics/PointCloudRepresentation.h
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_POINTCLOUDREPRESENTATION_H
+#define SURGSIM_GRAPHICS_POINTCLOUDREPRESENTATION_H
+
+#include <memory>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+typedef SurgSim::DataStructures::Vertices<SurgSim::DataStructures::EmptyData> PointCloud;
+
+/// Graphic representation of a point cloud, hase a very basic interface and is intentionally kept generic.
+class PointCloudRepresentation : public virtual Representation
+{
+public:
+
+ /// Constructor
+ explicit PointCloudRepresentation(const std::string& name);
+
+ virtual ~PointCloudRepresentation();
+
+ /// Pull the vertices.
+ /// \return The mesh.
+ virtual std::shared_ptr<PointCloud> getVertices() const = 0;
+
+ /// Sets point size for the point elements.
+ /// \param val The value.
+ virtual void setPointSize(double val) = 0;
+
+ /// Gets point size.
+ /// \return The point size.
+ virtual double getPointSize() const = 0;
+
+ /// Sets a color for all of the points together.
+ /// \param color The color.
+ virtual void setColor(const SurgSim::Math::Vector4d& color) = 0;
+
+ /// Gets the color.
+ /// \return The current color.
+ virtual SurgSim::Math::Vector4d getColor() const = 0;
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif // SURGSIM_GRAPHICS_POINTCLOUDREPRESENTATION_H
diff --git a/SurgSim/Graphics/RenderPass.cpp b/SurgSim/Graphics/RenderPass.cpp
new file mode 100644
index 0000000..bfa14b2
--- /dev/null
+++ b/SurgSim/Graphics/RenderPass.cpp
@@ -0,0 +1,150 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/RenderPass.h"
+
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+#include "SurgSim/Graphics/View.h"
+#include "SurgSim/Graphics/Texture.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+RenderPass::RenderPass(const std::string& name) :
+ SceneElement(name)
+{
+ m_camera = std::make_shared<OsgCamera>(getName() + " camera");
+ m_camera->setRenderGroupReference(name);
+
+ m_material = std::make_shared<OsgMaterial>("material");
+ m_camera->setMaterial(m_material);
+}
+
+RenderPass::~RenderPass()
+{
+
+}
+
+bool RenderPass::doInitialize()
+{
+ addComponent(m_camera);
+ addComponent(m_material);
+ return true;
+}
+
+std::shared_ptr<Camera> RenderPass::getCamera()
+{
+ return m_camera;
+}
+
+bool RenderPass::setRenderTarget(std::shared_ptr<RenderTarget> target)
+{
+ bool result = m_camera->setRenderTarget(target);
+ if (result)
+ {
+ m_renderTarget = target;
+ }
+ return result;
+}
+
+std::shared_ptr<RenderTarget> RenderPass::getRenderTarget()
+{
+ return m_renderTarget;
+}
+
+bool RenderPass::setMaterial(std::shared_ptr<Material> material)
+{
+ bool result = m_camera->setMaterial(material);
+ if (result)
+ {
+ m_material = material;
+ }
+ return result;
+}
+
+std::shared_ptr<Material> RenderPass::getMaterial()
+{
+ return m_material;
+}
+
+void RenderPass::setRenderOrder(SurgSim::Graphics::Camera::RenderOrder order, int value)
+{
+ m_camera->setRenderOrder(order, value);
+}
+
+void RenderPass::showColorTarget(int x, int y, int width, int height)
+{
+ if (m_debugColor == nullptr && m_renderTarget->getColorTargetCount() > 0)
+ {
+ auto texture = m_renderTarget->getColorTarget(0);
+ m_debugColor = buildDebugQuad("debug color", texture);
+ }
+ if (m_debugColor != nullptr)
+ {
+ m_debugColor->setLocation(x, y);
+ m_debugColor->setSize(width, height);
+ m_debugColor->setVisible(true);
+ }
+}
+
+void RenderPass::hideColorTarget()
+{
+ if (m_debugColor != nullptr)
+ {
+ m_debugColor->setVisible(false);
+ }
+}
+
+void RenderPass::showDepthTarget(int x, int y, int width, int height)
+{
+ if (m_debugDepth == nullptr && m_renderTarget->doesUseDepthTarget())
+ {
+ auto texture = m_renderTarget->getDepthTarget();
+ m_debugDepth = buildDebugQuad("debug depth", texture);
+ }
+ if (m_debugDepth != nullptr)
+ {
+ m_debugDepth->setLocation(x, y);
+ m_debugDepth->setSize(width, height);
+ m_debugDepth->setVisible(true);
+ }
+}
+
+void RenderPass::hideDepthTarget()
+{
+ if (m_debugDepth != nullptr)
+ {
+ m_debugDepth->setVisible(false);
+ }
+}
+
+std::shared_ptr<ScreenSpaceQuadRepresentation> RenderPass::buildDebugQuad(const std::string& name,
+ std::shared_ptr<Texture> texture)
+{
+ auto result = std::make_shared<OsgScreenSpaceQuadRepresentation>(name);
+ result->setTexture(texture);
+ addComponent(result);
+ return result;
+}
+
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/RenderPass.h b/SurgSim/Graphics/RenderPass.h
new file mode 100644
index 0000000..46646f8
--- /dev/null
+++ b/SurgSim/Graphics/RenderPass.h
@@ -0,0 +1,131 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_RENDERPASS_H
+#define SURGSIM_GRAPHICS_RENDERPASS_H
+
+#include <memory>
+
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/Camera.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+
+class Component;
+
+}
+namespace Graphics
+{
+
+class Group;
+class Material;
+class RenderTarget;
+class ScreenSpaceQuadRepresentation;
+class Texture;
+class View;
+
+
+/// Encapsulation of all the components necessary needed to implement a full renderpass, this SceneElement contains
+/// a Camera and Group, it can also take a Material (for shaders and uniforms) and a RenderTarget for textures that
+/// are used as the output for the camera.
+/// Other components do not need to be added to the pass explicitly, they only need to refer to the passes name in
+/// their respective groupReferences to be added to the render pass. The passes attributes should all be set up through
+/// uniforms in the material.
+class RenderPass : public SurgSim::Framework::SceneElement
+{
+public:
+
+ /// Constructor
+ /// \param name The name for this SceneElement
+ explicit RenderPass(const std::string& name);
+ ~RenderPass();
+
+ /// Executes the initialize operation.
+ /// \return true if it succeeds, false if it fails.
+ virtual bool doInitialize() override;
+
+ /// Sets render target for the camera, this abstracts the textures that are being used for rendering into.
+ /// \param target The rendertarget structure.
+ /// \return true if the target was successfully set
+ bool setRenderTarget(std::shared_ptr<RenderTarget> target);
+
+ /// Gets render target that is being used in this pass.
+ /// \return The render target that should be used.
+ std::shared_ptr<RenderTarget> getRenderTarget();
+
+ /// Sets render order.
+ /// \param order The general render stage for this pass.
+ /// \param value An order value for this pass, lower means earlier.
+ virtual void setRenderOrder(SurgSim::Graphics::Camera::RenderOrder order, int value);
+
+ /// Gets the camera.
+ /// \return The camera.
+ std::shared_ptr<Camera> getCamera();
+
+ /// Sets the material used for rendering.
+ /// \param material The material.
+ /// \return true if it succeeds, false if it fails.
+ bool setMaterial(std::shared_ptr<Material> material);
+
+ /// Gets the current material.
+ /// \return The material.
+ std::shared_ptr<Material> getMaterial();
+
+ /// Shows a quad on the screen with the texture used as the color target for this pass.
+ /// \param x,y The x and y coordinates on the screen.
+ /// \param width,height The width and height on the scree.
+ void showColorTarget(int x, int y, int width, int height);
+
+ /// Hides the color target display.
+ void hideColorTarget();
+
+ /// Shows a quad on the screen with the texture used as the depth target for this pass.
+ /// \param x,y The x and y coordinates on the screen.
+ /// \param width,height The width and height on the screen.
+ void showDepthTarget(int x, int y, int width, int height);
+
+ /// Hides the depth target display.
+ void hideDepthTarget();
+
+private:
+
+
+ std::shared_ptr<Camera> m_camera; ///< The camera used for the pass
+ std::shared_ptr<Group> m_group; ///< The groupd used for the pass
+ std::shared_ptr<RenderTarget> m_renderTarget; ///< The camera's rendertarget
+ std::shared_ptr<Material> m_material; ///< The material, attached to the camera
+
+ int m_renderOrder; ///< The renderorder that is being used for this pass
+
+ std::shared_ptr<ScreenSpaceQuadRepresentation> m_debugColor;
+ std::shared_ptr<ScreenSpaceQuadRepresentation> m_debugDepth;
+
+ /// Utility function to build a debug quad.
+ /// \param name The name for the component.
+ /// \param texture The texture for redering.
+ /// \return a constructed quads.
+ std::shared_ptr<ScreenSpaceQuadRepresentation> buildDebugQuad(
+ const std::string& name,
+ std::shared_ptr<Texture> texture);
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif
diff --git a/SurgSim/Graphics/RenderTarget.h b/SurgSim/Graphics/RenderTarget.h
new file mode 100644
index 0000000..6897552
--- /dev/null
+++ b/SurgSim/Graphics/RenderTarget.h
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_RENDERTARGET_H
+#define SURGSIM_GRAPHICS_RENDERTARGET_H
+
+#include <memory>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class Texture;
+
+/// RenderTarget is an abstraction of the target buffers that a Camera should use to render it's scene
+/// valid targets are a given number of color buffers, and an optional depth buffer. The buffers need to be
+/// made available as textures so they can be reused in another rendering step. The type of texture is not
+/// determined at this point and will depend on the concrete RenderTarget that is instantiated.
+/// The RenderTarget is consider immutable after construction.
+class RenderTarget
+{
+public:
+
+ /// Constructor
+ RenderTarget()
+ {
+ };
+
+ virtual ~RenderTarget()
+ {
+ };
+
+ /// Gets a size.
+ /// \param [out] width, height The width and height of the RenderTarget textures.
+ virtual void getSize(int* width, int* height) const = 0;
+
+ /// Returns the number of textures that this RenderTarget uses to draw into.
+ /// \return The color target count.
+ virtual int getColorTargetCount() const = 0;
+
+ /// Gets the indicated texture that is used as a target.
+ /// \param index Zero-based index of the texture to be used.
+ /// \return The color target, nullptr if index exceeds getColorTargetCount().
+ virtual std::shared_ptr<Texture> getColorTarget(int index) const = 0;
+
+ /// Check wether this draws into a depth texture.
+ /// \return true if yes, otherwise false.
+ virtual bool doesUseDepthTarget() const = 0;
+
+ /// Returns the texture that is used for the depth map drawing.
+ /// \return The depth target.
+ virtual std::shared_ptr<Texture> getDepthTarget() const = 0;
+
+private:
+
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/RenderTests/CMakeLists.txt b/SurgSim/Graphics/RenderTests/CMakeLists.txt
new file mode 100644
index 0000000..c7d081f
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/CMakeLists.txt
@@ -0,0 +1,63 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories (
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ OsgBoxRepresentationRenderTests.cpp
+ OsgCameraRenderTests.cpp
+ OsgCapsuleRepresentationRenderTests.cpp
+ OsgCylinderRepresentationRenderTests.cpp
+ OsgManagerRenderTests.cpp
+ OsgMeshRepresentationRenderTests.cpp
+ OsgOctreeRepresentationRenderTests.cpp
+ OsgPlaneRepresentationRenderTests.cpp
+ OsgPointCloudRepresentationRenderTests.cpp
+ OsgRepresentationRenderTests.cpp
+ OsgSceneryRepresentationRenderTests.cpp
+ OsgScreenSpaceQuadRenderTests.cpp
+ OsgShaderRenderTests.cpp
+ OsgSphereRepresentationRenderTests.cpp
+ OsgVectorFieldRepresentationRenderTests.cpp
+ OsgViewElementRenderTests.cpp
+ RenderTest.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ RenderTest.h
+)
+
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/OsgSceneryRepresentationTests DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Data)
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+set(LIBS
+ SurgSimFramework
+ SurgSimGraphics
+ SurgSimMath
+ SurgSimTesting
+ SurgSimBlocks
+)
+
+surgsim_add_unit_tests(SurgSimGraphicsRenderTest)
+
+set_target_properties(SurgSimGraphicsRenderTest PROPERTIES FOLDER "Graphics")
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/cube.png b/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/cube.png
new file mode 100644
index 0000000..ae6f0af
Binary files /dev/null and b/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/cube.png differ
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound.png b/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound.png
new file mode 100644
index 0000000..75922e4
Binary files /dev/null and b/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound.png differ
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound_deformable.ply b/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound_deformable.ply
new file mode 100644
index 0000000..765f44f
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound_deformable.ply
@@ -0,0 +1,2391 @@
+ply
+format ascii 1.0
+comment This file is a part of the OpenSurgSim project.
+comment Copyright 2013, SimQuest Solutions Inc.
+comment
+comment Licensed under the Apache License, Version 2.0 (the "License");
+comment you may not use this file except in compliance with the License.
+comment You may obtain a copy of the License at
+comment
+comment http://www.apache.org/licenses/LICENSE-2.0
+comment
+comment Unless required by applicable law or agreed to in writing, software
+comment distributed under the License is distributed on an "AS IS" BASIS,
+comment WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+comment See the License for the specific language governing permissions and
+comment limitations under the License.
+comment
+comment This file provides the geometry which describes a tetrahedral volume
+comment mesh and triangular surface mesh of an arm wound.
+comment
+element vertex 391
+property double x
+property double y
+property double z
+property double s
+property double t
+element face 407
+property list uint uint vertex_indices
+element polyhedron 1476
+property list uint uint vertex_indices
+element boundary_condition 79
+property uint vertex_index
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+-0.017638 0.001427 -0.012363 0.707716 0.681378
+-0.018984 0.002069 -0.016867 0.736310 0.634209
+-0.014913 -0.001358 -0.015090 0.680908 0.590891
+-0.018090 -0.001832 -0.017359 0.707848 0.596931
+0.012643 -0.002455 0.001226 0.403742 0.411127
+0.009376 -0.006324 0.003409 0.0 0.0
+0.015479 -0.002289 0.003265 0.375952 0.394412
+0.014119 -0.002367 0.007046 0.0 0.0
+-0.000236 -0.006438 -0.015323 0.0 0.0
+-0.003732 -0.004010 -0.013609 0.0 0.0
+0.002155 -0.000546 -0.013612 0.0 0.0
+0.001262 -0.001523 -0.018049 0.0 0.0
+0.019667 -0.017785 0.008674 0.0 0.0
+0.029299 -0.005262 0.004546 0.0 0.0
+0.046990 -0.020879 0.007375 0.0 0.0
+0.037332 -0.020768 -0.005328 0.0 0.0
+0.033586 -0.016523 0.018450 0.0 0.0
+0.034101 -0.002031 0.016309 0.0 0.0
+0.033392 -0.002474 0.009623 0.0 0.0
+0.025349 -0.003774 0.014192 0.0 0.0
+0.019145 0.004850 0.012264 0.307965 0.463751
+0.020216 0.004879 0.011113 0.302951 0.437069
+0.020968 0.001399 0.010219 0.315752 0.400181
+0.018687 0.001314 0.008476 0.333716 0.411611
+-0.007352 0.001018 0.005860 0.538316 0.697416
+-0.007333 -0.005722 0.014291 0.0 0.0
+-0.013652 -0.000173 0.002866 0.599904 0.744518
+-0.007113 -0.008223 -0.002577 0.0 0.0
+-0.006477 -0.004144 -0.015932 0.0 0.0
+-0.010957 -0.004967 -0.018490 0.0 0.0
+-0.007629 -0.006377 -0.018801 0.0 0.0
+-0.009906 -0.007928 -0.014976 0.0 0.0
+0.014105 -0.020914 -0.009294 0.0 0.0
+0.009112 -0.006286 -0.008604 0.0 0.0
+0.006268 -0.009189 -0.014543 0.0 0.0
+0.002300 -0.008261 -0.009596 0.0 0.0
+0.006635 0.000026 -0.006193 0.478446 0.422446
+0.010115 -0.000309 -0.007852 0.0 0.0
+0.005102 -0.004418 -0.008996 0.0 0.0
+-0.006609 0.001420 -0.025080 0.683895 0.418703
+-0.011057 -0.007486 -0.026293 0.0 0.0
+-0.008811 0.000184 -0.034044 0.750872 0.396926
+-0.005338 -0.005470 -0.024596 0.0 0.0
+0.001259 0.002603 0.004418 0.477874 0.590676
+0.000123 0.002377 -0.000362 0.508779 0.579936
+0.002998 0.002713 0.001375 0.478745 0.561899
+0.002769 -0.002124 0.003090 0.0 0.0
+-0.020005 -0.002711 -0.020006 0.725914 0.579430
+-0.021803 0.001971 -0.019803 0.769228 0.624707
+-0.021059 0.001058 -0.014582 0.742789 0.686283
+0.014631 0.000524 0.027783 0.0 0.0
+0.022184 -0.002620 0.021520 0.0 0.0
+0.018739 -0.011699 0.030717 0.0 0.0
+0.015337 -0.003630 0.015467 0.0 0.0
+0.016049 0.003671 -0.002811 0.385336 0.303910
+0.018898 0.003898 -0.000351 0.355721 0.293064
+0.014287 0.000683 -0.000350 0.398533 0.377274
+0.019319 -0.000205 -0.003032 0.0 0.0
+0.046990 -0.007619 0.007375 0.0 0.0
+0.040075 -0.002131 -0.002012 0.275562 0.097586
+0.037332 -0.007509 -0.005328 0.0 0.0
+-0.011140 0.001039 -0.002776 0.605865 0.671648
+-0.014033 -0.006140 -0.001762 0.0 0.0
+-0.016684 0.000633 -0.005670 0.664721 0.711067
+-0.011809 -0.002190 -0.009170 0.0 0.0
+0.024204 0.005929 0.015670 0.261423 0.435611
+0.026606 0.006410 0.017058 0.241027 0.413001
+0.023576 0.006924 0.019679 0.249385 0.456824
+0.024566 0.001831 0.017759 0.0 0.0
+0.013743 0.004124 0.008547 0.363455 0.496876
+0.011935 0.004109 0.005502 0.386672 0.485966
+0.014670 0.004333 0.007344 0.356430 0.470987
+0.012704 0.000753 0.008033 0.0 0.0
+-0.003034 -0.009956 -0.020825 0.0 0.0
+-0.006038 -0.007999 -0.012282 0.0 0.0
+0.014736 0.003042 -0.006304 0.410541 0.288505
+0.014286 -0.003192 -0.009813 0.0 0.0
+0.014065 0.002151 -0.010793 0.441123 0.277848
+0.019818 -0.002939 -0.008113 0.0 0.0
+0.036755 0.006575 0.019271 0.171882 0.285622
+0.036907 0.001084 0.018946 0.0 0.0
+0.033263 0.001097 0.023070 0.0 0.0
+0.033376 0.002597 0.017918 0.0 0.0
+0.022543 0.001069 0.006601 0.319550 0.337176
+0.020245 -0.001947 0.007184 0.330922 0.370668
+0.019727 0.001041 0.004292 0.343634 0.348013
+0.023642 -0.002413 0.005541 0.0 0.0
+0.029219 0.001517 -0.003210 0.316280 0.186932
+0.022506 -0.007309 -0.002980 0.0 0.0
+0.013584 0.001132 0.004599 0.380549 0.438335
+0.015737 0.001218 0.006218 0.360614 0.429612
+0.015374 -0.005687 -0.005108 0.0 0.0
+0.019027 -0.006775 -0.014839 0.0 0.0
+0.030475 0.001257 0.015552 0.262159 0.346781
+0.027297 -0.001544 0.019319 0.0 0.0
+-0.006877 -0.002740 -0.012407 0.607383 0.519549
+-0.009683 -0.005474 -0.007552 0.0 0.0
+-0.008963 -0.000571 -0.011342 0.619375 0.564274
+-0.009123 -0.005772 -0.011405 0.0 0.0
+0.006753 0.004254 0.022359 0.380109 0.683747
+0.010408 0.004267 0.016270 0.359520 0.575078
+0.015903 0.005679 0.020105 0.304442 0.542677
+0.017492 0.000471 0.018668 0.0 0.0
+-0.018916 -0.006283 -0.013171 0.0 0.0
+-0.013552 -0.004660 -0.011679 0.0 0.0
+-0.015191 -0.008193 -0.008642 0.0 0.0
+-0.014454 -0.008852 -0.015804 0.0 0.0
+0.016241 0.004334 0.010184 0.336917 0.483939
+0.017210 0.004543 0.009049 0.332410 0.456022
+0.012207 0.000507 -0.002006 0.420299 0.390013
+0.015357 0.000245 -0.003791 0.0 0.0
+0.007154 -0.001854 0.005752 0.0 0.0
+0.011373 -0.002515 0.009976 0.0 0.0
+0.009600 -0.002702 0.003200 0.0 0.0
+0.022251 -0.002063 0.015960 0.0 0.0
+0.021468 -0.001789 0.012242 0.0 0.0
+0.017030 0.000365 0.013057 0.0 0.0
+0.020308 0.001792 0.013974 0.0 0.0
+0.013639 -0.003166 -0.002562 0.0 0.0
+0.011924 -0.003711 -0.006304 0.0 0.0
+-0.014146 -0.020429 -0.047689 0.0 0.0
+-0.014146 -0.007170 -0.047689 0.0 0.0
+-0.024140 -0.017967 -0.040188 0.0 0.0
+-0.012259 -0.006676 -0.033836 0.0 0.0
+0.002537 -0.008403 -0.037874 0.0 0.0
+-0.001499 -0.000057 -0.029561 0.672858 0.347281
+-0.000610 -0.002936 -0.039936 0.735672 0.277619
+0.031048 0.003384 0.005728 0.258962 0.227713
+0.031021 0.004616 0.009841 0.239710 0.251763
+0.029058 -0.002228 0.009819 0.0 0.0
+-0.017817 -0.005609 -0.020805 0.0 0.0
+-0.021077 -0.009144 -0.020053 0.0 0.0
+-0.019782 -0.005521 -0.017072 0.0 0.0
+-0.021431 -0.005952 -0.022335 0.0 0.0
+0.028581 0.004545 0.037841 0.0 0.0
+0.022848 0.002598 0.033651 0.0 0.0
+0.026518 0.008415 0.028378 0.199337 0.482877
+-0.008942 -0.024739 -0.008176 0.0 0.0
+-0.020167 -0.001455 -0.000035 0.663670 0.794459
+-0.019937 -0.006669 -0.033633 0.0 0.0
+-0.011533 -0.022757 -0.029013 0.0 0.0
+0.025681 0.001454 0.013298 0.276295 0.369130
+0.021352 0.005330 0.013823 0.286985 0.451165
+0.022555 0.005159 0.012575 0.281559 0.420259
+-0.018697 -0.003565 -0.023866 0.0 0.0
+-0.003741 -0.007976 -0.007476 0.0 0.0
+-0.004960 -0.004029 -0.006795 0.0 0.0
+0.035352 0.005531 0.016102 0.190755 0.270198
+0.033038 0.004995 0.012719 0.216780 0.264971
+0.036201 0.003833 0.011247 0.209145 0.220919
+-0.014781 0.002213 -0.020373 0.719005 0.520347
+-0.016880 -0.002824 -0.021921 0.0 0.0
+-0.016398 0.001977 -0.024375 0.754476 0.519638
+-0.013707 -0.002037 -0.021513 0.0 0.0
+-0.001967 0.002359 -0.017593 0.600145 0.399436
+-0.005030 0.002252 -0.019030 0.632366 0.420192
+-0.000406 0.001476 -0.021158 0.610992 0.370987
+-0.003015 -0.003057 -0.018944 0.0 0.0
+0.005527 0.000425 -0.000868 0.462746 0.487855
+0.005663 0.003045 0.003152 0.449655 0.544815
+0.003921 0.003399 0.000160 0.473347 0.534593
+-0.015527 -0.005152 -0.022539 0.0 0.0
+-0.015560 -0.006666 -0.026900 0.0 0.0
+-0.014996 -0.002896 -0.024296 0.0 0.0
+-0.013917 -0.007956 -0.024021 0.0 0.0
+0.004887 -0.021029 0.001820 0.0 0.0
+0.002057 -0.006870 -0.003212 0.0 0.0
+0.022552 -0.001675 0.009108 0.314427 0.363307
+-0.024489 0.000741 -0.016918 0.777588 0.684517
+-0.024391 0.001936 -0.022406 0.801319 0.612368
+-0.024601 -0.004739 -0.018778 0.0 0.0
+0.006850 -0.006797 -0.004277 0.0 0.0
+0.004032 -0.004089 -0.000551 0.0 0.0
+0.012333 0.004106 0.011509 0.361086 0.521100
+0.005130 0.003275 0.012820 0.411354 0.608085
+0.007317 0.003436 0.008224 0.411549 0.550627
+-0.004058 0.001878 0.001264 0.531496 0.626286
+-0.014203 0.001575 -0.029189 0.762778 0.468814
+-0.010570 -0.002455 -0.021860 0.0 0.0
+-0.012199 0.002106 -0.022621 0.713800 0.485157
+-0.025827 -0.009187 -0.021112 0.0 0.0
+-0.021601 -0.004637 -0.026044 0.0 0.0
+-0.017290 -0.008529 -0.023047 0.0 0.0
+-0.013967 -0.002130 -0.018341 0.682695 0.540233
+-0.014398 -0.004396 -0.019489 0.0 0.0
+-0.016250 -0.002623 -0.019238 0.704893 0.552676
+0.002921 -0.006003 -0.021793 0.0 0.0
+0.009600 0.002834 -0.010048 0.466575 0.318768
+0.012195 0.002939 -0.008156 0.437269 0.303618
+0.014492 -0.021349 -0.029300 0.0 0.0
+0.026117 -0.020765 -0.017965 0.0 0.0
+0.014492 -0.008090 -0.029300 0.0 0.0
+0.002621 -0.007830 -0.029082 0.0 0.0
+0.008423 0.000639 0.001153 0.432663 0.470788
+0.030678 0.007333 0.019744 0.205235 0.378927
+0.026656 0.007758 0.022058 0.221464 0.439517
+0.020949 0.007122 0.023900 0.253990 0.513600
+0.024451 0.002704 0.023354 0.0 0.0
+-0.023157 -0.024020 -0.019768 0.0 0.0
+0.029177 0.003608 0.018712 0.0 0.0
+0.012003 0.005360 0.025917 0.328309 0.649173
+0.004293 -0.015189 0.020860 0.0 0.0
+0.002537 -0.021663 -0.037874 0.0 0.0
+0.017564 -0.002515 -0.026912 0.535047 0.158602
+0.011421 -0.002833 -0.031689 0.602351 0.194529
+0.011215 0.000322 -0.020415 0.517135 0.253575
+0.002635 0.003021 -0.012333 0.529341 0.378660
+0.001235 0.002490 -0.015739 0.562597 0.374265
+0.004272 0.002632 -0.013690 0.525222 0.350674
+-0.002997 0.002057 -0.002458 0.541143 0.598152
+-0.002614 -0.004314 -0.002919 0.0 0.0
+0.003125 -0.022994 -0.021280 0.0 0.0
+-0.005395 -0.000823 -0.013764 0.605695 0.495888
+-0.008954 -0.001258 -0.015629 0.641873 0.519288
+-0.003834 0.002652 -0.016057 0.603320 0.424604
+-0.007047 -0.000971 -0.018908 0.0 0.0
+0.010433 -0.002408 -0.000327 0.425311 0.423985
+0.007502 -0.002301 -0.002415 0.454583 0.442237
+0.009372 0.000321 -0.004108 0.449153 0.405214
+0.025152 0.001195 0.009053 0.296717 0.330843
+0.016960 0.000956 0.001995 0.370882 0.360310
+0.019875 -0.003369 -0.000014 0.0 0.0
+0.030751 0.008190 0.022981 0.192338 0.395364
+0.006170 -0.006701 0.007879 0.0 0.0
+0.004293 -0.001930 0.020860 0.0 0.0
+-0.000653 -0.004919 0.008637 0.0 0.0
+-0.014173 -0.021266 0.011121 0.0 0.0
+-0.017277 -0.006019 -0.017796 0.0 0.0
+0.023544 -0.000939 0.002037 0.0 0.0
+-0.009768 -0.009134 -0.022027 0.0 0.0
+-0.009580 -0.002948 -0.014062 0.636321 0.535441
+-0.011797 -0.001691 -0.017176 0.666623 0.531485
+-0.010204 0.002309 -0.009891 0.632801 0.615829
+-0.011598 0.001574 -0.008061 0.635984 0.649988
+-0.008708 0.001627 -0.006285 0.605513 0.633471
+0.034635 0.001794 0.002947 0.257173 0.179657
+0.039472 0.002062 0.008665 0.205123 0.177019
+-0.015907 -0.007981 -0.018944 0.0 0.0
+-0.020598 0.001813 -0.025718 0.792131 0.555168
+-0.045621 -0.006163 -0.019740 0.935849 0.983660
+-0.040582 -0.008149 -0.025424 0.0 0.0
+-0.042936 -0.004004 -0.023210 0.932866 0.916952
+-0.038274 -0.003561 -0.017712 0.876293 0.872123
+-0.025030 -0.004697 -0.013625 0.0 0.0
+-0.023215 -0.000003 -0.009663 0.733754 0.749496
+-0.021118 -0.002997 -0.014161 0.0 0.0
+-0.004112 0.000695 0.015891 0.483989 0.762601
+-0.016389 -0.004557 -0.019826 0.0 0.0
+-0.003547 -0.002715 -0.010257 0.571529 0.501819
+-0.001840 -0.000524 -0.011726 0.569397 0.474849
+-0.000903 -0.002832 -0.008443 0.541931 0.487581
+-0.001161 -0.006198 -0.011121 0.0 0.0
+0.026165 -0.000632 0.016453 0.0 0.0
+0.025651 0.005503 0.014190 0.255812 0.397905
+-0.045621 -0.011579 -0.019740 0.0 0.0
+-0.040582 -0.021408 -0.025424 0.0 0.0
+-0.032217 -0.008366 -0.016728 0.0 0.0
+0.000124 -0.006312 0.001727 0.0 0.0
+0.017517 -0.002166 0.004909 0.355779 0.383708
+0.011949 -0.006639 -0.000263 0.0 0.0
+0.023286 0.001530 0.011808 0.299493 0.388218
+-0.013427 -0.006320 -0.020900 0.0 0.0
+0.005159 0.001722 -0.017378 0.541972 0.328988
+0.006008 -0.003221 -0.013176 0.0 0.0
+0.044680 -0.002154 0.004195 0.212358 0.092100
+-0.000907 0.002148 0.009167 0.474108 0.650299
+0.018182 0.005343 0.015586 0.301651 0.485315
+0.043927 0.001725 0.025605 0.0 0.0
+-0.012854 0.002261 -0.012050 0.666436 0.627285
+-0.014517 0.001574 -0.010230 0.670831 0.667926
+-0.011668 -0.001006 -0.012914 0.648374 0.577399
+-0.008316 0.002150 -0.020675 0.670180 0.448525
+-0.010664 0.002390 -0.018678 0.676647 0.483901
+-0.021013 -0.010291 0.007951 0.0 0.0
+-0.014146 -0.001754 -0.047689 0.871625 0.380651
+-0.008422 -0.002312 -0.044596 0.811809 0.335078
+-0.015684 0.000035 -0.039467 0.823954 0.423325
+-0.034331 -0.001228 -0.022520 0.867926 0.743772
+-0.031662 -0.007124 -0.023091 0.0 0.0
+-0.031224 -0.014703 0.002956 0.0 0.0
+-0.022628 -0.011995 -0.002126 0.0 0.0
+-0.025924 -0.004114 -0.023568 0.0 0.0
+0.027872 -0.000761 0.013734 0.287809 0.354222
+-0.034591 -0.000064 -0.031338 0.914569 0.689654
+-0.029145 0.000494 -0.024321 0.845319 0.655410
+-0.028017 -0.005001 -0.028032 0.0 0.0
+0.027902 0.001681 0.011326 0.276857 0.326315
+0.025087 -0.001165 0.011230 0.300071 0.358218
+-0.021752 0.000419 -0.042010 0.892791 0.514352
+-0.026528 0.000997 -0.038365 0.897612 0.549181
+-0.024140 -0.004708 -0.040188 0.0 0.0
+-0.020978 0.001288 -0.033343 0.828966 0.502825
+-0.040551 -0.026143 -0.010199 0.0 0.0
+-0.023197 -0.010713 -0.012877 0.0 0.0
+-0.040551 -0.012883 -0.010199 0.0 0.0
+0.004485 -0.000007 -0.025484 0.601465 0.304044
+0.010626 0.000849 0.002651 0.410322 0.457472
+-0.028255 0.000295 -0.019377 0.812070 0.694379
+0.013332 0.003514 -0.005048 0.413228 0.317771
+0.012798 0.000333 -0.005951 0.0 0.0
+-0.015803 0.002187 -0.014369 0.700775 0.636061
+-0.013341 -0.003209 -0.016395 0.673887 0.556538
+0.004675 -0.002400 -0.004481 0.483440 0.456956
+0.002779 0.000374 -0.003048 0.492235 0.501557
+0.026117 -0.007505 -0.017965 0.0 0.0
+0.028859 -0.001994 -0.015040 0.401776 0.121209
+-0.018952 -0.009957 -0.016014 0.0 0.0
+-0.017184 -0.003861 -0.014796 0.0 0.0
+0.020301 0.003439 -0.001538 0.351704 0.269778
+0.046716 -0.013576 0.023724 0.0 0.0
+0.055055 -0.021059 0.018567 0.0 0.0
+0.055055 -0.007800 0.018567 0.0 0.0
+-0.007396 0.002390 -0.007873 0.599971 0.601408
+0.001875 -0.002670 -0.006494 0.512456 0.471953
+0.038019 -0.008659 0.029998 0.0 0.0
+-0.008029 -0.002626 -0.003881 0.0 0.0
+-0.005500 -0.000144 -0.009081 0.582441 0.547991
+-0.016373 -0.003539 -0.018074 0.693376 0.563980
+-0.010554 -0.001307 0.012690 0.542110 0.813150
+0.049300 -0.002253 0.010554 0.156641 0.074718
+0.038759 -0.002166 0.014632 0.0 0.0
+0.005684 -0.003039 -0.035812 0.664057 0.231011
+0.038019 0.004600 0.029998 0.0 0.0
+0.038019 0.010016 0.029998 0.114875 0.333565
+-0.026286 -0.002491 -0.003954 0.729150 0.852638
+0.017414 0.003222 -0.004081 0.382513 0.280278
+0.044485 0.002423 0.015009 0.154850 0.173882
+0.040181 0.004389 0.016007 0.169553 0.218078
+-0.000017 0.000235 -0.005153 0.522576 0.519037
+-0.033970 -0.003806 -0.010656 0.813819 0.897478
+0.009082 0.001920 -0.014465 0.496261 0.303552
+0.017757 0.000902 -0.014705 0.442129 0.218874
+-0.030192 -0.001153 -0.014581 0.804471 0.778885
+0.025315 0.003889 0.003730 0.298239 0.254431
+0.024009 0.004399 0.004815 0.299684 0.276964
+-0.000443 0.002808 -0.014194 0.567019 0.401750
+0.005274 0.003135 -0.010676 0.501002 0.361565
+0.006826 0.002728 -0.011961 0.497165 0.333554
+0.008126 0.003261 -0.008819 0.469528 0.344355
+0.003879 -0.000228 -0.008173 0.508530 0.439124
+0.032342 0.006310 0.016942 0.206620 0.322798
+0.023610 0.001317 -0.009141 0.378376 0.199234
+0.028043 0.004252 0.006706 0.268835 0.252118
+0.028016 -0.000536 0.005381 0.0 0.0
+0.017259 0.006519 0.029649 0.274881 0.613415
+-0.032441 -0.018179 -0.033098 0.0 0.0
+0.022512 0.003646 0.000819 0.327614 0.262242
+0.027741 0.001325 0.014450 0.267345 0.359882
+0.028451 0.005830 0.015287 0.234742 0.367594
+-0.017863 -0.003577 0.009446 0.606336 0.873144
+0.021215 0.004148 0.001990 0.329253 0.284285
+0.025358 0.002875 -0.000357 0.319055 0.238054
+-0.025580 0.001252 -0.027579 0.834418 0.591281
+-0.032441 -0.004919 -0.033098 0.0 0.0
+-0.038228 -0.001462 -0.027638 0.921967 0.762041
+-0.019666 0.002058 -0.021535 0.760948 0.564549
+0.028581 -0.008715 0.037841 0.0 0.0
+0.010757 0.003389 -0.006914 0.440443 0.329599
+0.019618 0.002475 -0.006065 0.380279 0.255089
+-0.031224 -0.009287 0.002956 0.726653 0.983908
+-0.031224 -0.027962 0.002956 0.0 0.0
+0.034589 -0.002055 -0.008644 0.340553 0.106507
+-0.002831 -0.000029 -0.007070 0.552248 0.532828
+-0.005830 0.001763 -0.004361 0.571170 0.614866
+-0.038103 -0.007971 -0.006212 0.816889 0.989345
+0.031060 0.005869 0.013961 0.222658 0.302436
+-0.030292 0.001057 -0.034857 0.906371 0.609867
+-0.024162 -0.006173 0.006457 0.662433 0.924856
+-0.042999 -0.006964 -0.014187 0.888500 0.981682
+0.001834 0.002718 0.019362 0.427865 0.719550
+0.049505 -0.002359 0.021843 0.0 0.0
+0.011040 0.003784 0.006750 0.391457 0.513336
+-0.001834 0.002801 -0.003863 0.535555 0.568767
+0.001083 0.003102 -0.001764 0.503470 0.551983
+0.022848 0.008014 0.033651 0.219229 0.581941
+0.023375 -0.002185 -0.020891 0.468122 0.139723
+0.028581 0.009961 0.037841 0.135591 0.524833
+0.006652 0.003669 0.002024 0.444507 0.517603
+0.009164 0.003881 0.003676 0.417615 0.501784
+0.008207 0.003356 0.004901 0.421793 0.528731
+0.049505 0.003057 0.021843 0.084272 0.136500
+0.055055 -0.002384 0.018567 0.061146 0.038651
+0.001013 -0.000353 -0.009972 0.538504 0.457627
+0.029329 0.005348 0.010717 0.244768 0.281415
+-0.004550 0.002514 -0.005831 0.565049 0.584900
+-0.007071 0.002482 -0.017312 0.640794 0.454937
+0.046366 0.005522 0.023902 0.092927 0.186502
+-0.045621 -0.024839 -0.019740 0.0 0.0
+0.041488 0.008760 0.027307 0.105299 0.272334
+0.033206 0.010046 0.034162 0.125908 0.426442
+0.026563 0.004882 0.007661 0.273634 0.279815
+3 43 45 44
+3 65 67 66
+3 69 71 70
+3 99 101 100
+3 61 63 138
+3 65 143 142
+3 147 149 148
+3 154 156 155
+3 159 160 45
+3 168 48 169
+3 173 175 174
+3 43 44 176
+3 187 188 77
+3 67 196 195
+3 66 67 195
+3 150 179 152
+3 203 204 205
+3 206 208 207
+3 176 44 209
+3 232 233 234
+3 235 127 236
+3 48 49 1
+3 239 242 241
+3 253 143 65
+3 87 235 59
+3 152 179 177
+3 41 39 125
+3 179 272 271
+3 274 276 275
+3 283 277 284
+3 266 107 173
+3 288 289 291
+3 48 168 49
+3 208 262 207
+3 156 262 295
+3 297 169 284
+3 142 67 65
+3 188 298 75
+3 268 300 269
+3 300 0 269
+3 312 232 234
+3 1 49 0
+3 236 127 149
+3 233 63 61
+3 265 24 318
+3 319 264 236
+3 321 126 125
+3 79 222 323
+3 276 288 291
+3 107 71 69
+3 149 327 326
+3 269 63 233
+3 49 244 0
+3 77 331 330
+3 332 168 297
+3 268 269 233
+3 142 20 266
+3 325 54 55
+3 262 330 205
+3 0 244 63
+3 195 136 222
+3 336 338 337
+3 79 340 194
+3 246 174 265
+3 195 222 194
+3 26 138 349
+3 333 346 350
+3 351 333 127
+3 169 352 284
+3 325 55 308
+3 244 332 324
+3 241 242 277
+3 354 241 277
+3 39 271 155
+3 59 235 264
+3 169 355 238
+3 205 204 295
+3 20 108 107
+3 335 154 214
+3 87 341 358
+3 358 351 87
+3 107 69 173
+3 359 324 329
+3 41 177 39
+3 326 319 236
+3 100 173 174
+3 359 329 364
+3 214 154 155
+3 154 335 207
+3 283 354 277
+3 142 143 21
+3 289 366 291
+3 348 253 66
+3 138 367 349
+3 368 329 242
+3 361 305 341
+3 77 341 331
+3 232 268 233
+3 338 187 337
+3 174 369 100
+3 100 369 99
+3 43 265 174
+3 265 43 176
+3 127 333 342
+3 154 207 156
+3 196 200 344
+3 188 187 357
+3 188 75 77
+3 173 371 175
+3 340 348 194
+3 372 44 373
+3 147 340 79
+3 333 350 334
+3 126 41 125
+3 330 262 208
+3 291 366 352
+3 374 376 136
+3 351 346 333
+3 377 379 378
+3 331 375 205
+3 355 152 238
+3 380 381 326
+3 234 233 61
+3 367 324 359
+3 196 101 200
+3 342 128 127
+3 188 357 298
+3 379 371 378
+3 358 325 308
+3 383 148 128
+3 177 291 238
+3 138 324 367
+3 136 344 374
+3 173 101 266
+3 168 169 297
+3 149 326 236
+3 209 372 384
+3 108 71 107
+3 337 187 330
+3 149 127 128
+3 357 187 338
+3 373 45 160
+3 173 100 101
+3 265 318 246
+3 101 99 200
+3 21 108 20
+3 24 265 176
+3 55 346 308
+3 371 70 378
+3 152 177 238
+3 351 127 235
+3 168 244 49
+3 26 61 138
+3 305 375 331
+3 266 101 196
+3 156 295 125
+3 168 332 244
+3 308 346 351
+3 366 283 284
+3 329 332 242
+3 244 138 63
+3 177 179 39
+3 136 196 344
+3 361 87 59
+3 77 358 341
+3 373 44 45
+3 327 79 386
+3 207 262 156
+3 148 149 128
+3 55 350 346
+3 222 79 194
+3 332 297 277
+3 39 155 156
+3 147 148 365
+3 242 332 277
+3 187 77 330
+3 1 0 300
+3 372 209 44
+3 246 369 174
+3 26 349 318
+3 26 318 24
+3 332 329 324
+3 155 271 385
+3 156 125 39
+3 271 272 385
+3 364 329 368
+3 175 379 159
+3 348 66 194
+3 383 365 148
+3 136 389 222
+3 342 390 383
+3 326 327 386
+3 126 275 41
+3 323 222 389
+3 277 297 284
+3 142 266 67
+3 173 69 371
+3 75 298 54
+3 380 326 386
+3 381 319 326
+3 388 79 323
+3 308 351 358
+3 352 169 238
+3 386 79 388
+3 136 376 389
+3 239 368 242
+3 208 337 330
+3 358 75 325
+3 77 75 358
+3 253 65 66
+3 0 63 269
+3 361 341 87
+3 204 321 295
+3 266 20 107
+3 147 365 340
+3 43 174 175
+3 75 54 325
+3 128 342 383
+3 150 272 179
+3 266 196 67
+3 175 159 43
+3 312 363 384
+3 351 235 87
+3 355 150 152
+3 206 336 208
+3 214 155 385
+3 147 79 327
+3 208 336 337
+3 291 177 41
+3 363 234 61
+3 363 312 234
+3 371 69 70
+3 274 288 276
+3 159 377 160
+3 66 195 194
+3 330 331 205
+3 321 125 295
+3 342 334 390
+3 176 363 61
+3 26 176 61
+3 375 203 205
+3 379 175 371
+3 341 305 331
+3 291 41 276
+3 24 176 26
+3 262 205 295
+3 209 363 176
+3 276 41 275
+3 159 45 43
+3 335 206 207
+3 20 142 21
+3 366 284 352
+3 147 327 149
+3 39 179 271
+3 209 384 363
+3 379 377 159
+3 264 235 236
+3 333 334 342
+3 195 196 136
+3 244 324 138
+3 291 352 238
+3 1 2 3
+3 21 22 23
+3 47 48 3
+3 108 23 90
+3 48 1 3
+3 253 141 143
+3 268 270 2
+3 2 300 268
+3 97 232 312
+3 97 312 316
+3 23 22 167
+3 48 47 169
+3 6 90 258
+3 317 47 3
+3 193 4 216
+3 95 97 316
+3 4 193 296
+3 4 89 6
+3 282 347 93
+3 348 347 253
+3 316 250 248
+3 89 90 6
+3 372 373 303
+3 2 230 301
+3 143 141 260
+3 70 89 296
+3 377 378 296
+3 301 317 2
+3 282 141 347
+3 328 302 313
+3 141 253 347
+3 71 90 89
+3 328 384 372
+3 108 90 71
+3 328 303 302
+3 217 158 216
+3 373 160 158
+3 21 23 108
+3 378 70 296
+3 22 287 167
+3 312 362 316
+3 89 4 296
+3 143 260 21
+3 328 313 362
+3 1 300 2
+3 270 268 232
+3 22 260 287
+3 316 362 250
+3 84 90 23
+3 71 89 70
+3 141 282 287
+3 230 2 270
+3 193 216 158
+3 362 313 250
+3 270 232 97
+3 348 340 93
+3 84 258 90
+3 312 384 362
+3 377 296 193
+3 193 158 160
+3 373 158 303
+3 377 193 160
+3 317 3 2
+3 95 230 97
+3 230 270 97
+3 372 303 328
+3 217 303 158
+3 328 362 384
+3 248 95 316
+3 217 302 303
+3 84 23 167
+3 260 22 21
+3 260 141 287
+3 347 348 93
+3 54 56 55
+3 83 85 84
+3 56 54 109
+3 212 214 213
+3 216 218 217
+3 219 83 167
+3 230 213 231
+3 248 250 249
+3 56 220 55
+3 286 219 287
+3 85 220 258
+3 185 317 183
+3 150 183 272
+3 83 334 85
+3 336 339 338
+3 301 230 231
+3 95 248 212
+3 183 301 231
+3 109 216 4
+3 282 286 287
+3 169 47 355
+3 338 339 36
+3 335 214 212
+3 4 220 56
+3 339 313 302
+3 85 334 350
+3 47 317 185
+3 357 36 218
+3 382 206 335
+3 250 313 382
+3 334 83 219
+3 231 213 272
+3 218 36 217
+3 219 286 383
+3 220 4 6
+3 286 282 93
+3 183 317 301
+3 213 214 385
+3 167 83 84
+3 357 109 298
+3 36 357 338
+3 272 213 385
+3 55 85 350
+3 85 258 84
+3 336 382 339
+3 336 206 382
+3 249 250 382
+3 230 95 213
+3 47 185 355
+3 340 365 93
+3 248 249 212
+3 383 286 365
+3 219 383 390
+3 249 382 335
+3 218 216 109
+3 219 167 287
+3 286 93 365
+3 217 36 302
+3 54 298 109
+3 357 218 109
+3 185 150 355
+3 183 231 272
+3 36 339 302
+3 313 339 382
+3 334 219 390
+3 185 183 150
+3 85 55 220
+3 213 95 212
+3 258 220 6
+3 56 109 4
+3 335 212 249
+3 14 310 309
+4 0 1 2 3
+4 4 5 6 7
+4 8 9 10 11
+4 12 13 14 15
+4 16 17 18 19
+4 20 21 22 23
+4 24 25 26 27
+4 28 29 30 31
+4 32 33 34 35
+4 36 37 33 38
+4 39 40 41 42
+4 43 44 45 46
+4 47 3 48 49
+4 50 51 52 53
+4 54 55 56 57
+4 58 59 60 13
+4 61 62 63 64
+4 65 66 67 68
+4 69 70 71 72
+4 73 8 30 74
+4 75 76 77 78
+4 79 80 81 82
+4 83 84 85 86
+4 87 88 60 13
+4 89 7 90 72
+4 91 78 88 92
+4 93 82 94 17
+4 95 96 97 98
+4 99 100 101 102
+4 103 104 105 106
+4 107 108 23 90
+4 56 109 54 110
+4 111 7 112 113
+4 114 115 116 117
+4 91 118 110 119
+4 120 121 122 123
+4 124 125 126 123
+4 127 18 128 129
+4 130 131 132 133
+4 134 135 136 81
+4 105 27 137 74
+4 61 138 63 62
+4 122 139 140 123
+4 65 141 142 143
+4 47 144 130 133
+4 27 145 96 146
+4 147 148 149 82
+4 150 151 152 153
+4 154 155 156 157
+4 158 159 160 45
+4 161 162 163 164
+4 165 145 27 166
+4 167 12 84 86
+4 168 169 48 170
+4 5 171 166 172
+4 173 174 175 112
+4 43 176 44 46
+4 177 178 179 163
+4 180 181 182 139
+4 183 184 185 153
+4 8 186 157 11
+4 187 77 188 37
+4 173 116 112 72
+4 189 190 191 92
+4 192 186 125 42
+4 159 158 193 46
+4 79 81 194 82
+4 67 195 196 197
+4 122 198 140 182
+4 66 195 67 199
+4 150 152 179 153
+4 200 50 99 102
+4 12 201 52 53
+4 15 14 58 13
+4 202 120 140 123
+4 191 203 204 205
+4 152 151 163 153
+4 206 207 208 10
+4 176 209 44 210
+4 137 35 211 74
+4 212 213 214 215
+4 216 217 218 119
+4 219 167 83 86
+4 118 220 221 56
+4 195 199 222 197
+4 174 223 224 225
+4 137 226 165 27
+4 47 132 130 227
+4 88 228 221 86
+4 131 170 132 133
+4 106 31 137 229
+4 230 231 213 29
+4 232 233 97 234
+4 235 236 127 18
+4 48 3 1 49
+4 214 154 212 157
+4 130 237 182 161
+4 114 68 141 117
+4 125 123 192 42
+4 238 181 177 144
+4 239 240 241 242
+4 103 243 244 245
+4 193 111 159 46
+4 246 224 25 225
+4 184 161 247 151
+4 248 249 250 251
+4 94 199 93 252
+4 253 141 65 143
+4 254 255 240 256
+4 257 210 176 46
+4 258 221 12 259
+4 22 115 260 117
+4 260 115 114 117
+4 94 82 81 17
+4 96 146 145 98
+4 178 163 261 153
+4 262 263 208 10
+4 264 13 58 18
+4 24 265 25 225
+4 87 235 60 59
+4 152 177 179 163
+4 116 101 102 266
+4 34 35 33 38
+4 24 257 176 225
+4 41 125 39 42
+4 261 164 40 229
+4 94 17 16 19
+4 267 81 79 80
+4 268 269 2 270
+4 95 74 248 98
+4 179 271 272 178
+4 178 184 261 29
+4 25 27 273 62
+4 274 121 275 276
+4 277 256 242 278
+4 273 226 279 280
+4 210 145 250 166
+4 181 133 47 281
+4 282 17 94 19
+4 56 55 220 57
+4 283 284 277 285
+4 182 162 161 164
+4 286 287 219 129
+4 266 173 107 116
+4 174 224 246 225
+4 282 18 93 17
+4 51 114 94 19
+4 288 289 290 291
+4 223 257 165 225
+4 205 34 191 92
+4 47 48 168 49
+4 292 293 256 294
+4 208 207 262 10
+4 156 295 262 11
+4 149 80 147 82
+4 223 46 257 225
+4 296 111 193 113
+4 165 257 223 5
+4 297 284 169 281
+4 117 142 67 65
+4 188 75 298 299
+4 85 258 220 221
+4 170 133 180 281
+4 178 215 30 42
+4 270 104 230 64
+4 2 268 300 269
+4 184 106 301 227
+4 302 210 172 303
+4 106 237 261 229
+4 221 118 91 259
+4 300 0 2 269
+4 304 88 305 78
+4 182 131 198 306
+4 3 307 49 245
+4 55 308 220 57
+4 132 243 103 245
+4 14 309 310 311
+4 145 74 96 98
+4 178 29 183 153
+4 44 210 303 172
+4 97 312 232 234
+4 302 166 313 38
+4 1 49 3 0
+4 174 111 223 225
+4 94 81 314 17
+4 73 31 30 229
+4 177 139 291 123
+4 140 198 137 106
+4 211 192 73 186
+4 236 149 127 18
+4 100 224 174 112
+4 176 210 24 315
+4 261 29 184 237
+4 269 104 270 64
+4 292 294 280 293
+4 97 316 312 234
+4 250 145 248 251
+4 23 167 22 115
+4 135 51 50 197
+4 61 315 26 62
+4 48 169 47 170
+4 234 315 61 64
+4 178 229 40 42
+4 137 145 165 35
+4 233 61 63 64
+4 185 183 317 184
+4 46 113 5 172
+4 177 181 291 139
+4 6 258 90 7
+4 135 81 51 197
+4 265 24 25 318
+4 319 236 264 320
+4 181 162 177 144
+4 321 126 124 125
+4 8 10 263 11
+4 322 79 222 323
+4 244 103 324 243
+4 54 110 325 57
+4 73 34 211 186
+4 114 116 53 102
+4 189 211 191 192
+4 51 68 94 252
+4 317 3 47 132
+4 290 276 288 291
+4 282 94 93 252
+4 106 227 237 306
+4 91 299 110 76
+4 261 247 184 161
+4 294 292 280 279
+4 179 178 272 153
+4 150 272 183 153
+4 193 216 4 113
+4 107 90 69 71
+4 12 53 84 7
+4 33 171 302 38
+4 198 293 137 306
+4 149 326 327 320
+4 269 233 63 64
+4 49 0 244 245
+4 6 221 258 259
+4 166 171 35 38
+4 47 247 317 227
+4 291 139 276 123
+4 44 209 328 210
+4 168 170 47 245
+4 191 190 304 92
+4 181 285 291 139
+4 329 243 293 256
+4 95 316 97 96
+4 96 62 315 64
+4 60 88 15 13
+4 77 330 331 76
+4 293 103 105 306
+4 182 181 180 133
+4 137 27 165 145
+4 50 197 51 102
+4 332 297 168 170
+4 106 29 301 31
+4 34 76 33 92
+4 40 162 140 164
+4 320 80 149 17
+4 137 106 198 306
+4 30 157 73 42
+4 140 182 198 306
+4 268 233 269 64
+4 106 307 301 227
+4 142 266 20 117
+4 83 333 85 334
+4 47 170 169 281
+4 313 166 35 38
+4 325 55 54 57
+4 30 8 73 157
+4 262 205 330 263
+4 0 63 244 103
+4 125 186 156 42
+4 3 132 317 307
+4 12 112 53 7
+4 250 35 313 166
+4 94 114 51 252
+4 195 222 136 197
+4 335 10 249 11
+4 18 17 129 19
+4 111 46 223 225
+4 336 337 338 339
+4 266 68 101 102
+4 156 186 157 42
+4 13 86 12 19
+4 18 129 16 19
+4 294 255 254 256
+4 293 243 180 256
+4 137 73 140 229
+4 4 296 193 113
+4 79 194 340 82
+4 295 186 262 11
+4 305 78 341 92
+4 157 9 8 11
+4 342 286 219 343
+4 140 306 106 229
+4 196 135 344 197
+4 93 199 94 82
+4 198 182 345 180
+4 329 293 294 256
+4 5 46 111 113
+4 163 161 261 153
+4 242 254 240 256
+4 293 131 180 170
+4 216 259 4 113
+4 346 333 85 228
+4 207 10 335 11
+4 40 164 140 229
+4 246 265 174 225
+4 195 194 222 199
+4 66 347 348 199
+4 26 349 138 62
+4 333 346 85 350
+4 301 231 230 29
+4 347 199 68 252
+4 351 127 333 343
+4 95 212 248 9
+4 140 164 182 229
+4 73 192 40 42
+4 240 242 239 254
+4 114 68 51 252
+4 213 29 231 215
+4 248 145 250 146
+4 182 237 261 161
+4 52 51 12 53
+4 2 270 269 104
+4 258 6 5 7
+4 184 151 185 153
+4 81 199 94 197
+4 219 129 287 86
+4 97 96 316 64
+4 248 74 95 9
+4 183 231 301 29
+4 169 284 352 281
+4 314 80 320 17
+4 104 64 96 98
+4 217 5 216 172
+4 286 343 127 129
+4 292 198 255 256
+4 33 37 36 119
+4 121 276 290 139
+4 290 285 353 139
+4 130 131 237 227
+4 349 273 138 62
+4 169 181 238 144
+4 325 308 55 57
+4 33 76 37 119
+4 152 163 179 153
+4 111 46 193 113
+4 8 263 34 186
+4 244 324 332 243
+4 104 106 103 307
+4 109 4 216 118
+4 295 192 204 186
+4 241 242 240 277
+4 240 354 241 277
+4 204 192 191 186
+4 39 155 271 215
+4 36 37 218 119
+4 110 118 91 57
+4 353 122 290 139
+4 88 78 304 92
+4 282 287 286 129
+4 303 158 45 46
+4 314 322 267 81
+4 201 223 165 225
+4 230 31 28 98
+4 84 12 258 86
+4 28 29 213 215
+4 59 264 235 18
+4 4 6 89 7
+4 132 170 47 133
+4 51 197 68 102
+4 169 355 47 238
+4 141 68 114 252
+4 356 314 52 51
+4 35 251 8 74
+4 93 18 282 129
+4 218 37 357 299
+4 191 205 204 295
+4 20 23 107 108
+4 149 18 236 320
+4 39 215 178 42
+4 267 309 314 320
+4 47 181 169 144
+4 47 170 132 245
+4 338 337 36 339
+4 216 5 217 259
+4 335 154 212 214
+4 212 157 249 9
+4 173 112 175 72
+4 20 116 22 117
+4 87 358 341 57
+4 2 269 0 104
+4 358 87 351 57
+4 107 173 69 72
+4 217 113 158 172
+4 25 226 273 27
+4 359 329 324 280
+4 41 39 177 40
+4 360 226 137 280
+4 112 7 5 113
+4 4 220 118 56
+4 60 87 361 88
+4 257 5 165 166
+4 224 50 52 53
+4 326 236 319 320
+4 249 251 248 9
+4 218 299 109 118
+4 305 304 361 88
+4 47 133 170 281
+4 100 174 173 112
+4 121 139 122 123
+4 198 293 292 280
+4 316 362 363 146
+4 32 34 211 35
+4 226 280 273 62
+4 294 359 329 364
+4 85 228 83 86
+4 244 105 324 103
+4 40 178 177 163
+4 214 155 154 157
+4 161 151 184 153
+4 169 181 47 281
+4 302 33 36 171
+4 154 207 335 11
+4 302 171 217 172
+4 20 23 22 116
+4 293 103 324 280
+4 283 277 354 285
+4 255 180 345 278
+4 339 302 313 38
+4 154 249 212 157
+4 27 145 210 166
+4 365 93 148 82
+4 28 31 74 98
+4 282 93 347 252
+4 121 275 276 123
+4 261 178 40 164
+4 53 112 173 116
+4 142 21 143 117
+4 81 82 80 17
+4 289 291 366 285
+4 140 137 211 73
+4 360 137 198 280
+4 348 66 253 347
+4 137 27 105 280
+4 101 68 196 102
+4 138 349 367 273
+4 316 248 250 146
+4 51 81 94 197
+4 53 116 100 102
+4 140 73 211 192
+4 368 242 329 256
+4 63 104 269 64
+4 361 341 305 88
+4 182 164 237 229
+4 337 37 339 263
+4 180 243 293 170
+4 77 331 341 78
+4 140 162 182 164
+4 154 157 156 11
+4 44 46 210 172
+4 232 233 268 64
+4 263 10 262 11
+4 51 68 114 102
+4 363 315 316 146
+4 66 68 347 199
+4 140 162 40 123
+4 338 337 187 37
+4 327 320 326 80
+4 174 224 100 369
+4 100 224 99 369
+4 339 37 36 38
+4 43 111 174 265
+4 250 145 210 146
+4 265 176 43 225
+4 180 281 285 278
+4 127 342 333 343
+4 154 156 207 11
+4 69 71 90 72
+4 196 344 200 50
+4 209 210 176 315
+4 140 40 73 192
+4 165 5 12 259
+4 223 112 12 5
+4 220 118 221 6
+4 188 357 187 37
+4 188 77 75 299
+4 304 60 361 88
+4 220 228 221 57
+4 272 178 231 153
+4 182 161 261 164
+4 327 326 267 80
+4 319 58 370 320
+4 58 309 14 311
+4 211 34 73 35
+4 259 171 217 119
+4 238 163 152 144
+4 173 175 371 72
+4 12 53 51 19
+4 89 6 90 7
+4 83 343 219 86
+4 159 111 43 46
+4 182 180 198 131
+4 340 194 348 199
+4 177 162 181 139
+4 88 13 87 228
+4 303 46 44 172
+4 137 106 105 31
+4 69 90 107 72
+4 74 31 105 98
+4 372 303 373 44
+4 329 293 324 280
+4 258 12 84 7
+4 224 53 100 102
+4 47 238 355 144
+4 47 49 168 245
+4 314 51 356 81
+4 304 78 305 92
+4 136 134 374 135
+4 141 114 287 19
+4 165 27 226 257
+4 2 301 230 104
+4 90 7 23 72
+4 27 145 137 74
+4 263 186 8 11
+4 147 79 340 82
+4 359 273 279 280
+4 375 304 305 92
+4 5 7 4 113
+4 223 53 12 112
+4 137 105 293 280
+4 333 85 334 350
+4 148 93 149 17
+4 137 31 73 229
+4 34 33 32 92
+4 91 118 221 57
+4 126 125 41 123
+4 143 260 141 117
+4 330 208 262 263
+4 370 309 267 320
+4 47 185 317 247
+4 83 228 343 86
+4 304 203 191 92
+4 317 307 132 227
+4 30 29 178 229
+4 291 352 366 285
+4 91 57 221 78
+4 184 247 261 237
+4 374 134 136 376
+4 294 254 368 256
+4 371 70 89 296
+4 261 237 247 161
+4 357 218 36 37
+4 347 68 141 252
+4 59 13 264 18
+4 351 333 346 228
+4 124 120 202 123
+4 231 178 272 215
+4 316 315 96 146
+4 36 339 337 37
+4 152 144 163 151
+4 377 296 378 379
+4 255 198 345 180
+4 251 74 248 9
+4 324 103 105 280
+4 88 57 341 78
+4 354 277 240 278
+4 13 228 88 86
+4 192 123 40 42
+4 105 104 96 98
+4 100 53 173 116
+4 331 205 375 92
+4 352 181 169 281
+4 240 255 345 278
+4 355 238 152 144
+4 370 380 381 326
+4 234 61 233 64
+4 105 62 96 64
+4 30 229 178 42
+4 67 117 68 266
+4 63 103 0 104
+4 93 199 347 252
+4 301 2 317 307
+4 132 227 103 306
+4 110 57 91 78
+4 367 359 324 280
+4 293 243 132 170
+4 258 221 85 228
+4 332 256 297 170
+4 382 335 206 10
+4 250 382 313 38
+4 178 29 30 215
+4 333 334 83 219
+4 65 141 253 68
+4 53 116 114 115
+4 97 234 233 64
+4 237 164 261 229
+4 282 347 141 252
+4 326 320 267 80
+4 341 88 87 57
+4 210 328 302 313
+4 169 238 47 144
+4 34 263 8 35
+4 32 88 190 92
+4 40 162 177 123
+4 290 276 291 139
+4 231 272 213 215
+4 25 201 226 225
+4 196 200 101 50
+4 96 315 27 146
+4 342 286 127 128
+4 4 118 6 259
+4 317 247 184 227
+4 12 13 88 86
+4 188 298 357 299
+4 141 347 253 68
+4 290 274 276 121
+4 379 378 371 296
+4 63 62 105 64
+4 110 118 299 119
+4 166 171 302 172
+4 290 289 353 285
+4 358 308 325 57
+4 103 106 105 306
+4 193 46 158 113
+4 165 201 12 223
+4 383 148 286 128
+4 106 29 261 237
+4 12 5 112 7
+4 67 199 195 197
+4 177 238 291 181
+4 58 13 16 18
+4 138 367 324 280
+4 26 315 27 62
+4 37 263 330 76
+4 136 374 344 135
+4 116 173 101 266
+4 237 131 182 306
+4 35 251 250 38
+4 77 76 331 78
+4 168 297 169 170
+4 149 236 326 320
+4 71 89 90 72
+4 105 103 293 280
+4 209 328 384 372
+4 279 226 360 280
+4 354 285 277 278
+4 87 228 351 57
+4 68 199 94 252
+4 93 18 149 17
+4 368 254 242 256
+4 108 90 107 71
+4 196 50 101 102
+4 337 330 187 37
+4 328 210 302 303
+4 265 111 174 225
+4 341 78 331 92
+4 217 216 158 113
+4 345 285 180 139
+4 149 128 127 18
+4 226 257 27 225
+4 249 10 382 251
+4 105 106 137 306
+4 357 338 187 37
+4 130 144 47 151
+4 249 9 157 11
+4 373 158 160 45
+4 10 9 249 11
+4 114 115 287 19
+4 222 322 136 81
+4 173 116 101 100
+4 132 131 293 170
+4 165 223 12 5
+4 331 78 76 92
+4 345 182 122 139
+4 261 29 106 229
+4 265 25 246 318
+4 88 221 12 86
+4 101 200 99 102
+4 145 146 248 74
+4 32 33 91 92
+4 211 35 73 74
+4 177 162 163 144
+4 157 186 156 11
+4 218 217 36 119
+4 287 19 86 129
+4 145 35 137 74
+4 63 105 244 103
+4 6 118 221 259
+4 21 23 20 108
+4 321 125 124 192
+4 24 176 265 225
+4 283 354 353 285
+4 149 93 148 18
+4 165 166 5 171
+4 73 35 8 74
+4 25 27 24 225
+4 55 346 85 308
+4 36 171 33 119
+4 217 171 36 119
+4 103 307 132 245
+4 219 342 383 286
+4 371 378 70 296
+4 316 315 234 64
+4 152 238 177 163
+4 168 243 332 170
+4 138 280 105 62
+4 34 190 92 32
+4 223 5 257 172
+4 33 35 32 171
+4 18 343 13 129
+4 351 235 127 343
+4 13 12 14 16
+4 371 296 89 72
+4 97 233 232 64
+4 220 4 118 6
+4 16 14 58 309
+4 5 113 216 172
+4 73 229 30 42
+4 168 49 244 245
+4 26 138 61 62
+4 96 145 27 74
+4 210 145 27 146
+4 80 82 149 17
+4 305 331 375 92
+4 346 85 308 228
+4 0 307 103 245
+4 230 64 104 98
+4 266 196 101 68
+4 333 343 83 228
+4 226 27 25 225
+4 286 93 282 129
+4 116 53 84 115
+4 161 144 130 151
+4 6 258 5 259
+4 198 131 293 306
+4 50 224 99 102
+4 162 139 177 123
+4 220 221 56 57
+4 181 285 180 281
+4 168 48 47 170
+4 356 51 135 81
+4 103 227 106 306
+4 147 80 79 82
+4 76 78 91 92
+4 221 57 88 78
+4 156 125 295 186
+4 224 201 25 225
+4 183 301 317 184
+4 173 53 100 112
+4 226 201 165 225
+4 58 319 264 320
+4 250 35 145 251
+4 198 180 255 256
+4 230 104 301 31
+4 184 29 106 237
+4 208 263 337 10
+4 168 244 332 243
+4 236 18 264 320
+4 182 306 140 229
+4 283 353 366 285
+4 182 237 130 131
+4 284 285 352 281
+4 178 163 40 164
+4 181 144 47 133
+4 266 117 68 102
+4 308 351 346 228
+4 213 28 230 29
+4 261 237 182 164
+4 122 182 140 139
+4 22 167 287 115
+4 213 385 214 215
+4 267 380 370 326
+4 379 296 371 72
+4 297 281 170 278
+4 366 285 284 283
+4 210 46 257 172
+4 218 118 216 119
+4 146 74 145 98
+4 312 363 316 362
+4 260 114 141 117
+4 141 114 260 115
+4 329 242 332 256
+4 105 244 138 63
+4 262 186 263 11
+4 177 39 179 178
+4 94 199 68 197
+4 325 110 358 57
+4 167 84 83 86
+4 163 162 40 164
+4 136 344 196 135
+4 132 307 3 245
+4 263 251 382 10
+4 361 87 60 59
+4 60 235 87 13
+4 375 203 304 92
+4 321 124 191 192
+4 267 320 314 80
+4 47 132 3 245
+4 222 199 81 197
+4 176 210 44 46
+4 177 163 238 144
+4 299 357 109 298
+4 110 76 75 78
+4 77 341 358 78
+4 357 37 188 299
+4 224 223 201 225
+4 91 171 259 119
+4 34 263 33 76
+4 157 28 8 9
+4 36 338 357 37
+4 344 135 50 197
+4 271 178 39 215
+4 373 45 44 303
+4 105 31 104 98
+4 58 309 370 320
+4 250 210 362 146
+4 267 327 79 386
+4 211 34 190 189
+4 293 131 132 306
+4 8 35 263 251
+4 207 156 262 11
+4 40 192 140 123
+4 189 124 211 192
+4 105 104 63 64
+4 264 18 58 320
+4 272 385 213 215
+4 142 143 141 117
+4 205 295 191 186
+4 50 53 224 102
+4 148 128 149 18
+4 99 224 100 102
+4 261 184 178 153
+4 180 285 345 278
+4 4 7 89 113
+4 231 178 183 153
+4 55 346 350 85
+4 104 31 230 98
+4 222 194 79 81
+4 269 270 268 64
+4 258 5 12 7
+4 110 299 75 76
+4 5 166 257 172
+4 3 49 47 245
+4 93 94 282 17
+4 16 13 12 19
+4 292 255 294 256
+4 27 210 257 166
+4 337 263 336 10
+4 16 320 18 17
+4 332 277 297 256
+4 105 106 104 31
+4 41 40 177 123
+4 39 156 155 157
+4 161 162 182 144
+4 16 18 13 129
+4 131 227 132 306
+4 73 8 34 186
+4 89 296 4 113
+4 210 315 363 146
+4 132 307 103 227
+4 147 82 365 148
+4 339 263 382 10
+4 32 211 165 35
+4 345 180 182 139
+4 289 285 290 139
+4 106 306 237 229
+4 104 106 301 31
+4 242 277 332 256
+4 85 84 258 86
+4 103 132 293 243
+4 224 223 174 112
+4 143 21 260 117
+4 109 110 56 118
+4 336 339 382 10
+4 187 330 77 37
+4 328 362 313 210
+4 196 68 67 197
+4 58 311 370 309
+4 298 110 109 299
+4 1 0 2 300
+4 336 382 206 10
+4 372 44 209 328
+4 137 74 73 31
+4 272 178 271 215
+4 293 105 137 306
+4 145 35 250 166
+4 26 27 25 62
+4 301 106 184 29
+4 240 277 242 278
+4 263 251 35 38
+4 34 211 190 32
+4 249 382 250 251
+4 176 46 43 225
+4 294 293 329 280
+4 148 18 286 128
+4 39 178 40 42
+4 122 121 290 139
+4 261 161 163 164
+4 230 213 95 28
+4 12 223 201 53
+4 36 337 338 37
+4 75 299 77 76
+4 363 210 209 315
+4 216 118 4 259
+4 379 193 296 111
+4 163 144 161 151
+4 270 232 268 64
+4 335 249 154 11
+4 249 157 154 11
+4 130 237 247 227
+4 230 29 28 31
+4 294 255 292 387
+4 285 281 284 278
+4 263 10 8 251
+4 287 114 141 115
+4 282 252 141 19
+4 246 224 174 369
+4 47 247 130 151
+4 26 349 25 318
+4 26 25 24 318
+4 316 234 97 64
+4 182 162 181 144
+4 243 170 168 245
+4 279 329 359 280
+4 286 18 93 129
+4 84 12 167 115
+4 8 28 30 74
+4 32 15 190 88
+4 5 259 165 171
+4 22 287 260 115
+4 190 34 92 189
+4 221 228 88 57
+4 23 116 107 72
+4 353 354 240 278
+4 138 273 367 280
+4 222 81 136 197
+4 258 228 85 86
+4 107 116 173 72
+4 27 257 165 166
+4 254 255 294 387
+4 332 324 329 243
+4 230 28 95 98
+4 244 243 168 245
+4 47 355 185 151
+4 316 250 362 146
+4 40 163 177 162
+4 155 385 271 215
+4 180 256 243 170
+4 156 39 125 42
+4 248 146 95 98
+4 271 385 272 215
+4 84 23 90 7
+4 221 118 56 57
+4 364 368 329 294
+4 8 157 30 28
+4 175 159 379 111
+4 136 81 135 197
+4 204 321 191 192
+4 243 256 332 170
+4 217 259 5 171
+4 340 93 365 82
+4 180 285 181 139
+4 71 70 89 72
+4 84 53 12 115
+4 222 79 322 81
+4 58 14 16 13
+4 141 287 282 19
+4 303 45 44 46
+4 230 270 2 104
+4 33 263 37 76
+4 100 53 224 112
+4 112 116 53 7
+4 2 0 3 307
+4 105 280 27 62
+4 319 370 311 381
+4 35 171 33 38
+4 216 259 217 119
+4 337 339 336 263
+4 33 76 91 92
+4 188 37 77 299
+4 132 170 243 245
+4 248 212 249 9
+4 140 120 122 123
+4 181 162 182 139
+4 382 263 339 38
+4 22 116 23 115
+4 267 79 322 388
+4 41 123 125 42
+4 308 85 220 228
+4 284 281 297 278
+4 279 360 292 280
+4 175 112 174 111
+4 348 194 66 199
+4 18 320 149 17
+4 165 257 226 225
+4 77 299 37 76
+4 137 293 198 280
+4 51 53 50 102
+4 351 228 308 57
+4 32 165 12 259
+4 101 50 200 102
+4 184 29 178 153
+4 49 307 0 245
+4 97 64 230 98
+4 211 34 191 186
+4 282 129 18 17
+4 29 31 106 229
+4 136 135 196 197
+4 111 43 225 265
+4 205 263 34 76
+4 79 327 267 80
+4 291 181 352 285
+4 40 123 41 42
+4 67 68 66 199
+4 34 8 73 35
+4 352 285 181 281
+4 40 178 261 229
+4 43 111 225 46
+4 329 294 368 256
+4 383 365 286 148
+4 103 307 106 227
+4 193 158 216 113
+4 191 34 189 92
+4 293 132 103 306
+4 358 57 110 78
+4 140 106 137 229
+4 30 31 29 229
+4 301 106 104 307
+4 358 110 75 78
+4 52 201 224 53
+4 136 322 222 389
+4 342 219 383 390
+4 277 285 284 278
+4 16 51 314 94
+4 267 326 327 386
+4 23 7 116 72
+4 94 252 282 19
+4 356 135 134 81
+4 130 144 182 133
+4 126 41 275 123
+4 112 5 111 113
+4 134 322 136 389
+4 261 163 178 164
+4 203 205 191 92
+4 296 113 89 72
+4 53 114 51 19
+4 322 323 222 389
+4 257 166 210 172
+4 297 170 256 278
+4 105 74 137 31
+4 343 129 219 86
+4 0 103 244 245
+4 301 29 230 31
+4 88 228 87 57
+4 277 284 297 278
+4 81 199 194 82
+4 125 192 295 186
+4 142 117 67 266
+4 91 76 33 119
+4 173 371 69 72
+4 75 54 298 110
+4 166 302 210 172
+4 351 343 333 228
+4 183 29 184 153
+4 25 265 246 225
+4 324 105 138 280
+4 331 76 205 92
+4 53 116 84 7
+4 249 335 382 10
+4 380 326 267 386
+4 209 362 328 210
+4 370 381 319 326
+4 91 76 110 78
+4 218 109 216 118
+4 322 388 79 323
+4 13 343 87 228
+4 15 88 12 13
+4 353 285 354 278
+4 219 287 167 86
+4 240 256 255 278
+4 262 10 207 11
+4 308 358 351 57
+4 32 91 88 92
+4 170 281 180 278
+4 158 46 303 172
+4 352 238 169 181
+4 191 34 205 186
+4 27 96 105 62
+4 191 295 204 186
+4 267 386 79 388
+4 322 79 267 81
+4 73 74 30 31
+4 367 273 359 280
+4 324 103 293 243
+4 114 252 94 19
+4 136 376 134 389
+4 315 62 61 64
+4 8 74 251 9
+4 261 161 184 153
+4 198 180 293 131
+4 326 319 370 320
+4 214 157 212 215
+4 333 83 85 228
+4 33 171 91 119
+4 85 221 220 228
+4 159 193 379 111
+4 73 186 192 42
+4 353 289 366 285
+4 286 148 365 93
+4 10 251 249 9
+4 169 170 297 281
+4 95 146 96 98
+4 182 144 181 133
+4 182 131 130 133
+4 129 86 13 19
+4 141 252 114 19
+4 130 132 47 133
+4 148 82 93 17
+4 127 128 286 129
+4 128 18 286 129
+4 12 51 16 19
+4 314 320 16 17
+4 12 86 167 19
+4 140 192 124 123
+4 8 251 10 9
+4 239 242 368 254
+4 333 219 83 343
+4 217 302 36 171
+4 13 18 235 343
+4 208 330 337 263
+4 257 46 176 225
+4 118 259 216 119
+4 12 5 258 259
+4 304 190 60 88
+4 358 325 75 110
+4 116 7 112 72
+4 23 116 84 115
+4 32 35 165 171
+4 135 52 50 51
+4 51 114 53 102
+4 77 358 75 78
+4 27 315 96 62
+4 326 370 267 320
+4 253 66 65 68
+4 255 256 180 278
+4 209 363 362 210
+4 212 28 157 9
+4 16 129 13 19
+4 32 91 171 259
+4 149 320 327 80
+4 263 35 34 38
+4 0 269 63 104
+4 355 144 152 151
+4 361 87 341 88
+4 75 110 298 299
+4 290 291 289 139
+4 204 295 321 192
+4 297 256 277 278
+4 256 170 180 278
+4 223 111 112 5
+4 266 107 20 116
+4 28 74 95 98
+4 141 65 142 117
+4 362 250 313 210
+4 177 40 39 178
+4 211 137 165 35
+4 82 147 365 340
+4 191 211 189 34
+4 190 15 60 88
+4 111 43 174 175
+4 89 70 371 72
+4 163 151 161 153
+4 37 76 299 119
+4 330 37 337 263
+4 13 129 343 86
+4 54 109 298 110
+4 75 325 54 110
+4 356 52 135 51
+4 180 170 131 133
+4 273 27 226 62
+4 127 343 18 129
+4 27 257 24 225
+4 35 166 165 171
+4 52 314 16 51
+4 39 157 155 215
+4 345 285 353 278
+4 12 52 16 51
+4 198 106 140 306
+4 12 221 258 86
+4 251 263 382 38
+4 308 228 220 57
+4 286 128 342 383
+4 117 67 68 65
+4 73 157 8 186
+4 28 74 8 9
+4 3 0 49 307
+4 68 197 196 102
+4 293 292 256 198
+4 357 299 109 218
+4 179 163 178 153
+4 150 179 272 153
+4 257 46 223 172
+4 176 257 24 210
+4 266 67 196 68
+4 175 43 159 111
+4 235 13 59 18
+4 270 97 232 64
+4 124 121 120 123
+4 299 76 91 119
+4 348 93 340 199
+4 84 90 258 7
+4 91 259 118 119
+4 185 355 150 151
+4 313 35 250 38
+4 37 263 33 38
+4 301 184 183 29
+4 339 263 37 38
+4 231 29 178 215
+4 136 322 134 81
+4 312 362 384 363
+4 293 180 198 256
+4 68 117 114 102
+4 24 210 27 315
+4 158 113 46 172
+4 12 115 53 19
+4 53 115 114 19
+4 30 74 28 31
+4 185 184 317 247
+4 351 87 235 343
+4 124 192 125 123
+4 355 152 150 151
+4 301 104 2 307
+4 12 221 88 259
+4 212 28 213 215
+4 345 198 122 182
+4 211 73 137 74
+4 292 360 198 280
+4 138 105 63 62
+4 219 343 286 129
+4 206 208 336 10
+4 4 259 5 113
+4 377 296 379 193
+4 214 385 155 215
+4 12 15 32 88
+4 147 327 79 80
+4 217 171 5 172
+4 208 337 336 10
+4 183 272 231 153
+4 291 41 177 123
+4 73 40 140 229
+4 363 61 234 315
+4 36 302 339 38
+4 89 113 7 72
+4 30 215 157 42
+4 194 199 340 82
+4 193 160 158 159
+4 91 32 171 33
+4 60 59 235 13
+4 273 280 138 62
+4 363 234 312 316
+4 24 257 27 210
+4 235 18 127 343
+4 373 158 45 303
+4 16 309 58 320
+4 105 103 63 104
+4 287 115 167 19
+4 353 345 122 139
+4 247 161 130 151
+4 291 285 289 139
+4 313 210 250 166
+4 116 115 22 117
+4 0 104 103 307
+4 134 322 314 81
+4 248 251 145 74
+4 94 68 51 197
+4 60 15 58 13
+4 371 70 69 72
+4 248 74 146 98
+4 156 186 295 11
+4 330 263 205 76
+4 36 33 302 38
+4 18 148 286 93
+4 130 161 182 144
+4 94 199 81 82
+4 155 157 214 215
+4 129 17 282 19
+4 189 140 124 202
+4 290 274 288 276
+4 159 377 193 160
+4 182 162 140 139
+4 194 81 222 199
+4 22 21 20 117
+4 58 264 59 13
+4 379 111 296 72
+4 116 117 266 102
+4 66 194 195 199
+4 330 205 331 76
+4 16 94 314 17
+4 253 347 66 68
+4 185 247 47 151
+4 30 28 157 215
+4 2 104 0 307
+4 101 116 102 100
+4 88 91 32 259
+4 124 202 140 123
+4 84 116 23 7
+4 317 2 3 307
+4 321 295 125 192
+4 27 280 226 62
+4 313 382 339 38
+4 96 104 105 64
+4 342 334 219 390
+4 176 61 363 315
+4 26 61 176 315
+4 165 259 32 171
+4 47 144 355 151
+4 30 29 28 215
+4 95 97 230 98
+4 6 5 4 259
+4 157 186 73 42
+4 156 157 39 42
+4 149 82 148 17
+4 191 192 211 186
+4 185 151 150 153
+4 185 150 183 153
+4 96 315 316 64
+4 180 133 181 281
+4 230 97 270 64
+4 40 229 73 42
+4 157 28 212 215
+4 112 111 175 72
+4 88 221 91 259
+4 184 237 106 227
+4 353 285 345 139
+4 105 96 27 74
+4 301 307 317 227
+4 85 55 308 220
+4 77 37 330 76
+4 226 27 137 280
+4 247 237 130 161
+4 174 112 223 111
+4 126 275 124 123
+4 111 113 296 72
+4 329 279 294 280
+4 276 139 121 123
+4 12 88 32 259
+4 112 7 111 72
+4 110 299 91 119
+4 145 251 35 74
+4 332 243 329 256
+4 124 275 121 123
+4 375 205 203 92
+4 178 29 261 229
+4 379 175 72 371
+4 33 263 34 38
+4 56 110 54 57
+4 372 303 44 328
+4 302 166 210 313
+4 250 251 382 38
+4 341 331 305 92
+4 5 259 216 113
+4 68 199 67 197
+4 107 90 23 72
+4 7 113 111 72
+4 172 217 303 158
+4 184 247 185 151
+4 291 276 41 123
+4 141 68 65 117
+4 56 118 110 57
+4 24 26 176 315
+4 87 13 235 343
+4 262 295 205 186
+4 134 314 356 81
+4 279 294 359 329
+4 328 209 384 362
+4 165 35 145 166
+4 237 227 131 306
+4 24 27 26 315
+4 209 176 363 315
+4 27 315 210 146
+4 276 275 41 123
+4 266 116 20 117
+4 248 316 95 146
+4 340 199 93 82
+4 25 349 26 62
+4 96 74 105 98
+4 190 88 304 92
+4 58 18 16 320
+4 316 96 95 146
+4 159 43 45 46
+4 336 263 339 10
+4 217 172 303 302
+4 240 345 353 278
+4 213 212 95 28
+4 309 16 314 320
+4 84 167 23 115
+4 335 207 206 10
+4 159 45 158 46
+4 20 21 142 117
+4 314 94 51 81
+4 196 197 50 102
+4 285 366 284 352
+4 343 228 13 86
+4 34 263 205 186
+4 96 64 97 98
+4 260 21 22 117
+4 258 6 220 221
+4 333 342 219 343
+4 114 117 116 102
+4 180 131 182 133
+4 305 88 341 78
+4 56 4 109 118
+4 107 23 20 116
+4 247 237 184 227
+4 370 58 319 311
+4 147 149 327 80
+4 362 210 363 146
+4 260 287 141 115
+4 303 44 328 210
+4 335 212 154 249
+4 81 80 314 17
+4 39 271 179 178
+4 191 124 189 192
+4 209 362 363 384
+4 19 287 86 167
+4 91 221 88 78
+4 347 93 348 199
+4 205 263 262 186
+4 316 363 234 315
+4 237 306 182 229
+4 342 127 286 343
+4 317 132 47 227
+4 193 379 377 159
+4 221 228 258 86
+4 167 115 12 19
+4 140 139 162 123
+4 344 50 196 197
+4 242 256 240 278
+4 25 273 349 62
+4 341 57 358 78
+4 205 76 34 92
+4 216 113 217 172
+4 95 28 212 9
+4 189 140 211 124
+4 264 236 235 18
+4 183 178 231 29
+4 175 379 72 111
+4 132 131 130 227
+4 302 171 166 38
+4 317 184 301 227
+4 111 5 223 46
+4 333 334 219 342
+4 195 136 196 197
+4 109 299 110 118
+4 157 215 39 42
+4 130 247 47 227
+4 201 223 224 53
+4 244 105 138 324
+4 299 118 218 119
+4 224 53 223 112
+4 163 162 161 144
+4 287 129 282 19
+4 87 343 351 228
+4 124 140 211 192
+4 51 94 16 19
+4 324 293 329 243
+4 314 81 267 80
+4 95 74 28 9
+4 223 46 5 172
+4 291 238 352 181
+4 37 299 218 119
+389
+323
+388
+386
+380
+368
+364
+359
+274
+366
+283
+241
+239
+367
+349
+318
+369
+99
+200
+344
+374
+381
+319
+264
+59
+361
+305
+375
+204
+321
+126
+275
+203
+322
+267
+246
+309
+289
+288
+354
+376
+314
+370
+294
+279
+121
+353
+240
+254
+273
+25
+224
+50
+135
+311
+58
+60
+304
+191
+124
+290
+134
+292
+360
+120
+345
+255
+387
+226
+201
+52
+310
+14
+15
+190
+189
+202
+122
+356
+1000.0 0.45 7.5e4
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgScreenSpaceQuadRenderTests/CheckerBoard.png b/SurgSim/Graphics/RenderTests/Data/OsgScreenSpaceQuadRenderTests/CheckerBoard.png
new file mode 100644
index 0000000..aafbea2
Binary files /dev/null and b/SurgSim/Graphics/RenderTests/Data/OsgScreenSpaceQuadRenderTests/CheckerBoard.png differ
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgScreenSpaceQuadRenderTests/Rectangle.png b/SurgSim/Graphics/RenderTests/Data/OsgScreenSpaceQuadRenderTests/Rectangle.png
new file mode 100644
index 0000000..0a7b2cf
Binary files /dev/null and b/SurgSim/Graphics/RenderTests/Data/OsgScreenSpaceQuadRenderTests/Rectangle.png differ
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.frag b/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.frag
new file mode 100644
index 0000000..9b08aef
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.frag
@@ -0,0 +1,7 @@
+varying vec4 geomColor;
+
+/// Outputs the input color
+void main(void)
+{
+ gl_FragColor = geomColor;
+};
\ No newline at end of file
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.geom b/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.geom
new file mode 100644
index 0000000..7bf2207
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.geom
@@ -0,0 +1,30 @@
+#version 150
+#extension GL_EXT_gpu_shader4 : enable
+#extension GL_EXT_geometry_shader4 : enable
+
+layout(triangles) in;
+layout(triangle_strip, max_vertices=6) out;
+
+in vec4 vertColor[3];
+out vec4 geomColor;
+
+/// Outputs the input geometry plus it's mirror in the X-axis
+void main()
+{
+ for (int i = 0; i < gl_VerticesIn; ++i)
+ {
+ gl_Position = gl_PositionIn[i];
+ geomColor = vertColor[i];
+ EmitVertex();
+ }
+ EndPrimitive();
+ for (int i = 0; i < gl_VerticesIn; ++i)
+ {
+ vec4 vertex = gl_PositionIn[i];
+ vertex.x = - vertex.x;
+ gl_Position = vertex;
+ geomColor = vertColor[i];
+ EmitVertex();
+ }
+ EndPrimitive();
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.vert b/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.vert
new file mode 100644
index 0000000..eb353ff
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.vert
@@ -0,0 +1,9 @@
+varying vec4 vertColor;
+
+/// Outputs the local normal direction as the vertex color
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+ vertColor.rgb = gl_Normal;
+ vertColor.a = 1.0;
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/RenderTests/OsgBoxRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgBoxRepresentationRenderTests.cpp
new file mode 100644
index 0000000..86b7e16
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgBoxRepresentationRenderTests.cpp
@@ -0,0 +1,180 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Render Tests for the OsgBoxRepresentation class.
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/MathUtilities.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+using SurgSim::Testing::interpolatePose;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+struct OsgBoxRepresentationRenderTests : public RenderTest
+{
+
+};
+
+TEST_F(OsgBoxRepresentationRenderTests, MovingBoxesTest)
+{
+ /// Initial and final position (X, Y, Z) of box 1
+ Vector3d startPosition1(-0.1, 0.0, -0.2), finalPosition1(0.1, 0.0, -0.2);
+ /// Initial angles (X, Y, Z) and final of the cylinder 1
+ Vector3d startAngles1(0.0, 0.0, 0.0), finalAngles1(-M_PI_4, -M_PI_4, -M_PI_4);
+ /// Initial box 1 sizeX;
+ double startSizeX1 = 0.001;
+ /// Final box 1 sizeX;
+ double endSizeX1 = 0.01;
+ /// Initial box 1 sizeY;
+ double startSizeY1 = 0.011;
+ /// Final box 1 sizeY;
+ double endSizeY1 = 0.02;
+ /// Initial box 1 sizeZ;
+ double startSizeZ1 = 0.021;
+ /// Final box 1 sizeZ;
+ double endSizeZ1 = 0.03;
+
+ /// Initial and final position (X, Y, Z) box 2
+ Vector3d startPosition2(0.0, -0.1, -0.2), finalPosition2(0.0, 0.1, -0.2);
+ /// Initial and final angles (X, Y, Z) of the box 2
+ Vector3d startAngles2(-M_PI_2, -M_PI_2, -M_PI_2), finalAngles2(M_PI, M_PI, M_PI);
+ /// Initial box 2 sizeX;
+ double startSizeX2 = 0.001;
+ /// Final box 2 sizeX;
+ double endSizeX2 = 0.01;
+ /// Initial box 2 sizeX;
+ double startSizeY2 = 0.011;
+ /// Final box 2 sizeX;
+ double endSizeY2 = 0.02;
+ /// Initial box 2 sizeX;
+ double startSizeZ2 = 0.021;
+ /// Final box 2 sizeX;
+ double endSizeZ2 = 0.03;
+ /// Initial box 2 angleX;
+
+
+
+
+ /// Number of times to step the box position and radius from start to end.
+ /// This number of steps will be done in 1 second.
+ int numSteps = 100;
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Box 1");
+ auto boxRepresentation1 = std::make_shared<OsgBoxRepresentation>("Graphics");
+ sceneElement->addComponent(boxRepresentation1);
+ scene->addSceneElement(sceneElement);
+
+ sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Box 2");
+ auto boxRepresentation2 = std::make_shared<OsgBoxRepresentation>("Graphics");
+ sceneElement->addComponent(boxRepresentation2);
+ scene->addSceneElement(sceneElement);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ enum BoxSetterType {BoxSetterTypeIndividual,
+ BoxSetterTypeTogether,
+ BoxSetterTypeVector3d,
+ // Add more setter types above this line.
+ BoxSetterTypeCount
+ };
+ int boxSetterType = 0;
+ Vector3d box1Size, box2Size;
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ /// Calculate t in [0.0, 1.0]
+ double t = static_cast<double>(i) / numSteps;
+ /// Interpolate position and orientation
+ boxRepresentation1->getSceneElement()->setPose(
+ interpolatePose(startAngles1, finalAngles1, startPosition1, finalPosition1, t));
+ boxRepresentation2->getSceneElement()->setPose(
+ interpolatePose(startAngles2, finalAngles2, startPosition1, finalPosition2, t));
+
+ if (boxSetterType == static_cast<int>(BoxSetterTypeIndividual))
+ {
+ boxRepresentation1->setSizeX((1 - t) * startSizeX1 + t * endSizeX1);
+ boxRepresentation1->setSizeY((1 - t) * startSizeY1 + t * endSizeY1);
+ boxRepresentation1->setSizeZ((1 - t) * startSizeZ1 + t * endSizeZ1);
+
+ boxRepresentation2->setSizeX((1 - t) * startSizeX2 + t * endSizeX2);
+ boxRepresentation2->setSizeY((1 - t) * startSizeY2 + t * endSizeY2);
+ boxRepresentation2->setSizeZ((1 - t) * startSizeZ2 + t * endSizeZ2);
+ }
+ else if (boxSetterType == static_cast<int>(BoxSetterTypeTogether))
+ {
+ boxRepresentation1->setSizeXYZ((1 - t) * startSizeX1 + t * endSizeX1,
+ (1 - t) * startSizeY1 + t * endSizeY1,
+ (1 - t) * startSizeZ1 + t * endSizeZ1);
+
+ boxRepresentation2->setSizeXYZ((1 - t) * startSizeX2 + t * endSizeX2,
+ (1 - t) * startSizeY2 + t * endSizeY2,
+ (1 - t) * startSizeZ2 + t * endSizeZ2);
+ }
+ else if (boxSetterType == static_cast<int>(BoxSetterTypeVector3d))
+ {
+ box1Size.x() = (1 - t) * startSizeX1 + t * endSizeX1;
+ box1Size.y() = (1 - t) * startSizeY1 + t * endSizeY1;
+ box1Size.z() = (1 - t) * startSizeZ1 + t * endSizeZ1;
+ boxRepresentation1->setSize(box1Size);
+
+ box2Size.x() = (1 - t) * startSizeX2 + t * endSizeX2;
+ box2Size.y() = (1 - t) * startSizeY2 + t * endSizeY2;
+ box2Size.z() = (1 - t) * startSizeZ2 + t * endSizeZ2;
+ boxRepresentation2->setSize(box2Size);
+ }
+ boxSetterType = (boxSetterType + 1) % BoxSetterTypeCount;
+ /// The total number of steps should complete in 1 second
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+
+ runtime->stop();
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgCameraRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgCameraRenderTests.cpp
new file mode 100644
index 0000000..d9dfb17
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgCameraRenderTests.cpp
@@ -0,0 +1,178 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+#include "SurgSim/Blocks/SphereElement.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Testing/MathUtilities.h"
+
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector3f;
+using SurgSim::Math::RigidTransform3d;
+
+std::string vertexShaderSource =
+ "uniform vec3 ambientColor;\n"
+ "uniform vec3 otherColor;\n"
+ "varying vec4 color;\n"
+ "void main(void)\n"
+ "{\n"
+ " vec3 lightVector = normalize(vec3(1.0,-1.0,1.0));\n"
+ " gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;\n"
+// This is not a realy lighting calculation ... just faking something ...
+ " color.rgb = otherColor * max(dot(gl_Normal, lightVector), 0.0) + ambientColor;\n"
+ " color.a = 1.0;\n"
+ "}";
+std::string fragmentShaderSource =
+ "varying vec4 color;\n"
+ "void main(void)\n"
+ "{\n"
+ " gl_FragColor = color;\n"
+ "}";
+
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+struct OsgCameraRenderTests: public RenderTest
+{
+
+};
+
+TEST_F(OsgCameraRenderTests, PassTest)
+{
+ auto defaultCamera = viewElement->getCamera();
+ auto renderPass = std::make_shared<OsgCamera>("RenderPass");
+
+ renderPass->setProjectionMatrix(defaultCamera->getProjectionMatrix());
+ renderPass->setRenderGroupReference("RenderPass");
+ renderPass->setGroupReference(SurgSim::Graphics::Representation::DefaultGroupName);
+
+ std::array<int, 2> dimensions = viewElement->getView()->getDimensions();
+
+ auto renderTargetOsg = std::make_shared<OsgRenderTarget2d>(dimensions[0], dimensions[1], 1.0, 2, true);
+ renderPass->setRenderTarget(renderTargetOsg);
+ renderPass->setRenderOrder(Camera::RENDER_ORDER_PRE_RENDER, 0);
+
+ auto shader = std::make_shared<OsgShader>();
+ shader->setFragmentShaderSource(fragmentShaderSource);
+ shader->setVertexShaderSource(vertexShaderSource);
+
+ auto material1 = std::make_shared<OsgMaterial>("material1");
+ auto material2 = std::make_shared<OsgMaterial>("material2");
+
+ material1->setShader(shader);
+ material2->setShader(shader);
+
+ viewElement->addComponent(material1);
+ viewElement->addComponent(material2);
+
+ renderPass->setMaterial(material2);
+
+
+ auto uniform = std::make_shared<OsgUniform<Vector3f>>("ambientColor");
+ uniform->set(Vector3f(0.2, 0.2, 0.2));
+ material1->addUniform(uniform);
+
+ uniform = std::make_shared<OsgUniform<Vector3f>>("otherColor");
+ uniform->set(Vector3f(1.0, 0.0, 0.0));
+ material1->addUniform(uniform);
+
+ uniform = std::make_shared<OsgUniform<Vector3f>>("otherColor");
+ uniform->set(Vector3f(0.0, 1.0, 0.0));
+ material2->addUniform(uniform);
+
+
+ viewElement->addComponent(renderPass);
+
+ int screenWidth = dimensions[0];
+ int screenHeight = dimensions[1];
+
+ int width = dimensions[0] / 3;
+ int height = dimensions[1] / 3;
+
+ std::shared_ptr<ScreenSpaceQuadRepresentation> quad;
+ quad = makeQuad("Color1", width, height, screenWidth - width, screenHeight - height);
+ quad->setTexture(renderTargetOsg->getColorTargetOsg(0));
+ viewElement->addComponent(quad);
+
+ quad = makeQuad("Color2", width, height, screenWidth - width, screenHeight - height * 2);
+ quad->setTexture(renderTargetOsg->getColorTargetOsg(1));
+ viewElement->addComponent(quad);
+
+ quad = makeQuad("Depth", width, height, 0.0, screenHeight - height);
+ quad->setTexture(renderTargetOsg->getDepthTargetOsg());
+ viewElement->addComponent(quad);
+
+ Quaterniond quat = Quaterniond::Identity();
+ RigidTransform3d startPose = SurgSim::Math::makeRigidTransform(quat, Vector3d(0.0, 0.0, -0.2));
+ quat = SurgSim::Math::makeRotationQuaternion(M_PI, Vector3d::UnitY().eval());
+ RigidTransform3d endPose = SurgSim::Math::makeRigidTransform(quat, Vector3d(0.0, 0.0, -0.2));
+
+ auto box = std::make_shared<OsgBoxRepresentation>("Graphics");
+ box->setSizeXYZ(0.05, 0.05, 0.05);
+ box->setGroupReference("RenderPass");
+
+ auto boxElement1 = std::make_shared<BasicSceneElement>("Box 1");
+ boxElement1->addComponent(box);
+ boxElement1->setPose(startPose);
+ scene->addSceneElement(boxElement1);
+
+ box = std::make_shared<OsgBoxRepresentation>("Graphics");
+ box->setSizeXYZ(0.05, 0.05, 0.05);
+ box->setMaterial(material1);
+
+ auto boxElement2 = std::make_shared<BasicSceneElement>("Box 2");
+ boxElement2->addComponent(box);
+ boxElement2->setPose(startPose);
+ scene->addSceneElement(boxElement2);
+
+ /// Run the thread
+ runtime->start();
+
+ int numSteps = 100;
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+ for (int i = 0; i < numSteps; ++i)
+ {
+ double t = static_cast<double>(i) / numSteps;
+ boxElement1->setPose(SurgSim::Testing::interpolate<RigidTransform3d>(endPose, startPose, t));
+ boxElement2->setPose(SurgSim::Testing::interpolate<RigidTransform3d>(startPose, endPose, t));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / 100));
+ }
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
+
diff --git a/SurgSim/Graphics/RenderTests/OsgCapsuleRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgCapsuleRepresentationRenderTests.cpp
new file mode 100644
index 0000000..23e28a8
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgCapsuleRepresentationRenderTests.cpp
@@ -0,0 +1,153 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Render Tests for the OsgCapsuleRepresentation class.
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgCapsuleRepresentation.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/MathUtilities.h"
+
+#include <gtest/gtest.h>
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+using SurgSim::Testing::interpolate;
+using SurgSim::Testing::interpolatePose;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+struct OsgCapsuleRepresentationRenderTests : public RenderTest
+{
+
+};
+
+TEST_F(OsgCapsuleRepresentationRenderTests, MovingCapsuleTest)
+{
+ /// Add the two capsule representation to the view element
+ std::shared_ptr<CapsuleRepresentation> capsuleRepresentation1 =
+ std::make_shared<OsgCapsuleRepresentation>("capsule representation 1");
+ viewElement->addComponent(capsuleRepresentation1);
+ std::shared_ptr<CapsuleRepresentation> capsuleRepresentation2 =
+ std::make_shared<OsgCapsuleRepresentation>("capsule representation 2");
+ viewElement->addComponent(capsuleRepresentation2);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ enum SetterType {SetterTypeIndividual,
+ SetterTypeTogether,
+ SetterTypeVector2d,
+ // Add more setter types above this line.
+ BoxSetterTypeCount
+ };
+ int setterType = 0;
+
+ Vector2d capsule1Size, capsule2Size;
+
+ /// Initial position of capsule 1
+ Vector3d startPosition1(-0.1, 0.0, -0.2);
+ /// Final position of capsule 1
+ Vector3d endPosition1(0.1, 0.0, -0.2);
+ /// Initial size (radius, height) of capsule 1
+ Vector2d startSize1(0.001, 0.011);
+ /// Final size (radius, height) of capsule 1
+ Vector2d endSize1(0.01, 0.02);
+ /// Initial angles (X, Y, Z) of the capsule 1
+ Vector3d startAngles1(0.0, 0.0, 0.0);
+ /// Final angles (X, Y, Z) of the capsule 1
+ Vector3d endAngles1(-M_PI_4, -M_PI_4, -M_PI_4);
+
+ /// Initial position capsule 2
+ Vector3d startPosition2(0.0, -0.1, -0.2);
+ /// Final position capsule 2
+ Vector3d endPosition2(0.0, 0.1, -0.2);
+ /// Initial size (radius, height) of capsule 2
+ Vector2d startSize2(0.001, 0.01);
+ /// Final size (radius, height) of capsule 2
+ Vector2d endSize2(0.011, 0.02);
+ /// Initial angles (X, Y, Z) of the capsule 2
+ Vector3d startAngles2(-M_PI_2, -M_PI_2, -M_PI_2);
+ /// Final angles (X, Y, Z) of the capsule 2
+ Vector3d endAngles2(M_PI, M_PI, M_PI);
+
+ /// Number of times to step the capsule position and radius from start to end.
+ /// This number of steps will be done in 1 second.
+ int numSteps = 100;
+ for (int i = 0; i < numSteps; ++i)
+ {
+ /// Calculate t in [0.0, 1.0]
+ double t = static_cast<double>(i) / numSteps;
+ /// Interpolate position and angle
+ capsuleRepresentation1->setLocalPose(
+ interpolatePose(startAngles1, endAngles1, startPosition1, endPosition1, t));
+ capsuleRepresentation2->setLocalPose(
+ interpolatePose(startAngles2, endAngles2, startPosition2, endPosition2, t));
+
+ /// Test different ways to set the size of capsule
+ if (setterType == static_cast<int>(SetterTypeIndividual))
+ {
+ capsuleRepresentation1->setRadius(interpolate(startSize1.x(), endSize1.x(), t));
+ capsuleRepresentation1->setHeight(interpolate(startSize1.y(), endSize1.y(), t));
+
+ capsuleRepresentation2->setRadius(interpolate(startSize2.x(), endSize2.x(), t));
+ capsuleRepresentation2->setHeight(interpolate(startSize2.y(), endSize2.y(), t));
+ }
+ else if (setterType == static_cast<int>(SetterTypeTogether))
+ {
+ capsuleRepresentation1->setSize(interpolate(startSize1.x(), endSize1.x(), t),
+ interpolate(startSize1.y(), endSize1.y(), t));
+
+ capsuleRepresentation2->setSize(interpolate(startSize2.x(), endSize2.x(), t),
+ interpolate(startSize2.y(), endSize2.y(), t));
+ }
+ else if (setterType == static_cast<int>(SetterTypeVector2d))
+ {
+ capsule1Size.x() = interpolate(startSize1.x(), endSize1.x(), t);
+ capsule1Size.y() = interpolate(startSize1.y(), endSize1.y(), t);
+ capsuleRepresentation1->setSize(capsule1Size);
+
+ capsule2Size.x() = interpolate(startSize2.x(), endSize2.x(), t);
+ capsule2Size.y() = interpolate(startSize2.y(), endSize2.y(), t);
+ capsuleRepresentation2->setSize(capsule2Size);
+ }
+ setterType = (setterType + 1) % BoxSetterTypeCount;
+ /// The total number of steps should complete in 1 second
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgCylinderRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgCylinderRepresentationRenderTests.cpp
new file mode 100644
index 0000000..47ea951
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgCylinderRepresentationRenderTests.cpp
@@ -0,0 +1,123 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Render Tests for the OsgCylinderRepresentation class.
+
+#include "SurgSim/Graphics/OsgCylinderRepresentation.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/MathUtilities.h"
+
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Testing::interpolate;
+using SurgSim::Testing::interpolatePose;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+struct OsgCylinderRepresentationRenderTests : public RenderTest
+{
+};
+
+TEST_F(OsgCylinderRepresentationRenderTests, MovingCylinderTest)
+{
+ /// Add the two cylinder representation to the view element
+ std::shared_ptr<CylinderRepresentation> cylinderRepresentation1 =
+ std::make_shared<OsgCylinderRepresentation>("cylinder representation 1");
+ viewElement->addComponent(cylinderRepresentation1);
+ std::shared_ptr<CylinderRepresentation> cylinderRepresentation2 =
+ std::make_shared<OsgCylinderRepresentation>("cylinder representation 2");
+ viewElement->addComponent(cylinderRepresentation2);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ enum SetterType {SetterTypeIndividual,
+ SetterTypeTogether,
+ SetterTypeVector2d,
+ // Add more setter types above this line.
+ BoxSetterTypeCount};
+ int setterType = 0;
+
+ Vector2d cylinder1Size, cylinder2Size;
+
+ /// Initial and final position (X, Y, Z) of cylinder 1
+ Vector3d startPosition1(-0.1, 0.0, -0.2), finalPosition1(0.1, 0.0, -0.2);
+ /// Initial size (radius, height) and final size of cylinder 1
+ Vector2d startSize1(0.001, 0.011), finalSize1(0.01, 0.02);
+ /// Initial angles (X, Y, Z) and final of the cylinder 1
+ Vector3d startAngles1(0.0, 0.0, 0.0), finalAngles1(-M_PI_4, -M_PI_4, -M_PI_4);
+
+ /// Initial and final position (X, Y, Z) cylinder 2
+ Vector3d startPosition2(0.0, -0.1, -0.2), finalPosition2(0.0, 0.1, -0.2);
+ /// Initial and final size (radius, height) of cylinder 2
+ Vector2d startSize2(0.001, 0.011), finalSize2(0.01, 0.02);
+ /// Initial and final angles (X, Y, Z) of the cylinder 2
+ Vector3d startAngles2(-M_PI_2, -M_PI_2, -M_PI_2), finalAngles2(M_PI, M_PI, M_PI);
+
+ /// Number of times to step the cylinder position and radius from start to end.
+ /// This number of steps will be done in 1 second.
+ int numSteps = 100;
+ for (int i = 0; i < numSteps; ++i)
+ {
+ /// Calculate t in [0.0, 1.0]
+ double t = static_cast<double>(i) / numSteps;
+ /// Interpolate position and radius
+ cylinderRepresentation1->setLocalPose(
+ interpolatePose(startAngles1, finalAngles1, startPosition1, finalPosition1, t));
+ cylinderRepresentation2->setLocalPose(
+ interpolatePose(startAngles2, finalAngles2, startPosition1, finalPosition2, t));
+
+ if(setterType == static_cast<int>(SetterTypeIndividual))
+ {
+ cylinderRepresentation1->setRadius(interpolate(startSize1.x(), finalSize1.x(), t));
+ cylinderRepresentation1->setHeight(interpolate(startSize1.y(), finalSize1.y(), t));
+
+ cylinderRepresentation2->setRadius(interpolate(startSize2.x(), finalSize2.x(), t));
+ cylinderRepresentation2->setHeight(interpolate(startSize2.y(), finalSize2.y(), t));
+ }
+ else if(setterType == static_cast<int>(SetterTypeTogether))
+ {
+ cylinderRepresentation1->setSize(interpolate(startSize1.x(), finalSize1.x(), t),
+interpolate(startSize1.y(), finalSize1.y(), t));
+
+ cylinderRepresentation2->setSize(interpolate(startSize2.x(), finalSize2.x(), t),
+interpolate(startSize2.y(), finalSize2.y(), t));
+ }
+ else if(setterType == static_cast<int>(SetterTypeVector2d))
+ {
+ cylinder1Size.x() = interpolate(startSize1.x(), finalSize1.x(), t);
+ cylinder1Size.y() = interpolate(startSize1.y(), finalSize1.y(), t);
+ cylinderRepresentation1->setSize(cylinder1Size);
+
+ cylinder2Size.x() = interpolate(startSize2.x(), finalSize2.x(), t);
+ cylinder2Size.y() = interpolate(startSize2.y(), finalSize2.y(), t);
+ cylinderRepresentation2->setSize(cylinder2Size);
+ }
+ setterType = (setterType + 1) % BoxSetterTypeCount;
+ /// The total number of steps should complete in 1 second
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgManagerRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgManagerRenderTests.cpp
new file mode 100644
index 0000000..666a2f3
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgManagerRenderTests.cpp
@@ -0,0 +1,59 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Render Tests for the OsgManager class.
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+
+#include <gtest/gtest.h>
+
+#include <algorithm>
+#include <random>
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+// This does not acutally display anything
+TEST(OsgManagerRenderTests, StartUpTest)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<OsgManager> manager = std::make_shared<OsgManager>();
+
+ runtime->addManager(manager);
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ /// Add a graphics component to the scene
+ std::shared_ptr<OsgViewElement> viewElement = std::make_shared<OsgViewElement>("test view element");
+ scene->addSceneElement(viewElement);
+
+ /// Run the thread for a moment
+ runtime->start();
+ EXPECT_TRUE(manager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+ runtime->stop();
+}
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgMeshRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgMeshRepresentationRenderTests.cpp
new file mode 100644
index 0000000..cb98c47
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgMeshRepresentationRenderTests.cpp
@@ -0,0 +1,400 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Graphics/MeshPlyReaderDelegate.h"
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/MathUtilities.h"
+#include "SurgSim/Testing/TestCube.h"
+
+#include <string>
+
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Vector4f;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+using SurgSim::Testing::interpolate;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+struct OsgMeshRepresentationRenderTests : public RenderTest
+{
+
+protected:
+ std::vector<Vector3d> cubeVertices;
+ std::vector<size_t> cubeTriangles;
+ std::vector<Vector4d> cubeColors;
+ std::vector<Vector2d> cubeTextures;
+
+ std::shared_ptr<MeshRepresentation> makeRepresentation(const std::string& name)
+ {
+ auto meshRepresentation = std::make_shared<OsgMeshRepresentation>(name);
+ meshRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.0)));
+ return meshRepresentation;
+ }
+
+ std::shared_ptr<SurgSim::Graphics::Mesh> loadGraphicsMesh(const std::string& fileName)
+ {
+ // The PlyReader and TriangleMeshPlyReaderDelegate work together to load triangle meshes.
+ SurgSim::DataStructures::PlyReader reader(fileName);
+ auto delegate = std::make_shared<SurgSim::Graphics::MeshPlyReaderDelegate>();
+ SURGSIM_ASSERT(reader.parseWithDelegate(delegate)) << "The input file " << fileName << " is malformed.";
+
+ return delegate->getMesh();
+ }
+};
+
+TEST_F(OsgMeshRepresentationRenderTests, BasicCubeTest)
+{
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Scene");
+ scene->addSceneElement(element);
+
+ SurgSim::Testing::Cube::makeCube(&cubeVertices, &cubeColors, &cubeTextures, &cubeTriangles);
+
+ // make a colored cube
+ auto meshRepresentation1 = makeRepresentation("colormesh");
+ meshRepresentation1->getMesh()->initialize(cubeVertices, cubeColors, std::vector<Vector2d>(), cubeTriangles);
+ meshRepresentation1->setUpdateOptions(MeshRepresentation::UPDATE_OPTION_COLORS |
+ MeshRepresentation::UPDATE_OPTION_VERTICES);
+
+ // make a textured cube
+ auto meshRepresentation2 = makeRepresentation("textureMesh");
+ meshRepresentation2->getMesh()->initialize(cubeVertices, std::vector<Vector4d>(), cubeTextures, cubeTriangles);
+
+ auto material = std::make_shared<OsgMaterial>("material");
+ auto texture = std::make_shared<OsgTexture2d>();
+ texture->loadImage(applicationData->findFile("OsgMeshRepresentationRenderTests/cube.png"));
+
+ auto uniform2d = std::make_shared<OsgUniform<std::shared_ptr<SurgSim::Graphics::OsgTexture2d>>>("oss_diffuseMap");
+ uniform2d->set(texture);
+ material->addUniform(uniform2d);
+ meshRepresentation2->setMaterial(material);
+
+ element->addComponent(material);
+ element->addComponent(meshRepresentation1);
+ element->addComponent(meshRepresentation2);
+
+ auto axes = std::make_shared<OsgAxesRepresentation>("Origin");
+ viewElement->addComponent(axes);
+
+ struct InterpolationData
+ {
+ public:
+ std::pair<RigidTransform3d, RigidTransform3d> transform;
+ std::pair<double, double> scale;
+ };
+
+ std::vector<InterpolationData> interpolators;
+ InterpolationData interpolator;
+
+ interpolator.transform.first =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d(-0.1, 0.0, -0.2));
+ interpolator.scale.first = 0.001;
+ interpolator.transform.second =
+ makeRigidTransform(makeRotationQuaternion(M_PI_2, Vector3d(1.0, -1.0, 1.0)), Vector3d(0.1, 0.0, -0.2));
+ interpolator.scale.second = 0.03;
+ interpolators.push_back(interpolator);
+
+ interpolator.transform.first =
+ makeRigidTransform(makeRotationQuaternion(-M_PI_2, Vector3d(-1.0, -1.0, 0.0)), Vector3d(0.0, -0.1, -0.2));
+ interpolator.scale.first = 0.001;
+ interpolator.transform.second =
+ makeRigidTransform(makeRotationQuaternion(-M_PI_2, Vector3d(-1.0, 1.0 , 0.0)), Vector3d(0.0, 0.1, -0.2));
+ interpolator.scale.second = 0.03;
+ interpolators.push_back(interpolator);
+
+ std::vector<std::shared_ptr<Mesh>> meshes;
+ meshes.push_back(meshRepresentation1->getMesh());
+ meshes.push_back(meshRepresentation2->getMesh());
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+
+ int numSteps = 1000;
+
+ std::vector<Vector3d> newVertices(cubeVertices.size());
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ double t = static_cast<double>(i) / numSteps;
+
+ for (size_t j = 0; j < interpolators.size(); ++j)
+ {
+ InterpolationData data = interpolators[j];
+
+ double scale = interpolate(data.scale, t);
+ RigidTransform3d transform = interpolate(data.transform, t);
+ for (size_t index = 0; index < cubeVertices.size(); ++index)
+ {
+ newVertices[index] = transform * (cubeVertices[index] * scale);
+ }
+ meshes[j]->setVertexPositions(newVertices, true);
+ }
+
+ if (i == 500)
+ {
+ for (size_t v = 0; v < cubeColors.size(); ++v)
+ {
+ //meshes[0]->getVertex(v).data.color.setValue(cubeColors[(v+numSteps)%cubeColors.size()]);
+ meshes[0]->getVertex(v).data.color.setValue(Vector4d(1.0, 0.0, 0.5, 1.0));
+ }
+ }
+
+ /// The total number of steps should complete in 1 second
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps) * 4);
+ }
+}
+
+TEST_F(OsgMeshRepresentationRenderTests, TextureTest)
+{
+
+ std::string woundFilename;
+ ASSERT_TRUE(runtime->getApplicationData()->tryFindFile("OsgMeshRepresentationRenderTests/wound_deformable.ply",
+ &woundFilename));
+
+ std::string textureFilename;
+ ASSERT_TRUE(runtime->getApplicationData()->tryFindFile("Textures/checkered.png",
+ &textureFilename));
+
+ // Create a triangle mesh for visualizing the surface of the finite element model
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgMeshRepresentation>("Mesh");
+ *graphics->getMesh() = SurgSim::Graphics::Mesh(*loadGraphicsMesh(woundFilename));
+
+
+ // Create material to transport the Textures
+ auto material = std::make_shared<SurgSim::Graphics::OsgMaterial>("material");
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ texture->loadImage(textureFilename);
+ auto diffuseMapUniform =
+ std::make_shared<SurgSim::Graphics::OsgTextureUniform<SurgSim::Graphics::OsgTexture2d>>("diffuseMap");
+ diffuseMapUniform->set(texture);
+ material->addUniform(diffuseMapUniform);
+ graphics->setMaterial(material);
+
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Wound");
+ sceneElement->addComponent(graphics);
+ sceneElement->addComponent(material);
+
+ scene->addSceneElement(sceneElement);
+ viewElement->setPose(SurgSim::Math::makeRigidTransform(
+ Vector3d(-0.1, 0.1, -0.1),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 0.0, 1.0)));
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+ runtime->stop();
+
+}
+
+TEST_F(OsgMeshRepresentationRenderTests, ShaderTest)
+{
+
+ std::string woundFilename;
+ ASSERT_TRUE(runtime->getApplicationData()->tryFindFile("OsgMeshRepresentationRenderTests/wound_deformable.ply",
+ &woundFilename));
+
+ std::string textureFilename;
+ ASSERT_TRUE(runtime->getApplicationData()->tryFindFile("OsgMeshRepresentationRenderTests/wound.png",
+ &textureFilename));
+
+ // Create a triangle mesh for visualizing the surface of the finite element model
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgMeshRepresentation>("Mesh");
+ *graphics->getMesh() = SurgSim::Graphics::Mesh(*loadGraphicsMesh(woundFilename));
+
+
+ // Create material to transport the Textures
+
+ auto material = std::make_shared<OsgMaterial>("material");
+ auto shader = SurgSim::Graphics::loadShader(*runtime->getApplicationData(), "Shaders/ds_mapping_material");
+ ASSERT_TRUE(shader != nullptr);
+ material->setShader(shader);
+ {
+ auto uniform = std::make_shared<SurgSim::Graphics::OsgUniform<Vector4f>>("diffuseColor");
+ material->addUniform(uniform);
+ material->setValue("diffuseColor", SurgSim::Math::Vector4f(0.8, 0.8, 0.8, 1.0));
+ }
+
+ {
+ auto uniform = std::make_shared<SurgSim::Graphics::OsgUniform<Vector4f>>("specularColor");
+ material->addUniform(uniform);
+ material->setValue("specularColor", SurgSim::Math::Vector4f(0.01, 0.01, 0.01, 1.0));
+ }
+
+ {
+ auto uniform = std::make_shared<SurgSim::Graphics::OsgUniform<float>>("shininess");
+ material->addUniform(uniform);
+ material->setValue("shininess", 32.0f);
+ }
+
+ {
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ texture->loadImage(textureFilename);
+ auto uniform =
+ std::make_shared<SurgSim::Graphics::OsgTextureUniform<SurgSim::Graphics::OsgTexture2d>>("diffuseMap");
+ uniform->set(texture);
+ material->addUniform(uniform);
+ }
+
+ {
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ std::string blackTexture;
+ ASSERT_TRUE(applicationData->tryFindFile("Textures/black.png", &blackTexture));
+
+ texture->loadImage(blackTexture);
+ auto uniform =
+ std::make_shared<SurgSim::Graphics::OsgTextureUniform<SurgSim::Graphics::OsgTexture2d>>("shadowMap");
+ uniform->set(texture);
+ uniform->setMinimumTextureUnit(8);
+ material->addUniform(uniform);
+ }
+
+
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Wound");
+ sceneElement->addComponent(graphics);
+
+ auto light = std::make_shared<SurgSim::Graphics::OsgLight>("Light");
+ light->setDiffuseColor(Vector4d(1.0, 1.0, 1.0, 1.0));
+ light->setSpecularColor(Vector4d(0.8, 0.8, 0.8, 1.0));
+ light->setLightGroupReference(SurgSim::Graphics::Representation::DefaultGroupName);
+
+ scene->addSceneElement(sceneElement);
+ viewElement->setPose(SurgSim::Math::makeRigidTransform(
+ Vector3d(-0.1, 0.1, -0.1),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 0.0, 1.0)));
+
+ viewElement->getCamera()->setAmbientColor(Vector4d(0.2, 0.2, 0.2, 1.0));
+ viewElement->getCamera()->setMaterial(material);
+ viewElement->addComponent(material);
+ viewElement->addComponent(light);
+
+ std::dynamic_pointer_cast<SurgSim::Graphics::OsgView>(viewElement->getView())->setOsgMapsUniforms(true);
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+ runtime->stop();
+
+}
+
+TEST_F(OsgMeshRepresentationRenderTests, TriangleDeletionTest)
+{
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Scene");
+ scene->addSceneElement(element);
+
+ SurgSim::Testing::Cube::makeCube(&cubeVertices, &cubeColors, &cubeTextures, &cubeTriangles);
+
+ // make a colored cube
+ auto meshRepresentation1 = makeRepresentation("colormesh");
+ meshRepresentation1->getMesh()->initialize(cubeVertices, cubeColors, std::vector<Vector2d>(), cubeTriangles);
+ meshRepresentation1->setUpdateOptions(MeshRepresentation::UPDATE_OPTION_COLORS |
+ MeshRepresentation::UPDATE_OPTION_VERTICES |
+ MeshRepresentation::UPDATE_OPTION_TRIANGLES);
+
+ element->addComponent(meshRepresentation1);
+
+ auto axes = std::make_shared<OsgAxesRepresentation>("Origin");
+ viewElement->addComponent(axes);
+ viewElement->setPose(SurgSim::Math::makeRigidTransform(
+ Vector3d(-2.0, -2.0, -2.0),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 0.0, 1.0)));
+// auto view = std::dynamic_pointer_cast<SurgSim::Graphics::OsgView>(viewElement->getView());
+// view->enableManipulator(true);
+
+ struct InterpolationData
+ {
+ public:
+ std::pair<RigidTransform3d, RigidTransform3d> transform;
+ std::pair<double, double> scale;
+ };
+
+ InterpolationData interpolator;
+ interpolator.transform.first =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d(-0.1, 0.0, -0.2));
+ interpolator.scale.first = 0.03;
+ interpolator.transform.second =
+ makeRigidTransform(makeRotationQuaternion(M_PI_2 * 2, Vector3d(1.0, -1.0, 1.0)), Vector3d(0.1, 0.0, -0.2));
+ interpolator.scale.second = 0.03;
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+
+ int numSteps = 1000;
+
+ std::vector<Vector3d> newVertices(cubeVertices.size());
+
+ size_t triangleCount = meshRepresentation1->getMesh()->getNumTriangles();
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ double t = static_cast<double>(i) / numSteps;
+
+ if (i % (numSteps / triangleCount) == 0)
+ {
+ size_t triangle = meshRepresentation1->getMesh()->getNumTriangles() - 1;
+
+ // Leave one triangle for the mesh
+ if (triangle > 0)
+ {
+ meshRepresentation1->getMesh()->removeTriangle(triangle);
+ }
+ }
+
+ RigidTransform3d transform = interpolate(interpolator.transform, t);
+ element->setPose(transform);
+
+ /// The total number of steps should complete in 1 second
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps) * 4);
+ }
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgOctreeRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgOctreeRepresentationRenderTests.cpp
new file mode 100644
index 0000000..ba3ef05
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgOctreeRepresentationRenderTests.cpp
@@ -0,0 +1,122 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// Render Tests for the OsgOctreeRepresentation class.
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/OctreeNode.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Graphics/OctreeRepresentation.h"
+#include "SurgSim/Graphics/OsgOctreeRepresentation.h"
+#include "SurgSim/Math/OctreeShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Graphics::OsgOctreeRepresentation;
+using SurgSim::Graphics::OctreeRepresentation;
+using SurgSim::Math::OctreeShape;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+
+struct OsgOctreeRepresentationRenderTests : public SurgSim::Graphics::RenderTest
+{
+};
+
+// An Octree(Node) is traversed in following order (the 2nd OctreeNode, i.e. OctreeNode with "1" is now shown):
+/*
+ ________
+ /3 / 7/|
+ /-------/ |
+ /2__/_6_/| |
+ | | | |/|
+ |___|___|/|5|
+ | | | |/
+ |0__|__4|/
+*/
+TEST_F(OsgOctreeRepresentationRenderTests, OctreeSubdivide)
+{
+ SurgSim::DataStructures::EmptyData emptyData;
+
+ SurgSim::Math::OctreeShape::NodeType::AxisAlignedBoundingBox boundingBox;
+ boundingBox.min() = Vector3d::Ones() * -0.2;
+ boundingBox.max() = Vector3d::Ones() * 0.2;
+
+ std::array<Vector3d, 8> secondLevelPositions = {{
+ Vector3d(-0.1, -0.1, -0.1),
+ Vector3d(0.1, -0.1, -0.1),
+ Vector3d(-0.1, 0.1, -0.1),
+ Vector3d(0.1, 0.1, -0.1),
+ Vector3d(-0.1, -0.1, 0.1),
+ Vector3d(0.1, -0.1, 0.1),
+ Vector3d(-0.1, 0.1, 0.1),
+ Vector3d(0.1, 0.1, 0.1)
+ }
+ };
+ auto octree = std::make_shared<OctreeShape::NodeType>(boundingBox);
+ octree->addData(secondLevelPositions[0], emptyData, 2);
+ octree->addData(secondLevelPositions[1], emptyData, 2);
+ octree->addData(secondLevelPositions[3], emptyData, 2);
+ octree->addData(secondLevelPositions[7], emptyData, 2);
+ octree->addData(Vector3d(0.2, 0.2, 0.01), emptyData, 3);
+ octree->addData(Vector3d(-0.2, -0.2, 0.2), emptyData, 3);
+ octree->addData(Vector3d(0.01, 0.01, 0.11), emptyData, 3);
+ octree->addData(Vector3d(0.01, 0.01, 0.11), emptyData, 4);
+
+ auto octreeShape = std::make_shared<SurgSim::Math::OctreeShape>(*octree);
+
+ auto octreeRepresentation = std::make_shared<OsgOctreeRepresentation>("Octree Representation");
+ octreeRepresentation->setLocalPose(makeRigidTransform(
+ makeRotationQuaternion(M_PI_4, Vector3d(1.0, 1.0, 1.0)),
+ Vector3d(0.0, 0.0, -1.0))
+ );
+ viewElement->addComponent(octreeRepresentation);
+ octreeRepresentation->setOctreeShape(octreeShape);
+
+ // Path to a leaf node
+ SurgSim::DataStructures::OctreePath path0;
+ path0.push_back(2);
+
+ // Path to a leaf node
+ SurgSim::DataStructures::OctreePath path1;
+ path1.push_back(7);
+ path1.push_back(3);
+
+ // Path to a internal OctreeNode
+ SurgSim::DataStructures::OctreePath path2;
+ path2.push_back(4);
+
+ SurgSim::DataStructures::OctreePath invalidPath;
+ invalidPath.push_back(0);
+ invalidPath.push_back(1);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ octreeRepresentation->setNodeVisible(path0, true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+
+ octreeRepresentation->setNodeVisible(path1, false);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+
+ octreeRepresentation->setNodeVisible(path2, false);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+
+ EXPECT_ANY_THROW(octreeRepresentation->setNodeVisible(invalidPath, true));
+}
diff --git a/SurgSim/Graphics/RenderTests/OsgPlaneRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgPlaneRepresentationRenderTests.cpp
new file mode 100644
index 0000000..656cba9
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgPlaneRepresentationRenderTests.cpp
@@ -0,0 +1,114 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Render Tests for the OsgPlaneRepresentation class.
+
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgPlaneRepresentation.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Pops up a window with two planes that are translating and rotating
+TEST(OsgPlaneRepresentationRenderTests, MovingPlanesTest)
+{
+ /// Initial plane 1 position
+ Vector3d startPosition1(0.0, -0.1, 0.0);
+ /// Final plane 1 position
+ Vector3d endPosition1(0.1, -0.2, 0.0);
+ /// Initial plane 1 angle
+ double startAngle1 = 0.0;
+ /// Final plane 1 angle
+ double endAngle1 = - M_PI / 4.0;
+ /// Initial plane 2 position
+ Vector3d startPosition2(-0.1, -0.2, 0.0);
+ /// Final plane 2 position
+ Vector3d endPosition2(0.1, -0.1, 0.1);
+ /// Initial plane 2 angle
+ double startAngle2 = -M_PI / 2.0;
+ /// Final plane 2 angle
+ double endAngle2 = M_PI;
+
+ /// Number of times to step the sphere position and radius from start to end.
+ /// This number of steps will be done in 1 second.
+ int numSteps = 100;
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<OsgManager> manager = std::make_shared<OsgManager>();
+
+ runtime->addManager(manager);
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ /// Add a graphics view element to the scene
+ std::shared_ptr<OsgViewElement> viewElement = std::make_shared<OsgViewElement>("view element");
+ scene->addSceneElement(viewElement);
+
+ /// Add the sphere representation to the view element, no need to make another scene element
+ std::shared_ptr<PlaneRepresentation> planeRepresentation1 =
+ std::make_shared<OsgPlaneRepresentation>("plane representation 1");
+ viewElement->addComponent(planeRepresentation1);
+ std::shared_ptr<PlaneRepresentation> planeRepresentation2 =
+ std::make_shared<OsgPlaneRepresentation>("plane representation 2");
+ viewElement->addComponent(planeRepresentation2);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(manager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ /// Calculate t in [0.0, 1.0]
+ double t = static_cast<double>(i) / numSteps;
+ /// Interpolate pose
+ planeRepresentation1->setLocalPose(makeRigidTransform(
+ makeRotationQuaternion((1.0 - t) * startAngle1 + t * endAngle1, Vector3d(1.0, 0.0, 0.0)),
+ (1.0 - t) * startPosition1 + t * endPosition1));
+ planeRepresentation2->setLocalPose(makeRigidTransform(
+ makeRotationQuaternion((1.0 - t) * startAngle2 + t * endAngle2, Vector3d(0.0, 0.0, 1.0)),
+ (1.0 - t) * startPosition2 + t * endPosition2));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+
+ runtime->stop();
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgPointCloudRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgPointCloudRepresentationRenderTests.cpp
new file mode 100644
index 0000000..d207e6d
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgPointCloudRepresentationRenderTests.cpp
@@ -0,0 +1,205 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <vector>
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/PointCloudRepresentation.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/MathUtilities.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+
+using SurgSim::Testing::interpolate;
+using SurgSim::Testing::interpolatePose;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+using SurgSim::Graphics::PointCloud;
+
+struct OsgPointCloudRepresentationRenderTests : public RenderTest
+{
+
+
+protected:
+ std::vector<Vector3d> makeCube()
+ {
+ std::vector<Vector3d> result;
+ result.push_back(Vector3d(0.01, -0.01, 0.01));
+ result.push_back(Vector3d(0.01, -0.01, 0.01));
+ result.push_back(Vector3d(-0.01, -0.01, 0.01));
+ result.push_back(Vector3d(-0.01, -0.01, -0.01));
+ result.push_back(Vector3d(0.01, -0.01, -0.01));
+
+ result.push_back(Vector3d(0.01, 0.01, 0.01));
+ result.push_back(Vector3d(-0.01, 0.01, 0.01));
+ result.push_back(Vector3d(-0.01, 0.01, -0.01));
+ result.push_back(Vector3d(0.01, 0.01, -0.01));
+ return result;
+ }
+
+ std::shared_ptr<PointCloudRepresentation> makeCloud(std::vector<Vector3d> vertices)
+ {
+ std::shared_ptr<PointCloudRepresentation> representation =
+ std::make_shared<OsgPointCloudRepresentation>("cloud representation");
+
+ representation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.0, -0.2)));
+ for (auto it = std::begin(vertices); it != std::end(vertices); ++it)
+ {
+ representation->getVertices()->addVertex(SurgSim::Graphics::PointCloud::VertexType(*it));
+ }
+
+ viewElement->addComponent(representation);
+
+ return representation;
+ }
+};
+
+TEST_F(OsgPointCloudRepresentationRenderTests, PointAdd)
+{
+ std::vector<Vector3d> vertices = makeCube();
+
+ auto representation = std::make_shared<OsgPointCloudRepresentation>("pointcloud representation");
+ auto pointCloud = representation->getVertices();
+ representation->setPointSize(2.0);
+
+ RigidTransform3d pose = makeRigidTransform(makeRotationQuaternion(0.2, Vector3d(1.0, 1.0, 1.0)),
+ Vector3d(0.0, 0.0, -0.2));
+ representation->setLocalPose(pose);
+
+ viewElement->addComponent(representation);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+
+
+ for (size_t i = 0; i < vertices.size(); ++i)
+ {
+ pointCloud->addVertex(PointCloud::VertexType(vertices[i]));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(250));
+ }
+}
+
+TEST_F(OsgPointCloudRepresentationRenderTests, StaticRotate)
+{
+ std::shared_ptr<PointCloudRepresentation> representation = makeCloud(makeCube());
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+
+ int numSteps = 100;
+
+ Vector3d startAngles(0.0, 0.0, 0.0);
+ Vector3d endAngles(M_PI_4, M_PI_2, M_PI_2);
+ Vector3d startPosition(-0.1, 0.0, -0.0);
+ Vector3d endPosition(0.1, 0.0, -0.4);
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ /// Calculate t in [0.0, 1.0]
+ double t = static_cast<double>(i) / numSteps;
+ representation->setLocalPose(interpolatePose(startAngles, endAngles, startPosition, endPosition, t));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+}
+
+TEST_F(OsgPointCloudRepresentationRenderTests, DynamicRotate)
+{
+ std::vector<Vector3d> startVertices = makeCube();
+ std::shared_ptr<PointCloudRepresentation> representation = makeCloud(startVertices);
+ std::shared_ptr<PointCloud> pointCloud = representation->getVertices();
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+
+ int numSteps = 100;
+
+ RigidTransform3d start = makeRigidTransform(makeRotationQuaternion(-M_PI_2, Vector3d(1.0, 1.0, 1.0)),
+ Vector3d(-0.1, 0.0, 0.2));
+
+ RigidTransform3d end = makeRigidTransform(makeRotationQuaternion(M_PI_2, Vector3d(1.0, 1.0, 1.0)),
+ Vector3d(0.1, 0.0, -0.2));
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ /// Calculate t in [0.0, 1.0]
+ double t = static_cast<double>(i) / numSteps;
+ RigidTransform3d currentPose = interpolate(start, end, t);
+
+ int id = 0;
+ for (auto it = std::begin(startVertices); it != std::end(startVertices); ++it, ++id)
+ {
+ pointCloud->setVertexPosition(id, currentPose * (*it));
+ }
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+}
+
+TEST_F(OsgPointCloudRepresentationRenderTests, PointSizeAndColor)
+{
+ std::shared_ptr<PointCloudRepresentation> representation = makeCloud(makeCube());
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+
+ int numSteps = 100;
+ std::pair<double, double> size = std::make_pair(0.0, 20.0);
+ std::pair<Vector4d, Vector4d> color =
+ std::make_pair(Vector4d(0.0, 1.0, 0.0, 1.0), Vector4d(1.0, 0.0, 1.0, 1.0));
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ /// Calculate t in [0.0, 1.0]
+ double t = static_cast<double>(i) / numSteps;
+ representation->setPointSize(interpolate(size, t));
+ representation->setColor(interpolate(color, t));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+}
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgRepresentationRenderTests.cpp
new file mode 100644
index 0000000..113d3cd
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgRepresentationRenderTests.cpp
@@ -0,0 +1,163 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Render Tests for the OsgBoxRepresentation class.
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgCapsuleRepresentation.h"
+#include "SurgSim/Graphics/OsgCylinderRepresentation.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+struct OsgRepresentationRenderTests : public ::testing::Test
+{
+ virtual void SetUp()
+ {
+ runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ manager = std::make_shared<SurgSim::Graphics::OsgManager>();
+
+ runtime->addManager(manager);
+
+ scene = runtime->getScene();
+
+ viewElement = std::make_shared<OsgViewElement>("view element");
+ scene->addSceneElement(viewElement);
+
+ }
+
+ virtual void TearDown()
+ {
+ runtime->stop();
+ }
+
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime;
+ std::shared_ptr<SurgSim::Graphics::OsgManager> manager;
+ std::shared_ptr<SurgSim::Framework::Scene> scene;
+ std::shared_ptr<OsgViewElement> viewElement;
+
+protected:
+
+};
+
+/// This test will put all shape one by one along the X-axis
+/// To make sure all shapes are aligned.
+/// X-axis points horizontally to the right
+/// Y-axis points vertically up
+/// Z-axis is perpendicular to the screen and points out
+TEST_F(OsgRepresentationRenderTests, RepresentationTest)
+{
+ /// Box position
+ Vector3d boxPosition(0.05, 0.0, -0.2);
+ /// Capsule position
+ Vector3d capsulePosition(-0.05, 0.0, -0.2);
+ /// Cylinder position
+ Vector3d cylinderPosition(-0.025, 0.0, -0.2);
+ /// Sphere position
+ Vector3d spherePosition(0.025, 0.0, -0.2);
+ /// Size of the box
+ Vector3d boxSize(0.01, 0.015, 0.01);
+ /// Size of the capsule (radius, height)
+ Vector2d capsuleSize(0.005, 0.015);
+ /// Size of the cylinder
+ Vector2d cylinderSize(0.005, 0.015);
+ /// Radius of the sphere
+ double sphereRadius = 0.005;
+
+ /// Add representations to the view element so we don't need to make another concrete scene element
+ std::shared_ptr<BoxRepresentation> boxRepresentation =
+ std::make_shared<OsgBoxRepresentation>("box representation");
+ viewElement->addComponent(boxRepresentation);
+
+ std::shared_ptr<CapsuleRepresentation> capsuleRepresentation =
+ std::make_shared<OsgCapsuleRepresentation>("capsule representation");
+ viewElement->addComponent(capsuleRepresentation);
+
+ std::shared_ptr<CylinderRepresentation> cylinderRepresentation =
+ std::make_shared<OsgCylinderRepresentation>("cylinder representation");
+ viewElement->addComponent(cylinderRepresentation);
+
+ std::shared_ptr<SphereRepresentation> sphereRepresentation =
+ std::make_shared<OsgSphereRepresentation>("sphere representation");
+ viewElement->addComponent(sphereRepresentation);
+
+
+ std::shared_ptr<AxesRepresentation> axesRepresentation =
+ std::make_shared<OsgAxesRepresentation>("axes");
+ viewElement->addComponent(axesRepresentation);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(manager->isInitialized());
+
+ boxRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), boxPosition));
+ capsuleRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), capsulePosition));
+ cylinderRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), cylinderPosition));
+ sphereRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), spherePosition));
+ axesRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.0, -0.2)));
+
+ /// Set the size of box
+ boxRepresentation->setSizeXYZ(boxSize.x(), boxSize.y(), boxSize.z());
+ /// Set the size of capsule
+ /// Capsule should use Y-axis as its axis
+ capsuleRepresentation->setSize(capsuleSize.x(), capsuleSize.y());
+ /// Set the size of cylinder
+ /// Cylinder should use Y-axis as its axis
+ cylinderRepresentation->setSize(cylinderSize.x(), cylinderSize.y());
+ /// Set the size of sphere
+ sphereRepresentation->setRadius(sphereRadius);
+
+ axesRepresentation->setSize(0.01);
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1500));
+
+ boxRepresentation->setDrawAsWireFrame(true);
+ capsuleRepresentation->setDrawAsWireFrame(true);
+ cylinderRepresentation->setDrawAsWireFrame(true);
+ sphereRepresentation->setDrawAsWireFrame(true);
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1500));
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgSceneryRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgSceneryRepresentationRenderTests.cpp
new file mode 100644
index 0000000..f70506e
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgSceneryRepresentationRenderTests.cpp
@@ -0,0 +1,92 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Render Tests for OsgSceneryRepresentation class.
+
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/MathUtilities.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Testing::interpolatePose;
+
+struct OsgSceneryRepresentationRenderTests : public RenderTest
+{
+};
+
+TEST_F(OsgSceneryRepresentationRenderTests, RenderTest)
+{
+ /// Initial position of object 1
+ Vector3d startPosition1(-5.0, 0.0, -5.0);
+ /// Final position of object 1
+ Vector3d endPosition1(5.0, 0.0, -5.0);
+ /// Initial angles (X, Y, Z) of object 1
+ Vector3d startAngles1(0.0, 0.0, 0.0);
+ /// Final angles (X, Y, Z) of object 1
+ Vector3d endAngles1(-M_PI_4, -M_PI_4, -M_PI_4);
+
+ /// Initial position of object 2
+ Vector3d startPosition2(0.0, -5.0, -5.0);
+ /// Final position of object 2
+ Vector3d endPosition2(0.0, 5.0, -5.0);
+ /// Initial angles (X, Y, Z) of object 2
+ Vector3d startAngles2(-M_PI_2, -M_PI_2, -M_PI_2);
+ /// Final angles (X, Y, Z) of object 2
+ Vector3d endAngles2(M_PI, M_PI, M_PI);
+
+ /// Number of times to step the objects' position from start to end.
+ /// This number of steps will be done in 1 second.
+ int numSteps = 100;
+
+ auto sceneryObject1 = std::make_shared<OsgSceneryRepresentation>("Torus1");
+ sceneryObject1->setFileName("OsgSceneryRepresentationTests/Torus.obj");
+ sceneryObject1->setLocalPose(SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::Quaterniond::Identity(), startPosition1));
+ viewElement->addComponent(sceneryObject1);
+
+ auto sceneryObject2 = std::make_shared<OsgSceneryRepresentation>("Torus2");
+ sceneryObject2->setFileName("OsgSceneryRepresentationTests/Torus.osgb");
+ sceneryObject2->setLocalPose(SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::Quaterniond::Identity(), startPosition2));
+ viewElement->addComponent(sceneryObject2);
+
+ runtime->start();
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ /// Calculate t in [0.0, 1.0]
+ double t = static_cast<double>(i) / numSteps;
+
+ /// Interpolate position
+ sceneryObject1->setLocalPose(interpolatePose(startAngles1, endAngles1, startPosition1, endPosition1, t));
+ sceneryObject2->setLocalPose(interpolatePose(startAngles2, endAngles2, startPosition2, endPosition2, t));
+
+ /// The total number of steps should complete in 1 second
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+}
+
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgScreenSpaceQuadRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgScreenSpaceQuadRenderTests.cpp
new file mode 100644
index 0000000..d13f27e
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgScreenSpaceQuadRenderTests.cpp
@@ -0,0 +1,224 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+
+#include "SurgSim/Graphics/Material.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+#include "SurgSim/Graphics/OsgTexture.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/OsgTextureRectangle.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Graphics/View.h"
+
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+#include "SurgSim/Testing/MathUtilities.h"
+
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+struct OsgScreenSpaceQuadRenderTests : public RenderTest
+{
+
+};
+
+
+TEST_F(OsgScreenSpaceQuadRenderTests, InitTest)
+{
+ std::shared_ptr<OsgScreenSpaceQuadRepresentation> quad =
+ std::make_shared<OsgScreenSpaceQuadRepresentation>("Screen Quad");
+
+ viewElement->addComponent(quad);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ quad->setSize(100, 100);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ auto dimensions = viewElement->getView()->getDimensions();
+
+ SurgSim::Math::Vector3d startPosition(0.0, 0.0, 0.0);
+ SurgSim::Math::Vector3d endPosition(dimensions[0], dimensions[1], 0.0);
+
+ SurgSim::Math::Vector2d startSize(0.0, 0.0);
+ SurgSim::Math::Vector2d endSize(200, 200);
+
+ int numSteps = 100;
+ for (int i = 0; i < numSteps; ++i)
+ {
+ /// Calculate t in [0.0, 1.0]
+ double t = static_cast<double>(i) / numSteps;
+ RigidTransform3d currentPose = SurgSim::Testing::interpolatePose(
+ Vector3d::Identity(), Vector3d::Identity(),
+ startPosition, endPosition, t);
+
+ quad->setLocalPose(currentPose);
+
+ SurgSim::Math::Vector2d size = SurgSim::Testing::interpolate(startSize, endSize, t);
+ quad->setSize(size.x(), size.y());
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+
+}
+
+TEST_F(OsgScreenSpaceQuadRenderTests, TextureTest)
+{
+ std::string checkerTexturePath = applicationData->findFile("OsgScreenSpaceQuadRenderTests/CheckerBoard.png");
+ std::string rectangleTexturePath = applicationData->findFile("OsgScreenSpaceQuadRenderTests/Rectangle.png");
+
+ EXPECT_NE("", checkerTexturePath) << "Could not find checker texture shader!";
+ EXPECT_NE("", rectangleTexturePath) << "Could not find rectangle texture!";
+
+ std::shared_ptr<OsgTexture2d> checkerTexture = std::make_shared<OsgTexture2d>();
+ EXPECT_TRUE(checkerTexture->loadImage(checkerTexturePath));
+
+ std::shared_ptr<OsgTextureRectangle> rectTexture = std::make_shared<OsgTextureRectangle>();
+ EXPECT_TRUE(rectTexture->loadImage(rectangleTexturePath));
+
+
+ std::shared_ptr<OsgScreenSpaceQuadRepresentation> quad1 =
+ std::make_shared<OsgScreenSpaceQuadRepresentation>("Screen Quad 1");
+
+ auto dimensions = viewElement->getView()->getDimensions();
+
+ quad1->setSize(256, 256);
+ quad1->setLocalPose(SurgSim::Math::makeRigidTransform(
+ Quaterniond::Identity(),
+ Vector3d(dimensions[0] - 256, dimensions[1] - 256, -0.2)));
+ EXPECT_TRUE(quad1->setTexture(checkerTexture));
+ viewElement->addComponent(quad1);
+
+
+ std::shared_ptr<OsgScreenSpaceQuadRepresentation> quad2 =
+ std::make_shared<OsgScreenSpaceQuadRepresentation>("Screen Quad 2");
+
+ int width, height;
+ rectTexture->getSize(&width, &height);
+ EXPECT_TRUE(quad2->setTexture(rectTexture));
+ quad2->setSize(width, height);
+ viewElement->addComponent(quad2);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+}
+
+
+
+// Should show two rotating cubes, one in the middle of the screen being rendered normally, the
+// other one in the top right hand corner, being rendered onto a texture mapped on a quad
+TEST_F(OsgScreenSpaceQuadRenderTests, RenderTextureTest)
+{
+
+ auto defaultCamera = viewElement->getCamera();
+ auto camera = std::make_shared<OsgCamera>("RenderPass");
+ camera->setProjectionMatrix(defaultCamera->getProjectionMatrix());
+ camera->setRenderGroupReference("RenderPass");
+ camera->setGroupReference(SurgSim::Graphics::Representation::DefaultGroupName);
+
+ auto dimensions = viewElement->getView()->getDimensions();
+
+ std::shared_ptr<OsgRenderTarget2d> renderTargetOsg =
+ std::make_shared<OsgRenderTarget2d>(dimensions[0], dimensions[1], 1.0, 2, true);
+ camera->setRenderTarget(renderTargetOsg);
+
+ viewElement->addComponent(camera);
+
+ int screenWidth = dimensions[0];
+ int screenHeight = dimensions[1];
+
+ int width = dimensions[0] / 3;
+ int height = dimensions[1] / 3;
+
+ std::shared_ptr<ScreenSpaceQuadRepresentation> quad;
+ quad = makeQuad("Color1", width, height, screenWidth - width, screenHeight - height);
+ quad->setTexture(renderTargetOsg->getColorTargetOsg(0));
+ viewElement->addComponent(quad);
+
+ quad = makeQuad("Color2", width, height, screenWidth - width, screenHeight - height * 2);
+ quad->setTexture(renderTargetOsg->getColorTargetOsg(1));
+ viewElement->addComponent(quad);
+
+ quad = makeQuad("Depth", width, height, 0.0, screenHeight - height);
+ quad->setTexture(renderTargetOsg->getDepthTargetOsg());
+ viewElement->addComponent(quad);
+
+ Quaterniond quat = Quaterniond::Identity();
+ RigidTransform3d startPose = SurgSim::Math::makeRigidTransform(quat, Vector3d(0.0, 0.0, -0.2));
+ quat = SurgSim::Math::makeRotationQuaternion(M_PI, Vector3d::UnitY().eval());
+ RigidTransform3d endPose = SurgSim::Math::makeRigidTransform(quat, Vector3d(0.0, 0.0, -0.2));
+
+ auto box = std::make_shared<OsgBoxRepresentation>("Graphics");
+ box->setSizeXYZ(0.05, 0.05, 0.05);
+ box->setGroupReference("RenderPass");
+ auto boxElement1 = std::make_shared<BasicSceneElement>("Box 1");
+ boxElement1->addComponent(box);
+ boxElement1->setPose(startPose);
+ scene->addSceneElement(boxElement1);
+
+ box = std::make_shared<OsgBoxRepresentation>("Graphics");
+ box->setSizeXYZ(0.05, 0.05, 0.05);
+ auto boxElement2 = std::make_shared<BasicSceneElement>("Box 2");
+ boxElement2->addComponent(box);
+ boxElement2->setPose(startPose);
+ scene->addSceneElement(boxElement2);
+
+ /// Run the thread
+ runtime->start();
+
+ int numSteps = 100;
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+ for (int i = 0; i < numSteps; ++i)
+ {
+ double t = static_cast<double>(i) / numSteps;
+ boxElement1->setPose(SurgSim::Testing::interpolate<RigidTransform3d>(startPose, endPose, t));
+ boxElement2->setPose(SurgSim::Testing::interpolate<RigidTransform3d>(endPose, startPose, t));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / 100));
+ }
+
+ graphicsManager->dumpDebugInfo();
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgShaderRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgShaderRenderTests.cpp
new file mode 100644
index 0000000..17c731c
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgShaderRenderTests.cpp
@@ -0,0 +1,253 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Render Tests for the OsgShader class.
+
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <boost/filesystem.hpp>
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+std::shared_ptr<Shader> loadExampleShader(const SurgSim::Framework::ApplicationData& data)
+{
+ std::shared_ptr<Shader> shader = std::make_shared<OsgShader>();
+
+ std::string vertexShaderPath = data.findFile("OsgShaderRenderTests/shader.vert");
+ std::string geometryShaderPath = data.findFile("OsgShaderRenderTests/shader.geom");
+ std::string fragmentShaderPath = data.findFile("OsgShaderRenderTests/shader.frag");
+
+ EXPECT_NE("", vertexShaderPath) << "Could not find vertex shader!";
+ EXPECT_NE("", geometryShaderPath) << "Could not find geometry shader!";
+ EXPECT_NE("", fragmentShaderPath) << "Could not find fragment shader!";
+
+ shader->loadVertexShaderSource(vertexShaderPath);
+ shader->loadGeometryShaderSource(geometryShaderPath);
+ shader->loadFragmentShaderSource(fragmentShaderPath);
+
+ return shader;
+}
+
+
+std::shared_ptr<Material> createShinyMaterial(const SurgSim::Framework::ApplicationData& data)
+{
+ auto material = std::make_shared<SurgSim::Graphics::OsgMaterial>("material");
+ auto shader = SurgSim::Graphics::loadShader(data, "Shaders/material");
+ material->setShader(shader);
+
+ std::shared_ptr<SurgSim::Graphics::UniformBase>
+ uniform = std::make_shared<OsgUniform<SurgSim::Math::Vector4f>>("diffuseColor");
+ material->addUniform(uniform);
+
+ uniform = std::make_shared<OsgUniform<SurgSim::Math::Vector4f>>("specularColor");
+ material->addUniform(uniform);
+
+ uniform = std::make_shared<OsgUniform<float>>("shininess");
+ material->addUniform(uniform);
+
+ return material;
+}
+
+struct OsgShaderRenderTests : public RenderTest
+{
+
+};
+
+/// Pops up a window with a sphere colored by its normals and its mirror along the x-axis is also drawn using the
+/// geometry shader
+TEST_F(OsgShaderRenderTests, SphereShaderTest)
+{
+ /// Add the sphere representation to the view element, no need to make another scene element
+ std::shared_ptr<SphereRepresentation> sphereRepresentation =
+ std::make_shared<OsgSphereRepresentation>("sphere representation");
+ sphereRepresentation->setRadius(0.25);
+ sphereRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.25, 0.0, -1.0)));
+
+ /// Add a shader to the sphere
+ std::shared_ptr<OsgMaterial> material = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<Shader> shader = loadExampleShader(*applicationData);
+
+ material->setShader(shader);
+ sphereRepresentation->setMaterial(material);
+
+ viewElement->addComponent(sphereRepresentation);
+ viewElement->addComponent(material);
+
+ /// Run the thread
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+ runtime->stop();
+
+}
+
+TEST_F(OsgShaderRenderTests, ShinyShaderTest)
+{
+ /// Add the sphere representation to the view element, no need to make another scene element
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Sphere");
+ std::shared_ptr<SphereRepresentation> sphereRepresentation =
+ std::make_shared<OsgSphereRepresentation>("sphere representation");
+ sphereRepresentation->setRadius(0.25);
+
+ auto material = createShinyMaterial(*runtime->getApplicationData());
+ material->setValue("diffuseColor", SurgSim::Math::Vector4f(0.8, 0.8, 0.1, 1.0));
+ material->setValue("specularColor", SurgSim::Math::Vector4f(1.0, 1.0, 0.4, 1.0));
+ material->setValue("shininess", 64.0f);
+ sphereRepresentation->setMaterial(material);
+ sceneElement->addComponent(material);
+ sceneElement->addComponent(sphereRepresentation);
+ sceneElement->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("axes"));
+ scene->addSceneElement(sceneElement);
+
+
+ sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Light");
+ auto light = std::make_shared<SurgSim::Graphics::OsgLight>("Light");
+ light->setDiffuseColor(SurgSim::Math::Vector4d(0.8, 0.8, 0.8, 1.0));
+ light->setSpecularColor(SurgSim::Math::Vector4d(0.8, 0.8, 0.8, 1.0));
+ light->setLightGroupReference(SurgSim::Graphics::Representation::DefaultGroupName);
+ sceneElement->addComponent(light);
+ sceneElement->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("axes"));
+ sceneElement->setPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(-2.0, -2.0, -4.0)));
+
+ scene->addSceneElement(sceneElement);
+
+ viewElement->setPose(
+ makeRigidTransform(Vector3d(0.0, 0.0, -2.0), Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 1.0, 0.0)));
+ viewElement->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("axes"));
+
+
+ /// Run the thread
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+ runtime->stop();
+
+}
+
+TEST_F(OsgShaderRenderTests, TexturedShinyShaderTest)
+{
+ // The textured Sphere
+ std::shared_ptr<SphereRepresentation> sphereRepresentation =
+ std::make_shared<OsgSphereRepresentation>("sphere representation");
+ sphereRepresentation->setRadius(0.25);
+
+ auto material = std::make_shared<OsgMaterial>("material");
+ auto shader = SurgSim::Graphics::loadShader(*runtime->getApplicationData(), "Shaders/ds_mapping_material");
+ ASSERT_TRUE(shader != nullptr);
+ material->setShader(shader);
+
+ std::shared_ptr<SurgSim::Graphics::UniformBase>
+ uniform = std::make_shared<OsgUniform<SurgSim::Math::Vector4f>>("diffuseColor");
+ material->addUniform(uniform);
+
+ uniform = std::make_shared<OsgUniform<SurgSim::Math::Vector4f>>("specularColor");
+ material->addUniform(uniform);
+
+ uniform = std::make_shared<OsgUniform<float>>("shininess");
+ material->addUniform(uniform);
+
+ material->setValue("diffuseColor", SurgSim::Math::Vector4f(0.8, 0.8, 0.1, 1.0));
+ material->setValue("specularColor", SurgSim::Math::Vector4f(1.0, 1.0, 0.4, 1.0));
+ material->setValue("shininess", 1.0f);
+
+ // Provide a texture for the diffuse color
+ std::string filename;
+ EXPECT_TRUE(runtime->getApplicationData()->tryFindFile("Textures/checkered.png", &filename));
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ texture->loadImage(filename);
+ auto textureUniform =
+ std::make_shared<OsgTextureUniform<OsgTexture2d>>("diffuseMap");
+ textureUniform->set(texture);
+ material->addUniform(textureUniform);
+
+ // Provide a fake shadow map, it's all black so no shadow contribution
+ EXPECT_TRUE(runtime->getApplicationData()->tryFindFile("Textures/black.png", &filename));
+ texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ texture->loadImage(filename);
+ textureUniform = std::make_shared<OsgTextureUniform<OsgTexture2d>>("shadowMap");
+ textureUniform->set(texture);
+ textureUniform->setMinimumTextureUnit(8);
+ material->addUniform(textureUniform);
+
+ sphereRepresentation->setMaterial(material);
+
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Sphere");
+ sceneElement->addComponent(sphereRepresentation);
+ sceneElement->addComponent(material);
+ sceneElement->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("axes"));
+
+ scene->addSceneElement(sceneElement);
+
+ sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Light");
+ auto light = std::make_shared<SurgSim::Graphics::OsgLight>("Light");
+ light->setDiffuseColor(SurgSim::Math::Vector4d(0.8, 0.8, 0.8, 1.0));
+ light->setSpecularColor(SurgSim::Math::Vector4d(0.8, 0.8, 0.8, 1.0));
+ light->setLightGroupReference(SurgSim::Graphics::Representation::DefaultGroupName);
+ sceneElement->addComponent(light);
+ sceneElement->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("axes"));
+ sceneElement->setPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(-2.0, -2.0, -4.0)));
+ scene->addSceneElement(sceneElement);
+
+ viewElement->enableManipulator(true);
+ viewElement->getCamera()->setAmbientColor(SurgSim::Math::Vector4d(0.2, 0.2, 0.2, 1.0));
+
+ viewElement->setPose(makeRigidTransform(Vector3d(0.0, 0.0, -2.0),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0)));
+
+ viewElement->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("axes"));
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+ runtime->stop();
+
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgSphereRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgSphereRepresentationRenderTests.cpp
new file mode 100644
index 0000000..05144a9
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgSphereRepresentationRenderTests.cpp
@@ -0,0 +1,114 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Render Tests for the OsgSphereRepresentation class.
+
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Pops up a window with two spheres that are translating and changing radius
+TEST(OsgSphereRepresentationRenderTests, MovingSpheresTest)
+{
+ /// Initial sphere 1 position
+ Vector3d startPosition1(-0.1, 0.0, -0.2);
+ /// Final sphere 1 position
+ Vector3d endPosition1(0.1, 0.0, -0.2);
+ /// Initial sphere 1 radius;
+ double startRadius1 = 0.001;
+ /// Final sphere 1 radius;
+ double endRadius1 = 0.01;
+ /// Initial sphere 2 position
+ Vector3d startPosition2(0.0, -0.1, -0.2);
+ /// Final sphere 2 position
+ Vector3d endPosition2(0.0, 0.1, -0.2);
+ /// Initial sphere 2 radius;
+ double startRadius2 = 0.01;
+ /// Final sphere 2 radius;
+ double endRadius2 = 0.05;
+
+ /// Number of times to step the sphere position and radius from start to end.
+ /// This number of steps will be done in 1 second.
+ int numSteps = 100;
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<OsgManager> manager = std::make_shared<OsgManager>();
+
+ runtime->addManager(manager);
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ /// Add a graphics view element to the scene
+ std::shared_ptr<OsgViewElement> viewElement = std::make_shared<OsgViewElement>("view element");
+ scene->addSceneElement(viewElement);
+
+ /// Add the sphere representation to the view element, no need to make another scene element
+ std::shared_ptr<SphereRepresentation> sphereRepresentation1 =
+ std::make_shared<OsgSphereRepresentation>("sphere representation 1");
+ viewElement->addComponent(sphereRepresentation1);
+ std::shared_ptr<SphereRepresentation> sphereRepresentation2 =
+ std::make_shared<OsgSphereRepresentation>("sphere representation 2");
+ viewElement->addComponent(sphereRepresentation2);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(manager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ /// Calculate t in [0.0, 1.0]
+ double t = static_cast<double>(i) / numSteps;
+ /// Interpolate position and radius
+ sphereRepresentation1->setLocalPose(makeRigidTransform(Quaterniond::Identity(), (1 - t) * startPosition1 +
+ t * endPosition1));
+ sphereRepresentation1->setRadius((1 - t) * startRadius1 + t * endRadius1);
+ sphereRepresentation2->setLocalPose(makeRigidTransform(Quaterniond::Identity(), (1 - t) * startPosition2 +
+ t * endPosition2));
+ sphereRepresentation2->setRadius((1 - t) * startRadius2 + t * endRadius2);
+ /// The total number of steps should complete in 1 second
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+
+ runtime->stop();
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgVectorFieldRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgVectorFieldRepresentationRenderTests.cpp
new file mode 100644
index 0000000..28cf3cf
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgVectorFieldRepresentationRenderTests.cpp
@@ -0,0 +1,218 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// Render Tests for the OsgVectorFieldRepresentation class.
+
+#include "SurgSim/DataStructures/Vertex.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Graphics/VectorField.h"
+#include "SurgSim/Graphics/VectorFieldRepresentation.h"
+#include "SurgSim/Graphics/OsgVectorFieldRepresentation.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Testing/MathUtilities.h"
+
+using SurgSim::DataStructures::Vertex;
+using SurgSim::Graphics::OsgVectorFieldRepresentation;
+using SurgSim::Graphics::VectorFieldRepresentation;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Testing::interpolate;
+
+struct OsgVectorFieldRepresentationRenderTests : public SurgSim::Graphics::RenderTest
+{
+protected:
+ // A point is a location (X,Y,Z) in 3D space
+ std::vector<Vector3d> makeStartingPoints()
+ {
+ std::vector<Vector3d> points(8);
+ points[0] = Vector3d(1.0, 0.0, 0.0);
+ points[1] = Vector3d(0.0, 1.0, 0.0);
+ points[2] = Vector3d(-1.0, 0.0, 0.0);
+ points[3] = Vector3d(0.0, -1.0, 0.0);
+
+ points[4] = Vector3d(2.0, 0.0, 0.0);
+ points[5] = Vector3d(0.0, 2.0, 0.0);
+ points[6] = Vector3d(-2.0, 0.0, 0.0);
+ points[7] = Vector3d(0.0, -2.0, 0.0);
+ return points;
+ }
+
+ std::vector<Vector3d> makeEndingPoints()
+ {
+ std::vector<Vector3d> points(8);
+ points[0] = Vector3d(1.0, 1.0, 0.0);
+ points[1] = Vector3d(-1.0, 1.0, 0.0);
+ points[2] = Vector3d(-1.0, -1.0, 0.0);
+ points[3] = Vector3d(1.0, -1.0, 0.0);
+
+ points[4] = Vector3d(2.0, 2.0, 0.0);
+ points[5] = Vector3d(-2.0, 2.0, 0.0);
+ points[6] = Vector3d(-2.0, -2.0, 0.0);
+ points[7] = Vector3d(2.0, -2.0, 0.0);
+ return points;
+ }
+
+ // Color is represented as (R, G, B, alpha)
+ std::vector<Vector4d> makeStartingColors()
+ {
+ std::vector<Vector4d> colors(8);
+ colors[0] = Vector4d(1.0, 0.0, 0.0, 0.0);
+ colors[1] = Vector4d(0.0, 1.0, 0.0, 0.0);
+ colors[2] = Vector4d(0.0, 0.0, 1.0, 0.0);
+ colors[3] = Vector4d(1.0, 1.0, 0.0, 0.0);
+
+ colors[4] = Vector4d(1.0, 0.0, 1.0, 0.0);
+ colors[5] = Vector4d(1.0, 1.0, 1.0, 0.0);
+ colors[6] = Vector4d(1.0, 0.5, 0.8, 0.0);
+ colors[7] = Vector4d(0.5, 1.0, 0.5, 0.0);
+ return colors;
+ }
+
+ std::vector<Vector4d> makeEndingColors()
+ {
+ std::vector<Vector4d> colors(8);
+ colors[0] = Vector4d(0.0, 1.0, 0.0, 0.0);
+ colors[1] = Vector4d(0.0, 0.0, 1.0, 0.0);
+ colors[2] = Vector4d(1.0, 0.0, 0.0, 0.0);
+ colors[3] = Vector4d(0.0, 1.0, 1.0, 0.0);
+
+ colors[4] = Vector4d(0.0, 1.0, 0.0, 0.0);
+ colors[5] = Vector4d(0.0, 0.0, 1.0, 0.0);
+ colors[6] = Vector4d(1.0, 1.0, 0.0, 0.0);
+ colors[7] = Vector4d(1.0, 0.0, 1.0, 0.0);
+ return colors;
+ }
+
+ std::vector<SurgSim::Graphics::VectorFieldData> makeVectors(const std::vector<Vector3d>& points,
+ const std::vector<Vector4d>& colors)
+ {
+ std::vector<SurgSim::Graphics::VectorFieldData> vecs(8);
+ for (auto i = 0; i < 8; ++i)
+ {
+ vecs[i].direction = points[i];
+ vecs[i].color.setValue(colors[i]);
+ }
+ return vecs;
+ }
+
+ std::shared_ptr<VectorFieldRepresentation>
+ makeVectorFieldRepresentation(const std::vector<Vector3d>& points,
+ const std::vector<SurgSim::Graphics::VectorFieldData>& vectors)
+ {
+ auto representation = std::make_shared<OsgVectorFieldRepresentation>("Vector Field Representation");
+ auto vertices = representation->getVectorField();
+ // Binding vectors to points
+ auto it = std::begin(points);
+ auto v = std::begin(vectors);
+ for (; it != std::end(points); ++it, ++v)
+ {
+ vertices->addVertex(Vertex<SurgSim::Graphics::VectorFieldData>((*it), *v));
+ }
+ return representation;
+ }
+};
+
+TEST_F(OsgVectorFieldRepresentationRenderTests, AddVectors)
+{
+ auto vectorRepresentation = std::make_shared<OsgVectorFieldRepresentation>("vector field representation");
+ auto points = makeStartingPoints();
+ auto colors = makeStartingColors();
+ auto vectors = makeVectors(points, colors);
+ auto vectorField = vectorRepresentation->getVectorField();
+
+ vectorRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.0, -8.0)));
+ viewElement->addComponent(vectorRepresentation);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+
+ auto it = std::begin(points);
+ auto v = std::begin(vectors);
+ for (; it != std::end(points); ++it, ++v)
+ {
+ vectorField->addVertex(Vertex<SurgSim::Graphics::VectorFieldData>((*it), *v));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(250));
+ }
+}
+
+TEST_F(OsgVectorFieldRepresentationRenderTests, LineWidth)
+{
+ auto points = makeStartingPoints();
+ auto colors = makeStartingColors();
+ auto vectors = makeVectors(points, colors);
+ auto vectorRepresentation = makeVectorFieldRepresentation(points, vectors);
+
+ vectorRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.0, -8.0)));
+
+ viewElement->addComponent(vectorRepresentation);
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+
+ int numSteps = 100;
+ double startWidth = 0.0;
+ double endWidth = 10.0;
+
+ // Vary line widths as time changes
+ for (int i = 0; i < numSteps; ++i)
+ {
+ double t = static_cast<double>(i) / numSteps;
+ vectorRepresentation->setLineWidth(interpolate(startWidth, endWidth, t));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+}
+
+TEST_F(OsgVectorFieldRepresentationRenderTests, ChangingVectorField)
+{
+ auto startPoints = makeStartingPoints();
+ auto endPoints = makeEndingPoints();
+
+ auto startColors = makeStartingColors();
+ auto endColors = makeStartingColors();
+
+ auto startVectors = makeVectors(startPoints, startColors);
+ auto endVectors = makeVectors(endPoints, endColors);
+
+ auto vectorRepresentation = makeVectorFieldRepresentation(startPoints, endVectors);
+ vectorRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.0, -8.0)));
+
+ viewElement->addComponent(vectorRepresentation);
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(250));
+
+ auto& vertexList = vectorRepresentation->getVectorField()->getVertices();
+ int numSteps = 100;
+ for (int i = 0; i < numSteps; ++i)
+ {
+ double t = static_cast<double>(i) / numSteps;
+ for (int j = 0; j < 8; ++j)
+ {
+ vertexList[j].position = interpolate(startPoints[j], endPoints[j], t);
+ vertexList[j].data.direction = interpolate(endVectors[j].direction, startVectors[j].direction, t);
+ vertexList[j].data.color.setValue(interpolate(startColors[j], endColors[j], t));
+ }
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
+ }
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/RenderTests/OsgViewElementRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgViewElementRenderTests.cpp
new file mode 100644
index 0000000..20e1e4f
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgViewElementRenderTests.cpp
@@ -0,0 +1,196 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Render Tests for the OsgViewElement class.
+
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Blocks/PoseInterpolator.h"
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+// Note: This tests the ViewElement, don't derive from RenderTest
+
+
+TEST(OsgViewElementRenderTests, MoveAndResizeWindowTest)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<OsgManager> manager = std::make_shared<OsgManager>();
+
+ runtime->addManager(manager);
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ /// Add a graphics component to the scene
+ std::shared_ptr<OsgViewElement> viewElement = std::make_shared<OsgViewElement>("test element");
+ scene->addSceneElement(viewElement);
+
+ /// Set initial position to (50, 60), dimensions to 200 x 100 and disable the window border
+
+ std::array<int, 2> position = {50, 60};
+ std::array<int, 2> dimensions = {200, 100};
+
+ viewElement->getView()->setPosition(position);
+ viewElement->getView()->setDimensions(dimensions);
+ viewElement->getView()->setWindowBorderEnabled(false);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(manager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ /// Move the window to (100, 200)
+ position[0] = 100;
+ position[1] = 200;
+
+ viewElement->getView()->setPosition(position);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ /// Enable the window border and resize the window to 400 x 500
+ viewElement->getView()->setWindowBorderEnabled(true);
+ dimensions[0] = 400;
+ dimensions[1] = 500;
+ viewElement->getView()->setDimensions(dimensions);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ runtime->stop();
+}
+
+// Just an empty screen but it should be fullscreen
+TEST(OsgViewElementRenderTest, FullScreenView)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<OsgManager> manager = std::make_shared<OsgManager>();
+
+ runtime->addManager(manager);
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ /// Add a graphics component to the scene
+ std::shared_ptr<OsgViewElement> viewElement = std::make_shared<OsgViewElement>("test element");
+ scene->addSceneElement(viewElement);
+
+ auto osgView = std::static_pointer_cast<OsgView>(viewElement->getView());
+ osgView->setFullScreen(true);
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+ runtime->stop();
+}
+
+TEST(OsgViewElementRenderTest, StereoView)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<OsgManager> manager = std::make_shared<OsgManager>();
+
+ runtime->addManager(manager);
+ runtime->addManager(std::make_shared<SurgSim::Framework::BehaviorManager>());
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ /// Add a graphics component to the scene
+ std::shared_ptr<OsgViewElement> viewElement = std::make_shared<OsgViewElement>("view");
+
+ auto boxElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("box");
+
+ RigidTransform3d pose =
+ makeRigidTransform(Vector3d(1.0, 1.0, 1.0), Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 1.0, 0.0));
+ viewElement->setPose(pose);
+ scene->addSceneElement(viewElement);
+
+ auto box = std::make_shared<OsgBoxRepresentation>("box");
+ box->setSizeXYZ(0.1, 0.1, 0.2);
+ boxElement->addComponent(box);
+
+ RigidTransform3d from =
+ makeRigidTransform(Vector3d(0.2, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 1.0, 0.0));
+ RigidTransform3d to =
+ makeRigidTransform(Vector3d(-0.2, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 1.0, 0.0));
+ auto interpolator = std::make_shared<SurgSim::Blocks::PoseInterpolator>("interpolator");
+
+ interpolator->setDuration(2.0);
+ interpolator->setStartingPose(from);
+ interpolator->setEndingPose(to);
+ interpolator->setPingPong(true);
+ interpolator->setTarget(boxElement);
+
+ boxElement->addComponent(interpolator);
+
+ scene->addSceneElement(boxElement);
+
+ auto osgView = std::static_pointer_cast<OsgView>(viewElement->getView());
+ osgView->setStereoMode(View::STEREO_MODE_HORIZONTAL_SPLIT);
+ osgView->setDisplayType(View::DISPLAY_TYPE_MONITOR);
+ osgView->setEyeSeparation(0.06);
+ osgView->setScreenWidth(0.486918);
+ osgView->setScreenHeight(0.273812);
+ osgView->setScreenDistance(1.0);
+
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+ runtime->stop();
+}
+
+// Verify a bug fix on the OsgView that had to do with the deallocation of the mouse and
+// keyboard handler.
+TEST(OsgViewElementRenderTest, KeyBoardMouseTest)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<OsgManager> manager = std::make_shared<OsgManager>();
+
+ runtime->addManager(manager);
+ runtime->addManager(std::make_shared<SurgSim::Framework::BehaviorManager>());
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ /// Add a graphics component to the scene
+ std::shared_ptr<OsgViewElement> viewElement = std::make_shared<OsgViewElement>("view");
+ scene->addSceneElement(viewElement);
+
+ auto osgView = std::static_pointer_cast<OsgView>(viewElement->getView());
+ osgView->enableKeyboardDevice(true);
+ osgView->enableMouseDevice(true);
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+ runtime->stop();
+}
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/RenderTest.cpp b/SurgSim/Graphics/RenderTests/RenderTest.cpp
new file mode 100644
index 0000000..864e047
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/RenderTest.cpp
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/RenderTests//RenderTest.h"
+
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+
+void RenderTest::SetUp()
+{
+ runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ applicationData = runtime->getApplicationData();
+ graphicsManager = std::make_shared<SurgSim::Graphics::OsgManager>();
+
+ runtime->addManager(graphicsManager);
+ runtime->addManager(std::make_shared<SurgSim::Framework::BehaviorManager>());
+
+ scene = runtime->getScene();
+
+ viewElement = std::make_shared<OsgViewElement>("view element");
+ std::array<int, 2> position = {100, 100};
+ viewElement->getView()->setPosition(position);
+ viewElement->getView()->setWindowBorderEnabled(true);
+
+ scene->addSceneElement(viewElement);
+}
+
+void RenderTest::TearDown()
+{
+ runtime->stop();
+}
+
+std::shared_ptr<ScreenSpaceQuadRepresentation> RenderTest::makeQuad(
+ const std::string& name,
+ int width,
+ int height,
+ int x,
+ int y)
+{
+ std::shared_ptr<OsgScreenSpaceQuadRepresentation> quad =
+ std::make_shared<OsgScreenSpaceQuadRepresentation>(name);
+ quad->setSize(width, height);
+ Quaterniond quat;
+ quat = SurgSim::Math::makeRotationQuaternion(0.0, Vector3d::UnitY().eval());
+ quad->setLocalPose(SurgSim::Math::makeRigidTransform(quat, Vector3d(x, y, -0.2)));
+ return quad;
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/RenderTests/RenderTest.h b/SurgSim/Graphics/RenderTests/RenderTest.h
new file mode 100644
index 0000000..878a1e6
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/RenderTest.h
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_RENDERTESTS_RENDERTEST_H
+#define SURGSIM_GRAPHICS_RENDERTESTS_RENDERTEST_H
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/ApplicationData.h"
+
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+class Runtime;
+class Scene;
+
+}
+
+namespace Graphics
+{
+
+struct RenderTest : public ::testing::Test
+{
+public:
+
+ virtual void SetUp();
+
+ virtual void TearDown();
+
+ std::shared_ptr<ScreenSpaceQuadRepresentation> makeQuad(
+ const std::string& name,
+ int width,
+ int height,
+ int x,
+ int y);
+
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime;
+ std::shared_ptr<OsgManager> graphicsManager;
+ std::shared_ptr<SurgSim::Framework::Scene> scene;
+ std::shared_ptr<OsgViewElement> viewElement;
+ std::shared_ptr<const SurgSim::Framework::ApplicationData> applicationData;
+
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/RenderTests/config.txt.in b/SurgSim/Graphics/RenderTests/config.txt.in
new file mode 100644
index 0000000..f614965
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/config.txt.in
@@ -0,0 +1,3 @@
+${SURGSIM_SOURCE_DIR}/Data/
+${SURGSIM_SOURCE_DIR}/SurgSim/Testing/
+${CMAKE_CURRENT_SOURCE_DIR}/Data/
\ No newline at end of file
diff --git a/SurgSim/Graphics/Representation.cpp b/SurgSim/Graphics/Representation.cpp
new file mode 100644
index 0000000..096e83f
--- /dev/null
+++ b/SurgSim/Graphics/Representation.cpp
@@ -0,0 +1,132 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/Representation.h"
+
+#include "SurgSim/Graphics/Material.h"
+#include "SurgSim/Framework/Log.h"
+
+using SurgSim::Graphics::Material;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+const std::string Representation::DefaultGroupName = "__OssDefault__";
+const std::string Representation::DefaultHudGroupName = "__OssDefaulHud__";
+
+Representation::Representation(const std::string& name) :
+ SurgSim::Framework::Representation(name),
+ m_isVisible(true)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, bool, Visible, isVisible, setVisible);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, std::vector<std::string>,
+ GroupReferences, getGroupReferences, setGroupReferences);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, bool, DrawAsWireFrame,
+ getDrawAsWireFrame, setDrawAsWireFrame);
+
+ addGroupReference(DefaultGroupName);
+}
+
+bool Representation::addGroupReference(const std::string& name)
+{
+ bool result = false;
+ if (!checkAwake("addGroupReference"))
+ {
+ auto insertion = m_groups.insert(name);
+ result = insertion.second;
+ }
+ return result;
+}
+
+void Representation::addGroupReferences(const std::vector<std::string>& groups)
+{
+ if (!checkAwake("addGroupReferences"))
+ {
+ for (auto it = groups.cbegin(); it != groups.cend(); ++it)
+ {
+ addGroupReference(*it);
+ }
+ }
+}
+
+void Representation::setGroupReferences(const std::vector<std::string>& groups)
+{
+ if (!checkAwake("setGroupReferences"))
+ {
+ m_groups.clear();
+ for (auto it = groups.cbegin(); it != groups.cend(); ++it)
+ {
+ addGroupReference(*it);
+ }
+ }
+}
+
+std::vector<std::string> Representation::getGroupReferences()
+{
+ return std::vector<std::string>(std::begin(m_groups), std::end(m_groups));
+}
+
+void Representation::clearGroupReferences()
+{
+ if (!checkAwake("clearGroupReference"))
+ {
+ m_groups.clear();
+ }
+}
+
+bool Representation::removeGroupReference(const std::string& name)
+{
+ bool result = false;
+ if (!checkAwake("removeGroupReference"))
+ {
+ result = (m_groups.erase(name) != 0u);
+ }
+ return result;
+}
+
+void Representation::setGroupReference(const std::string& group)
+{
+ if (!checkAwake("setGroupReference"))
+ {
+ clearGroupReferences();
+ m_groups.insert(group);
+ }
+}
+
+bool Representation::checkAwake(const std::string& functionName)
+{
+ if (isAwake())
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getLogger("Graphics")) <<
+ "Representation::" << functionName << "() was called while the component " <<
+ "was already awake for component " << getName() << " this has no effect and should be avoided.";
+ }
+ return isAwake();
+}
+
+Representation::~Representation()
+{
+
+}
+
+void Representation::setLocalActive(bool val)
+{
+ Component::setLocalActive(val);
+ setVisible(m_isVisible);
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/Representation.h b/SurgSim/Graphics/Representation.h
new file mode 100644
index 0000000..cf63a3d
--- /dev/null
+++ b/SurgSim/Graphics/Representation.h
@@ -0,0 +1,139 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_REPRESENTATION_H
+#define SURGSIM_GRAPHICS_REPRESENTATION_H
+
+#include "SurgSim/Framework/Representation.h"
+
+#include "SurgSim/Math/RigidTransform.h"
+
+#include <unordered_set>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+class Manager;
+class Material;
+
+/// Base graphics representation class, which defines the interface that all graphics representations must implement.
+///
+/// A Graphics::Representation is the visual Framework::Representation of a Framework::SceneElement in the
+/// Framework::Scene. Graphical representations can request to be assigned to groups, groups are used to select certain
+/// elements for rendering in various special effects.
+class Representation : public SurgSim::Framework::Representation
+{
+public:
+
+ static const std::string DefaultGroupName;
+ static const std::string DefaultHudGroupName;
+
+ /// Constructor
+ /// \param name Name of the representation
+ explicit Representation(const std::string& name);
+
+ /// Destructor
+ virtual ~Representation();
+
+ /// Sets whether the representation is currently visible
+ /// \note If the representation is inactive, this method has no visible effect.
+ /// \param visible True for visible, false for invisible
+ virtual void setVisible(bool visible) = 0;
+
+ /// Gets whether the representation is currently visible
+ /// \return visible True for visible, false for invisible
+ virtual bool isVisible() const = 0;
+
+ virtual void setLocalActive(bool val) override;
+
+ /// Sets the material that defines the visual appearance of the representation
+ /// \param material Graphics material
+ /// \return True if set successfully, otherwise false
+ virtual bool setMaterial(std::shared_ptr<Material> material) = 0;
+
+ /// Gets the material that defines the visual appearance of the representation
+ /// \return Graphics material
+ virtual std::shared_ptr<Material> getMaterial() const = 0;
+
+ /// Removes the material from the representation
+ virtual void clearMaterial() = 0;
+
+ /// Sets the representation to render as a wire frame.
+ /// \param val true if this representation should be rendered as a wireframe.
+ virtual void setDrawAsWireFrame(bool val) = 0;
+
+ /// Return if the representation is rendered as a wire frame.
+ /// \return True if this representation is rendered as a wireframe; false if not.
+ virtual bool getDrawAsWireFrame() const = 0;
+
+ /// Updates the representation
+ /// \param dt The time in seconds of the preceding timestep.
+ virtual void update(double dt) = 0;
+
+ /// Add a reference to a group, this will eventual add this representation to the group with the
+ /// the same name.
+ /// \param name The name of the group.
+ /// \return true if it succeeds, false if the group reference already exists.
+ virtual bool addGroupReference(const std::string& name);
+
+
+ /// Adds a list of group references.
+ /// \param groups The references.
+ void addGroupReferences(const std::vector<std::string>& groups);
+
+ /// Sets the list of group references. Clearing all the old references
+ /// \param groups The references.
+ void setGroupReferences(const std::vector<std::string>& groups);
+
+ /// Helper functions, this clears all the references and sets, only the reference
+ /// given in the parameter
+ /// \param group The reference to be used for this representation
+ void setGroupReference(const std::string& group);
+
+ /// Gets group references.
+ /// \return The group references.
+ std::vector<std::string> getGroupReferences();
+
+ /// Function to remove an unwanted reference.
+ /// \param group The name of the reference that should be removed
+ /// \return true If the reference was found and removed
+ bool removeGroupReference(const std::string& group);
+
+ /// Clear all the Group references
+ void clearGroupReferences();
+
+protected:
+ bool m_isVisible;
+
+private:
+
+ /// List of groups that this representation would like to be added
+ std::unordered_set<std::string> m_groups;
+
+ /// Check if the representation is awake and print a warning message if it is.
+ /// \param functionName the name of the calling function to be used in the error message
+ /// \return the value of isAwake()
+ bool checkAwake(const std::string& functionName);
+
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_REPRESENTATION_H
diff --git a/SurgSim/Graphics/SceneryRepresentation.h b/SurgSim/Graphics/SceneryRepresentation.h
new file mode 100644
index 0000000..cf064e2
--- /dev/null
+++ b/SurgSim/Graphics/SceneryRepresentation.h
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_SCENERYREPRESENTATION_H
+#define SURGSIM_GRAPHICS_SCENERYREPRESENTATION_H
+
+#include "SurgSim/Graphics/Representation.h"
+
+#include <string>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base class defining the interface for a Graphics Scenery Object.
+class SceneryRepresentation : public virtual Representation
+{
+public:
+
+ /// Constructor.
+ /// \param name The name of the representation.
+ explicit SceneryRepresentation(const std::string& name): Representation(name)
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SceneryRepresentation, std::string, FileName, getFileName, setFileName);
+ }
+
+ /// Return file name of the object
+ /// \return File name of the object
+ virtual std::string getFileName() const = 0;
+
+ /// Set file name of the object to be loaded
+ /// \param fileName Name of the file to be loaded
+ virtual void setFileName(const std::string& fileName) = 0;
+};
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_SCENERYREPRESENTATION_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/ScreenSpaceQuadRepresentation.h b/SurgSim/Graphics/ScreenSpaceQuadRepresentation.h
new file mode 100644
index 0000000..bcd0804
--- /dev/null
+++ b/SurgSim/Graphics/ScreenSpaceQuadRepresentation.h
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_SCREENSPACEQUADREPRESENTATION_H
+#define SURGSIM_GRAPHICS_SCREENSPACEQUADREPRESENTATION_H
+
+#include "SurgSim/Graphics/Representation.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class View;
+class Texture;
+
+/// A quad to display on the screen in screen space coordinates, use setPose() to set the position but
+/// x,y are presumed to be in screen space with 0|0 being in the lower left corner
+class ScreenSpaceQuadRepresentation : public virtual Representation
+{
+public:
+
+ /// Constructor.
+ /// \param name The name.
+ explicit ScreenSpaceQuadRepresentation(const std::string name) : Representation(name)
+ {
+
+ }
+
+ ~ScreenSpaceQuadRepresentation()
+ {
+
+ }
+
+ /// Sets the location in screen space.
+ /// \param x,y The x and y coordinates.
+ virtual void setLocation(double x, double y) = 0;
+
+ /// Gets the location in screen space.
+ /// \param [out] x,y If non-null the x and y coordinates, may throw if null is passed.
+ virtual void getLocation(double* x, double* y) = 0;
+
+ /// Sets the size for the quad in screen coordinates.
+ /// \param width The width of the quad in screen coordinates.
+ /// \param height The height of the quad in screen coordinates.
+ virtual void setSize(double width, double height) = 0;
+
+ /// Gets the size of the quad.
+ /// \param [out] width If non-null, the width, may throw if null is passed.
+ /// \param [out] height If non-null, the height, may throw if null is passed.
+ virtual void getSize(double* width, double* height) const = 0;
+
+ /// Sets a Texture for this quad, this should replace a current texture, this is a convenience function and
+ /// this will use the uniform name "diffuseMap" for the uniform in this operation. This can be accomplished
+ /// from the outside as well by using the material.
+ /// \param texture The texture to be set on the quad.
+ /// \return true if it succeeds, false if it fails.
+ virtual bool setTexture(std::shared_ptr<Texture> texture) = 0;
+
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/Shader.h b/SurgSim/Graphics/Shader.h
new file mode 100644
index 0000000..5d5e89f
--- /dev/null
+++ b/SurgSim/Graphics/Shader.h
@@ -0,0 +1,123 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_SHADER_H
+#define SURGSIM_GRAPHICS_SHADER_H
+
+#include <string>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base class that defines the interface for graphics shaders.
+///
+/// Shaders are the programs executed on the GPU to render the scene geometry.
+class Shader
+{
+public:
+ /// Destructor
+ virtual ~Shader() = 0;
+
+ /// Returns true if the vertex shader has been set, otherwise false.
+ virtual bool hasVertexShader() const = 0;
+
+ /// Removes the vertex shader, returning that portion of the shader program to fixed-function.
+ virtual void clearVertexShader() = 0;
+
+ /// Loads the vertex shader source code from a file
+ /// \param filePath Path to file containing shader source code
+ /// \return True if the source is successfully loaded, otherwise false.
+ virtual bool loadVertexShaderSource(const std::string& filePath) = 0;
+
+ /// Set the vertex shader source code
+ /// \param source Shader source code
+ virtual void setVertexShaderSource(const std::string& source) = 0;
+
+ /// Gets the vertex shader source code
+ /// \return Shader source code
+ virtual bool getVertexShaderSource(std::string* source) const = 0;
+
+ /// Returns true if the geometry shader has been set, otherwise false.
+ virtual bool hasGeometryShader() const = 0;
+
+ /// Removes the geometry shader, returning that portion of the shader program to fixed-function.
+ virtual void clearGeometryShader() = 0;
+
+ /// Loads the geometry shader source code from a file
+ /// \param filePath Path to file containing shader source code
+ /// \return True if the source is successfully loaded, otherwise false.
+ virtual bool loadGeometryShaderSource(const std::string& filePath) = 0;
+
+ /// Set the geometry shader source code
+ /// \param source Shader source code
+ virtual void setGeometryShaderSource(const std::string& source) = 0;
+
+ /// Gets the geometry shader source code
+ /// \return Shader source code
+ virtual bool getGeometryShaderSource(std::string* source) const = 0;
+
+
+ /// Returns true if the fragment shader has been set, otherwise false.
+ virtual bool hasFragmentShader() const = 0;
+
+ /// Removes the fragment shader, returning that portion of the shader program to fixed-function.
+ virtual void clearFragmentShader() = 0;
+
+ /// Loads the fragment shader source code from a file
+ /// \param filePath Path to file containing shader source code
+ /// \return True if the source is successfully loaded, otherwise false.
+ virtual bool loadFragmentShaderSource(const std::string& filePath) = 0;
+
+ /// Set the fragment shader source code
+ /// \param source Shader source code
+ virtual void setFragmentShaderSource(const std::string& source) = 0;
+
+ /// Gets the fragment shader source code
+ /// \return Shader source code
+ virtual bool getFragmentShaderSource(std::string* source) const = 0;
+
+ /// Clears the entire shader, returning to fixed-function pipeline.
+ virtual void clear()
+ {
+ clearVertexShader();
+ clearGeometryShader();
+ clearFragmentShader();
+ }
+
+ /// When this is set to true, this shader should be used instead of other shaders that might apply, depending
+ /// on the hierarchy that is set out. E.g if this shader is on a camera, the shaders that occur in a group
+ /// attached to that camera will be overridden.
+ /// This will usually be used in conjunction with \sa RenderPass.
+ /// \param val If true the shader should override shaders in lower levels.
+ virtual void setGlobalScope(bool val) = 0;
+
+ /// Query if this shader is of global scope.
+ /// \return true if global scope, false if not.
+ virtual bool isGlobalScope() const = 0;
+
+};
+
+inline Shader::~Shader()
+{
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_SHADER_H
diff --git a/SurgSim/Graphics/SphereRepresentation.h b/SurgSim/Graphics/SphereRepresentation.h
new file mode 100644
index 0000000..9aeb1b5
--- /dev/null
+++ b/SurgSim/Graphics/SphereRepresentation.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_SPHEREREPRESENTATION_H
+#define SURGSIM_GRAPHICS_SPHEREREPRESENTATION_H
+
+#include "SurgSim/Graphics/Representation.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base graphics sphere representation class, which defines the basic interface for a sphere that can be visualized.
+/// The sphere center is at (0, 0, 0).
+class SphereRepresentation : public virtual Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ /// \post The sphere radius is 1.0.
+ explicit SphereRepresentation(const std::string& name) : Representation(name)
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphereRepresentation, double, Radius, getRadius, setRadius);
+ }
+
+ /// Sets the radius of the sphere
+ virtual void setRadius(double radius) = 0;
+ /// Returns the radius of the sphere
+ virtual double getRadius() const = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_SPHEREREPRESENTATION_H
diff --git a/SurgSim/Graphics/Texture.h b/SurgSim/Graphics/Texture.h
new file mode 100644
index 0000000..41bcb61
--- /dev/null
+++ b/SurgSim/Graphics/Texture.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_TEXTURE_H
+#define SURGSIM_GRAPHICS_TEXTURE_H
+
+#include <string>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base class defining the interface for a Graphics Texture.
+class Texture
+{
+public:
+ /// Destructor
+ virtual ~Texture()
+ {
+ }
+
+ /// Loads an image into the texture from a file
+ /// \param filePath Path to the image file
+ /// \return True if the image is successfully loaded, otherwise false
+ virtual bool loadImage(const std::string& filePath) = 0;
+
+ /// Removes the image from the texture
+ virtual void clearImage() = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_TEXTURE_H
diff --git a/SurgSim/Graphics/Texture1d.h b/SurgSim/Graphics/Texture1d.h
new file mode 100644
index 0000000..2640e8a
--- /dev/null
+++ b/SurgSim/Graphics/Texture1d.h
@@ -0,0 +1,47 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_TEXTURE1D_H
+#define SURGSIM_GRAPHICS_TEXTURE1D_H
+
+#include "SurgSim/Graphics/OsgTexture.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base class defining the interface for a 1D Graphics Texture.
+/// A 1D Texture only has width.
+/// \note Normalized texture coordinates are used to access this texture.
+class Texture1d : public virtual Texture
+{
+public:
+ /// Sets the size of the texture
+ /// \param width Width of the texture
+ /// \note Use this to setup a texture as a render target rather than loading from file.
+ virtual void setSize(int width) = 0;
+
+ /// Gets the size of the texture
+ /// \return width Width of the texture
+ virtual void getSize(int* width) const = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_TEXTURE1D_H
diff --git a/SurgSim/Graphics/Texture2d.h b/SurgSim/Graphics/Texture2d.h
new file mode 100644
index 0000000..22fb922
--- /dev/null
+++ b/SurgSim/Graphics/Texture2d.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_TEXTURE2D_H
+#define SURGSIM_GRAPHICS_TEXTURE2D_H
+
+#include "SurgSim/Graphics/Texture.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base class defining the interface for a 2D Graphics Texture.
+/// A 2D Texture has width and height.
+/// \note Normalized texture coordinates are used to access this texture.
+class Texture2d : public virtual Texture
+{
+public:
+ /// Sets the size of the texture
+ /// \param width Width of the texture
+ /// \param height Height of the texture
+ /// \note Use this to setup a texture as a render target rather than loading from file.
+ virtual void setSize(int width, int height) = 0;
+
+ /// Gets the size of the texture
+ /// \param[out] width Width of the texture
+ /// \param[out] height Height of the texture
+ virtual void getSize(int* width, int* height) const = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_TEXTURE2D_H
diff --git a/SurgSim/Graphics/Texture3d.h b/SurgSim/Graphics/Texture3d.h
new file mode 100644
index 0000000..f7ee415
--- /dev/null
+++ b/SurgSim/Graphics/Texture3d.h
@@ -0,0 +1,59 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_TEXTURE3D_H
+#define SURGSIM_GRAPHICS_TEXTURE3D_H
+
+#include "SurgSim/Graphics/Texture.h"
+
+#include <vector>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base class defining the interface for a 3D Graphics Texture.
+/// A 3D Texture has width, height, and depth.
+/// \note Normalized texture coordinates are used to access this texture.
+class Texture3d : public virtual Texture
+{
+public:
+ /// Sets the size of the texture
+ /// \param width Width of the texture
+ /// \param height Height of the texture
+ /// \param depth Depth of the texture
+ /// \note Use this to setup a texture as a render target rather than loading from file.
+ virtual void setSize(int width, int height, int depth) = 0;
+
+ /// Gets the size of the texture
+ /// \param[out] width Width of the texture
+ /// \param[out] height Height of the texture
+ /// \param[out] depth Depth of the texture
+ virtual void getSize(int* width, int* height, int* depth) const = 0;
+
+ /// Loads images slices from files into the 3D texture
+ /// \param filePaths Paths to the image files
+ /// \return True if the image is successfully loaded, otherwise false
+ /// \note The slices are stacked in the order provided to create the depth of the 3D texture.
+ virtual bool loadImageSlices(const std::vector<std::string>& filePaths) = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_TEXTURE3D_H
diff --git a/SurgSim/Graphics/TextureCubeMap.h b/SurgSim/Graphics/TextureCubeMap.h
new file mode 100644
index 0000000..7ca4c1e
--- /dev/null
+++ b/SurgSim/Graphics/TextureCubeMap.h
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_TEXTURECUBEMAP_H
+#define SURGSIM_GRAPHICS_TEXTURECUBEMAP_H
+
+#include "SurgSim/Graphics/Texture.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base class defining the interface for a Cube Map Graphics Texture.
+/// A Cube Map Texture has a width and height, which is the same for each face of the cube.
+class TextureCubeMap : public virtual Texture
+{
+public:
+ /// Sets the size of the texture, which is the same for each face of the cube
+ /// \param width Width of the texture
+ /// \param height Height of the texture
+ /// \note Use this to setup a texture as a render target rather than loading from file.
+ virtual void setSize(int width, int height) = 0;
+
+ /// Gets the size of the texture, which is the same for each face of the cube
+ /// \param[out] width Width of the texture
+ /// \param[out] height Height of the texture
+ virtual void getSize(int* width, int* height) const = 0;
+
+ /// Loads images from files into the faces of the cube map
+ /// \param negativeX Path to the image for the (-X) face
+ /// \param positiveX Path to the image for the (+X) face
+ /// \param negativeY Path to the image for the (-Y) face
+ /// \param positiveY Path to the image for the (+Y) face
+ /// \param negativeZ Path to the image for the (-Z) face
+ /// \param positiveZ Path to the image for the (+Z) face
+ /// \return True if the image is successfully loaded, otherwise false
+ virtual bool loadImageFaces(const std::string& negativeX, const std::string& positiveX,
+ const std::string& negativeY, const std::string& positiveY,
+ const std::string& negativeZ, const std::string& positiveZ) = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_TEXTURECUBEMAP_H
diff --git a/SurgSim/Graphics/TextureRectangle.h b/SurgSim/Graphics/TextureRectangle.h
new file mode 100644
index 0000000..2ef862d
--- /dev/null
+++ b/SurgSim/Graphics/TextureRectangle.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_TEXTURERECTANGLE_H
+#define SURGSIM_GRAPHICS_TEXTURERECTANGLE_H
+
+#include "SurgSim/Graphics/Texture.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base class defining the interface for a Rectangle Graphics Texture.
+/// A Rectangle Texture has width and height.
+/// \note Texel coordinates are used to access this texture.
+class TextureRectangle : public virtual Texture
+{
+public:
+ /// Sets the size of the texture
+ /// \param width Width of the texture
+ /// \param height Height of the texture
+ /// \note Use this to setup a texture as a render target rather than loading from file.
+ virtual void setSize(int width, int height) = 0;
+
+ /// Gets the size of the texture
+ /// \param[out] width Width of the texture
+ /// \param[out] height Height of the texture
+ virtual void getSize(int* width, int* height) const = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_TEXTURERECTANGLE_H
diff --git a/SurgSim/Graphics/TriangleNormalGenerator.cpp b/SurgSim/Graphics/TriangleNormalGenerator.cpp
new file mode 100644
index 0000000..634e48b
--- /dev/null
+++ b/SurgSim/Graphics/TriangleNormalGenerator.cpp
@@ -0,0 +1,96 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/TriangleNormalGenerator.h"
+#include "SurgSim/Framework/Log.h"
+
+#include <osg/TriangleIndexFunctor>
+#include <osg/Vec3>
+#include <osg/Array>
+
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TriangleNormalGenerator::TriangleNormalGenerator() :
+ m_vertexArray(nullptr),
+ m_normalArray(nullptr)
+{
+}
+
+void TriangleNormalGenerator::set(
+ osg::Vec3Array* vertexArray,
+ osg::Vec3Array* normalArray)
+{
+
+ SURGSIM_ASSERT(vertexArray != nullptr) << "Need vertex array to generate normals!";
+ SURGSIM_ASSERT(normalArray != nullptr) << "Need normal array to store normals!";
+ SURGSIM_ASSERT(normalArray->size() == vertexArray->size()) << "Vertex and normal array need to have same size";
+
+ m_vertexArray = vertexArray;
+ m_normalArray = normalArray;
+ m_size = vertexArray->size();
+}
+
+void TriangleNormalGenerator::normalize()
+{
+ for(osg::Vec3Array::iterator normalIt = m_normalArray->begin(); normalIt != m_normalArray->end(); ++normalIt)
+ {
+ (*normalIt).normalize();
+ }
+}
+
+void TriangleNormalGenerator::reset()
+{
+ for(osg::Vec3Array::iterator normalIt = m_normalArray->begin(); normalIt != m_normalArray->end(); ++normalIt)
+ {
+ (*normalIt).set(0.0f, 0.0f, 0.0f);
+ }
+
+}
+
+void TriangleNormalGenerator::operator() (size_t vertexIndex1,
+ size_t vertexIndex2,
+ size_t vertexIndex3)
+{
+ if (vertexIndex1 == vertexIndex2 || vertexIndex2 == vertexIndex3 || vertexIndex1 == vertexIndex3)
+ {
+ return;
+ }
+
+ const osg::Vec3& v1 = (*m_vertexArray)[vertexIndex1];
+ const osg::Vec3& v2 = (*m_vertexArray)[vertexIndex2];
+ const osg::Vec3& v3 = (*m_vertexArray)[vertexIndex3];
+ osg::Vec3 normal = (v2-v1) ^ (v3-v1);
+ normal.normalize();
+
+ (*m_normalArray)[vertexIndex1] += normal;
+ (*m_normalArray)[vertexIndex2] += normal;
+ (*m_normalArray)[vertexIndex3] += normal;
+}
+
+osg::TriangleIndexFunctor<TriangleNormalGenerator> createNormalGenerator(
+ osg::Vec3Array* vertexArray,
+ osg::Vec3Array* normalArray)
+{
+ osg::TriangleIndexFunctor<TriangleNormalGenerator> result;
+ result.set(vertexArray, normalArray);
+ return result;
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/TriangleNormalGenerator.h b/SurgSim/Graphics/TriangleNormalGenerator.h
new file mode 100644
index 0000000..34a8a7e
--- /dev/null
+++ b/SurgSim/Graphics/TriangleNormalGenerator.h
@@ -0,0 +1,80 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_TRIANGLENORMALGENERATOR_H
+#define SURGSIM_GRAPHICS_TRIANGLENORMALGENERATOR_H
+
+#include <osg/NodeVisitor>
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osg/TriangleIndexFunctor>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+
+/// Triangle index functor which calculates normals for the vertices of a geometry, use
+/// createNormalGenerator to instantiate this
+class TriangleNormalGenerator
+{
+public:
+
+ /// Sets the arrays required to generate normals
+ /// \pre vertexArray and normalArray, need to have the same number of entries and not be nullptr
+ /// \param vertexArray Array containing vertex positions
+ /// \param normalArray Array to store calculated normals
+ void set(osg::Vec3Array* vertexArray,
+ osg::Vec3Array* normalArray);
+
+ /// Normalizes the calculated normals, this needs to be called after the pass to normalize all the normals
+ /// Due to the osg way this object is called there is no real good way of having this called automatically
+ void normalize();
+
+ /// Resets all calculated normals to 0.
+ void reset();
+
+ /// Calculates the triangle normal and adds it to each adjacent vertex normal.
+ /// \param vertexIndex1 First triangle vertex index
+ /// \param vertexIndex2 Second triangle vertex index
+ /// \param vertexIndex3 Third triangle vertex index
+ void operator() (size_t vertexIndex1, size_t vertexIndex2, size_t vertexIndex3);
+
+protected:
+ /// Constructor
+ TriangleNormalGenerator();
+
+private:
+
+ /// Array containing vertex positions
+ osg::ref_ptr<osg::Vec3Array> m_vertexArray;
+
+ /// Array storing calculated normals
+ osg::ref_ptr<osg::Vec3Array> m_normalArray;
+
+ /// Size of vertex and normal array
+ size_t m_size;
+};
+
+osg::TriangleIndexFunctor<TriangleNormalGenerator> createNormalGenerator(
+ osg::Vec3Array* vertexArray,
+ osg::Vec3Array* normalArray);
+
+
+}; // Graphics
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/Uniform.h b/SurgSim/Graphics/Uniform.h
new file mode 100644
index 0000000..a3c48a5
--- /dev/null
+++ b/SurgSim/Graphics/Uniform.h
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_UNIFORM_H
+#define SURGSIM_GRAPHICS_UNIFORM_H
+
+#include "SurgSim/Graphics/UniformBase.h"
+
+#include <vector>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Base class for a graphics uniform with a value of type T.
+/// \tparam Value type
+template <class T>
+class Uniform : public virtual UniformBase
+{
+public:
+
+ Uniform() {
+ SURGSIM_ADD_RW_PROPERTY(Uniform, T, Value, get, set);
+ }
+
+ /// Sets the value of the uniform
+ virtual void set(const T& value) = 0;
+
+ /// Returns the value of the uniform
+ virtual const T& get() const = 0;
+};
+
+/// Specialization of Uniform for vectors of values.
+/// \tparam Value type stored in the vector
+template <class T>
+class Uniform<std::vector<T>> : public virtual UniformBase
+{
+public:
+ /// Returns the number of elements
+ virtual size_t getNumElements() const = 0;
+
+ /// Sets the value of one of the uniform's elements
+ /// \param index Index of the element
+ /// \param value Value to set
+ virtual void setElement(size_t index, const T& value) = 0;
+
+ /// Sets the value of all of the uniform's elements
+ /// \param value Vector of values
+ virtual void set(const std::vector<T>& value) = 0;
+
+ /// Gets the value of one of the uniform's elements
+ /// \param index Index of the element
+ /// \return Value of the element
+ virtual typename std::vector<T>::const_reference getElement(size_t index) const = 0;
+
+ /// Gets the value of all of the uniform's elements
+ /// \return Vector of values
+ virtual const std::vector<T>& get() const = 0;
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_UNIFORM_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/UniformBase.h b/SurgSim/Graphics/UniformBase.h
new file mode 100644
index 0000000..d7625af
--- /dev/null
+++ b/SurgSim/Graphics/UniformBase.h
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_UNIFORMBASE_H
+#define SURGSIM_GRAPHICS_UNIFORMBASE_H
+
+#include "SurgSim/Framework/Accessible.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Common base class for all graphics uniforms.
+///
+/// Graphics uniforms act as parameters to shader programs.
+/// \note
+/// SurgSim::Graphics::Uniform is templated on the type of value, so this base class allows a pointer to any type
+/// of Uniform.
+class UniformBase : public SurgSim::Framework::Accessible
+{
+public:
+ /// Destructor
+ virtual ~UniformBase() = 0;
+};
+
+inline UniformBase::~UniformBase()
+{
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_UNIFORMBASE_H
diff --git a/SurgSim/Graphics/UnitTests/CMakeLists.txt b/SurgSim/Graphics/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..19546da
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/CMakeLists.txt
@@ -0,0 +1,92 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories (
+ ${gtest_SOURCE_DIR}/include
+ ${gmock_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ GroupTests.cpp
+ ManagerTests.cpp
+ MeshTests.cpp
+ OsgBoxRepresentationTests.cpp
+ OsgCameraTests.cpp
+ OsgCapsuleRepresentationTests.cpp
+ OsgCylinderRepresentationTests.cpp
+ OsgGroupTests.cpp
+ OsgLightTests.cpp
+ OsgLogTests.cpp
+ OsgManagerTests.cpp
+ OsgMaterialTests.cpp
+ OsgMatrixConversionsTests.cpp
+ OsgMeshRepresentationTests.cpp
+ OsgOctreeRepresentationTests.cpp
+ OsgPlaneRepresentationTests.cpp
+ OsgPlaneTests.cpp
+ OsgPointCloudRepresentationTests.cpp
+ OsgQuaternionConversionsTests.cpp
+ OsgRenderTargetTests.cpp
+ OsgRepresentationTests.cpp
+ OsgRigidTransformConversionsTests.cpp
+ OsgSceneryRepresentationTests.cpp
+ OsgScreenSpaceQuadTests.cpp
+ OsgShaderTests.cpp
+ OsgSphereRepresentationTests.cpp
+ OsgTexture1dTests.cpp
+ OsgTexture2dTests.cpp
+ OsgTexture3dTests.cpp
+ OsgTextureCubeMapTests.cpp
+ OsgTextureRectangleTests.cpp
+ OsgTextureTests.cpp
+ OsgTextureUniformTests.cpp
+ OsgUniformBaseTests.cpp
+ OsgUniformTests.cpp
+ OsgUniformTypesTests.cpp
+ OsgUnitSphereTests.cpp
+ OsgVectorConversionsTests.cpp
+ OsgVectorFieldRepresentationTests.cpp
+ OsgViewElementTests.cpp
+ OsgViewTests.cpp
+ RenderPassTests.cpp
+ ViewElementTests.cpp
+ ViewTests.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ MockObjects.h
+ MockOsgObjects.h
+)
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/OsgSceneryRepresentationTests DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Data)
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/Data/Geometry DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Data)
+
+
+set(LIBS
+ SurgSimGraphics
+ SurgSimMath
+ SurgSimTesting
+)
+
+surgsim_add_unit_tests(SurgSimGraphicsTest)
+
+set_target_properties(SurgSimGraphicsTest PROPERTIES FOLDER "Graphics")
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgMeshRepresentationTests/Cube.ply b/SurgSim/Graphics/UnitTests/Data/OsgMeshRepresentationTests/Cube.ply
new file mode 100644
index 0000000..82ad16a
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/Data/OsgMeshRepresentationTests/Cube.ply
@@ -0,0 +1,53 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 26
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+property float s
+property float t
+element face 12
+property list uchar uint vertex_indices
+end_header
+1.000000 1.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.500000
+1.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000 0.010000 0.510000
+-1.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000 0.020000 0.520000
+1.000000 0.999999 1.000000 -0.000000 -0.000000 1.000000 0.030000 0.530000
+-1.000000 1.000000 1.000000 -0.000000 -0.000000 1.000000 0.040000 0.540000
+0.999999 -1.000001 1.000000 -0.000000 -0.000000 1.000000 0.050000 0.550000
+1.000000 1.000000 -1.000000 1.000000 0.000000 -0.000000 0.060000 0.560000
+1.000000 0.999999 1.000000 1.000000 0.000000 -0.000000 0.070000 0.570000
+1.000000 -1.000000 -1.000000 1.000000 0.000000 -0.000000 0.080000 0.580000
+1.000000 -1.000000 -1.000000 -0.000000 -1.000000 -0.000000 0.090000 0.590000
+0.999999 -1.000001 1.000000 -0.000000 -1.000000 -0.000000 0.100000 0.600000
+-1.000000 -1.000000 -1.000000 -0.000000 -1.000000 -0.000000 0.110000 0.610000
+-1.000000 -1.000000 -1.000000 -1.000000 0.000000 -0.000000 0.120000 0.620000
+-1.000000 -1.000000 1.000000 -1.000000 0.000000 -0.000000 0.130000 0.630000
+-1.000000 1.000000 1.000000 -1.000000 0.000000 -0.000000 0.140000 0.640000
+1.000000 0.999999 1.000000 0.000000 1.000000 0.000000 0.150000 0.650000
+1.000000 1.000000 -1.000000 0.000000 1.000000 0.000000 0.160000 0.660000
+-1.000000 1.000000 1.000000 0.000000 1.000000 0.000000 0.170000 0.670000
+-1.000000 1.000000 -1.000000 0.000000 0.000000 -1.000000 0.180000 0.680000
+1.000000 0.999999 1.000000 1.000000 -0.000001 0.000000 0.190000 0.690000
+0.999999 -1.000001 1.000000 1.000000 -0.000001 0.000000 0.200000 0.700000
+1.000000 -1.000000 -1.000000 1.000000 -0.000001 0.000000 0.210000 0.710000
+-1.000000 1.000000 -1.000000 0.000000 1.000000 0.000000 0.220000 0.720000
+-1.000000 -1.000000 1.000000 0.000000 -0.000000 1.000000 0.230000 0.730000
+-1.000000 1.000000 -1.000000 -1.000000 0.000000 -0.000000 0.240000 0.740000
+-1.000000 -1.000000 1.000000 -0.000000 -1.000000 0.000000 0.250000 0.750000
+3 0 1 2
+3 3 4 5
+3 6 7 8
+3 9 10 11
+3 12 13 14
+3 15 16 17
+3 18 0 2
+3 19 20 21
+3 16 22 17
+3 4 23 5
+3 24 12 14
+3 10 25 11
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.frag b/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.frag
new file mode 100644
index 0000000..9b08aef
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.frag
@@ -0,0 +1,7 @@
+varying vec4 geomColor;
+
+/// Outputs the input color
+void main(void)
+{
+ gl_FragColor = geomColor;
+};
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.geom b/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.geom
new file mode 100644
index 0000000..1fed5fc
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.geom
@@ -0,0 +1,21 @@
+#version 150
+#extension GL_EXT_gpu_shader4 : enable
+#extension GL_EXT_geometry_shader4 : enable
+
+layout(triangles) in;
+layout(triangle_strip, max_vertices=3) out;
+
+in vec4 vertColor[3];
+out vec4 geomColor;
+
+/// Outputs the same geometry as is input
+void main()
+{
+ for (int i = 0; i < gl_VerticesIn; ++i)
+ {
+ gl_Position = gl_PositionIn[i];
+ geomColor = vertColor[i];
+ EmitVertex();
+ }
+ EndPrimitive();
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.vert b/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.vert
new file mode 100644
index 0000000..eb353ff
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.vert
@@ -0,0 +1,9 @@
+varying vec4 vertColor;
+
+/// Outputs the local normal direction as the vertex color
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+ vertColor.rgb = gl_Normal;
+ vertColor.a = 1.0;
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Brdf0.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Brdf0.png
new file mode 100644
index 0000000..ad9b553
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Brdf0.png differ
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Brdf1.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Brdf1.png
new file mode 100644
index 0000000..4bf64fd
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Brdf1.png differ
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CheckerBoard.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CheckerBoard.png
new file mode 100644
index 0000000..aafbea2
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CheckerBoard.png differ
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CubeMap.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CubeMap.png
new file mode 100644
index 0000000..2ea449d
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CubeMap.png differ
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Gradient.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Gradient.png
new file mode 100644
index 0000000..539ffe2
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Gradient.png differ
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeX.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeX.png
new file mode 100644
index 0000000..47ec4fb
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeX.png differ
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeY.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeY.png
new file mode 100644
index 0000000..5ca5a59
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeY.png differ
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeZ.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeZ.png
new file mode 100644
index 0000000..2f8aa5b
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeZ.png differ
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveX.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveX.png
new file mode 100644
index 0000000..bb38aca
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveX.png differ
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveY.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveY.png
new file mode 100644
index 0000000..2117d81
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveY.png differ
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveZ.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveZ.png
new file mode 100644
index 0000000..2538d01
Binary files /dev/null and b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveZ.png differ
diff --git a/SurgSim/Graphics/UnitTests/GroupTests.cpp b/SurgSim/Graphics/UnitTests/GroupTests.cpp
new file mode 100644
index 0000000..617fb37
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/GroupTests.cpp
@@ -0,0 +1,153 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Group class.
+
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST(GroupTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<Group> group = std::make_shared<MockGroup>("test name");});
+}
+
+TEST(GroupTests, VisibilityTest)
+{
+ std::shared_ptr<Group> group = std::make_shared<MockGroup>("test name");
+
+ group->setVisible(true);
+ EXPECT_TRUE(group->isVisible());
+
+ group->setVisible(false);
+ EXPECT_FALSE(group->isVisible());
+}
+
+TEST(GroupTests, AddRemoveTest)
+{
+ std::shared_ptr<Group> group = std::make_shared<MockGroup>("test name");
+
+ std::shared_ptr<Representation> representation1 = std::make_shared<MockRepresentation>("test representation 1");
+ std::shared_ptr<Representation> representation2 = std::make_shared<MockRepresentation>("test representation 2");
+ std::shared_ptr<MockGroup> group1 = std::make_shared<MockGroup>("test group 1");
+ std::shared_ptr<MockGroup> group2 = std::make_shared<MockGroup>("test group 2");
+
+ EXPECT_EQ(0u, group->getMembers().size());
+
+ /// Add an representation
+ EXPECT_TRUE(group->add(representation1));
+ EXPECT_EQ(1u, group->getMembers().size());
+ EXPECT_NE(group->getMembers().end(),
+ std::find(group->getMembers().begin(), group->getMembers().end(), representation1));
+
+ /// Add another representation
+ EXPECT_TRUE(group->add(representation2));
+ EXPECT_EQ(2u, group->getMembers().size());
+ EXPECT_NE(group->getMembers().end(),
+ std::find(group->getMembers().begin(), group->getMembers().end(), representation2));
+
+
+ /// Try to add a duplicate representation
+ EXPECT_FALSE(group->add(representation1));
+ EXPECT_EQ(2u, group->getMembers().size());
+
+
+ /// Remove an representation
+ EXPECT_TRUE(group->remove(representation1));
+ EXPECT_EQ(group->getMembers().end(),
+ std::find(group->getMembers().begin(), group->getMembers().end(), representation1));
+
+
+ /// Try to remove an representation that is not in the group
+ EXPECT_FALSE(group->remove(representation1));
+ EXPECT_EQ(group->getMembers().end(),
+ std::find(group->getMembers().begin(), group->getMembers().end(), representation1));
+}
+
+TEST(GroupTests, AppendTest)
+{
+ std::shared_ptr<Group> group1 = std::make_shared<MockGroup>("test group 1");
+ EXPECT_EQ(0u, group1->getMembers().size());
+
+ std::shared_ptr<Representation> representation1 = std::make_shared<MockRepresentation>("test representation 1");
+ std::shared_ptr<Representation> representation2 = std::make_shared<MockRepresentation>("test representation 2");
+
+ /// Add 2 representations to group 1
+ EXPECT_TRUE(group1->add(representation1));
+ EXPECT_TRUE(group1->add(representation2));
+ EXPECT_EQ(2u, group1->getMembers().size());
+
+ std::shared_ptr<Representation> representation3 = std::make_shared<MockRepresentation>("test representation 3");
+
+ std::shared_ptr<Group> group2 = std::make_shared<MockGroup>("test group 2");
+ EXPECT_EQ(0u, group2->getMembers().size());
+
+ /// Add an representation to group 2 and append group 1 to group 2.
+ EXPECT_TRUE(group2->add(representation3));
+ EXPECT_TRUE(group2->append(group1));
+ EXPECT_EQ(3u, group2->getMembers().size());
+
+ // Check that the representations from group 1 were added to group 2, and that it still has the representation
+ // that was added directly to it.
+ EXPECT_NE(group2->getMembers().end(),
+ std::find(group2->getMembers().begin(), group2->getMembers().end(), representation1));
+ EXPECT_NE(group2->getMembers().end(),
+ std::find(group2->getMembers().begin(), group2->getMembers().end(), representation2));
+ EXPECT_NE(group2->getMembers().end(),
+ std::find(group2->getMembers().begin(), group2->getMembers().end(), representation3));
+
+ /// Try to append a group that has already been appended - this will try to add duplicate representations.
+ EXPECT_FALSE(group2->append(group1)) << "Append should return false if any representation is a duplicate!";
+ EXPECT_EQ(3u, group2->getMembers().size());
+
+ /// Check that group 1 was not modified by appending it to group 2.
+ EXPECT_EQ(2u, group1->getMembers().size());
+ EXPECT_NE(group1->getMembers().end(),
+ std::find(group1->getMembers().begin(), group1->getMembers().end(), representation1));
+ EXPECT_NE(group1->getMembers().end(),
+ std::find(group1->getMembers().begin(), group1->getMembers().end(), representation2));
+}
+
+TEST(GroupTests, ClearTest)
+{
+ std::shared_ptr<Group> group = std::make_shared<MockGroup>("test name");
+
+ std::shared_ptr<Representation> representation1 = std::make_shared<MockRepresentation>("test representation 1");
+ std::shared_ptr<Representation> representation2 = std::make_shared<MockRepresentation>("test representation 2");
+ std::shared_ptr<Representation> representation3 = std::make_shared<MockRepresentation>("test representation 3");
+
+ EXPECT_EQ(0u, group->getMembers().size());
+
+ /// Add 3 representations
+ EXPECT_TRUE(group->add(representation1));
+ EXPECT_TRUE(group->add(representation2));
+ EXPECT_TRUE(group->add(representation3));
+ EXPECT_EQ(3u, group->getMembers().size());
+
+ /// Remove all representations and make sure that they are removed correctly
+ group->clear();
+ EXPECT_EQ(0u, group->getMembers().size());
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/ManagerTests.cpp b/SurgSim/Graphics/UnitTests/ManagerTests.cpp
new file mode 100644
index 0000000..5bd9059
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/ManagerTests.cpp
@@ -0,0 +1,240 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Graphics Manager class.
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/ComponentManager.h"
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+
+#include <gtest/gtest.h>
+
+#include <algorithm>
+
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Framework::ComponentManager;
+using SurgSim::Framework::Component;
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+
+class GraphicsManagerTest : public ::testing::Test
+{
+public:
+ virtual void SetUp()
+ {
+ runtime = std::make_shared<Runtime>();
+ graphicsManager = std::make_shared<MockManager>();
+
+ runtime->addManager(graphicsManager);
+ // runtime->start();
+ }
+
+ virtual void TearDown()
+ {
+ runtime->stop();
+ }
+
+
+ bool testDoAddComponent(const std::shared_ptr<Component>& component)
+ {
+ return graphicsManager->executeAdditions(component);
+ }
+
+ bool testDoRemoveComponent(const std::shared_ptr<Component>& component)
+ {
+ return graphicsManager->executeRemovals(component);
+ }
+
+ void doProcessComponents()
+ {
+ graphicsManager->processComponents();
+ }
+
+ std::shared_ptr<Runtime> runtime;
+ std::shared_ptr<MockManager> graphicsManager;
+};
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST_F(GraphicsManagerTest, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<MockManager> manager = std::make_shared<MockManager>();});
+}
+
+TEST_F(GraphicsManagerTest, StartUpTest)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<MockManager> manager = std::make_shared<MockManager>();
+
+ runtime->addManager(manager);
+ EXPECT_EQ(0, manager->getNumUpdates());
+ EXPECT_EQ(0.0, manager->getSumDt());
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ /// Run the thread for a moment
+ runtime->start();
+ EXPECT_TRUE(manager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ runtime->stop();
+
+ /// Check that the manager did update when the thread was running
+ EXPECT_GT(manager->getNumUpdates(), 0);
+ EXPECT_GT(manager->getSumDt(), 0.0);
+}
+
+TEST_F(GraphicsManagerTest, AddRemoveTest)
+{
+ /// Perform add and remove from a pointer to a ComponentManager to check that the intended polymorphism is working.
+ std::shared_ptr<ComponentManager> componentManager = graphicsManager;
+
+ std::shared_ptr<Representation> representation1 = std::make_shared<MockRepresentation>("test representation 1");
+ std::shared_ptr<Representation> representation2 = std::make_shared<MockRepresentation>("test representation 2");
+ representation2->addGroupReference("other_group");
+ std::shared_ptr<MockView> view1 = std::make_shared<MockView>("test view 1");
+ std::shared_ptr<MockView> view2 = std::make_shared<MockView>("test view 2");
+
+ std::shared_ptr<SurgSim::Framework::Representation> nonGraphicsComponent =
+ std::make_shared<NonGraphicsRepresentation>("non-graphics component");
+
+ EXPECT_EQ(0u, graphicsManager->getRepresentations().size());
+ EXPECT_EQ(0u, graphicsManager->getGroups().size());
+ EXPECT_EQ(0u, graphicsManager->getViews().size());
+
+ /// Add an representation
+ EXPECT_TRUE(testDoAddComponent(representation1));
+ EXPECT_EQ(1u, graphicsManager->getRepresentations().size());
+ EXPECT_NE(graphicsManager->getRepresentations().end(), std::find(graphicsManager->getRepresentations().begin(),
+ graphicsManager->getRepresentations().end(), representation1));
+
+ // Should have added the default group
+ EXPECT_EQ(1u, graphicsManager->getGroups().size());
+ EXPECT_NE(std::end(graphicsManager->getGroups()),
+ graphicsManager->getGroups().find(Representation::DefaultGroupName));
+
+ /// Add a view
+ EXPECT_TRUE(testDoAddComponent(view1));
+ EXPECT_EQ(1u, graphicsManager->getViews().size());
+ EXPECT_NE(graphicsManager->getViews().end(), std::find(graphicsManager->getViews().begin(),
+ graphicsManager->getViews().end(), view1));
+
+
+ /// Add another view
+ EXPECT_TRUE(testDoAddComponent(view2));
+ EXPECT_EQ(2u, graphicsManager->getViews().size());
+ EXPECT_NE(graphicsManager->getViews().end(), std::find(graphicsManager->getViews().begin(),
+ graphicsManager->getViews().end(), view2));
+
+ /// Add another representation
+ EXPECT_TRUE(testDoAddComponent(representation2));
+ EXPECT_EQ(2u, graphicsManager->getRepresentations().size());
+ EXPECT_NE(graphicsManager->getRepresentations().end(), std::find(graphicsManager->getRepresentations().begin(),
+ graphicsManager->getRepresentations().end(), representation2));
+
+ /// There should be another group in there now
+ EXPECT_EQ(2u, graphicsManager->getGroups().size());
+
+ /// Try to add a duplicate representation
+ /// the public interface functions addComponent and removeComponent always return true when the allocation
+ /// succeeded
+ EXPECT_TRUE(componentManager->enqueueAddComponent(representation1));
+ doProcessComponents();
+ EXPECT_EQ(2u, graphicsManager->getRepresentations().size());
+
+ /// Try to add a duplicate view
+ EXPECT_TRUE(componentManager->enqueueAddComponent(view1));
+ doProcessComponents();
+ EXPECT_EQ(2u, graphicsManager->getViews().size());
+
+ /// Try to add a component that is not graphics-related
+ EXPECT_TRUE(componentManager->enqueueAddComponent(nonGraphicsComponent)) <<
+ "Adding a component that this manager is not concerned with should return false";
+
+ /// Remove a view
+ EXPECT_TRUE(componentManager->enqueueRemoveComponent(view2));
+ doProcessComponents();
+ EXPECT_EQ(graphicsManager->getViews().end(), std::find(graphicsManager->getViews().begin(),
+ graphicsManager->getViews().end(), view2));
+
+ /// Remove an representation
+ EXPECT_TRUE(componentManager->enqueueRemoveComponent(representation1));
+ doProcessComponents();
+ EXPECT_EQ(graphicsManager->getRepresentations().end(), std::find(graphicsManager->getRepresentations().begin(),
+ graphicsManager->getRepresentations().end(), representation1));
+
+ /// Try to remove an representation that is not in the manager
+ EXPECT_TRUE(componentManager->enqueueRemoveComponent(representation1));
+ doProcessComponents();
+ EXPECT_EQ(graphicsManager->getRepresentations().end(), std::find(graphicsManager->getRepresentations().begin(),
+ graphicsManager->getRepresentations().end(), representation1));
+
+ /// Try to remove a view that is not in the manager
+ EXPECT_TRUE(componentManager->enqueueRemoveComponent(view2));
+ doProcessComponents();
+ EXPECT_EQ(graphicsManager->getViews().end(), std::find(graphicsManager->getViews().begin(),
+ graphicsManager->getViews().end(), view2));
+
+
+ /// Try to remove a component that is not graphics-related
+ EXPECT_TRUE(componentManager->enqueueRemoveComponent(nonGraphicsComponent)) <<
+ "Removing a component that this manager is not concerned with should return true";
+}
+
+TEST_F(GraphicsManagerTest, UpdateTest)
+{
+ auto mockRepresentation = std::make_shared<MockRepresentation>("MockRepresentation");
+ auto sceneElement = std::make_shared<BasicSceneElement>("BasicSceneElement");
+ sceneElement->addComponent(mockRepresentation);
+ runtime->getScene()->addSceneElement(sceneElement);
+
+ EXPECT_TRUE(mockRepresentation->isVisible());
+
+ // When a graphics representation is inactive, it will be set to invisible by Graphics Manager.
+ // And it won't be updated by Graphics Manager.
+ mockRepresentation->setLocalActive(false);
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_EQ(0, mockRepresentation->getNumUpdates());
+ EXPECT_FALSE(mockRepresentation->isVisible());
+
+ // When a graphics representation is active, it will be set to visible by Graphics Manager.
+ // And it will be updated by Graphics Manager.
+ mockRepresentation->setLocalActive(true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_GT(mockRepresentation->getNumUpdates(), 0);
+ EXPECT_TRUE(mockRepresentation->isVisible());
+
+ // Turn off an active graphics representation, the update of it will be stopped and it will be invisible.
+ mockRepresentation->setLocalActive(false);
+ auto updateCount = mockRepresentation->getNumUpdates();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_EQ(updateCount, mockRepresentation->getNumUpdates());
+ EXPECT_FALSE(mockRepresentation->isVisible());
+
+ runtime->stop();
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/MeshTests.cpp b/SurgSim/Graphics/UnitTests/MeshTests.cpp
new file mode 100644
index 0000000..6caee26
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/MeshTests.cpp
@@ -0,0 +1,136 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <vector>
+#include <gtest/gtest.h>
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Testing/TestCube.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+struct MeshTests : public ::testing::Test
+{
+public:
+
+ virtual void SetUp()
+ {
+ SurgSim::Testing::Cube::makeCube(&cubeVertices, &cubeColors, &cubeTextures, &cubeTriangles);
+ }
+
+
+ std::vector<Vector3d> cubeVertices;
+ std::vector<size_t> cubeTriangles;
+ std::vector<Vector4d> cubeColors;
+ std::vector<Vector2d> cubeTextures;
+};
+
+
+TEST_F(MeshTests, MakeMeshTestWorking)
+{
+ std::shared_ptr<Mesh> mesh = std::make_shared<Mesh>();
+ ASSERT_NO_THROW({
+ mesh->initialize(cubeVertices, cubeColors, cubeTextures, cubeTriangles);}
+ );
+
+ EXPECT_TRUE(mesh->isValid());
+
+ EXPECT_EQ(cubeVertices.size(), mesh->getNumVertices());
+ EXPECT_EQ(cubeTriangles.size()/3, mesh->getNumTriangles());
+
+ for (size_t i=0; i< cubeVertices.size(); ++i)
+ {
+ EXPECT_TRUE(cubeColors[i] == mesh->getVertex(i).data.color.getValue());
+ EXPECT_TRUE(cubeTextures[i] == mesh->getVertex(i).data.texture.getValue());
+ }
+}
+
+TEST_F(MeshTests, MakeMeshColors)
+{
+ std::vector<Vector4d> emptyCubeColors;
+ std::vector<Vector4d> tooFewCubeColors;
+ std::vector<Vector4d> tooManyCubeColors(cubeColors);
+ tooFewCubeColors.push_back(Vector4d(0.0,0.0,0.0,0.0));
+ tooManyCubeColors.push_back(Vector4d(0.0,0.0,0.0,0.0));
+
+ std::shared_ptr<Mesh> mesh = std::make_shared<Mesh>();
+ ASSERT_NO_THROW({
+ mesh->initialize(cubeVertices, emptyCubeColors, cubeTextures, cubeTriangles);}
+ );
+
+ for (size_t i=0; i< cubeVertices.size(); ++i)
+ {
+ EXPECT_FALSE(mesh->getVertex(i).data.color.hasValue());
+ }
+
+
+ mesh = std::make_shared<Mesh>();
+ ASSERT_ANY_THROW({
+ mesh->initialize(cubeVertices, tooFewCubeColors, cubeTextures, cubeTriangles);}
+ );
+
+ mesh = std::make_shared<Mesh>();
+ ASSERT_NO_THROW({
+ mesh->initialize(cubeVertices, tooManyCubeColors, cubeTextures, cubeTriangles);}
+ );
+
+}
+
+TEST_F(MeshTests, MakeMeshTextures)
+{
+ std::vector<Vector2d> emptyCubeTextures;
+ std::vector<Vector2d> tooFewCubeTextures;
+ std::vector<Vector2d> tooManyCubeTextures(cubeTextures);
+ tooFewCubeTextures.push_back(Vector2d(0.0,0.0));
+ tooManyCubeTextures.push_back(Vector2d(0.0,0.0));
+
+ std::shared_ptr<Mesh> mesh = std::make_shared<Mesh>();
+ ASSERT_NO_THROW({
+ mesh->initialize(cubeVertices, cubeColors, emptyCubeTextures, cubeTriangles);}
+ );
+
+ for (size_t i=0; i< cubeVertices.size(); ++i)
+ {
+ EXPECT_FALSE(mesh->getVertex(i).data.texture.hasValue());
+ }
+
+ mesh = std::make_shared<Mesh>();
+ ASSERT_ANY_THROW({
+ mesh->initialize(cubeVertices, cubeColors, tooFewCubeTextures, cubeTriangles);}
+ );
+
+ mesh = std::make_shared<Mesh>();
+ ASSERT_NO_THROW({
+ mesh->initialize(cubeVertices, cubeColors, tooManyCubeTextures, cubeTriangles);}
+ );
+
+}
+
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/MockObjects.h b/SurgSim/Graphics/UnitTests/MockObjects.h
new file mode 100644
index 0000000..45d4979
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/MockObjects.h
@@ -0,0 +1,597 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_UNITTESTS_MOCKOBJECTS_H
+#define SURGSIM_GRAPHICS_UNITTESTS_MOCKOBJECTS_H
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Graphics/Group.h"
+#include "SurgSim/Graphics/Manager.h"
+#include "SurgSim/Graphics/Material.h"
+#include "SurgSim/Graphics/RenderTarget.h"
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Graphics/Shader.h"
+#include "SurgSim/Graphics/UniformBase.h"
+#include "SurgSim/Graphics/View.h"
+#include "SurgSim/Graphics/ViewElement.h"
+#include "SurgSim/Graphics/Texture.h"
+
+#include <array>
+
+class MockGroup : public SurgSim::Graphics::Group
+{
+public:
+ /// Constructor. The group is initially empty.
+ /// \param name Name of the group
+ explicit MockGroup(const std::string& name) : SurgSim::Graphics::Group(name)
+ {
+ }
+
+ /// Sets whether the group is currently visible
+ /// \param visible True for visible, false for invisible
+ virtual void setVisible(bool visible)
+ {
+ m_isVisible = visible;
+ }
+
+ /// Gets whether the group is currently visible
+ /// \return visible True for visible, false for invisible
+ virtual bool isVisible() const
+ {
+ return m_isVisible;
+ }
+
+private:
+ /// Whether this group is currently visible or not
+ bool m_isVisible;
+};
+
+/// Manager class for testing
+class MockManager : public SurgSim::Graphics::Manager
+{
+public:
+
+ friend class GraphicsManagerTest;
+
+ /// Constructor
+ /// \post m_numUpdates and m_sumDt are initialized to 0
+ MockManager() : SurgSim::Graphics::Manager(),
+ m_numUpdates(0),
+ m_sumDt(0.0)
+ {
+ }
+
+ /// Returns the number of times the manager has been updated
+ int getNumUpdates() const
+ {
+ return m_numUpdates;
+ }
+ /// Returns the sum of the dt that the manager has been updated with
+ double getSumDt() const
+ {
+ return m_sumDt;
+ }
+
+ void dumpDebugInfo() const
+ {
+ return;
+ }
+
+ virtual int getType() const override
+ {
+ return SurgSim::Framework::MANAGER_TYPE_NONE;
+ }
+
+ virtual std::shared_ptr<SurgSim::Graphics::Group> getOrCreateGroup(const std::string& name)
+ {
+ if (getGroups().find(name) == std::end(getGroups()))
+ {
+ addGroup(std::make_shared<MockGroup>(name));
+ }
+ return getGroups().at(name);
+ }
+
+private:
+ /// Updates the manager.
+ /// \param dt The time in seconds of the preceding timestep.
+ /// \post m_numUpdates is incremented and dt is added to m_sumDt
+ virtual bool doUpdate(double dt)
+ {
+ if (Manager::doUpdate(dt))
+ {
+ ++m_numUpdates;
+ m_sumDt += dt;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+ /// Number of times the manager has been updated
+ int m_numUpdates;
+ /// Sum of the dt that the manager has been updated with
+ double m_sumDt;
+};
+
+/// Representation class for testing
+class MockRepresentation : public SurgSim::Graphics::Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ /// \post m_numUpdates and m_sumDt are initialized to 0
+ /// \post m_transform is set to identity
+ /// \post m_isInitialized and m_isAwoken are set to false
+ /// \post m_isVisible is set to true
+ explicit MockRepresentation(const std::string& name) : SurgSim::Graphics::Representation(name),
+ m_isVisible(true),
+ m_numUpdates(0),
+ m_sumDt(0.0),
+ m_isInitialized(false),
+ m_isAwoken(false),
+ m_drawAsWireFrame(false)
+ {
+ m_transform.setIdentity();
+ }
+
+ /// Sets whether the representation is currently visible
+ /// \param visible True for visible, false for invisible
+ virtual void setVisible(bool visible)
+ {
+ m_isVisible = visible;
+ }
+
+ /// Gets whether the representation is currently visible
+ /// \return visible True for visible, false for invisible
+ virtual bool isVisible() const
+ {
+ return isActive() && m_isVisible;
+ }
+
+ /// Returns the number of times the representation has been updated
+ int getNumUpdates() const
+ {
+ return m_numUpdates;
+ }
+ /// Returns the sum of the dt that the representation has been updated with
+ double getSumDt() const
+ {
+ return m_sumDt;
+ }
+
+ /// Updates the representation.
+ /// \param dt The time in seconds of the preceding timestep.
+ /// \post m_numUpdates is incremented and dt is added to m_sumDt
+ virtual void update(double dt)
+ {
+ ++m_numUpdates;
+ m_sumDt += dt;
+ }
+
+ /// Gets whether the representation has been initialized
+ bool isInitialized() const
+ {
+ return m_isInitialized;
+ }
+ /// Gets whether the representation has been awoken
+ bool isAwoken() const
+ {
+ return m_isAwoken;
+ }
+
+ /// Sets the material that defines the visual appearance of the representation
+ /// \param material Graphics material
+ /// \return True if set successfully, otherwise false
+ virtual bool setMaterial(std::shared_ptr<SurgSim::Graphics::Material> material)
+ {
+ return false;
+ }
+
+ /// Gets the material that defines the visual appearance of the representation
+ /// \return Graphics material
+ virtual std::shared_ptr<SurgSim::Graphics::Material> getMaterial() const
+ {
+ return nullptr;
+ }
+
+ /// Removes the material from the representation
+ virtual void clearMaterial()
+ {
+ }
+
+ virtual void setDrawAsWireFrame(bool val)
+ {
+ m_drawAsWireFrame = val;
+ }
+
+ virtual bool getDrawAsWireFrame() const
+ {
+ return m_drawAsWireFrame;
+ }
+
+private:
+ /// Initializes the representation
+ /// \post m_isInitialized is set to true
+ virtual bool doInitialize()
+ {
+ m_isInitialized = true;
+ return true;
+ }
+ /// Wakes up the representation
+ /// \post m_isAwoken is set to true
+ virtual bool doWakeUp()
+ {
+ m_isAwoken = true;
+ return true;
+ }
+
+ /// Whether this representation is currently visible or not
+ bool m_isVisible;
+
+ /// Number of times the representation has been updated
+ int m_numUpdates;
+ /// Sum of the dt that the representation has been updated with
+ double m_sumDt;
+
+ /// Whether the representation has been initialized
+ bool m_isInitialized;
+ /// Whether the representation has been awoken
+ bool m_isAwoken;
+
+ /// Indicates if the representation is rendered as a wireframe.
+ bool m_drawAsWireFrame;
+
+ /// Rigid transform describing pose of the representation
+ SurgSim::Math::RigidTransform3d m_transform;
+};
+
+/// Camera class for testing
+class MockCamera : public SurgSim::Graphics::Camera
+{
+public:
+ /// Constructor
+ /// \param name Name of the camera
+ /// \post m_numUpdates and m_sumDt are initialized to 0
+ /// \post m_transform is set to identity, m_eye to (0,0,0), m_center to (0, 0, -1), and m_up to (0, 1, 0)
+ /// \post m_isVisible is set to true
+ explicit MockCamera(const std::string& name) :
+ SurgSim::Graphics::Representation(name),
+ SurgSim::Graphics::Camera(name),
+ m_numUpdates(0),
+ m_sumDt(0.0),
+ m_isVisible(true)
+ {
+ m_pose.setIdentity();
+ m_viewMatrix.setIdentity();
+ m_projectionMatrix.setIdentity();
+ }
+
+ /// Sets whether the camera is currently visible
+ /// When the camera is invisible, it does not produce an image.
+ /// \param visible True for visible, false for invisible
+ virtual void setVisible(bool visible)
+ {
+ m_isVisible = visible;
+ }
+
+ /// Gets whether the camera is currently visible
+ /// When the camera is invisible, it does not produce an image.
+ /// \return visible True for visible, false for invisible
+ virtual bool isVisible() const
+ {
+ return m_isVisible;
+ }
+
+ /// Returns the number of times the representation has been updated
+ int getNumUpdates() const
+ {
+ return m_numUpdates;
+ }
+ /// Returns the sum of the dt that the representation has been updated with
+ double getSumDt() const
+ {
+ return m_sumDt;
+ }
+
+ /// Sets the current pose of the camera
+ /// \param transform Rigid transformation that describes the current pose of the camera
+ virtual void setPose(const SurgSim::Math::RigidTransform3d& transform)
+ {
+ m_pose = transform;
+ }
+
+ /// Gets the pose of the camera
+ /// \return Rigid transformation that describes the pose of the representation
+ virtual SurgSim::Math::RigidTransform3d getPose() const
+ {
+ return m_pose;
+ }
+
+ /// Sets the view matrix of the camera
+ /// \param matrix View matrix
+ virtual void setViewMatrix(const SurgSim::Math::Matrix44d& matrix)
+ {
+ m_viewMatrix = matrix;
+ }
+
+ /// Gets the view matrix of the camera
+ /// \return View matrix
+ virtual SurgSim::Math::Matrix44d getViewMatrix() const
+ {
+ return m_viewMatrix;
+ }
+
+ /// Sets the projection matrix of the camera
+ /// \param matrix Projection matrix
+ virtual void setProjectionMatrix(const SurgSim::Math::Matrix44d& matrix)
+ {
+ m_projectionMatrix = matrix;
+ }
+
+ /// Gets the projection matrix of the camera
+ /// \return Projection matrix
+ virtual const SurgSim::Math::Matrix44d& getProjectionMatrix() const
+ {
+ return m_projectionMatrix;
+ }
+
+ /// Updates the camera.
+ /// \param dt The time in seconds of the preceding timestep.
+ /// \post m_numUpdates is incremented and dt is added to m_sumDt
+ virtual void update(double dt)
+ {
+ ++m_numUpdates;
+ m_sumDt += dt;
+ }
+
+ /// Sets the material that defines the visual appearance of the representation
+ /// \param material Graphics material
+ /// \return True if set successfully, otherwise false
+ virtual bool setMaterial(std::shared_ptr<SurgSim::Graphics::Material> material)
+ {
+ return false;
+ }
+
+ /// Gets the material that defines the visual appearance of the representation
+ /// \return Graphics material
+ virtual std::shared_ptr<SurgSim::Graphics::Material> getMaterial() const
+ {
+ return nullptr;
+ }
+
+ /// Removes the material from the representation
+ virtual void clearMaterial()
+ {
+ }
+
+ virtual void setDrawAsWireFrame(bool val)
+ {
+ }
+
+ virtual bool getDrawAsWireFrame() const
+ {
+ return false;
+ }
+
+ virtual bool setColorRenderTexture(std::shared_ptr<SurgSim::Graphics::Texture> texture)
+ {
+ return true;
+ }
+
+ virtual std::shared_ptr<SurgSim::Graphics::Texture> getColorRenderTexture() const
+ {
+ return nullptr;
+ }
+
+ virtual bool setRenderTarget(std::shared_ptr<SurgSim::Graphics::RenderTarget> renderTarget)
+ {
+ return true;
+ }
+
+ virtual std::shared_ptr<SurgSim::Graphics::RenderTarget> getRenderTarget() const
+ {
+ return nullptr;
+ }
+
+ virtual void setRenderOrder(RenderOrder bin, int value) override
+ {
+
+ }
+
+ virtual SurgSim::Math::Matrix44d getInverseViewMatrix() const
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+ void setAmbientColor(const SurgSim::Math::Vector4d& color)
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+ SurgSim::Math::Vector4d getAmbientColor()
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+
+private:
+ /// Number of times the camera has been updated
+ int m_numUpdates;
+ /// Sum of the dt that the camera has been updated with
+ double m_sumDt;
+
+ /// Rigid transform describing pose of the camera
+ SurgSim::Math::RigidTransform3d m_pose;
+
+ /// View matrix of the camera
+ SurgSim::Math::Matrix44d m_viewMatrix;
+
+ /// Projection matrix of the camera
+ SurgSim::Math::Matrix44d m_projectionMatrix;
+
+ /// Whether this camera is currently visible or not
+ /// When the camera is invisible, it does not produce an image.
+ bool m_isVisible;
+};
+
+/// View class for testing
+class MockView : public SurgSim::Graphics::View
+{
+public:
+ /// Constructor
+ /// \param name Name of the view
+ /// \post m_x and m_y are initialized to 0
+ /// \post m_width is initialized to 800, m_height to 600
+ /// \post m_isWindowBorderEnabled is initialized to true
+ /// \post m_numUpdates and m_sumDt are initialized to 0
+ /// \post m_transform is set to identity
+ explicit MockView(const std::string& name) : SurgSim::Graphics::View(name),
+ m_x(0),
+ m_y(0),
+ m_width(800),
+ m_height(600),
+ m_isWindowBorderEnabled(true),
+ m_numUpdates(0),
+ m_sumDt(0.0),
+ m_isInitialized(false),
+ m_isAwoken(false)
+ {
+ }
+
+ /// Set the position of this view
+ /// \param x,y Position on the screen (in pixels)
+ virtual void setPosition(const std::array<int, 2>& position) override
+ {
+ m_x = position[0];
+ m_y = position[1];
+ }
+
+ /// Get the position of this view
+ /// \param[out] x,y Position on the screen (in pixels)
+ virtual std::array<int, 2> getPosition() const override
+ {
+ std::array<int, 2> result = {m_x, m_y};
+ return std::move(result);
+ }
+
+ /// Set the dimensions of this view
+ /// \param width,height Dimensions on the screen (in pixels)
+ virtual void setDimensions(const std::array<int, 2>& dimensions) override
+ {
+ m_width = dimensions[0];
+ m_height = dimensions[1];
+ }
+
+ /// Set the dimensions of this view
+ /// \param[out] width,height Dimensions on the screen (in pixels)
+ virtual std::array<int, 2> getDimensions() const override
+ {
+ std::array<int, 2> result = {m_width, m_height};
+ return std::move(result);
+ }
+
+ /// Sets whether the view window has a border
+ /// \param enabled True to enable the border around the window; false for no border
+ virtual void setWindowBorderEnabled(bool enabled) override
+ {
+ m_isWindowBorderEnabled = enabled;
+ }
+ /// Returns whether the view window has a border
+ /// \return True to enable the border around the window; false for no border
+ virtual bool isWindowBorderEnabled() const override
+ {
+ return m_isWindowBorderEnabled;
+ }
+
+ /// Returns the number of times the view has been updated
+ int getNumUpdates() const
+ {
+ return m_numUpdates;
+ }
+ /// Returns the sum of the dt that the view has been updated with
+ double getSumDt() const
+ {
+ return m_sumDt;
+ }
+
+ /// Updates the view.
+ /// \param dt The time in seconds of the preceding timestep.
+ /// \post m_numUpdates is incremented and dt is added to m_sumDt
+ virtual void update(double dt)
+ {
+ ++m_numUpdates;
+ m_sumDt += dt;
+ }
+
+ /// Gets whether the view has been initialized
+ bool isInitialized() const
+ {
+ return m_isInitialized;
+ }
+ /// Gets whether the view has been awoken
+ bool isAwoken() const
+ {
+ return m_isAwoken;
+ }
+
+private:
+ /// Initialize the view
+ /// \post m_isInitialized is set to true
+ virtual bool doInitialize()
+ {
+ m_isInitialized = true;
+ return true;
+ }
+ /// Wake up the view
+ /// \post m_isAwoken is set to true
+ virtual bool doWakeUp()
+ {
+ m_isAwoken = true;
+ return true;
+ }
+
+ /// Position of the view on the screen (in pixels)
+ int m_x, m_y;
+ /// Dimensions of the view on the screen (in pixels)
+ int m_width, m_height;
+ /// Whether the view window has a border
+ bool m_isWindowBorderEnabled;
+
+ /// Number of times the view has been updated
+ int m_numUpdates;
+ /// Sum of the dt that the view has been updated with
+ double m_sumDt;
+
+ /// Whether the view has been initialized
+ bool m_isInitialized;
+ /// Whether the view has been awoken
+ bool m_isAwoken;
+};
+
+/// Representation that does not subclass any graphics components
+class NonGraphicsRepresentation : public SurgSim::Framework::Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit NonGraphicsRepresentation(const std::string& name) : SurgSim::Framework::Representation(name)
+ {
+ }
+};
+
+#endif // SURGSIM_GRAPHICS_UNITTESTS_MOCKOBJECTS_H
diff --git a/SurgSim/Graphics/UnitTests/MockOsgObjects.h b/SurgSim/Graphics/UnitTests/MockOsgObjects.h
new file mode 100644
index 0000000..0921cbe
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/MockOsgObjects.h
@@ -0,0 +1,177 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_UNITTESTS_MOCKOSGOBJECTS_H
+#define SURGSIM_GRAPHICS_UNITTESTS_MOCKOSGOBJECTS_H
+
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Framework/Runtime.h"
+
+#include <osg/Group>
+#include <osg/Notify>
+
+#include <sstream>
+
+/// Representation class for testing
+class MockOsgRepresentation : public SurgSim::Graphics::OsgRepresentation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ /// \post m_numUpdates and m_sumDt are initialized to 0
+ /// \post m_transform is set to identity
+ /// \post m_isInitialized and m_isAwoken are set to false
+ /// \post m_isVisible is set to true
+ explicit MockOsgRepresentation(const std::string& name) :
+ SurgSim::Graphics::Representation(name),
+ SurgSim::Graphics::OsgRepresentation(name),
+ m_isVisible(true),
+ m_numUpdates(0),
+ m_sumDt(0.0),
+ m_isInitialized(false),
+ m_isAwoken(false)
+ {
+ m_transform.setIdentity();
+ }
+
+ /// Sets whether the representation is currently visible
+ /// \param visible True for visible, false for invisible
+ virtual void setVisible(bool visible)
+ {
+ m_isVisible = visible;
+ }
+
+ /// Gets whether the representation is currently visible
+ /// \return visible True for visible, false for invisible
+ virtual bool isVisible() const
+ {
+ return m_isVisible;
+ }
+
+ /// Returns the number of times the representation has been updated
+ int getNumUpdates() const
+ {
+ return m_numUpdates;
+ }
+ /// Returns the sum of the dt that the representation has been updated with
+ double getSumDt() const
+ {
+ return m_sumDt;
+ }
+
+ /// Updates the representation.
+ /// \param dt The time in seconds of the preceding timestep.
+ /// \post m_numUpdates is incremented and dt is added to m_sumDt
+ virtual void update(double dt)
+ {
+ ++m_numUpdates;
+ m_sumDt += dt;
+ }
+
+ /// Gets whether the representation has been initialized
+ bool isInitialized() const
+ {
+ return m_isInitialized;
+ }
+ /// Gets whether the representation has been awoken
+ bool isAwoken() const
+ {
+ return m_isAwoken;
+ }
+
+private:
+ /// Initializes the representation
+ /// \post m_isInitialized is set to true
+ virtual bool doInitialize()
+ {
+ m_isInitialized = true;
+ return true;
+ }
+ /// Wakes up the representation
+ /// \post m_isAwoken is set to true
+ virtual bool doWakeUp()
+ {
+ m_isAwoken = true;
+ return true;
+ }
+
+ /// Whether this representation is currently visible or not
+ bool m_isVisible;
+
+ /// Number of times the representation has been updated
+ int m_numUpdates;
+ /// Sum of the dt that the representation has been updated with
+ double m_sumDt;
+
+ /// Whether the representation has been initialized
+ bool m_isInitialized;
+ /// Whether the representation has been awoken
+ bool m_isAwoken;
+
+ /// Rigid transform describing pose of the representation
+ SurgSim::Math::RigidTransform3d m_transform;
+};
+
+
+/// Enable Logging of OSG through SurgSim Logging System
+class MockOsgLog : public osg::NotifyHandler
+{
+public:
+ MockOsgLog()
+ {
+ osg::setNotifyLevel(osg::DEBUG_FP);
+ }
+
+ virtual void notify(osg::NotifySeverity severity, const char *message) override
+ {
+ reset();
+ if (severity <= osg::FATAL)
+ {
+ m_message << "CRITICAL " << message;
+ }
+ else if (osg::FATAL < severity && severity <= osg::WARN)
+ {
+ m_message << "WARNING " << message;
+ }
+ else if (osg::WARN < severity && severity <= osg::INFO)
+ {
+ m_message << "INFO " << message;
+ }
+ else if (osg::INFO < severity && severity <= osg::DEBUG_FP)
+ {
+ m_message << "DEBUG " << message;
+ }
+ else
+ {
+ m_message << "Unknown severity in OsgLog::notify()";
+ }
+ }
+
+ std::string getMessage() const
+ {
+ return m_message.str();
+ }
+
+ void reset()
+ {
+ m_message.clear();
+ m_message.str("");
+ }
+
+ std::stringstream m_message;
+};
+
+
+#endif // SURGSIM_GRAPHICS_UNITTESTS_MOCKOSGOBJECTS_H
diff --git a/SurgSim/Graphics/UnitTests/OsgBoxRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgBoxRepresentationTests.cpp
new file mode 100644
index 0000000..1cd095c
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgBoxRepresentationTests.cpp
@@ -0,0 +1,237 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgBoxRepresentation class.
+
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST(OsgBoxRepresentationTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<Representation> representation =
+ std::make_shared<OsgBoxRepresentation>("test name");
+ });
+
+ std::shared_ptr<Representation> representation = std::make_shared<OsgBoxRepresentation>("test name");
+ EXPECT_EQ("test name", representation->getName());
+}
+
+TEST(OsgBoxRepresentationTests, AccessibleTest)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component;
+ ASSERT_NO_THROW(component = SurgSim::Framework::Component::getFactory().create(
+ "SurgSim::Graphics::OsgBoxRepresentation",
+ "box"));
+
+ EXPECT_EQ("SurgSim::Graphics::OsgBoxRepresentation", component->getClassName());
+
+ SurgSim::Math::Vector3d size(1.0, 2.0, 3.0);
+
+ component->setValue("Size", size);
+ YAML::Node node(YAML::convert<SurgSim::Framework::Component>::encode(*component));
+
+ auto decoded = std::dynamic_pointer_cast<SurgSim::Graphics::OsgBoxRepresentation>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>());
+
+ EXPECT_NE(nullptr, decoded);
+ EXPECT_TRUE(size.isApprox(decoded->getValue<SurgSim::Math::Vector3d>("Size")));
+}
+
+TEST(OsgBoxRepresentationTests, VisibilityTest)
+{
+ std::shared_ptr<Representation> representation = std::make_shared<OsgBoxRepresentation>("test name");
+
+ representation->setVisible(true);
+ EXPECT_TRUE(representation->isVisible());
+
+ representation->setVisible(false);
+ EXPECT_FALSE(representation->isVisible());
+}
+
+TEST(OsgBoxRepresentationTests, SizeXTest)
+{
+ std::shared_ptr<BoxRepresentation> boxRepresentation = std::make_shared<OsgBoxRepresentation>("test name");
+
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> distribution(1.0, 10.0);
+
+ double randomSize = distribution(generator);
+
+ boxRepresentation->setSizeX(randomSize);
+ EXPECT_EQ(randomSize, boxRepresentation->getSizeX());
+}
+
+TEST(OsgBoxRepresentationTests, SizeYTest)
+{
+ std::shared_ptr<BoxRepresentation> boxRepresentation = std::make_shared<OsgBoxRepresentation>("test name");
+
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> distribution(1.0, 10.0);
+
+ double randomSize = distribution(generator);
+
+ boxRepresentation->setSizeY(randomSize);
+ EXPECT_EQ(randomSize, boxRepresentation->getSizeY());
+}
+
+TEST(OsgBoxRepresentationTests, SizeZTest)
+{
+ std::shared_ptr<BoxRepresentation> boxRepresentation = std::make_shared<OsgBoxRepresentation>("test name");
+
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> distribution(1.0, 10.0);
+
+ double randomSize = distribution(generator);
+
+ boxRepresentation->setSizeZ(randomSize);
+ EXPECT_EQ(randomSize, boxRepresentation->getSizeZ());
+}
+
+TEST(OsgBoxRepresentationTests, SizeTest)
+{
+ std::shared_ptr<BoxRepresentation> boxRepresentation = std::make_shared<OsgBoxRepresentation>("test name");
+
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> distribution(1.0, 10.0);
+
+ double randomSizeX = distribution(generator);
+ double randomSizeY = distribution(generator);
+ double randomSizeZ = distribution(generator);
+
+ boxRepresentation->setSizeXYZ(randomSizeX, randomSizeY, randomSizeZ);
+ EXPECT_EQ(randomSizeX, boxRepresentation->getSizeX());
+ EXPECT_EQ(randomSizeY, boxRepresentation->getSizeY());
+ EXPECT_EQ(randomSizeZ, boxRepresentation->getSizeZ());
+}
+
+TEST(OsgBoxRepresentationTests, SizeVector3dTest)
+{
+ std::shared_ptr<BoxRepresentation> boxRepresentation = std::make_shared<OsgBoxRepresentation>("test name");
+
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> distribution(1.0, 10.0);
+
+ Vector3d randomSize(distribution(generator), distribution(generator), distribution(generator));
+
+ boxRepresentation->setSize(randomSize);
+ EXPECT_EQ(randomSize.x(), boxRepresentation->getSizeX());
+ EXPECT_EQ(randomSize.y(), boxRepresentation->getSizeY());
+ EXPECT_EQ(randomSize.z(), boxRepresentation->getSizeZ());
+}
+
+TEST(OsgBoxRepresentationTests, PoseTest)
+{
+ std::shared_ptr<Representation> representation = std::make_shared<OsgBoxRepresentation>("test name");
+ std::shared_ptr<BasicSceneElement> element = std::make_shared<BasicSceneElement>("element");
+ element->addComponent(representation);
+ element->initialize();
+ representation->wakeUp();
+
+ {
+ SCOPED_TRACE("Check Initial Pose");
+ EXPECT_TRUE(representation->getLocalPose().isApprox(RigidTransform3d::Identity()));
+ EXPECT_TRUE(representation->getPose().isApprox(RigidTransform3d::Identity()));
+ }
+
+ RigidTransform3d localPose;
+ {
+ SCOPED_TRACE("Set Local Pose");
+ localPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ representation->setLocalPose(localPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(localPose));
+ }
+
+ RigidTransform3d elementPose;
+ {
+ SCOPED_TRACE("Set Element Pose");
+ elementPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ element->setPose(elementPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(elementPose * localPose));
+ }
+
+ {
+ SCOPED_TRACE("Change Local Pose");
+ localPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ representation->setLocalPose(localPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(elementPose * localPose));
+ }
+
+ YAML::Node node = representation->encode();
+}
+
+TEST(OsgBoxRepresentationTests, MaterialTest)
+{
+ std::shared_ptr<OsgBoxRepresentation> osgRepresentation = std::make_shared<OsgBoxRepresentation>("test name");
+ std::shared_ptr<Representation> representation = osgRepresentation;
+
+ std::shared_ptr<OsgMaterial> osgMaterial = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<Material> material = osgMaterial;
+ {
+ SCOPED_TRACE("Set material");
+ EXPECT_TRUE(representation->setMaterial(material));
+ EXPECT_EQ(material, representation->getMaterial());
+
+ osg::Switch* switchNode = dynamic_cast<osg::Switch*>(osgRepresentation->getOsgNode().get());
+ ASSERT_NE(nullptr, switchNode) << "Could not get OSG switch node!";
+ ASSERT_EQ(1u, switchNode->getNumChildren()) << "OSG switch node should have 1 child, the transform node!";
+ EXPECT_EQ(osgMaterial->getOsgStateSet(), switchNode->getChild(0)->getStateSet()) <<
+ "State set should be the material's state set!";
+ }
+
+ {
+ SCOPED_TRACE("Clear material");
+ representation->clearMaterial();
+ EXPECT_EQ(nullptr, representation->getMaterial());
+
+ osg::Switch* switchNode = dynamic_cast<osg::Switch*>(osgRepresentation->getOsgNode().get());
+ ASSERT_NE(nullptr, switchNode) << "Could not get OSG switch node!";
+ ASSERT_EQ(1u, switchNode->getNumChildren()) << "OSG switch node should have 1 child, the transform node!";
+ EXPECT_NE(osgMaterial->getOsgStateSet(), switchNode->getChild(0)->getStateSet()) <<
+ "State set should have been cleared!";
+ }
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgCameraTests.cpp b/SurgSim/Graphics/UnitTests/OsgCameraTests.cpp
new file mode 100644
index 0000000..58a55d6
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgCameraTests.cpp
@@ -0,0 +1,257 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgCamera class.
+
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgMatrixConversions.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Math/Quaternion.h"
+
+#include <osg/Camera>
+#include <osg/ref_ptr>
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgCameraTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<Camera> camera = std::make_shared<OsgCamera>("test name");});
+
+ std::shared_ptr<OsgCamera> osgCamera = std::make_shared<OsgCamera>("test name");
+ std::shared_ptr<Camera> camera = osgCamera;
+
+ EXPECT_EQ("test name", camera->getName());
+
+ EXPECT_TRUE(camera->isVisible());
+
+ EXPECT_TRUE(camera->getPose().matrix().isApprox(
+ fromOsg(osgCamera->getOsgCamera()->getViewMatrix()).inverse())) <<
+ "Camera's pose should be initialized to the inverse of the osg::Camera's view matrix!";
+
+ EXPECT_TRUE(camera->getViewMatrix().isApprox(fromOsg(osgCamera->getOsgCamera()->getViewMatrix()))) <<
+ "Camera's view matrix should be initialized to the osg::Camera's view matrix!";
+
+ EXPECT_TRUE(camera->getProjectionMatrix().isApprox(fromOsg(osgCamera->getOsgCamera()->getProjectionMatrix()))) <<
+ "Camera's projection matrix should be initialized to the osg::Camera's projection matrix!";
+
+ EXPECT_EQ(nullptr, camera->getRenderGroup());
+}
+
+TEST(OsgCameraTests, OsgNodesTest)
+{
+ std::shared_ptr<OsgCamera> osgCamera = std::make_shared<OsgCamera>("test name");
+ std::shared_ptr<OsgRepresentation> osgRepresentation = osgCamera;
+
+ /// Check that the OSG nodes of the camera are built correctly
+ osg::ref_ptr<osg::Node> node = osgRepresentation->getOsgNode();
+ osg::ref_ptr<osg::Switch> switchNode = dynamic_cast<osg::Switch*>(node.get());
+ EXPECT_TRUE(switchNode.valid());
+ EXPECT_EQ(1u, switchNode->getNumChildren());
+
+ osg::ref_ptr<osg::Camera> camera = osgCamera->getOsgCamera();
+ EXPECT_EQ(camera.get(), switchNode->getChild(0));
+}
+
+TEST(OsgCameraTests, VisibilityTest)
+{
+ std::shared_ptr<OsgCamera> osgCamera = std::make_shared<OsgCamera>("test name");
+ std::shared_ptr<OsgRepresentation> osgRepresentation = osgCamera;
+ std::shared_ptr<Camera> camera = osgCamera;
+
+ // Get the osg::Switch from the OsgRepresentation so that we can make sure that the osg::Camera has the
+ // correct visibility.
+ osg::ref_ptr<osg::Switch> switchNode = dynamic_cast<osg::Switch*>(osgRepresentation->getOsgNode().get());
+ EXPECT_TRUE(switchNode.valid());
+
+ EXPECT_TRUE(camera->isVisible());
+ EXPECT_TRUE(switchNode->getChildValue(osgCamera->getOsgCamera()));
+
+ camera->setVisible(false);
+ EXPECT_FALSE(camera->isVisible());
+ EXPECT_FALSE(switchNode->getChildValue(osgCamera->getOsgCamera()));
+
+ camera->setVisible(true);
+ EXPECT_TRUE(camera->isVisible());
+ EXPECT_TRUE(switchNode->getChildValue(osgCamera->getOsgCamera()));
+
+}
+
+TEST(OsgCameraTests, GroupTest)
+{
+ std::shared_ptr<OsgCamera> osgCamera = std::make_shared<OsgCamera>("test name");
+ std::shared_ptr<Camera> camera = osgCamera;
+
+ EXPECT_EQ(nullptr, camera->getRenderGroup());
+
+ /// Adding an OsgGroup should succeed
+ std::shared_ptr<OsgGroup> osgGroup = std::make_shared<OsgGroup>(camera->getRenderGroupReference());
+ std::shared_ptr<Group> group = osgGroup;
+ EXPECT_TRUE(camera->setRenderGroup(group));
+ EXPECT_EQ(group, camera->getRenderGroup());
+
+ /// Check that the OSG node of the group is added to the OSG camera correctly
+ EXPECT_EQ(osgGroup->getOsgGroup(), osgCamera->getOsgCamera()->getChild(0)->asGroup()->getChild(0));
+
+ /// Adding a group that does not derive from OsgGroup should fail
+ std::shared_ptr<Group> mockGroup = std::make_shared<MockGroup>(camera->getRenderGroupReference());
+ EXPECT_FALSE(camera->setRenderGroup(mockGroup));
+ EXPECT_EQ(group, camera->getRenderGroup());
+ EXPECT_EQ(osgGroup->getOsgGroup(), osgCamera->getOsgCamera()->getChild(0)->asGroup()->getChild(0));
+}
+
+
+TEST(OsgCameraTests, PoseTest)
+{
+ std::shared_ptr<OsgCamera> osgCamera = std::make_shared<OsgCamera>("test name");
+ std::shared_ptr<Camera> camera = osgCamera;
+ camera->setRenderGroupReference("Test");
+ std::shared_ptr<BasicSceneElement> element = std::make_shared<BasicSceneElement>("element");
+ element->addComponent(camera);
+ element->initialize();
+ camera->wakeUp();
+
+ RigidTransform3d elementPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ RigidTransform3d localPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ RigidTransform3d pose = elementPose * localPose;
+
+ {
+ SCOPED_TRACE("Check Initial Pose");
+ EXPECT_TRUE(camera->getLocalPose().isApprox(RigidTransform3d::Identity()));
+ EXPECT_TRUE(camera->getPose().isApprox(RigidTransform3d::Identity()));
+ }
+
+ {
+ SCOPED_TRACE("Set Local Pose");
+ camera->setLocalPose(localPose);
+ EXPECT_TRUE(camera->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(camera->getPose().isApprox(localPose));
+ EXPECT_TRUE(camera->getViewMatrix().isApprox(localPose.matrix().inverse()));
+ EXPECT_TRUE(camera->getViewMatrix().inverse().isApprox(camera->getInverseViewMatrix()));
+
+ camera->update(0.01);
+ EXPECT_TRUE(fromOsg(osgCamera->getOsgCamera()->getViewMatrix()).isApprox(localPose.matrix().inverse()));
+ }
+
+ {
+ SCOPED_TRACE("Set Element Pose");
+ element->setPose(elementPose);
+ EXPECT_TRUE(camera->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(camera->getPose().isApprox(pose));
+ EXPECT_TRUE(camera->getViewMatrix().isApprox(pose.matrix().inverse()));
+ EXPECT_TRUE(camera->getViewMatrix().inverse().isApprox(camera->getInverseViewMatrix()));
+
+ camera->update(0.01);
+ EXPECT_TRUE(fromOsg(osgCamera->getOsgCamera()->getViewMatrix()).isApprox(pose.matrix().inverse()));
+ }
+}
+
+TEST(OsgCameraTests, MatricesTest)
+{
+ std::shared_ptr<OsgCamera> osgCamera = std::make_shared<OsgCamera>("test name");
+ std::shared_ptr<Camera> camera = osgCamera;
+
+ Matrix44d projectionMatrix = Matrix44d::Random();
+ camera->setProjectionMatrix(projectionMatrix);
+ EXPECT_TRUE(camera->getProjectionMatrix().isApprox(projectionMatrix));
+}
+
+TEST(OsgCameraTests, RenderTargetTest)
+{
+ auto osgCamera = std::make_shared<OsgCamera>("test camera");
+ std::shared_ptr<Camera> camera = osgCamera;
+
+ std::shared_ptr<RenderTarget> renderTarget = std::make_shared<OsgRenderTarget2d>(256, 256, 1.0, 2, true);
+
+ EXPECT_NO_THROW(camera->setRenderTarget(renderTarget));
+ EXPECT_TRUE(osgCamera->getOsgCamera()->isRenderToTextureCamera());
+}
+
+TEST(OsgCameraTests, CameraGroupTest)
+{
+ std::shared_ptr<Camera> camera = std::make_shared<OsgCamera>("TestRepresentation");
+
+ camera->clearGroupReferences();
+ camera->addGroupReference("test1");
+ camera->addGroupReference("test2");
+
+ EXPECT_EQ(2u, camera->getGroupReferences().size());
+
+ camera->setRenderGroupReference("otherTest");
+ EXPECT_EQ(2u, camera->getGroupReferences().size());
+
+ // Setting the render group of a camera to the group that it is in should remove the group
+ // from the set
+ camera->setRenderGroupReference("test1");
+ EXPECT_EQ(1u, camera->getGroupReferences().size());
+
+ // Should not be able to set group reference to the current render group
+ EXPECT_FALSE(camera->addGroupReference("test1"));
+ EXPECT_EQ(1u, camera->getGroupReferences().size());
+}
+
+TEST(OsgCameraTests, Serialization)
+{
+ std::shared_ptr<OsgCamera> camera = std::make_shared<OsgCamera>("TestOsgCamera");
+
+ // Set values.
+ SurgSim::Math::Matrix44d projection = SurgSim::Math::Matrix44d::Random();
+ camera->setValue("ProjectionMatrix", projection);
+ camera->setValue("Visible", true);
+ camera->setValue("AmbientColor", SurgSim::Math::Vector4d(0.1, 0.2, 0.3, 0.4));
+
+ // Serialize.
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*camera););
+
+ // Deserialize.
+ std::shared_ptr<Camera> newCamera;
+ newCamera = std::dynamic_pointer_cast<OsgCamera>(node.as<std::shared_ptr<SurgSim::Framework::Component>>());
+ EXPECT_NE(nullptr, newCamera);
+
+ // Verify.
+ EXPECT_TRUE(boost::any_cast<SurgSim::Math::Matrix44d>(camera->getValue("ProjectionMatrix")).isApprox(
+ boost::any_cast<SurgSim::Math::Matrix44d>(newCamera->getValue("ProjectionMatrix"))));
+ EXPECT_EQ(boost::any_cast<bool>(camera->getValue("Visible")),
+ boost::any_cast<bool>(newCamera->getValue("Visible")));
+ EXPECT_TRUE(boost::any_cast<SurgSim::Math::Vector4d>(camera->getValue("AmbientColor")).isApprox(
+ boost::any_cast<SurgSim::Math::Vector4d>(newCamera->getValue("AmbientColor"))));
+}
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgCapsuleRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgCapsuleRepresentationTests.cpp
new file mode 100644
index 0000000..8416d15
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgCapsuleRepresentationTests.cpp
@@ -0,0 +1,112 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgCapsuleRepresentation class.
+
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Graphics/OsgCapsuleRepresentation.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+using SurgSim::Math::Vector2d;
+
+namespace
+{
+const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST(OsgCapsuleRepresentationTests, AccessibleTest)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component;
+ ASSERT_NO_THROW(component = SurgSim::Framework::Component::getFactory().create(
+ "SurgSim::Graphics::OsgCapsuleRepresentation",
+ "capsule"));
+
+ EXPECT_EQ("SurgSim::Graphics::OsgCapsuleRepresentation", component->getClassName());
+
+ double radius = 4.321;
+ double height = 1.234;
+
+ component->setValue("Height", height);
+ component->setValue("Radius", radius);
+
+ YAML::Node node(YAML::convert<SurgSim::Framework::Component>::encode(*component));
+
+ auto decoded = std::dynamic_pointer_cast<SurgSim::Graphics::OsgCapsuleRepresentation>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>());
+
+ EXPECT_NE(nullptr, decoded);
+ EXPECT_DOUBLE_EQ(radius, decoded->getValue<double>("Radius"));
+ EXPECT_DOUBLE_EQ(height, decoded->getValue<double>("Height"));
+}
+
+TEST(OsgCapsuleRepresentationTests, RadiusTest)
+{
+ std::shared_ptr<CapsuleRepresentation> capsuleRepresentation =
+ std::make_shared<OsgCapsuleRepresentation>("test name");
+
+ double radius = 1.0;
+ capsuleRepresentation->setRadius(radius);
+ EXPECT_NEAR(capsuleRepresentation->getRadius(), radius, epsilon);
+}
+
+TEST(OsgCapsuleRepresentationTests, HeightTest)
+{
+ std::shared_ptr<CapsuleRepresentation> capsuleRepresentation =
+ std::make_shared<OsgCapsuleRepresentation>("test name");
+
+ double height = 1.0;
+ capsuleRepresentation->setHeight(height);
+ EXPECT_NEAR(capsuleRepresentation->getHeight(), height, epsilon);
+}
+
+TEST(OsgCapsuleRepresentationTests, SizeTest)
+{
+ std::shared_ptr<CapsuleRepresentation> capsuleRepresentation =
+ std::make_shared<OsgCapsuleRepresentation>("test name");
+
+ double radius = 1.0, height = 1.0;
+ double retrievedRadius, retrievedHeight;
+
+ capsuleRepresentation->setSize(radius, height);
+ capsuleRepresentation->getSize(&retrievedRadius, &retrievedHeight);
+
+ EXPECT_NEAR(radius, retrievedRadius, epsilon);
+ EXPECT_NEAR(height, retrievedHeight, epsilon);
+}
+
+TEST(OsgCapsuleRepresentationTests, SizeVectordTest)
+{
+ std::shared_ptr<CapsuleRepresentation> capsuleRepresentation =
+ std::make_shared<OsgCapsuleRepresentation>("test name");
+
+ Vector2d size(1.0, 1.0);
+ capsuleRepresentation->setSize(size);
+ EXPECT_TRUE(capsuleRepresentation->getSize().isApprox(size));
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgCylinderRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgCylinderRepresentationTests.cpp
new file mode 100644
index 0000000..54df2b6
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgCylinderRepresentationTests.cpp
@@ -0,0 +1,116 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgCylinderRepresentation class.
+
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgCylinderRepresentation.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+using SurgSim::Math::Vector2d;
+
+namespace
+{
+const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST(OsgCylinderRepresentationTests, AccessibleTest)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component;
+ ASSERT_NO_THROW(component = SurgSim::Framework::Component::getFactory().create(
+ "SurgSim::Graphics::OsgCylinderRepresentation",
+ "sphere"));
+
+ EXPECT_EQ("SurgSim::Graphics::OsgCylinderRepresentation", component->getClassName());
+
+ double radius = 4.321;
+ double height = 1.234;
+
+ component->setValue("Height", height);
+ component->setValue("Radius", radius);
+
+ YAML::Node node(YAML::convert<SurgSim::Framework::Component>::encode(*component));
+
+ auto decoded = std::dynamic_pointer_cast<SurgSim::Graphics::OsgCylinderRepresentation>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>());
+
+ EXPECT_NE(nullptr, decoded);
+ EXPECT_DOUBLE_EQ(radius, decoded->getValue<double>("Radius"));
+ EXPECT_DOUBLE_EQ(height, decoded->getValue<double>("Height"));
+}
+
+TEST(OsgCylinderRepresentationTests, RadiusTest)
+{
+ std::shared_ptr<CylinderRepresentation> cylinderRepresentation =
+ std::make_shared<OsgCylinderRepresentation>("test name");
+
+ double radius = 1.0;
+
+ cylinderRepresentation->setRadius(radius);
+ EXPECT_NEAR(radius, cylinderRepresentation->getRadius(), epsilon);
+}
+
+TEST(OsgCylinderRepresentationTests, HeightTest)
+{
+ std::shared_ptr<CylinderRepresentation> cylinderRepresentation =
+ std::make_shared<OsgCylinderRepresentation>("test name");
+
+ double height = 1.0;
+
+ cylinderRepresentation->setHeight(height);
+ EXPECT_NEAR(height, cylinderRepresentation->getHeight(), epsilon);
+}
+
+TEST(OsgCylinderRepresentationTests, SizeTest)
+{
+ std::shared_ptr<CylinderRepresentation> cylinderRepresentation =
+ std::make_shared<OsgCylinderRepresentation>("test name");
+
+ double radius = 1.0, height = 1.0;
+
+ cylinderRepresentation->setSize(radius, height);
+ EXPECT_NEAR(radius, cylinderRepresentation->getRadius(), epsilon);
+ EXPECT_NEAR(height, cylinderRepresentation->getHeight(), epsilon);
+}
+
+TEST(OsgCylinderRepresentationTests, SizeVector2dTest)
+{
+ std::shared_ptr<CylinderRepresentation> cylinderRepresentation =
+ std::make_shared<OsgCylinderRepresentation>("test name");
+
+ Vector2d size(1.0, 1.0);
+
+ cylinderRepresentation->setSize(size);
+ EXPECT_TRUE(size.isApprox(cylinderRepresentation->getSize(), epsilon));
+}
+
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgGroupTests.cpp b/SurgSim/Graphics/UnitTests/OsgGroupTests.cpp
new file mode 100644
index 0000000..10ad965
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgGroupTests.cpp
@@ -0,0 +1,225 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Group class.
+
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include "SurgSim/Graphics/OsgGroup.h"
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgGroupTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<Group> group = std::make_shared<OsgGroup>("test name");});
+
+ std::shared_ptr<OsgGroup> osgGroup = std::make_shared<OsgGroup>("test group");
+ std::shared_ptr<Group> group = osgGroup;
+
+ EXPECT_EQ("test group", group->getName());
+ EXPECT_TRUE(group->isVisible());
+ EXPECT_EQ(0u, group->getMembers().size());
+ EXPECT_EQ(0u, osgGroup->getOsgGroup()->getNumChildren());
+}
+
+TEST(OsgGroupTests, OsgNodesTest)
+{
+ std::shared_ptr<OsgGroup> osgGroup = std::make_shared<OsgGroup>("test group");
+
+ osg::ref_ptr<osg::Switch> osgSwitch = dynamic_cast<osg::Switch*>(osgGroup->getOsgGroup().get());
+ ASSERT_TRUE(osgSwitch.valid()) << "Group's OSG node should be a switch!";
+}
+
+TEST(OsgGroupTests, VisibilityTest)
+{
+ std::shared_ptr<OsgGroup> osgGroup = std::make_shared<OsgGroup>("test group");
+ std::shared_ptr<Group> group = osgGroup;
+
+ group->setVisible(false);
+ EXPECT_FALSE(group->isVisible());
+
+ group->setVisible(true);
+ EXPECT_TRUE(group->isVisible());
+}
+
+TEST(OsgGroupTests, AddRemoveTest)
+{
+ std::shared_ptr<OsgGroup> osgGroup = std::make_shared<OsgGroup>("test group");
+ std::shared_ptr<Group> group = osgGroup;
+
+ EXPECT_EQ(0u, group->getMembers().size());
+
+ osg::ref_ptr<osg::Switch> osgSwitch = dynamic_cast<osg::Switch*>(osgGroup->getOsgGroup().get());
+ ASSERT_TRUE(osgSwitch.valid()) << "Group's OSG node should be a switch!";
+ EXPECT_EQ(0u, osgSwitch->getNumChildren());
+
+ /// Add an representation and make sure the osg::Switch value for it is set correctly (should be true)
+ std::shared_ptr<OsgRepresentation> representation1 =
+ std::make_shared<MockOsgRepresentation>("test representation 1");
+ group->add(representation1);
+ EXPECT_EQ(1u, group->getMembers().size());
+ EXPECT_EQ(1u, osgSwitch->getNumChildren());
+ EXPECT_EQ(0u, osgSwitch->getChildIndex(representation1->getOsgNode()));
+ EXPECT_TRUE(osgSwitch->getChildValue(representation1->getOsgNode())) << "Representation 1 should be visible!";
+
+ /// Set group to not visible and check the osg::Switch values
+ group->setVisible(false);
+ EXPECT_FALSE(group->isVisible());
+ EXPECT_FALSE(osgSwitch->getChildValue(representation1->getOsgNode())) << "Representation 1 should not be visible!";
+
+ /// Add another representation and make sure the osg::Switch value for it is set correctly (should be false)
+ std::shared_ptr<OsgRepresentation> representation2 =
+ std::make_shared<MockOsgRepresentation>("test representation 2");
+ group->add(representation2);
+ EXPECT_EQ(2u, group->getMembers().size());
+ EXPECT_EQ(2u, osgSwitch->getNumChildren());
+ EXPECT_EQ(1u, osgSwitch->getChildIndex(representation2->getOsgNode()));
+ EXPECT_FALSE(osgSwitch->getChildValue(representation2->getOsgNode())) << "Representation 2 should not be visible!";
+
+ /// Set group to visible and check the osg::Switch values
+ group->setVisible(true);
+ EXPECT_TRUE(group->isVisible());
+ EXPECT_TRUE(osgSwitch->getChildValue(representation1->getOsgNode())) << "Representation 1 should be visible!";
+ EXPECT_TRUE(osgSwitch->getChildValue(representation2->getOsgNode())) << "Representation 2 should be visible!";
+
+ /// Set group to not visible and check the osg::Switch values
+ group->setVisible(false);
+ EXPECT_FALSE(group->isVisible());
+ EXPECT_FALSE(osgSwitch->getChildValue(representation1->getOsgNode())) << "Representation 1 should not be visible!";
+ EXPECT_FALSE(osgSwitch->getChildValue(representation2->getOsgNode())) << "Representation 1 should not be visible!";
+
+ /// Try to add a duplicate representation
+ EXPECT_FALSE(group->add(representation1));
+ EXPECT_EQ(2u, group->getMembers().size());
+ EXPECT_EQ(2u, osgSwitch->getNumChildren());
+
+ /// Remove an representation
+ EXPECT_TRUE(group->remove(representation1));
+ EXPECT_EQ(1u, osgSwitch->getNumChildren());
+ EXPECT_EQ(1u, osgSwitch->getChildIndex(representation1->getOsgNode()));
+
+ /// Try to remove an representation that is not in the group
+ EXPECT_FALSE(group->remove(representation1));
+ EXPECT_EQ(1u, osgSwitch->getChildIndex(representation1->getOsgNode()));
+
+ /// Try to add a non-OSG representation
+ std::shared_ptr<MockRepresentation> nonOsgRepresentation =
+ std::make_shared<MockRepresentation>("non-osg representation");
+ EXPECT_FALSE(group->add(nonOsgRepresentation)) <<
+ "OsgGroup should only succeed on representations that derive from OsgRepresentation!";
+ EXPECT_EQ(1u, group->getMembers().size());
+ EXPECT_EQ(group->getMembers().end(),
+ std::find(group->getMembers().begin(), group->getMembers().end(), nonOsgRepresentation)) <<
+ "Only subclasses of OsgRepresentation should be in an OsgGroup!";
+ EXPECT_EQ(1u, osgSwitch->getNumChildren());
+}
+
+TEST(OsgGroupTests, AppendTest)
+{
+ std::shared_ptr<Group> group1 = std::make_shared<OsgGroup>("test group 1");
+ EXPECT_EQ(0u, group1->getMembers().size());
+
+ std::shared_ptr<Representation> representation1 = std::make_shared<MockOsgRepresentation>("test representation 1");
+ std::shared_ptr<Representation> representation2 = std::make_shared<MockOsgRepresentation>("test representation 2");
+
+ /// Add 2 representations to group 1
+ EXPECT_TRUE(group1->add(representation1));
+ EXPECT_TRUE(group1->add(representation2));
+ EXPECT_EQ(2u, group1->getMembers().size());
+
+ std::shared_ptr<Representation> representation3 = std::make_shared<MockOsgRepresentation>("test representation 3");
+
+ std::shared_ptr<Group> group2 = std::make_shared<OsgGroup>("test group 2");
+ EXPECT_EQ(0u, group2->getMembers().size());
+
+ /// Add an representation to group 2 and append group 1 to group 2.
+ EXPECT_TRUE(group2->add(representation3));
+ EXPECT_TRUE(group2->append(group1));
+ EXPECT_EQ(3u, group2->getMembers().size());
+
+ // Check that the representations from group 1 were added to group 2, and that it still has the representation
+ // that was added directly to it.
+ EXPECT_NE(group2->getMembers().end(),
+ std::find(group2->getMembers().begin(), group2->getMembers().end(), representation1));
+ EXPECT_NE(group2->getMembers().end(),
+ std::find(group2->getMembers().begin(), group2->getMembers().end(), representation2));
+ EXPECT_NE(group2->getMembers().end(),
+ std::find(group2->getMembers().begin(), group2->getMembers().end(), representation3));
+
+ /// Try to append a group that has already been appended - this will try to add duplicate representations.
+ EXPECT_FALSE(group2->append(group1)) << "Append should return false if any representation is a duplicate!";
+ EXPECT_EQ(3u, group2->getMembers().size());
+
+ /// Check that group 1 was not modified by appending it to group 2.
+ EXPECT_EQ(2u, group1->getMembers().size());
+ EXPECT_NE(group1->getMembers().end(),
+ std::find(group1->getMembers().begin(), group1->getMembers().end(), representation1));
+ EXPECT_NE(group1->getMembers().end(),
+ std::find(group1->getMembers().begin(), group1->getMembers().end(), representation2));
+
+ /// Try to append a group that is not a subclass of OsgGroup
+ std::shared_ptr<Group> nonOsgGroup = std::make_shared<MockGroup>("non-osg group");
+ std::shared_ptr<Representation> nonOsgRepresentation =
+ std::make_shared<MockRepresentation>("non-osg representation");
+ /// Add an OSG and non-OSG representation to this group.
+ EXPECT_TRUE(nonOsgGroup->add(representation3));
+ EXPECT_TRUE(nonOsgGroup->add(nonOsgRepresentation));
+
+ EXPECT_FALSE(group1->append(nonOsgGroup));
+ EXPECT_EQ(2u, group1->getMembers().size()) <<
+ "Nothing from the non-OSG group should have been added to the OsgGroup!";
+ EXPECT_EQ(group1->getMembers().end(),
+ std::find(group1->getMembers().begin(), group1->getMembers().end(), representation3));
+ EXPECT_EQ(group1->getMembers().end(),
+ std::find(group1->getMembers().begin(), group1->getMembers().end(), nonOsgRepresentation));
+}
+
+TEST(OsgGroupTests, ClearTest)
+{
+ std::shared_ptr<OsgGroup> osgGroup = std::make_shared<OsgGroup>("test name");
+ std::shared_ptr<Group> group = osgGroup;
+
+ std::shared_ptr<Representation> representation1 = std::make_shared<MockOsgRepresentation>("test representation 1");
+ std::shared_ptr<Representation> representation2 = std::make_shared<MockOsgRepresentation>("test representation 2");
+ std::shared_ptr<Representation> representation3 = std::make_shared<MockOsgRepresentation>("test representation 3");
+
+ osg::ref_ptr<osg::Switch> osgSwitch = dynamic_cast<osg::Switch*>(osgGroup->getOsgGroup().get());
+ ASSERT_TRUE(osgSwitch.valid()) << "Group's OSG node should be a switch!";
+
+ EXPECT_EQ(0u, group->getMembers().size());
+ EXPECT_EQ(0u, osgSwitch->getNumChildren());
+
+ // Add 3 representations
+ EXPECT_TRUE(group->add(representation1));
+ EXPECT_TRUE(group->add(representation2));
+ EXPECT_TRUE(group->add(representation3));
+ EXPECT_EQ(3u, group->getMembers().size());
+ EXPECT_EQ(3u, osgSwitch->getNumChildren());
+
+ // Remove all representations
+ group->clear();
+ EXPECT_EQ(0u, group->getMembers().size());
+ EXPECT_EQ(0u, osgSwitch->getNumChildren());
+}
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgLightTests.cpp b/SurgSim/Graphics/UnitTests/OsgLightTests.cpp
new file mode 100644
index 0000000..7682f1a
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgLightTests.cpp
@@ -0,0 +1,174 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Basic logic tests for OsgLight
+
+#include <gtest/gtest.h>
+#include "SurgSim/Graphics/Light.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgConversions.h"
+
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+
+namespace
+{
+const double epsilon = 1e-8;
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class OsgLightTests : public testing::Test
+{
+public:
+ testing::AssertionResult hasUniforms(
+ std::shared_ptr<OsgLight> light,
+ std::shared_ptr<OsgGroup> group, bool doesHave)
+ {
+ osg::ref_ptr<osg::Group> osgGroup = group->getOsgGroup();
+ osg::ref_ptr<osg::StateSet> stateSet = osgGroup->getStateSet();
+
+ for (auto it = std::begin(light->m_uniforms); it != std::end(light->m_uniforms); ++it)
+ {
+ osg::ref_ptr<osg::Uniform> uniform = stateSet->getUniform(it->second->getName());
+ if (doesHave && uniform == nullptr)
+ {
+ return testing::AssertionFailure() << "Expected uniform " << it->second->getName() << " not found.";
+ }
+ else if (!doesHave && uniform != nullptr)
+ {
+ return testing::AssertionFailure() << "Did not expect uniform " << it->second->getName() <<
+ " but found it.";
+ }
+ }
+ return testing::AssertionSuccess();
+ }
+
+ osg::ref_ptr<osg::Uniform> getUniform(std::shared_ptr<OsgLight> light, int type)
+ {
+ return light->m_uniforms[type];
+ }
+};
+
+TEST_F(OsgLightTests, InitTest)
+{
+ ASSERT_NO_THROW({auto light = std::make_shared<OsgLight>("TestLight");});
+
+ std::shared_ptr<OsgLight> light = std::make_shared<OsgLight>("TestLight");
+
+ ASSERT_EQ(nullptr, light->getGroup());
+}
+
+
+
+TEST_F(OsgLightTests, GroupAccessorTest)
+{
+ std::shared_ptr<OsgLight> light = std::make_shared<OsgLight>("TestLight");
+ std::shared_ptr<OsgGroup> group = std::make_shared<OsgGroup>("TestGroup");
+ std::shared_ptr<MockGroup> mockGroup = std::make_shared<MockGroup>("MockGroup");
+
+ // Light does not have a default group
+ EXPECT_EQ(nullptr, light->getGroup());
+
+ // Assigning a light to a group
+ EXPECT_TRUE(light->setGroup(group));
+ EXPECT_EQ(group, light->getGroup());
+
+ EXPECT_TRUE(hasUniforms(light, group, true));
+
+ // Light should not take a mock group as a group
+ EXPECT_FALSE(light->setGroup(mockGroup));
+ EXPECT_EQ(group, light->getGroup());
+
+ // Assigning an empty group should clear the group
+ EXPECT_TRUE(light->setGroup(nullptr));
+ EXPECT_EQ(nullptr, light->getGroup());
+
+ // Still should not accept a mock group
+ EXPECT_FALSE(light->setGroup(mockGroup));
+ EXPECT_EQ(nullptr, light->getGroup());
+
+ // Setting the nullptr should also work
+ EXPECT_TRUE(light->setGroup(nullptr));
+ EXPECT_EQ(nullptr, light->getGroup());
+
+ EXPECT_TRUE(hasUniforms(light, group, false));
+}
+
+TEST_F(OsgLightTests, ColorAccessorTests)
+{
+ std::shared_ptr<OsgLight> light = std::make_shared<OsgLight>("TestLight");
+ std::shared_ptr<OsgGroup> group = std::make_shared<OsgGroup>("TestGroup");
+
+ light->setGroup(group);
+
+ // We are using the indices directly from the enum, in a white box way, there are not really
+ // any good ways to do this better besides digging for the uniform by name ...
+ // make sure all the values are different from each other so not to get false positives
+ osg::Vec4f osgColor;
+ Vector4d diffuse(1.0, 2.0, 3.0, 4.0);
+ light->setDiffuseColor(diffuse);
+ EXPECT_TRUE(diffuse.isApprox(light->getDiffuseColor()));
+ getUniform(light, 1)->get(osgColor);
+ EXPECT_TRUE(diffuse.isApprox(fromOsg(osgColor).cast<double>()));
+
+ Vector4d specular(2.0, 3.0, 4.0, 5.0);
+ light->setSpecularColor(specular);
+ EXPECT_TRUE(specular.isApprox(light->getSpecularColor()));
+ getUniform(light, 2)->get(osgColor);
+ EXPECT_TRUE(specular.isApprox(fromOsg(osgColor).cast<double>()));
+}
+
+TEST_F(OsgLightTests, AttenuationAccessorTests)
+{
+ std::shared_ptr<OsgLight> light = std::make_shared<OsgLight>("TestLight");
+ std::shared_ptr<OsgGroup> group = std::make_shared<OsgGroup>("TestGroup");
+
+ light->setGroup(group);
+ float osgValue;
+ double constantAttenuation = 3.0;
+ light->setConstantAttenuation(constantAttenuation);
+ EXPECT_NEAR(constantAttenuation, light->getConstantAttenuation(), epsilon);
+ getUniform(light, 3)->get(osgValue);
+ EXPECT_NEAR(constantAttenuation, static_cast<double>(osgValue), epsilon);
+
+ double linearAttenuation = 4.0;
+ light->setLinearAttenuation(linearAttenuation);
+ EXPECT_NEAR(linearAttenuation , light->getLinearAttenuation(), epsilon);
+ getUniform(light, 4)->get(osgValue);
+ EXPECT_NEAR(linearAttenuation, static_cast<double>(osgValue), epsilon);
+
+ double quadraticAttenuation = 5.0;
+ light->setQuadraticAttenuation(quadraticAttenuation);
+ EXPECT_NEAR(quadraticAttenuation, light->getQuadraticAttenuation(), epsilon);
+ getUniform(light, 5)->get(osgValue);
+ EXPECT_NEAR(quadraticAttenuation, static_cast<double>(osgValue), epsilon);
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgLogTests.cpp b/SurgSim/Graphics/UnitTests/OsgLogTests.cpp
new file mode 100644
index 0000000..58ae829
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgLogTests.cpp
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <iostream>
+
+#include "SurgSim/Graphics/OsgLog.h"
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+TEST(OsgLogTests, MessageTest)
+{
+ osg::NotifyHandler* pOsgLog = new MockOsgLog;
+ osg::setNotifyHandler(pOsgLog);
+
+ EXPECT_EQ("", dynamic_cast<MockOsgLog*>(pOsgLog)->getMessage());
+
+ osg::notify(osg::ALWAYS) << "osg::ALWAYS Test" << std::endl;
+ EXPECT_EQ("CRITICAL osg::ALWAYS Test\n", dynamic_cast<MockOsgLog*>(pOsgLog)->getMessage());
+
+ osg::notify(osg::FATAL) << "osg::FATAL Test" << std::endl;
+ EXPECT_EQ("CRITICAL osg::FATAL Test\n", dynamic_cast<MockOsgLog*>(pOsgLog)->getMessage());
+
+ osg::notify(osg::WARN) << "osg::WARN Test" << std::endl;
+ EXPECT_EQ("WARNING osg::WARN Test\n", dynamic_cast<MockOsgLog*>(pOsgLog)->getMessage());
+
+ osg::notify(osg::NOTICE) << "osg::NOTICE Test" << std::endl;
+ EXPECT_EQ("INFO osg::NOTICE Test\n", dynamic_cast<MockOsgLog*>(pOsgLog)->getMessage());
+
+ osg::notify(osg::INFO) << "osg::INFO Test" << std::endl;
+ EXPECT_EQ("INFO osg::INFO Test\n", dynamic_cast<MockOsgLog*>(pOsgLog)->getMessage());
+
+ osg::notify(osg::DEBUG_INFO) << "osg::DEBUG_INFO Test" << std::endl;
+ EXPECT_EQ("DEBUG osg::DEBUG_INFO Test\n", dynamic_cast<MockOsgLog*>(pOsgLog)->getMessage());
+
+ osg::notify(osg::DEBUG_FP) << "osg::DEBUG_FP Test" << std::endl;
+ EXPECT_EQ("DEBUG osg::DEBUG_FP Test\n", dynamic_cast<MockOsgLog*>(pOsgLog)->getMessage());
+
+ osg::notify(osg::DEBUG_FP) << "osg::DEBUG_FP Test" << std::endl;
+ EXPECT_EQ("DEBUG osg::DEBUG_FP Test\n", dynamic_cast<MockOsgLog*>(pOsgLog)->getMessage());
+}
diff --git a/SurgSim/Graphics/UnitTests/OsgManagerTests.cpp b/SurgSim/Graphics/UnitTests/OsgManagerTests.cpp
new file mode 100644
index 0000000..3639f96
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgManagerTests.cpp
@@ -0,0 +1,266 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgManager class.
+
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include "SurgSim/Framework/Runtime.h"
+
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <algorithm>
+#include <random>
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::ComponentManager;
+using SurgSim::Framework::Representation;
+using SurgSim::Framework::Component;
+
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+
+class OsgManagerTest : public ::testing::Test
+{
+public:
+ virtual void SetUp()
+ {
+ runtime = std::make_shared<Runtime>();
+ graphicsManager = std::make_shared<OsgManager>();
+
+ runtime->addManager(graphicsManager);
+ }
+
+ virtual void TearDown()
+ {
+ runtime->stop();
+ }
+
+
+ bool testDoAddComponent(const std::shared_ptr<Component>& component)
+ {
+ return graphicsManager->executeAdditions(component);
+ }
+
+ bool testDoRemoveComponent(const std::shared_ptr<Component>& component)
+ {
+ return graphicsManager->executeRemovals(component);
+ }
+
+ void doProcessComponents()
+ {
+ graphicsManager->processComponents();
+ }
+
+ std::shared_ptr<Runtime> runtime;
+ std::shared_ptr<OsgManager> graphicsManager;
+};
+
+TEST_F(OsgManagerTest, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<OsgManager> manager = std::make_shared<OsgManager>();});
+}
+
+bool hasView(osgViewer::CompositeViewer* compositeViewer, osg::View* view)
+{
+ bool foundView = false;
+ for (unsigned int i = 0; i < compositeViewer->getNumViews(); ++i)
+ {
+ if (compositeViewer->getView(i) == view)
+ {
+ foundView = true;
+ }
+ }
+ return foundView;
+}
+
+TEST_F(OsgManagerTest, AddRemoveTest)
+{
+ osgViewer::CompositeViewer* compositeViewer = graphicsManager->getOsgCompositeViewer();
+ /// Perform add and remove from a pointer to a ComponentManager to check that the intended polymorphism is working.
+ std::shared_ptr<ComponentManager> componentManager = graphicsManager;
+
+ std::shared_ptr<OsgRepresentation> representation1 =
+ std::make_shared<MockOsgRepresentation>("test representation 1");
+ std::shared_ptr<OsgRepresentation> representation2 =
+ std::make_shared<MockOsgRepresentation>("test representation 2");
+ std::shared_ptr<OsgGroup> group1 = std::make_shared<OsgGroup>("test group 1");
+ std::shared_ptr<OsgGroup> group2 = std::make_shared<OsgGroup>("test group 2");
+
+ std::shared_ptr<OsgCamera> camera = std::make_shared<OsgCamera>("test camera");
+
+ std::shared_ptr<OsgView> view1 = std::make_shared<OsgView>("test view 1");
+ view1->setCamera(camera);
+ std::shared_ptr<OsgView> view2 = std::make_shared<OsgView>("test view 2");
+ view2->setCamera(camera);
+
+ std::shared_ptr<MockRepresentation> nonOsgRepresentation
+ = std::make_shared<MockRepresentation>("non-osg representation");
+ std::shared_ptr<MockGroup> nonOsgGroup = std::make_shared<MockGroup>("non-osg group");
+ std::shared_ptr<MockView> nonOsgView = std::make_shared<MockView>("non-osg view");
+ using SurgSim::Framework::Representation;
+ std::shared_ptr<Representation> nonGraphicsComponent = std::make_shared<NonGraphicsRepresentation>(
+ "non-graphics component");
+
+ EXPECT_EQ(0u, graphicsManager->getRepresentations().size());
+ EXPECT_EQ(0u, graphicsManager->getGroups().size());
+ EXPECT_EQ(0u, graphicsManager->getViews().size());
+
+ /// Add an representation
+ EXPECT_TRUE(componentManager->enqueueAddComponent(representation1));
+ doProcessComponents();
+ EXPECT_EQ(1u, graphicsManager->getRepresentations().size());
+ EXPECT_EQ(1u, graphicsManager->getGroups().size());
+ EXPECT_NE(graphicsManager->getRepresentations().end(), std::find(graphicsManager->getRepresentations().begin(),
+ graphicsManager->getRepresentations().end(), representation1));
+
+ /// Add a view
+ EXPECT_TRUE(componentManager->enqueueAddComponent(view1));
+ doProcessComponents();
+ EXPECT_EQ(1u, graphicsManager->getViews().size());
+ EXPECT_NE(graphicsManager->getViews().end(), std::find(graphicsManager->getViews().begin(),
+ graphicsManager->getViews().end(), view1));
+ EXPECT_TRUE(hasView(compositeViewer, view1->getOsgView()));
+
+ /// Add another view
+ EXPECT_TRUE(componentManager->enqueueAddComponent(view2));
+ doProcessComponents();
+ EXPECT_EQ(2u, graphicsManager->getViews().size());
+ EXPECT_NE(graphicsManager->getViews().end(), std::find(graphicsManager->getViews().begin(),
+ graphicsManager->getViews().end(), view2));
+
+ /// Add another representation
+ EXPECT_TRUE(componentManager->enqueueAddComponent(representation2));
+ doProcessComponents();
+ EXPECT_EQ(2u, graphicsManager->getRepresentations().size());
+ EXPECT_NE(graphicsManager->getRepresentations().end(), std::find(graphicsManager->getRepresentations().begin(),
+ graphicsManager->getRepresentations().end(), representation2));
+
+
+ /// Try to add a duplicate representation
+ EXPECT_TRUE(componentManager->enqueueAddComponent(representation1));
+ doProcessComponents();
+ EXPECT_EQ(2u, graphicsManager->getRepresentations().size());
+
+ /// Try to add a duplicate view
+ EXPECT_TRUE(componentManager->enqueueAddComponent(view1));
+ doProcessComponents();
+ EXPECT_EQ(2u, graphicsManager->getViews().size());
+
+ /// Try to add an representation that is not a subclass of OsgRepresentation
+ EXPECT_TRUE(componentManager->enqueueAddComponent(nonOsgRepresentation)) <<
+ "Adding an Representation that is not a subclass of OsgRepresentation should fail and return false";
+ doProcessComponents();
+ EXPECT_EQ(2u, graphicsManager->getRepresentations().size());
+
+ /// Try to add a view that is not a subclass of OsgView
+ EXPECT_TRUE(componentManager->enqueueAddComponent(nonOsgView)) <<
+ "Adding a View that is not a subclass of OsgView should fail and return false";
+ doProcessComponents();
+ EXPECT_EQ(2u, graphicsManager->getViews().size());
+
+ /// Try to add a component that is not graphics-related
+ EXPECT_TRUE(componentManager->enqueueAddComponent(nonGraphicsComponent)) <<
+ "Adding a component that this manager is not concerned with should return true";
+
+ /// Remove a view
+ EXPECT_TRUE(componentManager->enqueueRemoveComponent(view2));
+ doProcessComponents();
+ EXPECT_EQ(graphicsManager->getViews().end(), std::find(graphicsManager->getViews().begin(),
+ graphicsManager->getViews().end(), view2));
+
+ /// Remove an representation
+ EXPECT_TRUE(componentManager->enqueueRemoveComponent(representation1));
+ doProcessComponents();
+ EXPECT_EQ(graphicsManager->getRepresentations().end(), std::find(graphicsManager->getRepresentations().begin(),
+ graphicsManager->getRepresentations().end(), representation1));
+
+ /// Try to remove an representation that is not in the manager
+ EXPECT_TRUE(componentManager->enqueueRemoveComponent(representation1));
+ doProcessComponents();
+ EXPECT_EQ(graphicsManager->getRepresentations().end(), std::find(graphicsManager->getRepresentations().begin(),
+ graphicsManager->getRepresentations().end(), representation1));
+
+ /// Try to remove a view that is not in the manager
+ EXPECT_TRUE(componentManager->enqueueRemoveComponent(view2));
+ doProcessComponents();
+ EXPECT_EQ(graphicsManager->getViews().end(), std::find(graphicsManager->getViews().begin(),
+ graphicsManager->getViews().end(), view2));
+
+
+ /// Try to remove a component that is not graphics-related
+ EXPECT_TRUE(componentManager->enqueueRemoveComponent(nonGraphicsComponent)) <<
+ "Removing a component that this manager is not concerned with should return true";
+}
+
+TEST_F(OsgManagerTest, LazyGroupsTest)
+{
+ std::shared_ptr<OsgRepresentation> representation1 =
+ std::make_shared<MockOsgRepresentation>("TestRepresentation_1");
+ std::shared_ptr<OsgRepresentation> representation2 =
+ std::make_shared<MockOsgRepresentation>("TestRepresentation_2");
+ std::shared_ptr<OsgRepresentation> representation3 =
+ std::make_shared<MockOsgRepresentation>("TestRepresentation_3");
+
+ representation1->addGroupReference("TestGroup_1");
+ representation1->addGroupReference("TestGroup_2");
+ representation1->addGroupReference("TestGroup_3");
+ representation2->addGroupReference("TestGroup_2");
+ representation2->addGroupReference("TestGroup_3");
+ representation3->addGroupReference("TestGroup_3");
+
+ graphicsManager->enqueueAddComponent(representation1);
+ graphicsManager->enqueueAddComponent(representation2);
+ doProcessComponents();
+
+ auto defaultGroup = graphicsManager->getGroups().at(SurgSim::Graphics::Representation::DefaultGroupName);
+ auto group1 = graphicsManager->getGroups().at("TestGroup_1");
+ auto group2 = graphicsManager->getGroups().at("TestGroup_2");
+
+
+ EXPECT_EQ(2U, defaultGroup->getMembers().size());
+ EXPECT_EQ(1U, group1->getMembers().size());
+ EXPECT_EQ(2U, group2->getMembers().size());
+
+ graphicsManager->enqueueAddComponent(representation3);
+ doProcessComponents();
+
+ auto group3 = graphicsManager->getGroups().at("TestGroup_3");
+
+ EXPECT_EQ(3U, defaultGroup->getMembers().size());
+ EXPECT_EQ(1U, group1->getMembers().size());
+ EXPECT_EQ(2U, group2->getMembers().size());
+ EXPECT_EQ(3U, group3->getMembers().size());
+
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
+
diff --git a/SurgSim/Graphics/UnitTests/OsgMaterialTests.cpp b/SurgSim/Graphics/UnitTests/OsgMaterialTests.cpp
new file mode 100644
index 0000000..e2081ca
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgMaterialTests.cpp
@@ -0,0 +1,240 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgMaterial class.
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+
+#include <gtest/gtest.h>
+#include <gmock/gmock.h>
+
+using SurgSim::Math::Vector2f;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Uniform that does not subclass OsgUniformBase
+class MockUniform : public UniformBase
+{
+public:
+ /// Constructor
+ MockUniform() : UniformBase()
+ {
+ }
+};
+
+class MockShader : public Shader
+{
+public:
+ MOCK_CONST_METHOD0(hasGeometryShader, bool());
+ MOCK_CONST_METHOD0(hasVertexShader, bool());
+ MOCK_CONST_METHOD0(hasFragmentShader, bool());
+
+ MOCK_METHOD1(setGeometryShaderSource, void(const std::string&));
+ MOCK_METHOD1(setVertexShaderSource, void(const std::string&));
+ MOCK_METHOD1(setFragmentShaderSource, void(const std::string&));
+
+ MOCK_METHOD1(loadGeometryShaderSource, bool(const std::string&));
+ MOCK_METHOD1(loadVertexShaderSource, bool(const std::string&));
+ MOCK_METHOD1(loadFragmentShaderSource, bool(const std::string&));
+
+ MOCK_CONST_METHOD1(getGeometryShaderSource, bool(std::string*));
+ MOCK_CONST_METHOD1(getVertexShaderSource, bool(std::string*));
+ MOCK_CONST_METHOD1(getFragmentShaderSource, bool(std::string*));
+
+ MOCK_METHOD0(clearGeometryShader, void());
+ MOCK_METHOD0(clearVertexShader, void());
+ MOCK_METHOD0(clearFragmentShader, void());
+
+ MOCK_CONST_METHOD0(isGlobalScope, bool());
+ MOCK_METHOD1(setGlobalScope, void(bool)); //NOLINT
+};
+
+
+TEST(OsgMaterialTests, InitTest)
+{
+ auto material = std::make_shared<OsgMaterial>("material");
+
+ EXPECT_EQ(0u, material->getNumUniforms());
+ EXPECT_EQ(nullptr, material->getShader());
+
+ EXPECT_NE(nullptr, material->getOsgStateSet());
+}
+
+TEST(OsgMaterialTests, AddAndRemoveUniformsTest)
+{
+ std::shared_ptr<OsgMaterial> osgMaterial = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<Material> material = osgMaterial;
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ material->initialize(runtime);
+
+ EXPECT_EQ(0u, material->getNumUniforms());
+
+ std::shared_ptr<OsgUniform<float>> osgUniform1 = std::make_shared<OsgUniform<float>>("float uniform");
+ std::shared_ptr<Uniform<float>> uniform1 = osgUniform1;
+ std::shared_ptr<OsgUniform<Vector2f>> osgUniform2 = std::make_shared<OsgUniform<Vector2f>>("Vector2f uniform");
+ std::shared_ptr<Uniform<Vector2f>> uniform2 = osgUniform2;
+
+ const osg::StateSet::UniformList& uniforms = osgMaterial->getOsgStateSet()->getUniformList();
+
+ // Add a uniform to the material
+ EXPECT_TRUE(material->addUniform(uniform1));
+ EXPECT_EQ(1u, material->getNumUniforms());
+ EXPECT_EQ(uniform1, material->getUniform(0));
+
+ EXPECT_EQ(1u, uniforms.size());
+ EXPECT_EQ(osgUniform1->getOsgUniform(), uniforms.at("float uniform").first);
+
+ /// Add another uniform to the material
+ EXPECT_TRUE(material->addUniform(uniform2));
+ EXPECT_EQ(2u, material->getNumUniforms());
+ EXPECT_EQ(uniform2, material->getUniform(1));
+
+ EXPECT_EQ(2u, uniforms.size());
+ EXPECT_EQ(osgUniform1->getOsgUniform(), uniforms.at("float uniform").first);
+ EXPECT_EQ(osgUniform2->getOsgUniform(), uniforms.at("Vector2f uniform").first);
+
+ /// Remove the first uniform from the material
+ EXPECT_TRUE(material->removeUniform(uniform1));
+ EXPECT_EQ(1u, material->getNumUniforms());
+ EXPECT_EQ(uniform2, material->getUniform(0));
+
+ EXPECT_EQ(1u, uniforms.size());
+ EXPECT_EQ(osgUniform2->getOsgUniform(), uniforms.at("Vector2f uniform").first);
+
+ /// Try removing the same uniform again
+ EXPECT_FALSE(material->removeUniform(uniform1));
+ EXPECT_EQ(1u, material->getNumUniforms());
+
+ /// Try adding a non-OSG Uniform
+ std::shared_ptr<MockUniform> nonOsgUniform = std::make_shared<MockUniform>();
+ EXPECT_FALSE(material->addUniform(nonOsgUniform)) <<
+ "Should not be able to add a uniform that is not a subclass of OsgUniformBase!";
+ EXPECT_EQ(1u, material->getNumUniforms());
+
+ /// Try removing a non-OSG Uniform
+ EXPECT_FALSE(material->removeUniform(nonOsgUniform)) <<
+ "Should not be able to remove a uniform that is not a subclass of OsgUniformBase!";
+ EXPECT_EQ(1u, material->getNumUniforms());
+}
+
+TEST(OsgMaterialTests, SetAndClearShaderTest)
+{
+ std::shared_ptr<OsgMaterial> osgMaterial = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<Material> material = osgMaterial;
+
+ EXPECT_EQ(nullptr, material->getShader());
+
+ std::shared_ptr<OsgShader> osgShader = std::make_shared<OsgShader>();
+ std::shared_ptr<Shader> shader = osgShader;
+
+ const osg::StateSet::AttributeList& attributes = osgMaterial->getOsgStateSet()->getAttributeList();
+
+ // Set the material's shader
+ EXPECT_TRUE(material->setShader(shader));
+ EXPECT_EQ(shader, material->getShader());
+
+ EXPECT_EQ(1u, attributes.size());
+ EXPECT_EQ(osgShader->getOsgProgram(), attributes.at(osg::StateAttribute::TypeMemberPair(
+ osg::StateAttribute::PROGRAM, 0)).first) <<
+ "Shader should have been added to the material's state attributes!";
+
+ /// Try setting a non-OSG Shader
+ std::shared_ptr<MockShader> nonOsgShader = std::make_shared<MockShader>();
+ EXPECT_FALSE(material->setShader(nonOsgShader)) <<
+ "Should not be able to set a shader that is not a subclass of OsgShader!";
+ EXPECT_NE(nonOsgShader, material->getShader());
+
+ /// Clear the shader
+ material->clearShader();
+ EXPECT_EQ(nullptr, material->getShader());
+ EXPECT_EQ(0u, attributes.size()) << "Shader should have been removed from the material's state attributes!";
+}
+
+TEST(OsgMaterialTests, NamedAccessTest)
+{
+ std::shared_ptr<OsgMaterial> osgMaterial = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<Material> material = osgMaterial;
+
+ std::string uniform1Name = "float uniform";
+ std::shared_ptr<OsgUniform<float>> osgUniform1 = std::make_shared<OsgUniform<float>>(uniform1Name);
+ std::shared_ptr<Uniform<float>> uniform1 = osgUniform1;
+
+ std::string uniform2Name = "Vector2f uniform";
+ std::shared_ptr<OsgUniform<Vector2f>> osgUniform2 = std::make_shared<OsgUniform<Vector2f>>(uniform2Name);
+ std::shared_ptr<Uniform<Vector2f>> uniform2 = osgUniform2;
+
+ material->addUniform(uniform1);
+ material->addUniform(uniform2);
+
+ EXPECT_TRUE(material->hasUniform(uniform1Name));
+ EXPECT_EQ(uniform1.get(), material->getUniform(uniform1Name).get());
+
+ EXPECT_TRUE(material->hasUniform(uniform2Name));
+ EXPECT_EQ(uniform2.get(), material->getUniform(uniform2Name).get());
+
+ EXPECT_FALSE(material->hasUniform("xxx"));
+ EXPECT_EQ(nullptr, material->getUniform("xxx"));
+
+ EXPECT_TRUE(material->removeUniform(uniform1Name));
+ EXPECT_FALSE(material->hasUniform(uniform1Name));
+}
+
+TEST(OsgMaterialTests, AccessibleUniformTest)
+{
+ auto material = std::make_shared<OsgMaterial>("material");
+
+ std::string uniform1Name = "ossFloatUniform";
+ auto uniform1 = std::make_shared<OsgUniform<float>>(uniform1Name);
+
+
+ std::string uniform2Name = "ossVector2fUniform";
+ auto uniform2 = std::make_shared<OsgUniform<Vector2f>>(uniform2Name);
+
+ material->addUniform(uniform1);
+ material->addUniform(uniform2);
+
+ material->setValue(uniform1Name, 2.0f);
+
+ EXPECT_FLOAT_EQ(2.0, uniform1->get());
+
+ uniform1->set(4.0f);
+ EXPECT_FLOAT_EQ(4.0f, material->getValue<float>(uniform1Name));
+
+ Vector2f vector1(1.0f, 2.0f);
+ Vector2f vector2(3.0f, 4.0f);
+
+ material->setValue(uniform2Name, vector1);
+
+ EXPECT_TRUE(vector1.isApprox(uniform2->get()));
+
+ uniform2->set(vector2);
+ EXPECT_TRUE(vector2.isApprox(material->getValue<Vector2f>(uniform2Name)));
+
+ material->removeUniform(uniform1);
+
+ EXPECT_ANY_THROW(material->setValue(uniform1Name, 1.0f));
+
+}
+
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgMatrixConversionsTests.cpp b/SurgSim/Graphics/UnitTests/OsgMatrixConversionsTests.cpp
new file mode 100644
index 0000000..40f370b
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgMatrixConversionsTests.cpp
@@ -0,0 +1,114 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 conversions to and from OSG matrix types
+
+#include "SurgSim/Graphics/OsgMatrixConversions.h"
+#include "SurgSim/Graphics/OsgVectorConversions.h"
+
+#include <gtest/gtest.h>
+
+using SurgSim::Graphics::fromOsg;
+using SurgSim::Graphics::toOsg;
+using SurgSim::Math::Matrix22f;
+using SurgSim::Math::Matrix22d;
+using SurgSim::Math::Matrix33f;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Matrix44f;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Math::Vector2f;
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3f;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4f;
+using SurgSim::Math::Vector4d;
+
+
+TEST(OsgMatrixConversionsTests, Matrix22fTest)
+{
+ Matrix22f matrix = Matrix22f::Random();
+ osg::Matrix2 osgMatrix = toOsg(matrix);
+ EXPECT_TRUE(matrix.isApprox(fromOsg(osgMatrix)));
+}
+
+TEST(OsgMatrixConversionsTests, Matrix22dTest)
+{
+ Matrix22d matrix = Matrix22d::Random();
+ osg::Matrix2d osgMatrix = toOsg(matrix);
+ EXPECT_TRUE(matrix.isApprox(fromOsg(osgMatrix)));
+}
+
+TEST(OsgMatrixConversionsTests, Matrix33fTest)
+{
+ Matrix33f matrix = Matrix33f::Random();
+ osg::Matrix3 osgMatrix = toOsg(matrix);
+ EXPECT_TRUE(matrix.isApprox(fromOsg(osgMatrix)));
+}
+
+TEST(OsgMatrixConversionsTests, Matrix33dTest)
+{
+ Matrix33d matrix = Matrix33d::Random();
+ osg::Matrix3d osgMatrix = toOsg(matrix);
+ EXPECT_TRUE(matrix.isApprox(fromOsg(osgMatrix)));
+}
+
+TEST(OsgMatrixConversionsTests, Matrix44fConversionTest)
+{
+ Matrix44f matrix = Matrix44f::Random();
+ osg::Matrixf osgMatrix = toOsg(matrix);
+ EXPECT_TRUE(matrix.isApprox(fromOsg(osgMatrix)));
+}
+
+TEST(OsgMatrixConversionsTests, Matrix44fMultiplicationTest)
+{
+ Matrix44f matrix = Matrix44f::Random();
+ Vector4f vector = Vector4f::Random();
+
+ osg::Matrixf osgMatrix = toOsg(matrix);
+ osg::Vec4f osgVector = toOsg(vector);
+
+ /// Multiply with Eigen
+ Vector4f result = matrix * vector;
+ /// Multiply with OSG
+ osg::Vec4f osgResult = osgVector * osgMatrix;
+
+ /// Compare the two results
+ EXPECT_TRUE(result.isApprox(fromOsg(osgResult)));
+}
+
+TEST(OsgMatrixConversionsTests, Matrix44dTest)
+{
+ Matrix44d matrix = Matrix44d::Random();
+ osg::Matrixd osgMatrix = toOsg(matrix);
+ EXPECT_TRUE(matrix.isApprox(fromOsg(osgMatrix)));
+}
+
+TEST(OsgMatrixConversionsTests, Matrix44dMultiplicationTest)
+{
+ Matrix44d matrix = Matrix44d::Random();
+ Vector4d vector = Vector4d::Random();
+
+ osg::Matrixd osgMatrix = toOsg(matrix);
+ osg::Vec4d osgVector = toOsg(vector);
+
+ /// Multiply with Eigen
+ Vector4d result = matrix * vector;
+ /// Multiply with OSG
+ osg::Vec4d osgResult = osgVector * osgMatrix;
+
+ /// Compare the two results
+ EXPECT_TRUE(result.isApprox(fromOsg(osgResult)));
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgMeshRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgMeshRepresentationTests.cpp
new file mode 100644
index 0000000..310059f
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgMeshRepresentationTests.cpp
@@ -0,0 +1,168 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <vector>
+
+#include <gtest/gtest.h>
+#include <osg/ref_ptr>
+#include <osg/Geometry>
+#include <osg/Array>
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Graphics/MeshPlyReaderDelegate.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/TestCube.h"
+
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+
+using SurgSim::Framework::Runtime;
+
+namespace
+{
+std::vector<Vector3d> cubeVertices;
+std::vector<size_t> cubeTriangles;
+std::vector<Vector4d> cubeColors;
+std::vector<Vector2d> cubeTextures;
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgMeshRepresentationTests, InitTest)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ std::shared_ptr<MeshRepresentation> meshRepresentation;
+ ASSERT_NO_THROW(meshRepresentation = std::make_shared<OsgMeshRepresentation>("TestMesh"));
+
+ SurgSim::Testing::Cube::makeCube(&cubeVertices, &cubeColors, &cubeTextures, &cubeTriangles);
+
+ ASSERT_NE(nullptr, meshRepresentation->getMesh());
+ EXPECT_EQ(OsgMeshRepresentation::UPDATE_OPTION_VERTICES, meshRepresentation->getUpdateOptions());
+};
+
+TEST(OsgMeshRepresentationTests, InitialisationTest)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ auto meshRepresentation = std::make_shared<OsgMeshRepresentation>("TestMesh");
+
+ std::shared_ptr<Mesh> mesh = meshRepresentation->getMesh();
+ mesh->initialize(cubeVertices, cubeColors, cubeTextures, cubeTriangles);
+
+ EXPECT_TRUE(meshRepresentation->initialize(runtime));
+ EXPECT_TRUE(meshRepresentation->wakeUp());
+
+ ASSERT_NO_THROW(meshRepresentation->update(0.1));
+
+ osg::ref_ptr<osg::Geometry> geometry = meshRepresentation->getOsgGeometry();
+ EXPECT_NE(nullptr, geometry);
+
+ osg::ref_ptr<osg::Array> array = geometry->getVertexArray();
+ ASSERT_NE(nullptr, array);
+ EXPECT_EQ(cubeVertices.size(), array->getNumElements());
+
+ array = geometry->getColorArray();
+ ASSERT_NE(nullptr, array);
+ EXPECT_EQ(cubeColors.size(), array->getNumElements());
+
+ array = geometry->getTexCoordArray(0);
+ ASSERT_NE(nullptr, array);
+ EXPECT_EQ(cubeTextures.size(), array->getNumElements());
+
+ osg::ref_ptr<osg::PrimitiveSet> primitiveSet = geometry->getPrimitiveSet(0);
+ ASSERT_NE(nullptr, primitiveSet);
+ EXPECT_EQ(cubeTriangles.size(), primitiveSet->getNumIndices());
+
+}
+
+TEST(OsgMeshRepresentationTests, FilenameTest)
+{
+ auto meshRepresentation = std::make_shared<OsgMeshRepresentation>("TestMesh");
+ std::string filename = "Geometry/arm_collision.ply";
+
+ meshRepresentation->setFilename(filename);
+ EXPECT_EQ(filename, meshRepresentation->getFilename());
+}
+
+TEST(OsgMeshRepresentationTests, SerializationTest)
+{
+ std::shared_ptr<SurgSim::Framework::Component> osgMesh = std::make_shared<OsgMeshRepresentation>("TestMesh");
+ std::string filename = "Geometry/arm_collision.ply";
+
+ osgMesh->setValue("Filename", filename);
+ osgMesh->setValue("UpdateOptions", 2);
+ osgMesh->setValue("DrawAsWireFrame", true);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*osgMesh));
+
+ EXPECT_EQ(1u, node.size());
+ YAML::Node data;
+ data = node["SurgSim::Graphics::OsgMeshRepresentation"];
+ EXPECT_EQ(9u, data.size());
+
+ std::shared_ptr<SurgSim::Graphics::OsgMeshRepresentation> newOsgMesh;
+ ASSERT_NO_THROW(newOsgMesh =
+ std::dynamic_pointer_cast<OsgMeshRepresentation>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_EQ("SurgSim::Graphics::OsgMeshRepresentation", newOsgMesh->getClassName());
+ EXPECT_EQ(filename, newOsgMesh->getValue<std::string>("Filename"));
+ EXPECT_EQ(2u, newOsgMesh->getValue<int>("UpdateOptions"));
+ EXPECT_TRUE(newOsgMesh->getValue<bool>("DrawAsWireFrame"));
+}
+
+TEST(OsgMeshRepresentationTests, MeshDelegateTest)
+{
+ SurgSim::Framework::ApplicationData data("config.txt");
+ SurgSim::DataStructures::PlyReader reader(data.findFile("OsgMeshRepresentationTests/Cube.ply"));
+ auto delegate = std::make_shared<SurgSim::Graphics::MeshPlyReaderDelegate>();
+
+ EXPECT_NO_THROW(EXPECT_TRUE(reader.parseWithDelegate(delegate)));
+
+ auto mesh = delegate->getMesh();
+ EXPECT_EQ(26u, mesh->getNumVertices());
+ EXPECT_EQ(12u, mesh->getNumTriangles());
+
+ // The first and last vertices from the file
+ Vector3d vertex0(1.0, 1.0, -1.0);
+ Vector3d vertex25(-1.0, -1.0, 1.0);
+ Vector2d texture0(0.00, 0.50);
+ Vector2d texture25(0.25, 0.75);
+
+
+ EXPECT_TRUE(vertex0.isApprox(mesh->getVertex(0).position));
+ EXPECT_TRUE(vertex25.isApprox(mesh->getVertex(25).position));
+
+ EXPECT_TRUE(texture0.isApprox(mesh->getVertex(0).data.texture.getValue()));
+ EXPECT_TRUE(texture25.isApprox(mesh->getVertex(25).data.texture.getValue()));
+
+ std::array<size_t, 3> triangle0 = {0, 1, 2};
+ std::array<size_t, 3> triangle11 = {10, 25, 11};
+
+ EXPECT_EQ(triangle0, mesh->getTriangle(0).verticesId);
+ EXPECT_EQ(triangle11, mesh->getTriangle(11).verticesId);
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgOctreeRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgOctreeRepresentationTests.cpp
new file mode 100644
index 0000000..b3f44c1
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgOctreeRepresentationTests.cpp
@@ -0,0 +1,123 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Unit Tests for the OsgOctreeRepresentation class.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/Graphics/OsgOctreeRepresentation.h"
+#include "SurgSim/Math/OctreeShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Framework::Runtime;
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::Graphics::OsgOctreeRepresentation;
+using SurgSim::Math::OctreeShape;
+using SurgSim::Math::Vector3d;
+
+TEST(OsgOctreeRepresentationTests, InitilizationTest)
+{
+ ASSERT_NO_THROW(std::make_shared<OsgOctreeRepresentation>("Test Octree"));
+};
+
+TEST(OsgOctreeRepresentationTests, GetSetUpdateTest)
+{
+ OctreeShape::NodeType::AxisAlignedBoundingBox boundingBox(Vector3d::Zero(), Vector3d::Ones() * 4.0);
+ auto octreeNode = std::make_shared<OctreeShape::NodeType>(boundingBox);
+ auto octreeShape = std::make_shared<SurgSim::Math::OctreeShape>(*octreeNode);
+
+ auto runtime = std::make_shared<Runtime>();
+ auto octreeRepresentation = std::make_shared<OsgOctreeRepresentation>("Test Octree");
+ octreeRepresentation->setOctreeShape(octreeShape);
+
+ EXPECT_TRUE(octreeRepresentation->initialize(runtime));
+ EXPECT_TRUE(octreeRepresentation->wakeUp());
+
+ // Set the octree after wake up will cause a assertion failure.
+ ASSERT_ANY_THROW( { octreeRepresentation->setOctreeShape(octreeShape); } );
+
+ ASSERT_NO_THROW(octreeRepresentation->update(0.1));
+}
+
+TEST(OsgOctreeRepresentationTests, SetNodeVisibilityTest)
+{
+ SurgSim::DataStructures::EmptyData emptyData;
+
+ OctreeShape::NodeType::AxisAlignedBoundingBox boundingBox(Vector3d::Zero(), Vector3d::Ones() * 4.0);
+ auto octreeNode = std::make_shared<OctreeShape::NodeType>(boundingBox);
+ octreeNode->addData(Vector3d(0.0, 0.0, 0.0), emptyData, 2);
+ octreeNode->addData(Vector3d(0.0, 0.0, 1.0), emptyData, 3);
+
+ auto octreeShape = std::make_shared<SurgSim::Math::OctreeShape>(*octreeNode);
+ auto octreeRepresentation = std::make_shared<OsgOctreeRepresentation>("TestOctree");
+
+ // Path to leaf node
+ SurgSim::DataStructures::OctreePath path;
+ path.push_back(0);
+ path.push_back(0);
+
+ // Set node visibility when no octree is held by OsgOctreeRepresentation will throw.
+ EXPECT_ANY_THROW(octreeRepresentation->setNodeVisible(path, true));
+
+ octreeRepresentation->setOctreeShape(octreeShape);
+ EXPECT_NO_THROW(octreeRepresentation->setNodeVisible(path, false));
+
+ // Path to internal node
+ SurgSim::DataStructures::OctreePath path2;
+ path2.push_back(0);
+ EXPECT_NO_THROW(octreeRepresentation->setNodeVisible(path2, true));
+
+ // Invalid path
+ SurgSim::DataStructures::OctreePath invalidPath;
+ invalidPath.push_back(4);
+ invalidPath.push_back(1);
+ EXPECT_ANY_THROW(octreeRepresentation->setNodeVisible(invalidPath, true));
+}
+
+TEST(OsgOctreeRepresentationTests, SerializationTest)
+{
+ Runtime runtime("config.txt");
+ std::shared_ptr<SurgSim::Math::Shape> octreeShape = std::make_shared<SurgSim::Math::OctreeShape>();
+ std::string filename = "OctreeShapeData/staple.vox";
+ std::static_pointer_cast<SurgSim::Math::OctreeShape>(octreeShape)->load(filename);
+
+ std::shared_ptr<SurgSim::Framework::Component> osgOctree = std::make_shared<OsgOctreeRepresentation>("TestOctree");
+ osgOctree->setValue("OctreeShape", octreeShape);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*osgOctree));
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Graphics::OsgOctreeRepresentation"];
+ EXPECT_EQ(8u, data.size());
+
+ std::shared_ptr<SurgSim::Graphics::OsgOctreeRepresentation> newOsgOctree;
+ ASSERT_NO_THROW(newOsgOctree = std::dynamic_pointer_cast<OsgOctreeRepresentation>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ EXPECT_EQ("SurgSim::Graphics::OsgOctreeRepresentation", newOsgOctree->getClassName());
+
+ auto newOctree = newOsgOctree->getOctreeShape();
+ auto oldOctree = std::static_pointer_cast<OctreeShape>(octreeShape)->getRootNode();
+ EXPECT_TRUE(newOctree->getRootNode()->getBoundingBox().isApprox(oldOctree->getBoundingBox()));
+ EXPECT_TRUE(newOsgOctree->getOctreeShape()->isValid());
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_OCTREE, newOctree->getType());
+ EXPECT_THROW(newOctree->getVolume(), SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE((newOctree->getCenter() - Vector3d::Zero()).isZero());
+ EXPECT_THROW(newOctree->getSecondMomentOfVolume(), SurgSim::Framework::AssertionFailure);
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgPlaneRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgPlaneRepresentationTests.cpp
new file mode 100644
index 0000000..e8fd329
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgPlaneRepresentationTests.cpp
@@ -0,0 +1,194 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgPlaneRepresentation class.
+
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgPlaneRepresentation.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+#include <gtest/gtest.h>
+
+#include <osg/Geode>
+
+#include <random>
+
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST(OsgPlaneRepresentationTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<Representation> representation =
+ std::make_shared<OsgPlaneRepresentation>("test name");
+ });
+
+ std::shared_ptr<Representation> representation = std::make_shared<OsgPlaneRepresentation>("test name");
+ EXPECT_EQ("test name", representation->getName());
+}
+
+TEST(OsgPlaneRepresentationTests, AccessibleTest)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component;
+ ASSERT_NO_THROW(component = SurgSim::Framework::Component::getFactory().create(
+ "SurgSim::Graphics::OsgPlaneRepresentation",
+ "capsule"));
+
+ EXPECT_EQ("SurgSim::Graphics::OsgPlaneRepresentation", component->getClassName());
+
+ YAML::Node node(YAML::convert<SurgSim::Framework::Component>::encode(*component));
+
+ auto decoded = std::dynamic_pointer_cast<SurgSim::Graphics::OsgPlaneRepresentation>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>());
+
+ EXPECT_NE(nullptr, decoded);
+}
+
+TEST(OsgPlaneRepresentationTests, OsgNodeTest)
+{
+ std::shared_ptr<OsgRepresentation> representation = std::make_shared<OsgPlaneRepresentation>("test name");
+
+ ASSERT_NE(nullptr, representation->getOsgNode());
+
+ osg::Switch* switchNode = dynamic_cast<osg::Switch*>(representation->getOsgNode().get());
+ ASSERT_NE(nullptr, switchNode) << "Could not get OSG switch node!";
+
+ ASSERT_EQ(1u, switchNode->getNumChildren()) << "OSG switch node should have 1 child, the transform node!";
+
+ osg::PositionAttitudeTransform* transformNode =
+ dynamic_cast<osg::PositionAttitudeTransform*>(switchNode->getChild(0));
+ ASSERT_NE(nullptr, transformNode) << "Could not get OSG transform node!";
+
+ ASSERT_EQ(1u, transformNode->getNumChildren()) << "OSG transform node should have 1 child, the geode!";
+
+ osg::Geode* geode = dynamic_cast<osg::Geode*>(transformNode->getChild(0));
+ ASSERT_NE(nullptr, geode) << "Could not get OSG geode!";
+}
+
+TEST(OsgPlaneRepresentationTests, VisibilityTest)
+{
+ std::shared_ptr<OsgRepresentation> osgRepresentation = std::make_shared<OsgPlaneRepresentation>("test name");
+ std::shared_ptr<Representation> representation = osgRepresentation;
+
+ osg::Switch* switchNode = dynamic_cast<osg::Switch*>(osgRepresentation->getOsgNode().get());
+ ASSERT_NE(nullptr, switchNode) << "Could not get OSG switch node!";
+ ASSERT_EQ(1u, switchNode->getNumChildren()) << "OSG switch node should have 1 child, the transform node!";
+
+ EXPECT_TRUE(representation->isVisible());
+ EXPECT_TRUE(switchNode->getChildValue(switchNode->getChild(0)));
+
+ representation->setVisible(false);
+ EXPECT_FALSE(representation->isVisible());
+ EXPECT_FALSE(switchNode->getChildValue(switchNode->getChild(0)));
+
+ representation->setVisible(true);
+ EXPECT_TRUE(representation->isVisible());
+ EXPECT_TRUE(switchNode->getChildValue(switchNode->getChild(0)));
+}
+
+TEST(OsgPlaneRepresentationTests, PoseTest)
+{
+ std::shared_ptr<Representation> representation = std::make_shared<MockOsgRepresentation>("test name");
+ std::shared_ptr<BasicSceneElement> element = std::make_shared<BasicSceneElement>("element");
+ element->addComponent(representation);
+ element->initialize();
+ representation->wakeUp();
+
+ {
+ SCOPED_TRACE("Check Initial Pose");
+ EXPECT_TRUE(representation->getLocalPose().isApprox(RigidTransform3d::Identity()));
+ EXPECT_TRUE(representation->getPose().isApprox(RigidTransform3d::Identity()));
+ }
+
+ RigidTransform3d localPose;
+ {
+ SCOPED_TRACE("Set Local Pose");
+ localPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ representation->setLocalPose(localPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(localPose));
+ }
+
+ RigidTransform3d elementPose;
+ {
+ SCOPED_TRACE("Set Element Pose");
+ elementPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ element->setPose(elementPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(elementPose * localPose));
+ }
+
+ {
+ SCOPED_TRACE("Change Local Pose");
+ localPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ representation->setLocalPose(localPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(elementPose * localPose));
+ }
+}
+
+TEST(OsgPlaneRepresentationTests, MaterialTest)
+{
+ std::shared_ptr<OsgRepresentation> osgRepresentation = std::make_shared<OsgPlaneRepresentation>("test name");
+ std::shared_ptr<Representation> representation = osgRepresentation;
+
+ std::shared_ptr<OsgMaterial> osgMaterial = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<Material> material = osgMaterial;
+ {
+ SCOPED_TRACE("Set material");
+ EXPECT_TRUE(representation->setMaterial(material));
+ EXPECT_EQ(material, representation->getMaterial());
+
+ osg::Switch* switchNode = dynamic_cast<osg::Switch*>(osgRepresentation->getOsgNode().get());
+ ASSERT_NE(nullptr, switchNode) << "Could not get OSG switch node!";
+ ASSERT_EQ(1u, switchNode->getNumChildren()) << "OSG switch node should have 1 child, the transform node!";
+ EXPECT_EQ(osgMaterial->getOsgStateSet(), switchNode->getChild(0)->getStateSet()) <<
+ "State set should be the material's state set!";
+ }
+
+ {
+ SCOPED_TRACE("Clear material");
+ representation->clearMaterial();
+ EXPECT_EQ(nullptr, representation->getMaterial());
+
+ osg::Switch* switchNode = dynamic_cast<osg::Switch*>(osgRepresentation->getOsgNode().get());
+ ASSERT_NE(nullptr, switchNode) << "Could not get OSG switch node!";
+ ASSERT_EQ(1u, switchNode->getNumChildren()) << "OSG switch node should have 1 child, the transform node!";
+ EXPECT_NE(osgMaterial->getOsgStateSet(), switchNode->getChild(0)->getStateSet()) <<
+ "State set should have been cleared!";
+ }
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgPlaneTests.cpp b/SurgSim/Graphics/UnitTests/OsgPlaneTests.cpp
new file mode 100644
index 0000000..1e1a49e
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgPlaneTests.cpp
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgPlane class.
+
+#include "SurgSim/Graphics/OsgPlane.h"
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST(OsgPlaneTests, InitTest)
+{
+ ASSERT_NO_THROW({OsgPlane plane;});
+
+ ASSERT_NO_THROW({OsgPlane plane(100.0, 200.0);});
+
+ OsgPlane plane;
+
+ osg::ref_ptr<osg::Node> node = plane.getNode();
+ EXPECT_NE(nullptr, node.get());
+
+ osg::ref_ptr<osg::Geode> geode = dynamic_cast<osg::Geode*>(node.get());
+ EXPECT_NE(nullptr, geode.get());
+
+ EXPECT_EQ(1u, geode->getNumDrawables());
+
+ osg::ref_ptr<osg::Drawable> drawable = geode->getDrawable(0);
+ EXPECT_NE(nullptr, drawable.get());
+
+ osg::ref_ptr<osg::Geometry> geometry = dynamic_cast<osg::Geometry*>(drawable.get());
+ EXPECT_NE(nullptr, geometry.get());
+
+ osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray());
+ ASSERT_EQ(4u, vertices->size());
+
+ osg::Vec3 u = vertices->at(1) - vertices->at(0);
+ osg::Vec3 v = vertices->at(2) - vertices->at(0);
+ osg::Vec3 normal = u ^ v;
+ normal.normalize();
+
+ EXPECT_EQ(0.0, vertices->at(0).y()) << "The plane should be in the XZ plane (Y = 0)!";
+
+ EXPECT_NEAR(1.0, normal * osg::Vec3(0.0, 1.0, 0.0), 1.0e-10) << "The plane normal should be +Y!";
+
+ EXPECT_NEAR(0.0, vertices->at(3) * normal, 1.0e-10) << "All vertices should be co-planar!";
+
+ osg::Vec3 sum(0.0, 0.0, 0.0);
+ for (size_t i = 0; i < vertices->size(); ++i)
+ {
+ sum += vertices->at(i);
+ }
+ sum /= vertices->size();
+
+ EXPECT_NEAR(0.0, sum.length(), 1e-10) << "The center of the plane should be (0, 0, 0)!";
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgPointCloudRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgPointCloudRepresentationTests.cpp
new file mode 100644
index 0000000..1cd0d7a
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgPointCloudRepresentationTests.cpp
@@ -0,0 +1,118 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::Graphics::PointCloudRepresentation;
+using SurgSim::Graphics::OsgPointCloudRepresentation;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+
+namespace
+{
+const double epsilon = 1e-5;
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgPointCloudRepresentationTests, InitTest)
+{
+ ASSERT_NO_THROW(OsgPointCloudRepresentation("TestPointCloud"));
+};
+
+TEST(OsgPointCloudRepresentationTests, PointSizeTest)
+{
+ auto pointCloud = std::make_shared<OsgPointCloudRepresentation>("TestPointCloud");
+
+ double pointSize = 1.234;
+ pointCloud->setPointSize(pointSize);
+ EXPECT_NEAR(pointSize, pointCloud->getPointSize(), epsilon);
+}
+
+TEST(OsgPointCloudRepresentationTests, ColorTest)
+{
+ auto pointCloud = std::make_shared<OsgPointCloudRepresentation>("TestPointCloud");
+
+ Vector4d color = Vector4d(1.0, 2.0, 3.0, 4.0);
+ pointCloud->setColor(color);
+ EXPECT_TRUE(color.isApprox(pointCloud->getColor()));
+}
+
+TEST(OsgPointCloudRepresentationTests, VertexTest)
+{
+ auto pointCloud = std::make_shared<OsgPointCloudRepresentation>("TestPointCloud");
+ auto vertices = pointCloud->getVertices();
+ EXPECT_EQ(0u, vertices->getNumVertices());
+
+ std::vector<Vector3d> vertexList;
+ vertexList.push_back(Vector3d(0.01, -0.01, 0.01));
+ vertexList.push_back(Vector3d(0.01, -0.01, 0.01));
+ vertexList.push_back(Vector3d(-0.01, -0.01, 0.01));
+ vertexList.push_back(Vector3d(-0.01, -0.01, -0.01));
+ vertexList.push_back(Vector3d(0.01, -0.01, -0.01));
+
+ for (auto it = std::begin(vertexList); it != std::end(vertexList); ++it)
+ {
+ vertices->addVertex(SurgSim::Graphics::PointCloud::VertexType(*it));
+ }
+
+ auto updatedVertices = pointCloud->getVertices();
+ ASSERT_EQ(vertexList.size(), updatedVertices->getNumVertices());
+ for (size_t i = 0; i < vertexList.size(); ++i)
+ {
+ EXPECT_TRUE(vertexList[i].isApprox(updatedVertices->getVertexPosition(i)));
+ }
+}
+
+TEST(OsgPointCloudRepresentationTests, SerializationTest)
+{
+ auto pointCloud = std::make_shared<OsgPointCloudRepresentation>("TestPointCloud");
+
+ double pointSize = 1.234;
+ Vector4d color = Vector4d(1.0, 2.0, 3.0, 4.0);
+
+ pointCloud->setValue("PointSize", pointSize);
+ pointCloud->setValue("Color", color);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*pointCloud));
+
+ EXPECT_EQ(1u, node.size());
+ YAML::Node data;
+ data = node["SurgSim::Graphics::OsgPointCloudRepresentation"];
+ EXPECT_EQ(9u, data.size());
+
+ std::shared_ptr<SurgSim::Graphics::OsgPointCloudRepresentation> newOsgPointCloud;
+ ASSERT_NO_THROW(newOsgPointCloud = std::dynamic_pointer_cast<OsgPointCloudRepresentation>
+ (node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_EQ("SurgSim::Graphics::OsgPointCloudRepresentation", newOsgPointCloud->getClassName());
+ EXPECT_NEAR(pointSize, newOsgPointCloud->getValue<double>("PointSize"), epsilon);
+ EXPECT_TRUE(color.isApprox(newOsgPointCloud->getValue<Vector4d>("Color")));
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgQuaternionConversionsTests.cpp b/SurgSim/Graphics/UnitTests/OsgQuaternionConversionsTests.cpp
new file mode 100644
index 0000000..59be7a0
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgQuaternionConversionsTests.cpp
@@ -0,0 +1,45 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 conversions to and from OSG quaternion types
+
+#include "SurgSim/Graphics/OsgQuaternionConversions.h"
+
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+using SurgSim::Graphics::fromOsg;
+using SurgSim::Graphics::toOsg;
+using SurgSim::Math::Quaternionf;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector4f;
+using SurgSim::Math::Vector4d;
+
+
+TEST(OsgQuaternionConversionsTests, QuaternionfTest)
+{
+ Quaternionf quaternion = Quaternionf(Vector4f::Random());
+ osg::Quat osgQuaternion = toOsg(quaternion);
+ EXPECT_TRUE(quaternion.isApprox(fromOsg<float>(osgQuaternion)));
+}
+
+TEST(OsgQuaternionConversionsTests, QuaterniondTest)
+{
+ Quaterniond quaternion = Quaterniond(Vector4d::Random());
+ osg::Quat osgQuaternion = toOsg(quaternion);
+ EXPECT_TRUE(quaternion.isApprox(fromOsg<double>(osgQuaternion)));
+}
diff --git a/SurgSim/Graphics/UnitTests/OsgRenderTargetTests.cpp b/SurgSim/Graphics/UnitTests/OsgRenderTargetTests.cpp
new file mode 100644
index 0000000..465e1ed
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgRenderTargetTests.cpp
@@ -0,0 +1,86 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+
+TEST(OsgRenderTargetTests, DefaultConstructorTest)
+{
+ EXPECT_NO_THROW({OsgRenderTarget2d target;});
+ EXPECT_NO_THROW({OsgRenderTargetRectangle target;});
+}
+
+TEST(OsgRenderTargetTests, PotDefaultConstructorTest)
+{
+ EXPECT_NO_THROW({OsgRenderTarget2d target;});
+ OsgRenderTarget2d target;
+
+ int width, height;
+ target.getSize(&width, &height);
+ EXPECT_EQ(0.0, width);
+ EXPECT_EQ(0.0, height);
+
+ EXPECT_FALSE(target.doesUseDepthTarget());
+ EXPECT_TRUE(nullptr == target.getDepthTarget());
+
+ ASSERT_EQ(0, target.getColorTargetCount());
+ for(int i=0; i<16; ++i)
+ {
+ EXPECT_TRUE(nullptr == target.getColorTarget(i)) << "color target should be nullptr at index:" << i;
+ }
+}
+
+
+TEST(OsgRenderTargetTests, PotSpecificConstructorTest)
+{
+ ASSERT_NO_THROW({OsgRenderTarget2d target(256,256,1.0,16,true);});
+
+ OsgRenderTarget2d target(256,128,1.0,16,true);
+
+ int width, height;
+ target.getSize(&width, &height);
+ EXPECT_EQ(256, width);
+ EXPECT_EQ(128, height);
+
+ EXPECT_TRUE(target.doesUseDepthTarget());
+ EXPECT_FALSE(nullptr == target.getDepthTarget());
+
+ EXPECT_EQ(16, target.getColorTargetCount());
+ for(int i=0; i<16; ++i)
+ {
+ EXPECT_FALSE(nullptr == target.getColorTarget(i)) << "color target is nullptr at index:" << i;
+ }
+}
+
+TEST(OsgRenderTargetTests, CameraTest)
+{
+ auto camera = std::make_shared<OsgCamera>("Camera1");
+ auto renderTarget1 = std::make_shared<OsgRenderTarget2d>(256,256,1.0,8,true);
+ auto renderTarget2 = std::make_shared<OsgRenderTarget2d>(128,128,1.0,8,true);
+
+ EXPECT_NO_THROW(camera->setRenderTarget(renderTarget1));
+ EXPECT_NO_THROW(camera->setRenderTarget(renderTarget2));
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgRepresentationTests.cpp
new file mode 100644
index 0000000..758f084
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgRepresentationTests.cpp
@@ -0,0 +1,241 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgRepresentation class.
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgRepresentationTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<Representation> representation =
+ std::make_shared<MockOsgRepresentation>("test name");
+ });
+
+ std::shared_ptr<Representation> representation = std::make_shared<MockOsgRepresentation>("test name");
+
+ EXPECT_EQ("test name", representation->getName());
+ EXPECT_TRUE(representation->isVisible());
+}
+
+TEST(OsgRepresentationTests, OsgNodeTest)
+{
+ std::shared_ptr<OsgRepresentation> representation = std::make_shared<MockOsgRepresentation>("test name");
+
+ EXPECT_NE(nullptr, representation->getOsgNode());
+
+ // Check that the OSG node is a group (MockOsgRepresentation passes a new group as the node into the
+ // OsgRepresentation constructor)
+ osg::ref_ptr<osg::Group> osgGroup = dynamic_cast<osg::Group*>(representation->getOsgNode().get());
+ EXPECT_TRUE(osgGroup.valid()) << "Representation's OSG node should be a group!";
+}
+
+TEST(OsgRepresentationTests, VisibilityTest)
+{
+ std::shared_ptr<Representation> representation = std::make_shared<MockOsgRepresentation>("test name");
+
+ representation->setVisible(true);
+ EXPECT_TRUE(representation->isVisible());
+
+ representation->setVisible(false);
+ EXPECT_FALSE(representation->isVisible());
+}
+
+TEST(OsgRepresentationTests, WireFrameTest)
+{
+ std::shared_ptr<Representation> representation = std::make_shared<MockOsgRepresentation>("test name");
+
+ EXPECT_FALSE(representation->getDrawAsWireFrame());
+
+ representation->setDrawAsWireFrame(true);
+ EXPECT_TRUE(representation->getDrawAsWireFrame());
+}
+
+TEST(OsgRepresentationTests, PoseTest)
+{
+ std::shared_ptr<Representation> representation = std::make_shared<MockOsgRepresentation>("test name");
+ std::shared_ptr<BasicSceneElement> element = std::make_shared<BasicSceneElement>("element");
+ element->addComponent(representation);
+ element->initialize();
+ representation->wakeUp();
+
+ {
+ SCOPED_TRACE("Check Initial Pose");
+ EXPECT_TRUE(representation->getLocalPose().isApprox(RigidTransform3d::Identity()));
+ EXPECT_TRUE(representation->getPose().isApprox(RigidTransform3d::Identity()));
+ }
+
+ RigidTransform3d localPose;
+ {
+ SCOPED_TRACE("Set Local Pose");
+ localPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ representation->setLocalPose(localPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(localPose));
+ }
+
+ RigidTransform3d elementPose;
+ {
+ SCOPED_TRACE("Set Element Pose");
+ elementPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ element->setPose(elementPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(elementPose * localPose));
+ }
+
+ {
+ SCOPED_TRACE("Change Local Pose");
+ localPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ representation->setLocalPose(localPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(elementPose * localPose));
+ }
+}
+
+TEST(OsgRepresentationTests, MaterialTest)
+{
+ std::shared_ptr<Representation> representation = std::make_shared<MockOsgRepresentation>("test name");
+
+ {
+ SCOPED_TRACE("Set material");
+ std::shared_ptr<Material> material = std::make_shared<OsgMaterial>("material");
+ EXPECT_TRUE(representation->setMaterial(material));
+ EXPECT_EQ(material, representation->getMaterial());
+ }
+
+ {
+ SCOPED_TRACE("Clear material");
+ representation->clearMaterial();
+ EXPECT_EQ(nullptr, representation->getMaterial());
+ }
+}
+
+TEST(OsgRepresentationTests, UpdateTest)
+{
+ std::shared_ptr<MockOsgRepresentation> mockRepresentation = std::make_shared<MockOsgRepresentation>("test name");
+ std::shared_ptr<Representation> representation = mockRepresentation;
+
+ EXPECT_EQ(0, mockRepresentation->getNumUpdates());
+ EXPECT_EQ(0.0, mockRepresentation->getSumDt());
+
+ double sumDt = 0.0;
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> distribution(0.0, 1.0);
+
+ /// Do 10 updates with random dt and check each time that the number of updates and sum of dt are correct.
+ for (int i = 1; i <= 10; ++i)
+ {
+ double dt = distribution(generator);
+ sumDt += dt;
+
+ representation->update(dt);
+ EXPECT_EQ(i, mockRepresentation->getNumUpdates());
+ EXPECT_LT(fabs(sumDt - mockRepresentation->getSumDt()), Eigen::NumTraits<double>::dummy_precision());
+ }
+}
+
+TEST(OsgRepresentationTests, GroupTest)
+{
+ std::shared_ptr<Representation> rep = std::make_shared<MockOsgRepresentation>("TestRepresentation");
+
+ rep->clearGroupReferences();
+
+ EXPECT_TRUE(rep->addGroupReference("group1"));
+ EXPECT_FALSE(rep->addGroupReference("group1"));
+
+ EXPECT_TRUE(rep->addGroupReference("group2"));
+ EXPECT_TRUE(rep->addGroupReference("group3"));
+
+ std::vector<std::string> groups = rep->getGroupReferences();
+
+ EXPECT_EQ(3U, groups.size());
+
+ EXPECT_NE(std::end(groups), std::find(std::begin(groups), std::end(groups), "group1"));
+ EXPECT_NE(std::end(groups), std::find(std::begin(groups), std::end(groups), "group2"));
+ EXPECT_NE(std::end(groups), std::find(std::begin(groups), std::end(groups), "group3"));
+}
+
+TEST(OsgRepresentationTests, GroupsTest)
+{
+ std::shared_ptr<Representation> rep = std::make_shared<MockOsgRepresentation>("TestRepresentation");
+
+ rep->clearGroupReferences();
+
+ std::vector<std::string> newGroups;
+ newGroups.push_back("group1");
+ newGroups.push_back("group1");
+ newGroups.push_back("group2");
+ newGroups.push_back("group3");
+
+ rep->addGroupReferences(newGroups);
+ std::vector<std::string> groups = rep->getGroupReferences();
+
+ EXPECT_EQ(3U, groups.size());
+
+ EXPECT_NE(std::end(groups), std::find(std::begin(groups), std::end(groups), "group1"));
+ EXPECT_NE(std::end(groups), std::find(std::begin(groups), std::end(groups), "group2"));
+ EXPECT_NE(std::end(groups), std::find(std::begin(groups), std::end(groups), "group3"));
+
+}
+
+TEST(OsgRepresentationTests, SetGroupsTests)
+{
+ std::shared_ptr<Representation> rep = std::make_shared<MockOsgRepresentation>("TestRepresentation");
+
+ std::vector<std::string> newGroups;
+ newGroups.push_back("group1");
+ newGroups.push_back("group1");
+ newGroups.push_back("group2");
+ newGroups.push_back("group3");
+
+ rep->addGroupReference("OtherGroup");
+ rep->setGroupReferences(newGroups);
+
+ std::vector<std::string> groups = rep->getGroupReferences();
+
+ EXPECT_EQ(3U, groups.size());
+
+ EXPECT_NE(std::end(groups), std::find(std::begin(groups), std::end(groups), "group1"));
+ EXPECT_NE(std::end(groups), std::find(std::begin(groups), std::end(groups), "group2"));
+ EXPECT_NE(std::end(groups), std::find(std::begin(groups), std::end(groups), "group3"));
+}
+
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgRigidTransformConversionsTests.cpp b/SurgSim/Graphics/UnitTests/OsgRigidTransformConversionsTests.cpp
new file mode 100644
index 0000000..5122b52
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgRigidTransformConversionsTests.cpp
@@ -0,0 +1,118 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 conversions to and from OSG rigid transform types
+
+#include "SurgSim/Graphics/OsgQuaternionConversions.h"
+#include "SurgSim/Graphics/OsgRigidTransformConversions.h"
+#include "SurgSim/Graphics/OsgVectorConversions.h"
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+using SurgSim::Graphics::fromOsg;
+using SurgSim::Graphics::toOsg;
+using SurgSim::Math::Quaternionf;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform2f;
+using SurgSim::Math::RigidTransform2d;
+using SurgSim::Math::RigidTransform3f;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3f;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4f;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::makeRigidTransform;
+
+
+TEST(OsgRigidTransformConversionsTests, RigidTransform3fTest)
+{
+ Quaternionf rotation = Quaternionf(Vector4f::Random());
+ rotation.normalize();
+ Vector3f translation = Vector3f::Random();
+ RigidTransform3f transform = makeRigidTransform(rotation, translation);
+
+ /// Convert to OSG
+ std::pair<osg::Quat, osg::Vec3f> osgTransform = toOsg(transform);
+
+ /// Convert back to Eigen and compare with original
+ RigidTransform3f resultTransform = fromOsg(osgTransform);
+ EXPECT_TRUE(transform.isApprox(resultTransform));
+}
+
+TEST(OsgRigidTransformConversionsTests, RigidTransform3fMultiplyTest)
+{
+ Quaternionf rotation = Quaternionf(Vector4f::Random());
+ rotation.normalize();
+ Vector3f translation = Vector3f::Random();
+ RigidTransform3f transform = makeRigidTransform(rotation, translation);
+
+ Vector3f vector = Vector3f::Random();
+ osg::Vec3f osgVector = toOsg(vector);
+
+ /// Transform the vector using Eigen
+ Vector3f result = transform * vector;
+
+ std::pair<osg::Quat, osg::Vec3f> osgTransform = toOsg(transform);
+
+ /// Transform the vector using OSG
+ osg::Vec3f osgResult = osgTransform.first * osgVector + osgTransform.second;
+
+ /// Compare the transformations
+ EXPECT_TRUE(result.isApprox(fromOsg(osgResult)));
+}
+
+TEST(OsgRigidTransformConversionsTests, RigidTransform3dTest)
+{
+ Quaterniond rotation = Quaterniond(Vector4d::Random());
+ rotation.normalize();
+ Vector3d translation = Vector3d::Random();
+
+ RigidTransform3d transform = makeRigidTransform(rotation, translation);
+
+ /// Convert to OSG
+ std::pair<osg::Quat, osg::Vec3d> osgTransform = toOsg(transform);
+
+ /// Convert back to Eigen and compare with original
+ RigidTransform3d resultTransform = fromOsg(osgTransform);
+ EXPECT_TRUE(transform.isApprox(resultTransform));
+}
+
+TEST(OsgRigidTransformConversionsTests, RigidTransform3dMultiplyTest)
+{
+ Quaterniond rotation = Quaterniond(Vector4d::Random());
+ rotation.normalize();
+ Vector3d translation = Vector3d::Random();
+ RigidTransform3d transform = makeRigidTransform(rotation, translation);
+
+ Vector3d vector = Vector3d::Random();
+ osg::Vec3d osgVector = toOsg(vector);
+
+ /// Transform the vector using Eigen
+ Vector3d result = transform * vector;
+
+ std::pair<osg::Quat, osg::Vec3d> osgTransform = toOsg(transform);
+
+ /// Transform the vector using OSG
+ osg::Vec3d osgResult = osgTransform.first * osgVector + osgTransform.second;
+
+ /// Compare the transformations
+ EXPECT_TRUE(result.isApprox(fromOsg(osgResult)));
+}
diff --git a/SurgSim/Graphics/UnitTests/OsgSceneryRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgSceneryRepresentationTests.cpp
new file mode 100644
index 0000000..127428b
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgSceneryRepresentationTests.cpp
@@ -0,0 +1,106 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Unit Tests for the OsgSceneryRepresentation class.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+
+using SurgSim::Graphics::OsgSceneryRepresentation;
+using SurgSim::Graphics::OsgViewElement;
+using SurgSim::Graphics::SceneryRepresentation;
+
+class OsgSceneryRepresentationTest: public ::testing::Test
+{
+public:
+ virtual void SetUp() override
+ {
+ sceneryObject = std::make_shared<OsgSceneryRepresentation>("test");
+ sceneryObject2 = std::make_shared<OsgSceneryRepresentation>("test2");
+ runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ manager = std::make_shared<SurgSim::Graphics::OsgManager>();
+ scene = runtime->getScene();
+ viewElement = std::make_shared<OsgViewElement>("view element");
+
+ scene->addSceneElement(viewElement);
+ runtime->addManager(manager);
+
+ }
+
+ virtual void TearDown() override
+ {
+ }
+
+ std::shared_ptr<SurgSim::Graphics::OsgSceneryRepresentation> sceneryObject;
+ std::shared_ptr<SurgSim::Graphics::OsgSceneryRepresentation> sceneryObject2;
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime;
+ std::shared_ptr<SurgSim::Graphics::OsgManager> manager;
+ std::shared_ptr<SurgSim::Framework::Scene> scene;
+ std::shared_ptr<SurgSim::Graphics::OsgViewElement> viewElement;
+};
+
+TEST_F(OsgSceneryRepresentationTest, FileNameTest)
+{
+ sceneryObject->setFileName("OsgSceneryRepresentationTests/Torus.obj");
+ EXPECT_EQ("OsgSceneryRepresentationTests/Torus.obj", sceneryObject->getFileName());
+}
+
+TEST_F(OsgSceneryRepresentationTest, InitTest)
+{
+ sceneryObject->setFileName("OsgSceneryRepresentationTests/Torus.obj");
+ EXPECT_NO_THROW(viewElement->addComponent(sceneryObject));
+
+ sceneryObject2->setFileName("OsgSceneryRepresentationTests/Torus.osgb");
+ EXPECT_NO_THROW(viewElement->addComponent(sceneryObject2));
+}
+
+TEST_F(OsgSceneryRepresentationTest, AccessibleTest)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component;
+ ASSERT_NO_THROW(component = SurgSim::Framework::Component::getFactory().create(
+ "SurgSim::Graphics::OsgSceneryRepresentation",
+ "scenery"));
+
+ std::string fileName("TestFileName");
+ component->setValue("FileName", fileName);
+ EXPECT_EQ(fileName, component->getValue<std::string>("FileName"));
+}
+
+TEST_F(OsgSceneryRepresentationTest, SerializationTests)
+{
+ std::shared_ptr<SceneryRepresentation> scenery = std::make_shared<OsgSceneryRepresentation>("OsgScenery");
+
+ std::string fileName("TestFileName");
+ scenery->setFileName(fileName);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = scenery->encode());
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(6u, node.size());
+
+ std::shared_ptr<SceneryRepresentation> result = std::make_shared<OsgSceneryRepresentation>("OsgScenery");
+ ASSERT_NO_THROW(result->decode(node));
+ EXPECT_EQ("SurgSim::Graphics::OsgSceneryRepresentation", result->getClassName());
+ EXPECT_EQ(fileName, result->getFileName());
+}
diff --git a/SurgSim/Graphics/UnitTests/OsgScreenSpaceQuadTests.cpp b/SurgSim/Graphics/UnitTests/OsgScreenSpaceQuadTests.cpp
new file mode 100644
index 0000000..573077b
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgScreenSpaceQuadTests.cpp
@@ -0,0 +1,135 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgTexture class.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+#include "SurgSim/Graphics/OsgTexture.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+
+#include <osg/Node>
+#include <osg/Texture2D>
+#include <osg/StateSet>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgScreenSpaceQuadRepresentationTests, SetTexture2d)
+{
+ auto texture = std::make_shared<OsgTexture2d>();
+ texture->setSize(256, 256);
+ auto quad = std::make_shared<OsgScreenSpaceQuadRepresentation>("quad");
+
+ EXPECT_TRUE(quad->setTexture(texture));
+
+ osg::StateSet* stateSet = quad->getOsgNode()->getOrCreateStateSet();
+ EXPECT_EQ(1u, stateSet->getTextureAttributeList().size());
+
+ // Test Initialization
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ EXPECT_TRUE(quad->initialize(runtime));
+}
+
+TEST(OsgScreenSpaceQuadRepresentationTests, SetTextureRectangle)
+{
+ std::shared_ptr<TextureRectangle> texture = std::make_shared<OsgTextureRectangle>();
+ texture->setSize(10, 100);
+ std::shared_ptr<OsgScreenSpaceQuadRepresentation> quad =
+ std::make_shared<OsgScreenSpaceQuadRepresentation>("quad");
+
+ EXPECT_TRUE(quad->setTexture(texture));
+
+ osg::StateSet* stateSet = quad->getOsgNode()->getOrCreateStateSet();
+ EXPECT_EQ(1u, stateSet->getTextureAttributeList().size());
+
+ // Test Initialization
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ EXPECT_TRUE(quad->initialize(runtime));
+}
+
+TEST(OsgScreenSpaceQuadRepresentation, SetSize)
+{
+ std::shared_ptr<OsgScreenSpaceQuadRepresentation> quad =
+ std::make_shared<OsgScreenSpaceQuadRepresentation>("quad");
+
+ double width = 100.0;
+ double height = 100.0;
+
+ ASSERT_ANY_THROW(quad->getSize(nullptr, &height));
+ ASSERT_ANY_THROW(quad->getSize(&width, nullptr));
+ ASSERT_ANY_THROW(quad->getSize(nullptr, nullptr));
+
+ quad->getLocation(&width, &height);
+ EXPECT_DOUBLE_EQ(0.0, width);
+ EXPECT_DOUBLE_EQ(0.0, height);
+
+ quad->setSize(100.0, 200.0);
+ quad->getSize(&width, &height);
+
+ EXPECT_DOUBLE_EQ(100.0, width);
+ EXPECT_DOUBLE_EQ(200.0, height);
+
+}
+
+TEST(OsgScreenSpaceQuadRepresentationTests, SetLocation)
+{
+ std::shared_ptr<OsgScreenSpaceQuadRepresentation> quad =
+ std::make_shared<OsgScreenSpaceQuadRepresentation>("quad");
+
+ double x = 100.0;
+ double y = 100.0;
+
+ ASSERT_ANY_THROW(quad->getLocation(nullptr, &y));
+ ASSERT_ANY_THROW(quad->getLocation(&x, nullptr));
+ ASSERT_ANY_THROW(quad->getLocation(nullptr, nullptr));
+
+ quad->getLocation(&x, &y);
+ EXPECT_DOUBLE_EQ(0.0, x);
+ EXPECT_DOUBLE_EQ(0.0, y);
+
+ quad->setLocation(100.0, 200.0);
+ quad->getLocation(&x, &y);
+
+ EXPECT_DOUBLE_EQ(100.0, x);
+ EXPECT_DOUBLE_EQ(200.0, y);
+
+ Vector3d position(300.0, 400.0, 0.0);
+ quad->setLocalPose(SurgSim::Math::makeRigidTransform(Quaterniond::Identity(), position));
+
+ quad->getLocation(&x, &y);
+ EXPECT_DOUBLE_EQ(300.0, x);
+ EXPECT_DOUBLE_EQ(400.0, y);
+
+}
+
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgShaderTests.cpp b/SurgSim/Graphics/UnitTests/OsgShaderTests.cpp
new file mode 100644
index 0000000..0dabd8b
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgShaderTests.cpp
@@ -0,0 +1,297 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgShader class.
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Graphics/OsgShader.h"
+
+#include <boost/filesystem.hpp>
+#include <boost/filesystem/fstream.hpp>
+
+#include <gtest/gtest.h>
+
+#include <sstream>
+
+namespace
+{
+ /// Sample vertex shader code
+ const std::string vertexShader =
+ "varying vec4 vertColor;\n"
+ "void main(void)\n"
+ "{\n"
+ " gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;\n"
+ " vertColor.rgb = gl_Normal;\n"
+ " vertColor.a = 1.0;\n"
+ "}\n";
+
+ /// Sample geometry shader code
+ const std::string geometryShader =
+ "#version 150\n"
+ "#extension GL_EXT_gpu_shader4 : enable\n"
+ "#extension GL_EXT_geometry_shader4 : enable\n"
+ "layout(triangles) in;\n"
+ "layout(triangle_strip, max_vertices=3) out;\n"
+ "in vec4 vertColor[3];\n"
+ "out vec4 geomColor;\n"
+ "void main()\n"
+ "{\n"
+ " for (int i = 0; i < gl_VerticesIn; ++i)\n"
+ " {\n"
+ " gl_Position = gl_PositionIn[i];\n"
+ " geomColor = vertColor[i];\n"
+ " EmitVertex();\n"
+ " }\n"
+ " EndPrimitive();\n"
+ "};";
+
+ /// Sample fragment shader code
+ const std::string fragmentShader =
+ "varying vec4 geomColor;\n"
+ "void main(void)\n"
+ "{\n"
+ " gl_FragColor = geomColor;\n"
+ "}";
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgShaderTests, InitTest)
+{
+ OsgShader shader;
+
+ EXPECT_NE(nullptr, shader.getOsgProgram());
+
+ EXPECT_EQ(0u, shader.getOsgProgram()->getNumShaders());
+}
+
+TEST(OsgShaderTests, StateSetTest)
+{
+ OsgShader shader;
+
+ // Create an OSG state set
+ osg::ref_ptr<osg::StateSet> stateSet = new osg::StateSet();
+
+ const osg::StateSet::AttributeList& attributes = stateSet->getAttributeList();
+ EXPECT_EQ(0u, attributes.size());
+
+ /// Add the shader to the state set
+ shader.addToStateSet(stateSet.get());
+
+ EXPECT_EQ(1u, attributes.size()) << "State set has no attributes, one shader program should have been added!";
+ EXPECT_EQ(shader.getOsgProgram(), attributes.at(osg::StateAttribute::TypeMemberPair(
+ osg::StateAttribute::PROGRAM, 0)).first) << "First attribute in state set should be the added shader program!";
+
+ /// Remove the shader from the state set
+ shader.removeFromStateSet(stateSet.get());
+
+ EXPECT_EQ(0u, attributes.size()) <<
+ "State set should no longer have any attributes, the shader program should have been removed!";
+}
+
+TEST(OsgShaderTests, SetShaderSourceTest)
+{
+ std::shared_ptr<OsgShader> osgShader = std::make_shared<OsgShader>();
+ std::shared_ptr<Shader> shader = osgShader;
+
+ EXPECT_FALSE(shader->hasVertexShader());
+ EXPECT_FALSE(shader->hasGeometryShader());
+ EXPECT_FALSE(shader->hasFragmentShader());
+
+ {
+ SCOPED_TRACE("Set vertex shader source");
+ shader->setVertexShaderSource(vertexShader);
+
+ std::string resultVertexShader;
+ EXPECT_TRUE(shader->hasVertexShader());
+ EXPECT_TRUE(shader->getVertexShaderSource(&resultVertexShader));
+ EXPECT_EQ(vertexShader, resultVertexShader);
+
+ EXPECT_EQ(1u, osgShader->getOsgProgram()->getNumShaders());
+ }
+
+ {
+ SCOPED_TRACE("Set geometry shader source");
+ shader->setGeometryShaderSource(geometryShader);
+
+ std::string resultGeometryShader;
+ EXPECT_TRUE(shader->hasGeometryShader());
+ EXPECT_TRUE(shader->getGeometryShaderSource(&resultGeometryShader));
+ EXPECT_EQ(geometryShader, resultGeometryShader);
+
+ EXPECT_EQ(2u, osgShader->getOsgProgram()->getNumShaders());
+ }
+
+ {
+ SCOPED_TRACE("Set fragment shader source");
+ shader->setFragmentShaderSource(fragmentShader);
+
+ std::string resultFragmentShader;
+ EXPECT_TRUE(shader->hasFragmentShader());
+ EXPECT_TRUE(shader->getFragmentShaderSource(&resultFragmentShader));
+ EXPECT_EQ(fragmentShader, resultFragmentShader);
+
+ EXPECT_EQ(3u, osgShader->getOsgProgram()->getNumShaders());
+ }
+}
+
+void expectFileContents(const std::string& filePath, const std::string& contents)
+{
+ boost::filesystem::ifstream fileStream(filePath);
+ std::stringstream resultStream(contents);
+
+ ASSERT_FALSE(fileStream.bad());
+
+ std::string fileLine;
+ std::string resultLine;
+ while (! fileStream.eof() && ! resultStream.eof())
+ {
+ fileStream >> fileLine;
+ resultStream >> resultLine;
+
+ // Skip possible trailing newlines
+ fileStream >> std::ws;
+ resultStream >> std::ws;
+
+ EXPECT_EQ(fileLine, resultLine);
+ }
+ EXPECT_TRUE(fileStream.eof());
+ EXPECT_TRUE(resultStream.eof());
+ fileStream.close();
+}
+
+TEST(OsgShaderTests, LoadShaderSourceTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/OsgShaderTests");
+ SurgSim::Framework::ApplicationData data(paths);
+
+ std::string vertexShaderPath = data.findFile("shader.vert");
+ std::string geometryShaderPath = data.findFile("shader.geom");
+ std::string fragmentShaderPath = data.findFile("shader.frag");
+
+ ASSERT_NE("", vertexShaderPath) << "Could not find vertex shader!";
+ ASSERT_NE("", geometryShaderPath) << "Could not find geometry shader!";
+ ASSERT_NE("", fragmentShaderPath) << "Could not find fragment shader!";
+
+ std::shared_ptr<OsgShader> osgShader = std::make_shared<OsgShader>();
+ std::shared_ptr<Shader> shader = osgShader;
+
+ {
+ SCOPED_TRACE("Load vertex shader source from file");
+ EXPECT_TRUE(shader->loadVertexShaderSource(vertexShaderPath));
+ std::string resultVertexShader;
+ EXPECT_TRUE(shader->getVertexShaderSource(&resultVertexShader));
+ expectFileContents(vertexShaderPath, resultVertexShader);
+
+ EXPECT_EQ(1u, osgShader->getOsgProgram()->getNumShaders());
+ }
+
+ {
+ SCOPED_TRACE("Load geometry shader source from file");
+ EXPECT_TRUE(shader->loadGeometryShaderSource(geometryShaderPath));
+ std::string resultGeometryShader;
+ EXPECT_TRUE(shader->getGeometryShaderSource(&resultGeometryShader));
+ expectFileContents(geometryShaderPath, resultGeometryShader);
+
+ EXPECT_EQ(2u, osgShader->getOsgProgram()->getNumShaders());
+ }
+
+ {
+ SCOPED_TRACE("Load fragment shader source from file");
+ EXPECT_TRUE(shader->loadFragmentShaderSource(fragmentShaderPath));
+ std::string resultFragmentShader;
+ EXPECT_TRUE(shader->getFragmentShaderSource(&resultFragmentShader));
+ expectFileContents(fragmentShaderPath, resultFragmentShader);
+
+ EXPECT_EQ(3u, osgShader->getOsgProgram()->getNumShaders());
+ }
+}
+
+TEST(OsgShaderTests, ClearShaderTest)
+{
+ std::shared_ptr<OsgShader> osgShader = std::make_shared<OsgShader>();
+ std::shared_ptr<Shader> shader = osgShader;
+
+ // Set vertex, geometry, and fragment shaders
+ EXPECT_FALSE(shader->hasVertexShader());
+ shader->setVertexShaderSource(vertexShader);
+ EXPECT_TRUE(shader->hasVertexShader());
+
+ EXPECT_FALSE(shader->hasGeometryShader());
+ shader->setGeometryShaderSource(geometryShader);
+ EXPECT_TRUE(shader->hasGeometryShader());
+
+ EXPECT_FALSE(shader->hasFragmentShader());
+ shader->setFragmentShaderSource(fragmentShader);
+ EXPECT_TRUE(shader->hasFragmentShader());
+
+ EXPECT_EQ(3u, osgShader->getOsgProgram()->getNumShaders());
+
+ {
+ SCOPED_TRACE("Clear vertex shader");
+ shader->clearVertexShader();
+ EXPECT_FALSE(shader->hasVertexShader());
+ EXPECT_TRUE(shader->hasGeometryShader());
+ EXPECT_TRUE(shader->hasFragmentShader());
+
+ EXPECT_EQ(2u, osgShader->getOsgProgram()->getNumShaders());
+ }
+ shader->setVertexShaderSource(vertexShader);
+ EXPECT_TRUE(shader->hasVertexShader());
+
+ {
+ SCOPED_TRACE("Clear geometry shader");
+ shader->clearGeometryShader();
+ EXPECT_TRUE(shader->hasVertexShader());
+ EXPECT_FALSE(shader->hasGeometryShader());
+ EXPECT_TRUE(shader->hasFragmentShader());
+
+ EXPECT_EQ(2u, osgShader->getOsgProgram()->getNumShaders());
+ }
+ shader->setGeometryShaderSource(geometryShader);
+ EXPECT_TRUE(shader->hasGeometryShader());
+
+ {
+ SCOPED_TRACE("Clear fragment shader");
+ shader->clearFragmentShader();
+ EXPECT_TRUE(shader->hasVertexShader());
+ EXPECT_TRUE(shader->hasGeometryShader());
+ EXPECT_FALSE(shader->hasFragmentShader());
+
+ EXPECT_EQ(2u, osgShader->getOsgProgram()->getNumShaders());
+ }
+ shader->setFragmentShaderSource(fragmentShader);
+ EXPECT_TRUE(shader->hasFragmentShader());
+
+ {
+ SCOPED_TRACE("Clear the entire shader");
+ shader->clear();
+ EXPECT_FALSE(shader->hasVertexShader());
+ EXPECT_FALSE(shader->hasGeometryShader());
+ EXPECT_FALSE(shader->hasFragmentShader());
+
+ EXPECT_EQ(0u, osgShader->getOsgProgram()->getNumShaders());
+ }
+}
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgSphereRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgSphereRepresentationTests.cpp
new file mode 100644
index 0000000..871a3fb
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgSphereRepresentationTests.cpp
@@ -0,0 +1,210 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgSphereRepresentation class.
+
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+#include <gtest/gtest.h>
+
+#include <osg/Geode>
+
+#include <random>
+
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST(OsgSphereRepresentationTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<Representation> representation =
+ std::make_shared<OsgSphereRepresentation>("test name");
+ });
+
+ std::shared_ptr<Representation> representation = std::make_shared<OsgSphereRepresentation>("test name");
+ EXPECT_EQ("test name", representation->getName());
+}
+
+TEST(OsgSphereRepresentationTests, AccessibleTest)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component;
+ ASSERT_NO_THROW(component = SurgSim::Framework::Component::getFactory().create(
+ "SurgSim::Graphics::OsgSphereRepresentation",
+ "sphere"));
+
+ EXPECT_EQ("SurgSim::Graphics::OsgSphereRepresentation", component->getClassName());
+
+ double radius = 4.321;
+
+ component->setValue("Radius", radius);
+ YAML::Node node(YAML::convert<SurgSim::Framework::Component>::encode(*component));
+
+ auto decoded = std::dynamic_pointer_cast<SurgSim::Graphics::OsgSphereRepresentation>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>());
+
+ EXPECT_NE(nullptr, decoded);
+ EXPECT_DOUBLE_EQ(radius, decoded->getValue<double>("Radius"));
+}
+
+TEST(OsgSphereRepresentationTests, OsgNodeTest)
+{
+ std::shared_ptr<OsgRepresentation> representation = std::make_shared<OsgSphereRepresentation>("test name");
+
+ ASSERT_NE(nullptr, representation->getOsgNode());
+
+ osg::Switch* switchNode = dynamic_cast<osg::Switch*>(representation->getOsgNode().get());
+ ASSERT_NE(nullptr, switchNode) << "Could not get OSG switch node!";
+
+ ASSERT_EQ(1u, switchNode->getNumChildren()) << "OSG switch node should have 1 child, the transform node!";
+
+ osg::PositionAttitudeTransform* transformNode =
+ dynamic_cast<osg::PositionAttitudeTransform*>(switchNode->getChild(0));
+ ASSERT_NE(nullptr, transformNode) << "Could not get OSG transform node!";
+
+ ASSERT_EQ(1u, transformNode->getNumChildren()) << "OSG transform node should have 1 child, the geode!";
+
+ osg::Node* node = dynamic_cast<osg::Node*>(transformNode->getChild(0));
+ ASSERT_NE(nullptr, node) << "Could not get unit sphere OSG node!";
+}
+
+TEST(OsgSphereRepresentationTests, VisibilityTest)
+{
+ std::shared_ptr<OsgRepresentation> osgRepresentation = std::make_shared<OsgSphereRepresentation>("test name");
+ std::shared_ptr<Representation> representation = osgRepresentation;
+
+ osg::Switch* switchNode = dynamic_cast<osg::Switch*>(osgRepresentation->getOsgNode().get());
+ ASSERT_NE(nullptr, switchNode) << "Could not get OSG switch node!";
+ ASSERT_EQ(1u, switchNode->getNumChildren()) << "OSG switch node should have 1 child, the transform node!";
+
+ EXPECT_TRUE(representation->isVisible());
+ EXPECT_TRUE(switchNode->getChildValue(switchNode->getChild(0)));
+
+ representation->setVisible(false);
+ EXPECT_FALSE(representation->isVisible());
+ EXPECT_FALSE(switchNode->getChildValue(switchNode->getChild(0)));
+
+ representation->setVisible(true);
+ EXPECT_TRUE(representation->isVisible());
+ EXPECT_TRUE(switchNode->getChildValue(switchNode->getChild(0)));
+}
+
+TEST(OsgSphereRepresentationTests, RadiusTest)
+{
+ std::shared_ptr<SphereRepresentation> sphereRepresentation = std::make_shared<OsgSphereRepresentation>("test name");
+
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> distribution(1.0, 10.0);
+
+ double randomRadius = distribution(generator);
+
+ sphereRepresentation->setRadius(randomRadius);
+ EXPECT_EQ(randomRadius, sphereRepresentation->getRadius());
+}
+
+TEST(OsgSphereRepresentationTests, PoseTest)
+{
+ std::shared_ptr<Representation> representation = std::make_shared<MockOsgRepresentation>("test name");
+ std::shared_ptr<BasicSceneElement> element = std::make_shared<BasicSceneElement>("element");
+ element->addComponent(representation);
+ element->initialize();
+ representation->wakeUp();
+
+ {
+ SCOPED_TRACE("Check Initial Pose");
+ EXPECT_TRUE(representation->getLocalPose().isApprox(RigidTransform3d::Identity()));
+ EXPECT_TRUE(representation->getPose().isApprox(RigidTransform3d::Identity()));
+ }
+
+ RigidTransform3d localPose;
+ {
+ SCOPED_TRACE("Set Local Pose");
+ localPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ representation->setLocalPose(localPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(localPose));
+ }
+
+ RigidTransform3d elementPose;
+ {
+ SCOPED_TRACE("Set Element Pose");
+ elementPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ element->setPose(elementPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(elementPose * localPose));
+ }
+
+ {
+ SCOPED_TRACE("Change Local Pose");
+ localPose = SurgSim::Math::makeRigidTransform(
+ Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ representation->setLocalPose(localPose);
+ EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
+ EXPECT_TRUE(representation->getPose().isApprox(elementPose * localPose));
+ }
+}
+
+TEST(OsgSphereRepresentationTests, MaterialTest)
+{
+ std::shared_ptr<OsgRepresentation> osgRepresentation = std::make_shared<OsgSphereRepresentation>("test name");
+ std::shared_ptr<Representation> representation = osgRepresentation;
+
+ std::shared_ptr<OsgMaterial> osgMaterial = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<Material> material = osgMaterial;
+ {
+ SCOPED_TRACE("Set material");
+ EXPECT_TRUE(representation->setMaterial(material));
+ EXPECT_EQ(material, representation->getMaterial());
+
+ osg::Switch* switchNode = dynamic_cast<osg::Switch*>(osgRepresentation->getOsgNode().get());
+ ASSERT_NE(nullptr, switchNode) << "Could not get OSG switch node!";
+ ASSERT_EQ(1u, switchNode->getNumChildren()) << "OSG switch node should have 1 child, the transform node!";
+ EXPECT_EQ(osgMaterial->getOsgStateSet(), switchNode->getChild(0)->getStateSet()) <<
+ "State set should be the material's state set!";
+ }
+
+ {
+ SCOPED_TRACE("Clear material");
+ representation->clearMaterial();
+ EXPECT_EQ(nullptr, representation->getMaterial());
+
+ osg::Switch* switchNode = dynamic_cast<osg::Switch*>(osgRepresentation->getOsgNode().get());
+ ASSERT_NE(nullptr, switchNode) << "Could not get OSG switch node!";
+ ASSERT_EQ(1u, switchNode->getNumChildren()) << "OSG switch node should have 1 child, the transform node!";
+ EXPECT_NE(osgMaterial->getOsgStateSet(), switchNode->getChild(0)->getStateSet()) <<
+ "State set should have been cleared!";
+ }
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgTexture1dTests.cpp b/SurgSim/Graphics/UnitTests/OsgTexture1dTests.cpp
new file mode 100644
index 0000000..3ac4e9e
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgTexture1dTests.cpp
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgTexture1d class.
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Graphics/OsgTexture1d.h"
+
+#include <boost/filesystem.hpp>
+#include <boost/filesystem/fstream.hpp>
+
+#include <gtest/gtest.h>
+
+#include <sstream>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgTexture1dTests, InitTest)
+{
+ OsgTexture1d texture;
+
+ EXPECT_NE(nullptr, texture.getOsgTexture());
+
+ EXPECT_EQ(nullptr, texture.getOsgTexture()->getImage(0u));
+}
+
+TEST(OsgTexture1dTests, SetSizeTest)
+{
+ OsgTexture1d texture;
+
+ texture.setSize(256);
+
+ int width;
+ texture.getSize(&width);
+
+ EXPECT_EQ(256, width);
+}
+
+TEST(OsgTexture1dTests, LoadAndClearImageTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/OsgTextureTests");
+ SurgSim::Framework::ApplicationData data(paths);
+
+ std::string imagePath = data.findFile("Gradient.png");
+
+ ASSERT_NE("", imagePath) << "Could not find image file!";
+
+ // Load the image
+ std::shared_ptr<OsgTexture1d> osgTexture = std::make_shared<OsgTexture1d>();
+ std::shared_ptr<Texture> texture = osgTexture;
+
+ EXPECT_TRUE(texture->loadImage(imagePath)) << "Failed to load image!";
+
+ EXPECT_EQ(1u, osgTexture->getOsgTexture()->getNumImages());
+
+ EXPECT_NE(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture should have an image!";
+
+ // Make sure the image has the expected size
+ int width;
+ osgTexture->getSize(&width);
+ EXPECT_EQ(256, width);
+
+ osg::Image* image = osgTexture->getOsgTexture()->getImage(0u);
+ EXPECT_EQ(256, image->s());
+ EXPECT_EQ(1, image->t());
+ EXPECT_EQ(1, image->r());
+
+ // Remove the image
+ texture->clearImage();
+
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture image should have been cleared!";
+
+ // Try to load an image that does not exist
+ EXPECT_FALSE(texture->loadImage("NotHere.png")) << "Should not have been able to load image - it does not exist!";
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u));
+}
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgTexture2dTests.cpp b/SurgSim/Graphics/UnitTests/OsgTexture2dTests.cpp
new file mode 100644
index 0000000..4d1a44f
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgTexture2dTests.cpp
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgTexture2d class.
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+
+#include <boost/filesystem.hpp>
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgTexture2dTests, InitTest)
+{
+ OsgTexture2d texture;
+
+ EXPECT_NE(nullptr, texture.getOsgTexture());
+
+ EXPECT_EQ(nullptr, texture.getOsgTexture()->getImage(0u));
+}
+
+TEST(OsgTexture2dTests, SetSizeTest)
+{
+ OsgTexture2d texture;
+
+ texture.setSize(256, 512);
+
+ int width, height;
+ texture.getSize(&width, &height);
+
+ EXPECT_EQ(256, width);
+ EXPECT_EQ(512, height);
+}
+
+TEST(OsgTexture2dTests, LoadAndClearImageTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/OsgTextureTests");
+ SurgSim::Framework::ApplicationData data(paths);
+
+ std::string imagePath = data.findFile("CheckerBoard.png");
+
+ ASSERT_NE("", imagePath) << "Could not find image file!";
+
+ // Load the image
+ std::shared_ptr<OsgTexture2d> osgTexture = std::make_shared<OsgTexture2d>();
+ std::shared_ptr<Texture> texture = osgTexture;
+
+ EXPECT_TRUE(texture->loadImage(imagePath)) << "Failed to load image!";
+
+ EXPECT_EQ(1u, osgTexture->getOsgTexture()->getNumImages());
+
+ EXPECT_NE(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture should have an image!";
+
+ // Make sure the image has the expected size
+ int width, height;
+ osgTexture->getSize(&width, &height);
+ EXPECT_EQ(512, width);
+ EXPECT_EQ(512, height);
+
+ osg::Image* image = osgTexture->getOsgTexture()->getImage(0u);
+ EXPECT_EQ(512, image->s());
+ EXPECT_EQ(512, image->t());
+ EXPECT_EQ(1, image->r());
+
+ // Remove the image
+ texture->clearImage();
+
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture image should have been cleared!";
+
+ // Try to load an image that does not exist
+ EXPECT_FALSE(texture->loadImage("NotHere.png")) << "Should not have been able to load image - it does not exist!";
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u));
+}
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgTexture3dTests.cpp b/SurgSim/Graphics/UnitTests/OsgTexture3dTests.cpp
new file mode 100644
index 0000000..e016d2e
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgTexture3dTests.cpp
@@ -0,0 +1,151 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgTextureCubeMap class.
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Graphics/OsgTexture3d.h"
+
+#include <boost/filesystem.hpp>
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgTexture3dTests, InitTest)
+{
+ OsgTexture3d texture;
+
+ EXPECT_NE(nullptr, texture.getOsgTexture());
+
+ EXPECT_EQ(nullptr, texture.getOsgTexture()->getImage(0u));
+}
+
+TEST(OsgTexture3dTests, SetSizeTest)
+{
+ OsgTexture3d texture;
+
+ texture.setSize(256, 512, 1024);
+
+ int width, height, depth;
+ texture.getSize(&width, &height, &depth);
+
+ EXPECT_EQ(256, width);
+ EXPECT_EQ(512, height);
+ EXPECT_EQ(1024, depth);
+}
+
+TEST(OsgTexture3dTests, LoadAndClearImageTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/OsgTextureTests");
+ SurgSim::Framework::ApplicationData data(paths);
+
+ std::string imagePath = data.findFile("CheckerBoard.png");
+
+ ASSERT_NE("", imagePath) << "Could not find image file!";
+
+ // Load the image
+ std::shared_ptr<OsgTexture3d> osgTexture = std::make_shared<OsgTexture3d>();
+ std::shared_ptr<Texture> texture = osgTexture;
+
+ EXPECT_TRUE(texture->loadImage(imagePath)) << "Failed to load image!";
+
+ EXPECT_EQ(1u, osgTexture->getOsgTexture()->getNumImages());
+
+ EXPECT_NE(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture should have an image!";
+
+ // Make sure the image has the expected size
+ int width, height, depth;
+ osgTexture->getSize(&width, &height, &depth);
+ EXPECT_EQ(512, width);
+ EXPECT_EQ(512, height);
+ EXPECT_EQ(1, depth);
+
+ osg::Image* image = osgTexture->getOsgTexture()->getImage(0u);
+ EXPECT_EQ(512, image->s());
+ EXPECT_EQ(512, image->t());
+ EXPECT_EQ(1, image->r());
+
+ // Remove the image
+ texture->clearImage();
+
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture image should have been cleared!";
+
+ // Try to load an image that does not exist
+ EXPECT_FALSE(texture->loadImage("NotHere.png")) << "Should not have been able to load image - it does not exist!";
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u));
+}
+
+TEST(OsgTexture3dTests, LoadImageSlicesTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/OsgTextureTests");
+ SurgSim::Framework::ApplicationData data(paths);
+
+ std::string slice0Path = data.findFile("Brdf0.png");
+ ASSERT_NE("", slice0Path) << "Could not find image file for slice 0!";
+
+ std::string slice1Path = data.findFile("Brdf1.png");
+ ASSERT_NE("", slice1Path) << "Could not find image file for slice 1!";
+
+ std::vector<std::string> slicePaths;
+ slicePaths.push_back(slice0Path);
+ slicePaths.push_back(slice1Path);
+
+ // Load the images
+ std::shared_ptr<OsgTexture3d> osgTexture = std::make_shared<OsgTexture3d>();
+ std::shared_ptr<Texture> texture = osgTexture;
+
+ EXPECT_TRUE(osgTexture->loadImageSlices(slicePaths)) << "Failed to load images!";
+
+ EXPECT_EQ(1u, osgTexture->getOsgTexture()->getNumImages());
+
+ EXPECT_NE(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture should have an image!";
+
+ // Make sure the image has the expected size
+ int width, height, depth;
+ osgTexture->getSize(&width, &height, &depth);
+ EXPECT_EQ(256, width);
+ EXPECT_EQ(256, height);
+ EXPECT_EQ(2, depth);
+
+ osg::Image* image = osgTexture->getOsgTexture()->getImage(0u);
+ EXPECT_EQ(256, image->s());
+ EXPECT_EQ(256, image->t());
+ EXPECT_EQ(2, image->r());
+
+ // Remove the image
+ texture->clearImage();
+
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture image should have been cleared!";
+
+ // Try to load an image that does not exist
+ slicePaths.push_back("NotHere.png");
+ EXPECT_FALSE(osgTexture->loadImageSlices(slicePaths)) <<
+ "Should not have been able to load an image - it does not exist!";
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u));
+}
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgTextureCubeMapTests.cpp b/SurgSim/Graphics/UnitTests/OsgTextureCubeMapTests.cpp
new file mode 100644
index 0000000..754e2a9
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgTextureCubeMapTests.cpp
@@ -0,0 +1,163 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgTextureCubeMap class.
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Graphics/OsgTextureCubeMap.h"
+
+#include <boost/filesystem.hpp>
+#include <boost/filesystem/fstream.hpp>
+
+#include <gtest/gtest.h>
+
+#include <sstream>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgTextureCubeMapTests, InitTest)
+{
+ OsgTextureCubeMap texture;
+
+ EXPECT_NE(nullptr, texture.getOsgTexture());
+
+ EXPECT_EQ(nullptr, texture.getOsgTexture()->getImage(0u));
+}
+
+TEST(OsgTextureCubeMapTests, SetSizeTest)
+{
+ OsgTextureCubeMap texture;
+
+ texture.setSize(256, 512);
+
+ int width, height;
+ texture.getSize(&width, &height);
+
+ EXPECT_EQ(256, width);
+ EXPECT_EQ(512, height);
+}
+
+TEST(OsgTextureCubeMapTests, LoadAndClearImageTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/OsgTextureTests");
+ SurgSim::Framework::ApplicationData data(paths);
+
+ std::string imagePath = data.findFile("CubeMap.png");
+
+ ASSERT_NE("", imagePath) << "Could not find image file!";
+
+ // Load the image
+ std::shared_ptr<OsgTextureCubeMap> osgTexture = std::make_shared<OsgTextureCubeMap>();
+ std::shared_ptr<Texture> texture = osgTexture;
+
+ EXPECT_TRUE(texture->loadImage(imagePath)) << "Failed to load image!";
+
+ EXPECT_EQ(6u, osgTexture->getOsgTexture()->getNumImages());
+
+ // Make sure each face has the expected size
+ int width, height;
+ osgTexture->getSize(&width, &height);
+ EXPECT_EQ(256, width);
+ EXPECT_EQ(256, height);
+
+ for (size_t i = 0; i < 6; ++i)
+ {
+ osg::Image* image = osgTexture->getOsgTexture()->getImage(i);
+ ASSERT_NE(nullptr, image) << "The texture should have an image for each face!";
+ EXPECT_EQ(256, image->s());
+ EXPECT_EQ(256, image->t());
+ EXPECT_EQ(1, image->r());
+ }
+
+ // Remove the image
+ texture->clearImage();
+
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture image should have been cleared!";
+
+ // Try to load an image that does not exist
+ EXPECT_FALSE(texture->loadImage("NotHere.png")) << "Should not have been able to load image - it does not exist!";
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u));
+}
+
+TEST(OsgTextureCubeMapTests, LoadImageFacesTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/OsgTextureTests");
+ SurgSim::Framework::ApplicationData data(paths);
+
+ std::string negativeXPath = data.findFile("NegativeX.png");
+ ASSERT_NE("", negativeXPath) << "Could not find image file for (-X) face!";
+
+ std::string positiveXPath = data.findFile("PositiveX.png");
+ ASSERT_NE("", positiveXPath) << "Could not find image file for (+X) face!";
+
+ std::string negativeYPath = data.findFile("NegativeY.png");
+ ASSERT_NE("", negativeYPath) << "Could not find image file for (-Y) face!";
+
+ std::string positiveYPath = data.findFile("PositiveY.png");
+ ASSERT_NE("", positiveYPath) << "Could not find image file for (+Y) face!";
+
+ std::string negativeZPath = data.findFile("NegativeZ.png");
+ ASSERT_NE("", negativeZPath) << "Could not find image file for (-Z) face!";
+
+ std::string positiveZPath = data.findFile("PositiveZ.png");
+ ASSERT_NE("", positiveZPath) << "Could not find image file for (+Z) face!";
+
+ // Load the images
+ std::shared_ptr<OsgTextureCubeMap> osgTexture = std::make_shared<OsgTextureCubeMap>();
+ std::shared_ptr<Texture> texture = osgTexture;
+
+ EXPECT_TRUE(osgTexture->loadImageFaces(negativeXPath, positiveXPath, negativeYPath, positiveYPath,
+ negativeZPath, positiveZPath)) << "Failed to load images!";
+
+ EXPECT_EQ(6u, osgTexture->getOsgTexture()->getNumImages());
+
+ // Make sure each face has the expected size
+ int width, height;
+ osgTexture->getSize(&width, &height);
+ EXPECT_EQ(256, width);
+ EXPECT_EQ(256, height);
+
+ for (size_t i = 0; i < 6; ++i)
+ {
+ osg::Image* image = osgTexture->getOsgTexture()->getImage(i);
+ ASSERT_NE(nullptr, image) << "The texture should have an image for each face!";
+ EXPECT_EQ(256, image->s());
+ EXPECT_EQ(256, image->t());
+ EXPECT_EQ(1, image->r());
+ }
+
+ // Remove the image
+ texture->clearImage();
+
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture image should have been cleared!";
+
+ // Try to load an image that does not exist
+ EXPECT_FALSE(osgTexture->loadImageFaces(negativeXPath, positiveXPath, "NotHere.png", positiveYPath,
+ negativeZPath, positiveZPath)) << "Should not have been able to load an image - it does not exist!";
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u));
+}
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgTextureRectangleTests.cpp b/SurgSim/Graphics/UnitTests/OsgTextureRectangleTests.cpp
new file mode 100644
index 0000000..0eae628
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgTextureRectangleTests.cpp
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgTextureRectangle class.
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Graphics/OsgTextureRectangle.h"
+
+#include <boost/filesystem.hpp>
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgTextureRectangleTests, InitTest)
+{
+ OsgTextureRectangle texture;
+
+ EXPECT_NE(nullptr, texture.getOsgTexture());
+
+ EXPECT_EQ(nullptr, texture.getOsgTexture()->getImage(0u));
+}
+
+TEST(OsgTextureRectangleTests, SetSizeTest)
+{
+ OsgTextureRectangle texture;
+
+ texture.setSize(256, 512);
+
+ int width, height;
+ texture.getSize(&width, &height);
+
+ EXPECT_EQ(256, width);
+ EXPECT_EQ(512, height);
+}
+
+TEST(OsgTextureRectangleTests, LoadAndClearImageTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/OsgTextureTests");
+ SurgSim::Framework::ApplicationData data(paths);
+
+ std::string imagePath = data.findFile("CheckerBoard.png");
+
+ ASSERT_NE("", imagePath) << "Could not find image file!";
+
+ // Load the image
+ std::shared_ptr<OsgTextureRectangle> osgTexture = std::make_shared<OsgTextureRectangle>();
+ std::shared_ptr<Texture> texture = osgTexture;
+
+ EXPECT_TRUE(texture->loadImage(imagePath)) << "Failed to load image!";
+
+ EXPECT_EQ(1u, osgTexture->getOsgTexture()->getNumImages());
+
+ EXPECT_NE(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture should have an image!";
+
+ // Make sure the image has the expected size
+ int width, height;
+ osgTexture->getSize(&width, &height);
+ EXPECT_EQ(512, width);
+ EXPECT_EQ(512, height);
+
+ osg::Image* image = osgTexture->getOsgTexture()->getImage(0u);
+ EXPECT_EQ(512, image->s());
+ EXPECT_EQ(512, image->t());
+ EXPECT_EQ(1, image->r());
+
+ // Remove the image
+ texture->clearImage();
+
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture image should have been cleared!";
+
+ // Try to load an image that does not exist
+ EXPECT_FALSE(texture->loadImage("NotHere.png")) << "Should not have been able to load image - it does not exist!";
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u));
+}
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgTextureTests.cpp b/SurgSim/Graphics/UnitTests/OsgTextureTests.cpp
new file mode 100644
index 0000000..803c822
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgTextureTests.cpp
@@ -0,0 +1,81 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgTexture class.
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Graphics/OsgTexture.h"
+
+#include <boost/filesystem.hpp>
+#include <boost/filesystem/fstream.hpp>
+
+#include <osg/Texture2D>
+
+#include <gtest/gtest.h>
+
+#include <sstream>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Concrete OSG Texture for testing.
+/// Wraps an osg::Texture2D.
+class MockOsgTexture : public OsgTexture
+{
+public:
+ /// Constructor
+ MockOsgTexture() : OsgTexture(new osg::Texture2D())
+ {
+ }
+};
+
+TEST(OsgTextureTests, InitTest)
+{
+ MockOsgTexture texture;
+
+ EXPECT_NE(nullptr, texture.getOsgTexture());
+
+ EXPECT_EQ(nullptr, texture.getOsgTexture()->getImage(0u));
+}
+
+TEST(OsgTextureTests, LoadAndClearImageTest)
+{
+ ASSERT_TRUE(boost::filesystem::exists("Data"));
+
+ std::vector<std::string> paths;
+ paths.push_back("Data/OsgTextureTests");
+ SurgSim::Framework::ApplicationData data(paths);
+
+ std::string imagePath = data.findFile("CheckerBoard.png");
+
+ ASSERT_NE("", imagePath) << "Could not find image file!";
+
+ std::shared_ptr<OsgTexture> osgTexture = std::make_shared<MockOsgTexture>();
+ std::shared_ptr<Texture> texture = osgTexture;
+
+ EXPECT_TRUE(texture->loadImage(imagePath)) << "Failed to load image!";
+
+ EXPECT_NE(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture should have an image!";
+
+ texture->clearImage();
+
+ EXPECT_EQ(nullptr, osgTexture->getOsgTexture()->getImage(0u)) << "Texture image should have been cleared!";
+}
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgTextureUniformTests.cpp b/SurgSim/Graphics/UnitTests/OsgTextureUniformTests.cpp
new file mode 100644
index 0000000..9d53e4b
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgTextureUniformTests.cpp
@@ -0,0 +1,129 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgTexture1d.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/OsgTexture3d.h"
+#include "SurgSim/Graphics/OsgTextureRectangle.h"
+
+#include <osg/StateSet>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgTextureUniformTest, AddUniformTests)
+{
+ auto uniform2d = std::make_shared<OsgUniform<std::shared_ptr<OsgTexture2d>>>("TextureUniform");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+
+ {
+ // Material not initialized, should be able to add Uniform without Texture
+ auto material = std::make_shared<OsgMaterial>("material");
+ EXPECT_NO_THROW(material->addUniform(uniform2d));
+ }
+
+ {
+ // Material is initialized, should not be able to add Uniform without Texture
+ auto material = std::make_shared<OsgMaterial>("material");
+ material->initialize(runtime);
+ EXPECT_ANY_THROW(material->addUniform(uniform2d));
+ }
+
+ {
+ // Material is initialized, should be able to add Uniform with Texture
+ auto material = std::make_shared<OsgMaterial>("material");
+ material->initialize(runtime);
+ auto texture2d = std::make_shared<OsgTexture2d>();
+ texture2d->setSize(256, 256);
+ uniform2d->set(texture2d);
+ EXPECT_NO_THROW(material->addUniform(uniform2d));
+ }
+}
+
+// Check for correct assignment of uniforms to texture units
+TEST(OsgTextureUniformTests, TextureUnitAssignment)
+{
+ auto material = std::make_shared<OsgMaterial>("material");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ material->initialize(runtime);
+ auto uniform2d0 = std::make_shared<OsgUniform<std::shared_ptr<OsgTexture2d>>>("TextureUniform0");
+ auto uniform2d1 = std::make_shared<OsgUniform<std::shared_ptr<OsgTexture2d>>>("TextureUniform1");
+ auto uniform2d2 = std::make_shared<OsgUniform<std::shared_ptr<OsgTexture2d>>>("TextureUniform2");
+ uniform2d1->setMinimumTextureUnit(8);
+
+ auto texture2d0 = std::make_shared<OsgTexture2d>();
+ uniform2d0->set(texture2d0);
+
+ auto texture2d1 = std::make_shared<OsgTexture2d>();
+ uniform2d1->set(texture2d1);
+
+ auto texture2d2 = std::make_shared<OsgTexture2d>();
+ uniform2d2->set(texture2d2);
+
+ osg::StateSet* stateSet = material->getOsgStateSet();
+
+ EXPECT_EQ(0u, stateSet->getTextureAttributeList().size());
+
+ material->addUniform(uniform2d0);
+ EXPECT_EQ(1u, stateSet->getTextureAttributeList().size());
+ EXPECT_FALSE(stateSet->getTextureAttributeList()[0].empty());
+
+ material->addUniform(uniform2d1);
+ EXPECT_EQ(9u, stateSet->getTextureAttributeList().size());
+ EXPECT_FALSE(stateSet->getTextureAttributeList()[0].empty());
+ EXPECT_TRUE(stateSet->getTextureAttributeList()[1].empty());
+ EXPECT_FALSE(stateSet->getTextureAttributeList()[8].empty());
+
+ material->addUniform(uniform2d2);
+ EXPECT_EQ(9u, stateSet->getTextureAttributeList().size());
+ EXPECT_FALSE(stateSet->getTextureAttributeList()[0].empty());
+ EXPECT_FALSE(stateSet->getTextureAttributeList()[1].empty());
+ EXPECT_TRUE(stateSet->getTextureAttributeList()[2].empty());
+ EXPECT_FALSE(stateSet->getTextureAttributeList()[8].empty());
+}
+
+
+/// Expose a bug where a texture uniform could be created that is not really a correctly specialized
+/// uniform, if this fails then the uniform2d was not created correctly
+TEST(OsgTextureUniformTests, TextureUniformTemplateProblem)
+{
+ auto material = std::make_shared<OsgMaterial>("material");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ material->initialize(runtime);
+ auto uniform2d = std::make_shared<OsgUniform<std::shared_ptr<OsgTexture2d>>>("TextureUniform");
+ auto texture2d = std::make_shared<OsgTexture2d>();
+ texture2d->setSize(256, 256);
+ uniform2d->set(texture2d);
+
+ material->addUniform(uniform2d);
+
+ osg::StateSet* stateSet = material->getOsgStateSet();
+
+ EXPECT_EQ(1u, stateSet->getTextureAttributeList().size());
+}
+
+
+}; // namespace Graphics
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgUniformBaseTests.cpp b/SurgSim/Graphics/UnitTests/OsgUniformBaseTests.cpp
new file mode 100644
index 0000000..bd10de9
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgUniformBaseTests.cpp
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgUniformBase class.
+
+#include "SurgSim/Graphics/OsgUniformBase.h"
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// OsgUniformBase class for testing
+class MockOsgUniformBase : public OsgUniformBase
+{
+public:
+ /// Constructor
+ /// \param name Name used in shader code to access this uniform
+ explicit MockOsgUniformBase(const std::string& name) : OsgUniformBase(name)
+ {
+ }
+};
+
+TEST(OsgUniformBaseTests, InitTest)
+{
+ MockOsgUniformBase uniform("test name");
+
+ EXPECT_EQ("test name", uniform.getName());
+
+ EXPECT_NE(nullptr, uniform.getOsgUniform());
+}
+
+TEST(OsgUniformBaseTests, StateSetTest)
+{
+ MockOsgUniformBase uniform("test name");
+
+ // Create an OSG state set
+ osg::ref_ptr<osg::StateSet> stateSet = new osg::StateSet();
+
+ const osg::StateSet::UniformList& uniforms = stateSet->getUniformList();
+ EXPECT_EQ(0u, uniforms.size());
+
+ /// Add the uniform to the state set
+ uniform.addToStateSet(stateSet.get());
+
+ EXPECT_EQ(1u, uniforms.size()) << "State set has no uniforms, one should have been added!";
+ EXPECT_EQ(uniform.getOsgUniform(), uniforms.at("test name").first) <<
+ "First uniform in state set should be the added uniform!";
+
+ /// Remove the uniform from the state set
+ uniform.removeFromStateSet(stateSet.get());
+
+ EXPECT_EQ(0u, uniforms.size()) <<
+ "State set should no longer have any uniforms, the uniform should have been removed!";
+}
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgUniformTests.cpp b/SurgSim/Graphics/UnitTests/OsgUniformTests.cpp
new file mode 100644
index 0000000..f0d6261
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgUniformTests.cpp
@@ -0,0 +1,430 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgUniform class.
+
+#include "SurgSim/Graphics/OsgUniform.h"
+
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+using SurgSim::Math::Vector2f;
+using SurgSim::Math::Vector3f;
+using SurgSim::Math::Vector4f;
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Matrix22f;
+using SurgSim::Math::Matrix33f;
+using SurgSim::Math::Matrix44f;
+using SurgSim::Math::Matrix22d;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Matrix44d;
+
+namespace
+{
+/// Random number generator, used to generate random values for the tests.
+std::default_random_engine generator;
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Constructs an OsgUniform, sets it to the given value, and returns the result of Uniform::get() and the wrapped
+/// osg::Uniform::get().
+/// \tparam Type Uniform's value type
+/// \tparam OsgType Type stored in the osg::Uniform
+template <class Type, class OsgType>
+std::pair<Type, OsgType> testUniformConstruction(const Type& value)
+{
+ std::shared_ptr<OsgUniform<Type>> osgUniform =
+ std::make_shared<OsgUniform<Type>>("test name");
+ std::shared_ptr<OsgUniformBase> osgUniformBase = osgUniform;
+ std::shared_ptr<Uniform<Type>> uniform = osgUniform;
+ std::shared_ptr<UniformBase> uniformBase = osgUniform;
+
+ EXPECT_EQ("test name", osgUniformBase->getName());
+
+ uniform->set(value);
+
+ OsgType osgValue;
+ EXPECT_TRUE(osgUniformBase->getOsgUniform()->get(osgValue)) <<
+ "Failed to get osg::Uniform value. The Uniform type may be wrong!";
+
+ return std::make_pair(uniform->get(), osgValue);
+}
+
+template <class Type>
+std::pair<Type, boost::any> testAccessible(const Type& value)
+{
+ auto osgUniform = std::make_shared<OsgUniform<Type>>("test name");
+
+ osgUniform->setValue("Value", value);
+
+ return std::make_pair(osgUniform->get(), osgUniform->getValue("Value"));
+}
+
+/// Constructs an OsgUniform that stores a vector of values, sets it to the given vector values, and returns the result
+/// of Uniform::get() and the wrapped osg::Uniform::get().
+/// \tparam Type Uniform's value type
+/// \tparam OsgType Type stored in the osg::Uniform
+template <class Type, class OsgType>
+std::pair<std::vector<Type>, std::vector<OsgType>> testUniformElementsConstruction(
+ const std::vector<Type>& value, size_t numElements)
+{
+ std::shared_ptr<OsgUniform<std::vector<Type>>> osgUniform =
+ std::make_shared<OsgUniform<std::vector<Type>>>("test name", numElements);
+ std::shared_ptr<OsgUniformBase> osgUniformBase = osgUniform;
+ std::shared_ptr<Uniform<std::vector<Type>>> uniform = osgUniform;
+ std::shared_ptr<UniformBase> uniformBase = osgUniform;
+
+ EXPECT_EQ("test name", osgUniformBase->getName());
+
+ uniform->set(value);
+
+ std::vector<OsgType> osgValue;
+
+ for (unsigned int i = 0; i < osgUniformBase->getOsgUniform()->getNumElements(); ++i)
+ {
+ OsgType element;
+ EXPECT_TRUE(osgUniformBase->getOsgUniform()->getElement(i, element)) <<
+ "Failed to get osg::Uniform element value. The Uniform type may be wrong!";
+ osgValue.push_back(element);
+ }
+
+ return std::make_pair(uniform->get(), osgValue);
+}
+
+/// Tests OsgUniform with a random floating point type value.
+/// \tparam FloatType Floating point type (float, double, ...)
+/// \param min Minimum random value
+/// \param max Maximum random value
+template <class FloatType>
+void testUniformFloat(FloatType min, FloatType max)
+{
+ std::uniform_real_distribution<FloatType> distribution(min, max);
+ FloatType value = distribution(generator);
+ std::pair<FloatType, FloatType> result = testUniformConstruction<FloatType, FloatType>(value);
+ EXPECT_NEAR(value, result.first, Eigen::NumTraits<FloatType>::dummy_precision());
+ EXPECT_NEAR(value, result.second, Eigen::NumTraits<FloatType>::dummy_precision());
+
+ auto accessibleResult = testAccessible<FloatType>(value);
+ FloatType resultValue;
+ ASSERT_NO_THROW({boost::any_cast<FloatType>(accessibleResult.second);});
+ resultValue = boost::any_cast<FloatType>(accessibleResult.second);
+ EXPECT_NEAR(value, resultValue, Eigen::NumTraits<FloatType>::dummy_precision());
+}
+
+/// Tests OsgUniform with a vector of random floating point type values.
+/// \tparam FloatType Floating point type (float, double, ...)
+/// \param min Minimum random value
+/// \param max Maximum random value
+/// \param numElements Number of elements
+template <class FloatType>
+void testUniformElementsFloat(FloatType min, FloatType max, size_t numElements)
+{
+ std::uniform_real_distribution<FloatType> distribution(min, max);
+ std::vector<FloatType> elements;
+ for (size_t i = 0; i < numElements; ++i)
+ {
+ elements.push_back(distribution(generator));
+ }
+
+ std::pair<std::vector<FloatType>, std::vector<FloatType>> result =
+ testUniformElementsConstruction<FloatType, FloatType>(elements, numElements);
+
+ EXPECT_EQ(elements.size(), result.first.size()) << "Number of resulting float-type elements does not match input";
+ EXPECT_EQ(elements.size(), result.second.size()) << "Number of resulting OSG-type elements does not match input";
+
+ for (size_t i = 0; i < elements.size(); ++i)
+ {
+ EXPECT_NEAR(elements[i], result.first[i], Eigen::NumTraits<FloatType>::dummy_precision());
+ EXPECT_NEAR(elements[i], result.second[i], Eigen::NumTraits<FloatType>::dummy_precision());
+ }
+}
+
+/// Tests OsgUniform with a random integer type value.
+/// \tparam IntType Integer type (int, unsigned int, ...)
+/// \param min Minimum random value
+/// \param max Maximum random value
+template <class IntType>
+void testUniformInt(IntType min, IntType max)
+{
+ std::uniform_int_distribution<IntType> distribution(min, max);
+ IntType value = distribution(generator);
+ std::pair<IntType, IntType> result = testUniformConstruction<IntType, IntType>(value);
+ EXPECT_EQ(value, result.first);
+ EXPECT_EQ(value, result.second);
+
+ auto accessibleResult = testAccessible<IntType>(value);
+ IntType resultValue;
+ ASSERT_NO_THROW({resultValue = boost::any_cast<IntType>(accessibleResult.second);});
+ EXPECT_EQ(value, resultValue);
+}
+
+/// Tests OsgUniform with a vector of random integer type values.
+/// \tparam IntType Integer type (int, unsigned int, ...)
+/// \param min Minimum random value
+/// \param max Maximum random value
+/// \param numElements Number of elements
+template <class IntType>
+void testUniformElementsInt(IntType min, IntType max, size_t numElements)
+{
+ std::uniform_int_distribution<IntType> distribution(min, max);
+ std::vector<IntType> elements;
+ for (size_t i = 0; i < numElements; ++i)
+ {
+ elements.push_back(distribution(generator));
+ }
+
+ std::pair<std::vector<IntType>, std::vector<IntType>> result =
+ testUniformElementsConstruction<IntType, IntType>(elements, numElements);
+
+ EXPECT_EQ(elements.size(), result.first.size()) << "Number of resulting int-type elements does not match input";
+ EXPECT_EQ(elements.size(), result.second.size()) << "Number of resulting OSG-type elements does not match input";
+
+ for (size_t i = 0; i < elements.size(); ++i)
+ {
+ EXPECT_EQ(elements[i], result.first[i]);
+ EXPECT_EQ(elements[i], result.second[i]);
+ }
+}
+
+/// Tests OsgUniform with a random Eigen type values.
+/// \tparam Type Eigen type (Vector2f, Matrix44d, ...)
+/// \tparam OsgType OSG type which corresponds with the Eigen type (must have a fromOsg() defined for this type)
+template <class Type, class OsgType>
+void testUniformEigen()
+{
+ Type value = Type::Random();
+ std::pair<Type, OsgType> result = testUniformConstruction<Type, OsgType>(value);
+ EXPECT_TRUE(result.first.isApprox(value));
+ EXPECT_TRUE(fromOsg(result.second).isApprox(value));
+
+ auto accessibleResult = testAccessible<Type>(value);
+ Type resultValue;
+ ASSERT_NO_THROW({boost::any_cast<Type>(accessibleResult.second);});
+ resultValue = boost::any_cast<Type>(accessibleResult.second);
+ EXPECT_TRUE(value.isApprox(resultValue));
+}
+
+/// Tests OsgUniform with a vector of random Eigen type values.
+/// \tparam Type Eigen type (Vector2f, Matrix44d, ...)
+/// \tparam OsgType OSG type which corresponds with the Eigen type (must have a fromOsg() defined for this type)
+template <class Type, class OsgType>
+void testUniformElementsEigen(size_t numElements)
+{
+ std::vector<Type> elements;
+ for (size_t i = 0; i < numElements; ++i)
+ {
+ elements.push_back(Type::Random());
+ }
+
+ std::pair<std::vector<Type>, std::vector<OsgType>> result =
+ testUniformElementsConstruction<Type, OsgType>(elements, numElements);
+
+ EXPECT_EQ(elements.size(), result.first.size()) << "Number of resulting Eigen-type elements does not match input";
+ EXPECT_EQ(elements.size(), result.second.size()) << "Number of resulting OSG-type elements does not match input";
+
+ for (size_t i = 0; i < elements.size(); ++i)
+ {
+ const Type& eigenInput = elements[i];
+ const Type& eigenOutput = result.first[i];
+ const OsgType& osgOutput = result.second[i];
+
+ EXPECT_TRUE(eigenOutput.isApprox(eigenInput));
+ EXPECT_TRUE(fromOsg(osgOutput).isApprox(eigenInput));
+ }
+}
+
+TEST(OsgUniformTests, FloatTest)
+{
+ testUniformFloat<float>(-10.0f, 10.0f);
+}
+TEST(OsgUniformTests, DoubleTest)
+{
+ testUniformFloat<double>(-10.0, 10.0);
+}
+TEST(OsgUniformTests, IntTest)
+{
+ testUniformInt<int>(-10, 10);
+}
+TEST(OsgUniformTests, UnsignedIntTest)
+{
+ testUniformInt<unsigned int>(0, 10);
+}
+TEST(OsgUniformTests, BoolTest)
+{
+ {
+ std::pair<bool, bool> result = testUniformConstruction<bool, bool>(true);
+ EXPECT_TRUE(result.first);
+ EXPECT_TRUE(result.second);
+ }
+ {
+ std::pair<bool, bool> result = testUniformConstruction<bool, bool>(false);
+ EXPECT_FALSE(result.first);
+ EXPECT_FALSE(result.second);
+ }
+}
+
+TEST(OsgUniformTests, Vector2fTest)
+{
+ testUniformEigen<Vector2f, osg::Vec2f>();
+}
+TEST(OsgUniformTests, Vector3fTest)
+{
+ testUniformEigen<Vector3f, osg::Vec3f>();
+}
+TEST(OsgUniformTests, Vector4fTest)
+{
+ testUniformEigen<Vector4f, osg::Vec4f>();
+}
+
+TEST(OsgUniformTests, Vector2dTest)
+{
+ testUniformEigen<Vector2d, osg::Vec2d>();
+}
+TEST(OsgUniformTests, Vector3dTest)
+{
+ testUniformEigen<Vector3d, osg::Vec3d>();
+}
+TEST(OsgUniformTests, Vector4dTest)
+{
+ testUniformEigen<Vector4d, osg::Vec4d>();
+}
+
+TEST(OsgUniformTests, Matrix22fTest)
+{
+ testUniformEigen<Matrix22f, osg::Matrix2>();
+}
+TEST(OsgUniformTests, Matrix33fTest)
+{
+ testUniformEigen<Matrix33f, osg::Matrix3>();
+}
+TEST(OsgUniformTests, Matrix44fTest)
+{
+ testUniformEigen<Matrix44f, osg::Matrixf>();
+}
+
+TEST(OsgUniformTests, Matrix22dTest)
+{
+ testUniformEigen<Matrix22d, osg::Matrix2d>();
+}
+TEST(OsgUniformTests, Matrix33dTest)
+{
+ testUniformEigen<Matrix33d, osg::Matrix3d>();
+}
+TEST(OsgUniformTests, Matrix44dTest)
+{
+ testUniformEigen<Matrix44d, osg::Matrixd>();
+}
+
+TEST(OsgUniformTests, FloatElementsTest)
+{
+ testUniformElementsFloat<float>(-10.0f, 10.0f, 10);
+}
+TEST(OsgUniformTests, DoubleElementsTest)
+{
+ testUniformElementsFloat<double>(-10.0, 10.0, 10);
+}
+TEST(OsgUniformTests, IntElementsTest)
+{
+ testUniformElementsInt<int>(-10, 10, 10);
+}
+TEST(OsgUniformTests, UnsignedIntElementsTest)
+{
+ testUniformElementsInt<unsigned int>(0, 10, 10);
+}
+TEST(OsgUniformTests, BoolElementsTest)
+{
+ std::vector<bool> elements;
+ for (size_t i = 0; i < 10; ++i)
+ {
+ elements.push_back(i % 2 == 0);
+ }
+
+ std::pair<std::vector<bool>, std::vector<bool>> result =
+ testUniformElementsConstruction<bool, bool>(elements, 10);
+
+ EXPECT_EQ(elements.size(), result.first.size()) << "Number of resulting bool-type elements does not match input";
+ EXPECT_EQ(elements.size(), result.second.size()) << "Number of resulting OSG-type elements does not match input";
+
+ for (size_t i = 0; i < elements.size(); ++i)
+ {
+ EXPECT_EQ(elements[i], result.first[i]);
+ EXPECT_EQ(elements[i], result.second[i]);
+ }
+}
+
+TEST(OsgUniformTests, Vector2fElementsTest)
+{
+ testUniformElementsEigen<Vector2f, osg::Vec2f>(10);
+}
+TEST(OsgUniformTests, Vector3fElementsTest)
+{
+ testUniformElementsEigen<Vector3f, osg::Vec3f>(10);
+}
+TEST(OsgUniformTests, Vector4fElementsTest)
+{
+ testUniformElementsEigen<Vector4f, osg::Vec4f>(10);
+}
+
+TEST(OsgUniformTests, Vector2dElementsTest)
+{
+ testUniformElementsEigen<Vector2d, osg::Vec2d>(10);
+}
+TEST(OsgUniformTests, Vector3dElementsTest)
+{
+ testUniformElementsEigen<Vector3d, osg::Vec3d>(10);
+}
+TEST(OsgUniformTests, Vector4dElementsTest)
+{
+ testUniformElementsEigen<Vector4d, osg::Vec4d>(10);
+}
+
+TEST(OsgUniformTests, Matrix22fElementsTest)
+{
+ testUniformElementsEigen<Matrix22f, osg::Matrix2>(10);
+}
+TEST(OsgUniformTests, Matrix33fElementsTest)
+{
+ testUniformElementsEigen<Matrix33f, osg::Matrix3>(10);
+}
+TEST(OsgUniformTests, Matrix44fElementsTest)
+{
+ testUniformElementsEigen<Matrix44f, osg::Matrixf>(10);
+}
+
+TEST(OsgUniformTests, Matrix22dElementsTest)
+{
+ testUniformElementsEigen<Matrix22d, osg::Matrix2d>(10);
+}
+TEST(OsgUniformTests, Matrix33dElementsTest)
+{
+ testUniformElementsEigen<Matrix33d, osg::Matrix3d>(10);
+}
+TEST(OsgUniformTests, Matrix44dElementsTest)
+{
+ testUniformElementsEigen<Matrix44d, osg::Matrixd>(10);
+}
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgUniformTypesTests.cpp b/SurgSim/Graphics/UnitTests/OsgUniformTypesTests.cpp
new file mode 100644
index 0000000..e290006
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgUniformTypesTests.cpp
@@ -0,0 +1,104 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgUniformTypes functions to make sure they return the correct enum values.
+
+#include "SurgSim/Graphics/OsgUniformTypes.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgUniformTypesTests, FloatTest)
+{
+ EXPECT_EQ(osg::Uniform::FLOAT, getOsgUniformType<float>());
+}
+TEST(OsgUniformTypesTests, DoubleTest)
+{
+ EXPECT_EQ(osg::Uniform::DOUBLE, getOsgUniformType<double>());
+}
+TEST(OsgUniformTypesTests, IntTest)
+{
+ EXPECT_EQ(osg::Uniform::INT, getOsgUniformType<int>());
+}
+TEST(OsgUniformTypesTests, UnsignedIntTest)
+{
+ EXPECT_EQ(osg::Uniform::UNSIGNED_INT, getOsgUniformType<unsigned int>());
+}
+TEST(OsgUniformTypesTests, BoolTest)
+{
+ EXPECT_EQ(osg::Uniform::BOOL, getOsgUniformType<bool>());
+}
+
+TEST(OsgUniformTypesTests, Vector2fTest)
+{
+ EXPECT_EQ(osg::Uniform::FLOAT_VEC2, getOsgUniformType<SurgSim::Math::Vector2f>());
+}
+TEST(OsgUniformTypesTests, Vector3fTest)
+{
+ EXPECT_EQ(osg::Uniform::FLOAT_VEC3, getOsgUniformType<SurgSim::Math::Vector3f>());
+}
+TEST(OsgUniformTypesTests, Vector4fTest)
+{
+ EXPECT_EQ(osg::Uniform::FLOAT_VEC4, getOsgUniformType<SurgSim::Math::Vector4f>());
+}
+
+TEST(OsgUniformTypesTests, Vector2dTest)
+{
+ EXPECT_EQ(osg::Uniform::DOUBLE_VEC2, getOsgUniformType<SurgSim::Math::Vector2d>());
+}
+TEST(OsgUniformTypesTests, Vector3dTest)
+{
+ EXPECT_EQ(osg::Uniform::DOUBLE_VEC3, getOsgUniformType<SurgSim::Math::Vector3d>());
+}
+TEST(OsgUniformTypesTests, Vector4dTest)
+{
+ EXPECT_EQ(osg::Uniform::DOUBLE_VEC4, getOsgUniformType<SurgSim::Math::Vector4d>());
+}
+
+TEST(OsgUniformTypesTests, Matrix22fTest)
+{
+ EXPECT_EQ(osg::Uniform::FLOAT_MAT2, getOsgUniformType<SurgSim::Math::Matrix22f>());
+}
+TEST(OsgUniformTypesTests, Matrix33fTest)
+{
+ EXPECT_EQ(osg::Uniform::FLOAT_MAT3, getOsgUniformType<SurgSim::Math::Matrix33f>());
+}
+TEST(OsgUniformTypesTests, Matrix44fTest)
+{
+ EXPECT_EQ(osg::Uniform::FLOAT_MAT4, getOsgUniformType<SurgSim::Math::Matrix44f>());
+}
+
+TEST(OsgUniformTypesTests, Matrix22dTest)
+{
+ EXPECT_EQ(osg::Uniform::DOUBLE_MAT2, getOsgUniformType<SurgSim::Math::Matrix22d>());
+}
+TEST(OsgUniformTypesTests, Matrix33dTest)
+{
+ EXPECT_EQ(osg::Uniform::DOUBLE_MAT3, getOsgUniformType<SurgSim::Math::Matrix33d>());
+}
+TEST(OsgUniformTypesTests, Matrix44dTest)
+{
+ EXPECT_EQ(osg::Uniform::DOUBLE_MAT4, getOsgUniformType<SurgSim::Math::Matrix44d>());
+}
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgUnitSphereTests.cpp b/SurgSim/Graphics/UnitTests/OsgUnitSphereTests.cpp
new file mode 100644
index 0000000..2a7abbf
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgUnitSphereTests.cpp
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgUnitSphere class.
+
+#include "SurgSim/Graphics/OsgUnitSphere.h"
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST(OsgUnitSphereTests, InitTest)
+{
+ ASSERT_NO_THROW({OsgUnitSphere sphere;});
+
+ OsgUnitSphere sphere;
+
+ osg::ref_ptr<osg::Node> node = sphere.getNode();
+ EXPECT_NE(nullptr, node.get());
+
+ osg::ref_ptr<osg::PositionAttitudeTransform> transform = dynamic_cast<osg::PositionAttitudeTransform*>(node.get());
+ EXPECT_NE(nullptr, transform.get());
+
+ osg::Quat rotation;
+ rotation.makeRotate(osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(0.0, 1.0, 0.0));
+ EXPECT_EQ(rotation, transform->getAttitude());
+ EXPECT_EQ(osg::Vec3(0.0, 0.0, 0.0), transform->getPosition());
+ EXPECT_EQ(osg::Vec3(1.0, 1.0, 1.0), transform->getScale());
+
+ EXPECT_EQ(1u, transform->getNumChildren());
+
+ osg::ref_ptr<osg::Geode> geode = dynamic_cast<osg::Geode*>(transform->getChild(0u));
+ EXPECT_NE(nullptr, geode.get());
+
+ EXPECT_EQ(1u, geode->getNumDrawables());
+
+ osg::ref_ptr<osg::Drawable> drawable = geode->getDrawable(0);
+ EXPECT_NE(nullptr, drawable.get());
+
+ osg::ref_ptr<osg::ShapeDrawable> shapeDrawable = dynamic_cast<osg::ShapeDrawable*>(drawable.get());
+ EXPECT_NE(nullptr, shapeDrawable.get());
+
+ osg::ref_ptr<osg::Shape> shape = shapeDrawable->getShape();
+ EXPECT_NE(nullptr, shape.get());
+
+ osg::ref_ptr<osg::Sphere> sphereShape = dynamic_cast<osg::Sphere*>(shape.get());
+ EXPECT_NE(nullptr, sphereShape.get());
+
+ /// The sphere should be at (0, 0, 0) with radius 1
+ EXPECT_EQ(0.0f, sphereShape->getCenter().x());
+ EXPECT_EQ(0.0f, sphereShape->getCenter().y());
+ EXPECT_EQ(0.0f, sphereShape->getCenter().z());
+ EXPECT_EQ(1.0f, sphereShape->getRadius());
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgVectorConversionsTests.cpp b/SurgSim/Graphics/UnitTests/OsgVectorConversionsTests.cpp
new file mode 100644
index 0000000..03e4800
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgVectorConversionsTests.cpp
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 conversions to and from OSG vector types
+
+#include "SurgSim/Graphics/OsgVectorConversions.h"
+
+#include <gtest/gtest.h>
+
+using SurgSim::Graphics::fromOsg;
+using SurgSim::Graphics::toOsg;
+using SurgSim::Math::Vector2f;
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3f;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4f;
+using SurgSim::Math::Vector4d;
+
+TEST(OsgVectorConversionsTests, Vector2fTest)
+{
+ Vector2f vector = Vector2f::Random();
+ osg::Vec2f osgVector = toOsg(vector);
+ EXPECT_TRUE(vector.isApprox(fromOsg(osgVector)));
+}
+
+TEST(OsgVectorConversionsTests, Vector2dTest)
+{
+ Vector2d vector = Vector2d::Random();
+ osg::Vec2d osgVector = toOsg(vector);
+ EXPECT_TRUE(vector.isApprox(fromOsg(osgVector)));
+}
+
+TEST(OsgVectorConversionsTests, Vector3fTest)
+{
+ Vector3f vector = Vector3f::Random();
+ osg::Vec3f osgVector = toOsg(vector);
+ EXPECT_TRUE(vector.isApprox(fromOsg(osgVector)));
+}
+
+TEST(OsgVectorConversionsTests, Vector3dTest)
+{
+ Vector3d vector = Vector3d::Random();
+ osg::Vec3d osgVector = toOsg(vector);
+ EXPECT_TRUE(vector.isApprox(fromOsg(osgVector)));
+}
+
+TEST(OsgVectorConversionsTests, Vector4fTest)
+{
+ Vector4f vector = Vector4f::Random();
+ osg::Vec4f osgVector = toOsg(vector);
+ EXPECT_TRUE(vector.isApprox(fromOsg(osgVector)));
+}
+
+TEST(OsgVectorConversionsTests, Vector4dTest)
+{
+ Vector4d vector = Vector4d::Random();
+ osg::Vec4d osgVector = toOsg(vector);
+ EXPECT_TRUE(vector.isApprox(fromOsg(osgVector)));
+}
diff --git a/SurgSim/Graphics/UnitTests/OsgVectorFieldRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgVectorFieldRepresentationTests.cpp
new file mode 100644
index 0000000..0a05e9f
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgVectorFieldRepresentationTests.cpp
@@ -0,0 +1,63 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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
+/// Unit Tests for the OsgVectorFieldRepresentation class.
+
+#include "SurgSim/Graphics/VectorFieldRepresentation.h"
+#include "SurgSim/Graphics/OsgVectorFieldRepresentation.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+using SurgSim::Graphics::OsgVectorFieldRepresentation;
+using SurgSim::Graphics::VectorFieldRepresentation;
+
+TEST(OsgVectorFieldRepresentationTests, VerticesTest)
+{
+ std::shared_ptr<VectorFieldRepresentation> vectorFieldRepresentation =
+ std::make_shared<OsgVectorFieldRepresentation>("Vector Field");
+
+ EXPECT_EQ(0u, vectorFieldRepresentation->getVectorField()->getNumVertices());
+}
+
+TEST(OsgVectorFieldRepresentationTests, LineWidthTest)
+{
+ std::shared_ptr<VectorFieldRepresentation> vectorFieldRepresentation =
+ std::make_shared<OsgVectorFieldRepresentation>("Vector Field");
+ vectorFieldRepresentation->setLineWidth(1.25);
+ EXPECT_NEAR(1.25, vectorFieldRepresentation->getLineWidth(), epsilon);
+}
+
+TEST(OsgVectorFieldRepresentationTests, ScaleTest)
+{
+ std::shared_ptr<VectorFieldRepresentation> vectorFieldRepresentation =
+ std::make_shared<OsgVectorFieldRepresentation>("Vector Field");
+ vectorFieldRepresentation->setScale(1.25);
+ EXPECT_NEAR(1.25, vectorFieldRepresentation->getScale(), epsilon);
+}
+
+TEST(OsgVectorFieldRepresentationTests, PointSizeTest)
+{
+ std::shared_ptr<VectorFieldRepresentation> vectorFieldRepresentation =
+ std::make_shared<OsgVectorFieldRepresentation>("Vector Field");
+ vectorFieldRepresentation->setPointSize(1.25);
+ EXPECT_NEAR(1.25, vectorFieldRepresentation->getPointSize(), epsilon);
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgViewElementTests.cpp b/SurgSim/Graphics/UnitTests/OsgViewElementTests.cpp
new file mode 100644
index 0000000..40ec5f3
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgViewElementTests.cpp
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgViewElement class.
+
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgViewElementTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<ViewElement> viewElement = std::make_shared<OsgViewElement>("test name");});
+
+ std::shared_ptr<ViewElement> viewElement = std::make_shared<OsgViewElement>("test name");
+
+ std::shared_ptr<OsgView> osgView = std::dynamic_pointer_cast<OsgView>(viewElement->getView());
+ EXPECT_NE(nullptr, osgView);
+}
+
+TEST(OsgViewElementTests, ViewTest)
+{
+ std::shared_ptr<ViewElement> element = std::make_shared<OsgViewElement>("test name");
+
+ /// Setting an OsgView should succeed
+ std::shared_ptr<View> osgView = std::make_shared<OsgView>("test osg view");
+ EXPECT_TRUE(element->setView(osgView));
+ EXPECT_EQ(osgView, element->getView());
+
+ /// Any other View should fail
+ std::shared_ptr<View> mockView = std::make_shared<MockView>("test mock view");
+
+ EXPECT_FALSE(element->setView(mockView));
+ EXPECT_NE(mockView, element->getView());
+ EXPECT_EQ(osgView, element->getView());
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgViewTests.cpp b/SurgSim/Graphics/UnitTests/OsgViewTests.cpp
new file mode 100644
index 0000000..4e25191
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgViewTests.cpp
@@ -0,0 +1,229 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OsgView class.
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgView.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgViewTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<View> view = std::make_shared<OsgView>("test name");});
+
+ std::shared_ptr<View> view = std::make_shared<OsgView>("test name");
+
+ EXPECT_EQ("test name", view->getName());
+
+ EXPECT_EQ(nullptr, view->getCamera());
+
+ std::array<int, 2> position = view->getPosition();
+ EXPECT_EQ(0, position[0]);
+ EXPECT_EQ(0, position[1]);
+
+ std::array<int, 2> dimensions = view->getDimensions();
+ EXPECT_EQ(1024, dimensions[0]);
+ EXPECT_EQ(768, dimensions[1]);
+
+ EXPECT_TRUE(view->isWindowBorderEnabled());
+}
+
+TEST(OsgViewTests, PositionAndDimensionsTest)
+{
+ std::shared_ptr<OsgView> osgView = std::make_shared<OsgView>("test name");
+ std::shared_ptr<View> view = osgView;
+
+ std::default_random_engine generator;
+ std::uniform_int_distribution<int> distribution(0, 1000);
+
+ std::array<int, 2> position = {distribution(generator), distribution(generator)};
+ std::array<int, 2> dimensions = {distribution(generator), distribution(generator)};
+
+ /// Set position and check that it set correctly
+ view->setPosition(position);
+
+ auto test = view->getPosition();
+
+ EXPECT_EQ(position, test);
+
+ /// Set dimensions and check that it set correctly
+ view->setDimensions(dimensions);
+
+ test = view->getDimensions();
+
+ EXPECT_EQ(dimensions, test);
+
+ /// The window border should be enabled initially
+ EXPECT_TRUE(view->isWindowBorderEnabled());
+ /// Disable the window border
+ view->setWindowBorderEnabled(false);
+ EXPECT_FALSE(view->isWindowBorderEnabled());
+}
+
+TEST(OsgViewTests, CameraTest)
+{
+ std::shared_ptr<View> view = std::make_shared<OsgView>("test name");
+
+ std::shared_ptr<Camera> camera = std::make_shared<OsgCamera>("test camera");
+
+ /// Set the camera and check that it set correctly
+ EXPECT_NO_THROW(view->setCamera(camera));
+ EXPECT_EQ(camera, view->getCamera());
+
+ std::shared_ptr<Camera> mockCamera = std::make_shared<MockCamera>("non-osg camera");
+
+ /// Try to set a camera that does not derive from OsgCamera
+ EXPECT_ANY_THROW(view->setCamera(mockCamera));
+ EXPECT_EQ(camera, view->getCamera());
+}
+
+typedef std::array<int, 2> CoordinateType;
+using SurgSim::Math::Vector3d;
+
+void expectEqual(std::shared_ptr<OsgView> expected, std::shared_ptr<OsgView> actual)
+{
+ if (expected->getCamera() == nullptr)
+ {
+ EXPECT_EQ(nullptr, actual->getCamera());
+ }
+ else
+ {
+ EXPECT_NE(nullptr, actual->getCamera());
+ EXPECT_EQ(boost::any_cast<std::shared_ptr<Camera>>(expected->getValue("Camera"))->getName(),
+ boost::any_cast<std::shared_ptr<Camera>>(actual->getValue("Camera"))->getName());
+ }
+ EXPECT_EQ(boost::any_cast<CoordinateType>(expected->getValue("Position")),
+ boost::any_cast<CoordinateType>(actual->getValue("Position")));
+ EXPECT_EQ(boost::any_cast<CoordinateType>(expected->getValue("Dimensions")),
+ boost::any_cast<CoordinateType>(actual->getValue("Dimensions")));
+ EXPECT_EQ(boost::any_cast<bool>(expected->getValue("WindowBorder")),
+ boost::any_cast<bool>(actual->getValue("WindowBorder")));
+ EXPECT_EQ(boost::any_cast<int>(expected->getValue("StereoMode")),
+ boost::any_cast<int>(actual->getValue("StereoMode")));
+ EXPECT_EQ(boost::any_cast<int>(expected->getValue("DisplayType")),
+ boost::any_cast<int>(actual->getValue("DisplayType")));
+ EXPECT_EQ(boost::any_cast<bool>(expected->getValue("FullScreen")),
+ boost::any_cast<bool>(actual->getValue("FullScreen")));
+ EXPECT_EQ(boost::any_cast<int>(expected->getValue("TargetScreen")),
+ boost::any_cast<int>(actual->getValue("TargetScreen")));
+ EXPECT_EQ(boost::any_cast<double>(expected->getValue("EyeSeparation")),
+ boost::any_cast<double>(actual->getValue("EyeSeparation")));
+ EXPECT_EQ(boost::any_cast<double>(expected->getValue("ScreenDistance")),
+ boost::any_cast<double>(actual->getValue("ScreenDistance")));
+ EXPECT_EQ(boost::any_cast<double>(expected->getValue("ScreenWidth")),
+ boost::any_cast<double>(actual->getValue("ScreenWidth")));
+ EXPECT_EQ(boost::any_cast<double>(expected->getValue("ScreenHeight")),
+ boost::any_cast<double>(actual->getValue("ScreenHeight")));
+ EXPECT_EQ(boost::any_cast<bool>(expected->getValue("CameraManipulatorEnabled")),
+ boost::any_cast<bool>(actual->getValue("CameraManipulatorEnabled")));
+ EXPECT_TRUE(boost::any_cast<Vector3d>(expected->getValue("CameraPosition")).isApprox(
+ boost::any_cast<Vector3d>(actual->getValue("CameraPosition"))));
+ EXPECT_TRUE(boost::any_cast<Vector3d>(expected->getValue("CameraLookAt")).isApprox(
+ boost::any_cast<Vector3d>(actual->getValue("CameraLookAt"))));
+ EXPECT_EQ(boost::any_cast<bool>(expected->getValue("OsgMapUniforms")),
+ boost::any_cast<bool>(actual->getValue("OsgMapUniforms")));
+ EXPECT_EQ(boost::any_cast<bool>(expected->getValue("KeyboardDeviceEnabled")),
+ boost::any_cast<bool>(actual->getValue("KeyboardDeviceEnabled")));
+ EXPECT_EQ(boost::any_cast<bool>(expected->getValue("MouseDeviceEnabled")),
+ boost::any_cast<bool>(actual->getValue("MouseDeviceEnabled")));
+}
+
+TEST(OsgViewTests, Serialization)
+{
+ {
+ SCOPED_TRACE("Serialize with default values");
+
+ std::shared_ptr<OsgView> view = std::make_shared<OsgView>("test name");
+
+ /// Serialize
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*view););
+
+ /// Deserialize
+ std::shared_ptr<OsgView> newView;
+ EXPECT_NO_THROW(newView = std::dynamic_pointer_cast<OsgView>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ EXPECT_NE(nullptr, newView);
+
+ // Verify
+ expectEqual(view, newView);
+ }
+
+ {
+ SCOPED_TRACE("Serialize with user-defined values");
+
+ std::shared_ptr<OsgView> view = std::make_shared<OsgView>("test name");
+ std::shared_ptr<OsgCamera> camera = std::make_shared<OsgCamera>("test camera");
+
+ CoordinateType position;
+ position[0] = 100;
+ position[1] = 100;
+ view->setValue("Position", position);
+ CoordinateType dimensions;
+ dimensions[0] = 200;
+ dimensions[1] = 200;
+ view->setValue("Dimensions", dimensions);
+ view->setValue("WindowBorder", true);
+ view->setValue("Camera", std::dynamic_pointer_cast<SurgSim::Framework::Component>(camera));
+ view->setValue("StereoMode", static_cast<int>(SurgSim::Graphics::View::STEREO_MODE_QUAD_BUFFER));
+ view->setValue("DisplayType", static_cast<int>(SurgSim::Graphics::View::DISPLAY_TYPE_HMD));
+ view->setValue("FullScreen", true);
+ view->setValue("TargetScreen", 3);
+ view->setValue("EyeSeparation", 0.123);
+ view->setValue("ScreenDistance", 2.123);
+ view->setValue("ScreenWidth", 1.1);
+ view->setValue("ScreenHeight", 1.1);
+ view->setValue("CameraManipulatorEnabled", true);
+ view->setValue("CameraPosition", Vector3d(1.5, 1.5, 1.5));
+ view->setValue("CameraLookAt", Vector3d(10.5, 10.5, 10.5));
+ view->setValue("OsgMapUniforms", true);
+ view->setValue("KeyboardDeviceEnabled", true);
+ view->setValue("MouseDeviceEnabled", true);
+
+ /// Serialize
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*view););
+
+ /// Deserialize
+ std::shared_ptr<OsgView> newView;
+ EXPECT_NO_THROW(newView = std::dynamic_pointer_cast<OsgView>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ EXPECT_NE(nullptr, newView);
+
+ // Verify
+ expectEqual(view, newView);
+ YAML::Node cameraNode;
+ EXPECT_NO_THROW(cameraNode = YAML::convert<SurgSim::Framework::Component>::encode(*camera););
+ EXPECT_EQ(cameraNode[camera->getClassName()]["Id"].as<std::string>(),
+ node[view->getClassName()]["Camera"][camera->getClassName()]["Id"].as<std::string>());
+ }
+}
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/RenderPassTests.cpp b/SurgSim/Graphics/UnitTests/RenderPassTests.cpp
new file mode 100644
index 0000000..ed31cdd
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/RenderPassTests.cpp
@@ -0,0 +1,59 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+#include "SurgSim/Graphics/RenderPass.h"
+
+#include "SurgSim/Graphics/Group.h"
+#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(RenderPassTests, InitTest)
+{
+ std::shared_ptr<RenderPass> renderPass;
+
+ ASSERT_NO_THROW({renderPass = std::make_shared<RenderPass>("testpass");});
+
+ EXPECT_NE(nullptr, renderPass->getCamera());
+ EXPECT_NE(nullptr, renderPass->getMaterial());
+ EXPECT_EQ(renderPass->getCamera()->getMaterial(), renderPass->getMaterial());
+ EXPECT_EQ(renderPass->getCamera()->getRenderGroupReference(), renderPass->getName());
+
+}
+
+TEST(RenderPassTests, SettersTest)
+{
+ auto renderPass = std::make_shared<RenderPass>("testpass");
+
+ auto renderTarget = std::make_shared<OsgRenderTarget2d>(1024, 1024, 1.0, 1, true);
+
+ ASSERT_NO_THROW({renderPass->setRenderTarget(renderTarget);});
+ EXPECT_EQ(renderTarget, renderPass->getCamera()->getRenderTarget());
+
+ auto material = std::make_shared<OsgMaterial>("material");
+
+ ASSERT_NO_THROW({renderPass->setMaterial(material);});
+ EXPECT_EQ(material, renderPass->getMaterial());
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/ViewElementTests.cpp b/SurgSim/Graphics/UnitTests/ViewElementTests.cpp
new file mode 100644
index 0000000..172dbf6
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/ViewElementTests.cpp
@@ -0,0 +1,147 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 ViewElement class.
+
+#include "SurgSim/Graphics/ViewElement.h"
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+
+#include "SurgSim/Input/CommonDevice.h"
+
+
+#include <gtest/gtest.h>
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Scene;
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Concrete ViewElement subclass for testing
+class MockViewElement : public ViewElement
+{
+public:
+ explicit MockViewElement(const std::string& name) : ViewElement(name), m_isInitialized(false)
+ {
+ setView(std::make_shared<MockView>(name + " View"));
+ setCamera(std::make_shared<MockCamera>(name + " Camera"));
+ getCamera()->setRenderGroupReference("Test");
+ }
+
+ virtual std::shared_ptr<SurgSim::Input::CommonDevice> getKeyboardDevice() override
+ {
+ return nullptr;
+ }
+
+ virtual void enableKeyboardDevice(bool val) override
+ {
+ return;
+ }
+
+ virtual std::shared_ptr<SurgSim::Input::CommonDevice> getMouseDevice() override
+ {
+ return nullptr;
+ }
+
+ virtual void enableMouseDevice(bool val) override
+ {
+ return;
+ }
+
+private:
+ /// Initialize the view element
+ /// \post m_isInitialized is set to true
+ virtual bool doInitialize()
+ {
+ if (ViewElement::doInitialize())
+ {
+ m_isInitialized = true;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+ /// Whether the view has been initialized
+ bool m_isInitialized;
+};
+
+/// View class for testing adding a non-MockView
+class NotMockView : public View
+{
+public:
+ /// Constructor
+ /// \param name Name of the view
+ explicit NotMockView(const std::string& name) : View(name)
+ {
+ return;
+ }
+
+};
+
+TEST(ViewElementTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<ViewElement> element = std::make_shared<MockViewElement>("test name");});
+}
+
+TEST(ViewElementTests, StartUpTest)
+{
+ auto runtime = std::make_shared<Runtime>();
+ auto manager = std::make_shared<MockManager>();
+
+ runtime->addManager(manager);
+ EXPECT_EQ(0, manager->getNumUpdates());
+ EXPECT_EQ(0.0, manager->getSumDt());
+
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ /// Add a graphics component to the scene
+ auto viewElement = std::make_shared<MockViewElement>("Testing MockViewElement");
+ scene->addSceneElement(viewElement);
+
+ /// Run the thread for a moment
+ runtime->start();
+ EXPECT_TRUE(manager->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ runtime->stop();
+
+ /// Check that the view element was initialized and awoken
+ EXPECT_TRUE(viewElement->isInitialized());
+ EXPECT_TRUE(viewElement->getView()->isInitialized());
+ EXPECT_TRUE(viewElement->getView()->isAwake());
+}
+
+TEST(ViewElementTests, ViewTest)
+{
+ std::shared_ptr<ViewElement> element = std::make_shared<MockViewElement>("Testing MockViewElement");
+
+ /// Setting a MockView should succeed
+ std::shared_ptr<View> mockView = std::make_shared<MockView>("Testing MockView");
+ EXPECT_TRUE(element->setView(mockView));
+ EXPECT_EQ(mockView, element->getView());
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/ViewTests.cpp b/SurgSim/Graphics/UnitTests/ViewTests.cpp
new file mode 100644
index 0000000..d35135f
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/ViewTests.cpp
@@ -0,0 +1,113 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 View class.
+
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST(ViewTests, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<View> view = std::make_shared<MockView>("test name");});
+}
+
+TEST(ViewTests, NameTest)
+{
+ std::shared_ptr<View> view = std::make_shared<MockView>("test name");
+
+ EXPECT_EQ("test name", view->getName());
+}
+
+TEST(ViewTests, WindowSettingsTests)
+{
+ std::shared_ptr<View> view = std::make_shared<MockView>("test name");
+
+ std::default_random_engine generator;
+ std::uniform_int_distribution<int> distribution(0, 1000);
+
+ std::array<int, 2> position = {distribution(generator), distribution(generator)};
+ std::array<int, 2> dimensions = {distribution(generator), distribution(generator)};
+
+ /// Set position and check that it set correctly
+ view->setPosition(position);
+
+ auto test = view->getPosition();
+
+ EXPECT_EQ(position, test);
+
+ /// Set dimensions and check that it set correctly
+ view->setDimensions(dimensions);
+
+ test = view->getDimensions();
+
+ EXPECT_EQ(dimensions, test);
+
+ /// The window border should be enabled initially
+ EXPECT_TRUE(view->isWindowBorderEnabled());
+ /// Disable the window border
+ view->setWindowBorderEnabled(false);
+ EXPECT_FALSE(view->isWindowBorderEnabled());
+}
+
+TEST(ViewTests, CameraTest)
+{
+ std::shared_ptr<View> view = std::make_shared<MockView>("test name");
+
+ std::shared_ptr<Camera> camera = std::make_shared<OsgCamera>("test camera");
+
+ /// Set the camera and check that it set correctly
+ EXPECT_NO_THROW(view->setCamera(camera));
+
+ EXPECT_EQ(camera, view->getCamera());
+}
+
+TEST(ViewTests, UpdateTest)
+{
+ std::shared_ptr<MockView> mockView = std::make_shared<MockView>("test name");
+ std::shared_ptr<View> view = mockView;
+
+ EXPECT_EQ(0, mockView->getNumUpdates());
+ EXPECT_EQ(0.0, mockView->getSumDt());
+
+ double sumDt = 0.0;
+ std::default_random_engine generator;
+ std::uniform_real_distribution<double> distribution(0.0, 1.0);
+
+ /// Do 10 updates with random dt and check each time that the number of updates and sum of dt are correct.
+ for (int i = 1; i <= 10; ++i)
+ {
+ double dt = distribution(generator);
+ sumDt += dt;
+
+ view->update(dt);
+ EXPECT_EQ(i, mockView->getNumUpdates());
+ EXPECT_LT(fabs(sumDt - mockView->getSumDt()), Eigen::NumTraits<double>::dummy_precision());
+ }
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/config.txt.in b/SurgSim/Graphics/UnitTests/config.txt.in
new file mode 100644
index 0000000..f614965
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/config.txt.in
@@ -0,0 +1,3 @@
+${SURGSIM_SOURCE_DIR}/Data/
+${SURGSIM_SOURCE_DIR}/SurgSim/Testing/
+${CMAKE_CURRENT_SOURCE_DIR}/Data/
\ No newline at end of file
diff --git a/SurgSim/Graphics/VectorField.h b/SurgSim/Graphics/VectorField.h
new file mode 100644
index 0000000..1379b0a
--- /dev/null
+++ b/SurgSim/Graphics/VectorField.h
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_VECTORFIELD_H
+#define SURGSIM_GRAPHICS_VECTORFIELD_H
+
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// A (mathematical) vector is represented as (X,Y,Z) associated with an optional color (R,G,B,alpha) information
+struct VectorFieldData
+{
+ /// Direction (X,Y,Z) of the vector
+ SurgSim::Math::Vector3d direction;
+ /// Color (R,G,B,alpha) of the vector (Optional)
+ SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector4d> color;
+
+ /// Compare the vectors and return true if equal; Othwise, false.
+ /// \return True if vector1 and rhs have the same value; Otherwise, false.
+ bool operator==(const VectorFieldData& rhs) const
+ {
+ if (color.hasValue() && rhs.color.hasValue())
+ {
+ return direction == rhs.direction &&
+ color.getValue() == rhs.color.getValue();
+ }
+ else
+ {
+ return direction == rhs.direction;
+ }
+ }
+
+ /// Compare the vectors and return true if not equal, false if equal.
+ /// \return True if vector1 and rhs have different values; Otherwise, false.
+ bool operator!=(const VectorFieldData& rhs) const
+ {
+ return ! (*this == rhs);
+ }
+};
+
+typedef SurgSim::DataStructures::Vertices<VectorFieldData> VectorField;
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_VECTORFIELD_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/VectorFieldRepresentation.h b/SurgSim/Graphics/VectorFieldRepresentation.h
new file mode 100644
index 0000000..4afd5e4
--- /dev/null
+++ b/SurgSim/Graphics/VectorFieldRepresentation.h
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_VECTORFIELDREPRESENTATION_H
+#define SURGSIM_GRAPHICS_VECTORFIELDREPRESENTATION_H
+
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Graphics/VectorField.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Graphic representation of a vector field
+/// Each point/location, i.e. (X,Y,Z), in the vector field is associated with a vector and an optional color
+class VectorFieldRepresentation : public virtual Representation
+{
+public:
+ /// Constructor
+ /// \param name Name of VectorFieldRepresentation
+ explicit VectorFieldRepresentation(const std::string& name) : Representation(name)
+ {
+ }
+
+ /// Destructor
+ virtual ~VectorFieldRepresentation()
+ {
+ }
+
+ /// Gets the vector field
+ /// \return The vector field
+ virtual std::shared_ptr< SurgSim::Graphics::VectorField > getVectorField() const = 0;
+
+ /// Sets vector line width
+ /// \param width Width of vector line
+ virtual void setLineWidth(double width) = 0;
+ /// Gets line width
+ /// \return The line width
+ virtual double getLineWidth() const = 0;
+
+ /// Sets the scale to be applied to all vectors
+ /// \param scale The scale
+ virtual void setScale(double scale) = 0;
+ /// Gets the scale applied to all vectors
+ /// \return The scale
+ virtual double getScale() const = 0;
+
+ /// Sets the size of point indicating the starting of vector
+ /// \param size Size of starting point of a vector
+ virtual void setPointSize(double size) = 0;
+ /// Gets the size of starting point of a vector
+ /// \return The size of starting point of a vector
+ virtual double getPointSize() const = 0;
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif // SURGSIM_GRAPHICS_VECTORFIELDREPRESENTATION_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/View.cpp b/SurgSim/Graphics/View.cpp
new file mode 100644
index 0000000..192bf8e
--- /dev/null
+++ b/SurgSim/Graphics/View.cpp
@@ -0,0 +1,173 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/View.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Graphics/Camera.h"
+
+using SurgSim::Framework::Component;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+View::View(const std::string& name) :
+ SurgSim::Framework::Component(name),
+ m_stereoMode(STEREO_MODE_NONE),
+ m_displayType(DISPLAY_TYPE_MONITOR),
+ m_targetScreen(0),
+ m_isFullscreen(false),
+ m_eyeSeparation(0.06),
+ m_screenDistance(1.0),
+ m_screenWidth(0.0),
+ m_screenHeight(0.0)
+{
+ typedef std::array<int, 2> CoordinateType;
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, CoordinateType, Position, getPosition, setPosition);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, CoordinateType, Dimensions, getDimensions, setDimensions);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, std::shared_ptr<SurgSim::Framework::Component>, Camera,
+ getCamera, setCamera);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, bool, WindowBorder, isWindowBorderEnabled, setWindowBorderEnabled);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, int, StereoMode, getStereoMode, setStereoMode);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, int, DisplayType, getDisplayType, setDisplayType);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, bool, FullScreen, isFullScreen, setFullScreen);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, int, TargetScreen, getTargetScreen, setTargetScreen);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, double, EyeSeparation, getEyeSeparation, setEyeSeparation);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, double, ScreenDistance, getScreenDistance, setScreenDistance);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, double, ScreenWidth, getScreenWidth, setScreenWidth);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, double, ScreenHeight, getScreenHeight, setScreenHeight);
+}
+
+void View::setCamera(std::shared_ptr<Component> camera)
+{
+ auto castCamera = std::dynamic_pointer_cast<Camera>(camera);
+ SURGSIM_ASSERT(castCamera != nullptr) << "setCamera() passed not a camera.";
+ m_camera = castCamera;
+}
+
+std::shared_ptr<Camera> View::getCamera() const
+{
+ return m_camera;
+}
+
+bool View::doInitialize()
+{
+ SURGSIM_ASSERT(m_camera != nullptr) << "View cannot be created without a camera.";
+ return true;
+}
+
+bool View::isStereo() const
+{
+ return m_stereoMode != STEREO_MODE_NONE;
+}
+
+
+void View::setStereoMode(int mode)
+{
+ SURGSIM_ASSERT(!isAwake()) << "Can't change the view settings once the view has been woken up.";
+ SURGSIM_ASSERT(mode < STEREO_MODE_COUNT) << "Invalid StereoMode " << mode;
+ m_stereoMode = static_cast<StereoMode>(mode);
+}
+
+int View::getStereoMode() const
+{
+ return m_stereoMode;
+}
+
+void View::setDisplayType(int type)
+{
+ SURGSIM_ASSERT(!isAwake()) << "Can't change the view settings once the view has been woken up.";
+ SURGSIM_ASSERT(type < DISPLAY_TYPE_COUNT) << "Invalid DisplayType " << type;
+ m_displayType = type;
+}
+
+int View::getDisplayType() const
+{
+ return m_displayType;
+}
+
+void View::setFullScreen(bool val)
+{
+ SURGSIM_ASSERT(!isAwake()) << "Can't change the view settings once the view has been woken up.";
+ m_isFullscreen = val;
+}
+
+bool View::isFullScreen() const
+{
+ return m_isFullscreen;
+}
+
+void View::setTargetScreen(int val)
+{
+ SURGSIM_ASSERT(!isAwake()) << "Can't change the view settings once the view has been woken up.";
+ m_targetScreen = val;
+}
+
+int View::getTargetScreen() const
+{
+ return m_targetScreen;
+}
+
+void View::setScreenDistance(double val)
+{
+ SURGSIM_ASSERT(!isAwake()) << "Can't change the view settings once the view has been woken up.";
+ m_screenDistance = val;
+}
+
+double View::getScreenDistance() const
+{
+ return m_screenDistance;
+}
+
+void View::setEyeSeparation(double val)
+{
+ SURGSIM_ASSERT(!isAwake()) << "Can't change the view settings once the view has been woken up.";
+ m_eyeSeparation = val;
+}
+
+double View::getEyeSeparation() const
+{
+ return m_eyeSeparation;
+}
+
+double View::getScreenWidth() const
+{
+ SURGSIM_ASSERT(!isAwake()) << "Can't change the view settings once the view has been woken up.";
+ return m_screenWidth;
+}
+
+void View::setScreenWidth(double val)
+{
+ m_screenWidth = val;
+}
+
+double View::getScreenHeight() const
+{
+ return m_screenHeight;
+}
+
+void View::setScreenHeight(double val)
+{
+ SURGSIM_ASSERT(!isAwake()) << "Can't change the view settings once the view has been woken up.";
+ m_screenHeight = val;
+}
+
+}
+}
+
diff --git a/SurgSim/Graphics/View.h b/SurgSim/Graphics/View.h
new file mode 100644
index 0000000..1792e49
--- /dev/null
+++ b/SurgSim/Graphics/View.h
@@ -0,0 +1,204 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_VIEW_H
+#define SURGSIM_GRAPHICS_VIEW_H
+
+#include "SurgSim/Framework/Component.h"
+
+#include "SurgSim/Math/Vector.h"
+
+#include <memory>
+#include <array>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+class Camera;
+
+/// Base graphics view class, which defines the basic interface for all graphics views.
+///
+/// A Graphics::View provides a visualization of the scene to the user.
+///
+/// A Graphics::Camera controls the viewpoint of this View.
+class View : public SurgSim::Framework::Component
+{
+public:
+ /// Constructor
+ /// \param name Name of the view
+ explicit View(const std::string& name);
+
+ enum StereoMode
+ {
+ STEREO_MODE_NONE = -1,
+ STEREO_MODE_QUAD_BUFFER,
+ STEREO_MODE_ANAGLYPHIC,
+ STEREO_MODE_HORIZONTAL_SPLIT,
+ STEREO_MODE_VERTICAL_SPLIT,
+ STEREO_MODE_LEFT_EYE,
+ STEREO_MODE_RIGHT_EYE,
+ STEREO_MODE_HORIZONTAL_INTERLACE,
+ STEREO_MODE_VERTICAL_INTERLACE,
+ STEREO_MODE_CHECKERBOARD,
+ STEREO_MODE_COUNT
+ };
+
+ enum DisplayType
+ {
+ DISPLAY_TYPE_MONITOR,
+ DISPLAY_TYPE_HMD,
+ DISPLAY_TYPE_COUNT
+ };
+
+ /// Set the position of this view
+ /// \param position Position on the screen (in pixels)
+ /// \return True if it succeeded, false if it failed
+ virtual void setPosition(const std::array<int, 2>& position) = 0;
+
+ /// Get the position of this view
+ /// \return Position on the screen (in pixels)
+ virtual std::array<int, 2> getPosition() const = 0;
+
+ /// Set the dimensions of this view
+ /// \param dimensions Dimensions on the screen (in pixels)
+ virtual void setDimensions(const std::array<int, 2>& dimensions) = 0;
+
+ /// Get the dimensions of this view
+ /// \return Dimensions on the screen (in pixels)
+ virtual std::array<int, 2> getDimensions() const = 0;
+
+ /// Sets whether the view window has a border
+ /// \param enabled True to enable the border around the window; false for no border
+ virtual void setWindowBorderEnabled(bool enabled) = 0;
+
+ /// Returns whether the view window has a border
+ /// \return True to enable the border around the window; false for no border
+ virtual bool isWindowBorderEnabled() const = 0;
+
+ /// Sets the camera which provides the viewpoint in the scene
+ /// \param camera Camera whose image will be shown in this view
+ /// \return True if it succeeded, false if it failed
+ virtual void setCamera(std::shared_ptr<SurgSim::Framework::Component> camera);
+
+ /// Gets the camera which provides the viewpoint in the scene
+ /// \return camera Camera whose image will be shown in this view
+ std::shared_ptr<Camera> getCamera() const;
+
+ /// Updates the view
+ /// \param dt The time in seconds of the preceding timestep.
+ virtual void update(double dt) = 0;
+
+ /// \return true if the display is set to render a stereo view, or is currently rendering in stereo.
+ virtual bool isStereo() const;
+
+ /// Set the mode that this view should use for stereo display, see StereMode for all the modes.
+ /// \throws SurgSim::Framework::AssertionFailure if used after initialize has been called.
+ /// \param val The actual mode.
+ virtual void setStereoMode(int val);
+
+ /// \return What kind of stereo rendering is being used for this view.
+ int getStereoMode() const;
+
+ /// Set the kind of display.
+ /// \throws SurgSim::Framework::AssertionFailure if used after initialize has been called
+ /// \param type The type of display
+ void setDisplayType(int type);
+
+ /// \return The type of display that the view is on
+ int getDisplayType() const;
+
+ /// Request the display to use the whole screen.
+ /// \throws SurgSim::Framework::AssertionFailure if used after initialize has been called.
+ /// \param val If true the display will use up the whole screen, ignoring the dimension and location settings
+ void setFullScreen(bool val);
+
+ /// \return true if the display is set to use the whole screen, or is currently using the whole screen.
+ bool isFullScreen() const;
+
+ /// Request a certain screen to be used for this view.
+ /// \throws SurgSim::Framework::AssertionFailure if used after initialize has been called.
+ /// \param val The number of the screen (base 0) that should be used for this view.
+ void setTargetScreen(int val);
+
+ /// \return The number of the screen that this view is on.
+ int getTargetScreen() const;
+
+ /// Set the distance between the users eyes, this is necessary to calculate the correct projection matrices
+ /// for stereo rendering.
+ /// \throws SurgSim::Framework::AssertionFailure if used after initialize has been called.
+ /// \note this is only used when rendering stereo.
+ /// \param val The distance between the eyes in m.
+ void setEyeSeparation(double val);
+
+ /// \return The current distance between the eye points in m.
+ double getEyeSeparation() const;
+
+ /// Set the distance of the user from the screen, this is necessary to calculate the correct projection matrices
+ /// for stereo rendering.
+ /// \throws SurgSim::Framework::AssertionFailure if used after initialize has been called.
+ /// \note this is only used when rendering stereo.
+ /// \param val The distance from the user to the screen in m.
+ void setScreenDistance(double val);
+
+ /// \return The current distance from user to screen in m.
+ double getScreenDistance() const;
+
+ /// Set the width of the screen, this is necessary to calculate the correct projection matrices
+ /// for stereo rendering.
+ /// \throws SurgSim::Framework::AssertionFailure if used after initialize has been called.
+ /// \note this is only used when rendering stereo.
+ /// \param val The width of the screen used, in m.
+ void setScreenWidth(double val);
+
+ /// \return The currently used width of the screen in m.
+ double getScreenWidth() const;
+
+
+ /// Set the height of the screen, this is necessary to calculate the correct projection matrices
+ /// for stereo rendering.
+ /// \throws SurgSim::Framework::AssertionFailure if used after initialize has been called.
+ /// \note this is only used when rendering stereo.
+ /// \param val The height of the screen used, in m.
+ void setScreenHeight(double val);
+
+ /// \return The currently used height of the screen in m.
+ double getScreenHeight() const;
+
+private:
+
+ virtual bool doInitialize() override;
+
+ /// Camera whose image will be shown in this view
+ std::shared_ptr<Camera> m_camera;
+
+ int m_stereoMode; ///< The stereo mode, that is being used.
+ int m_displayType; ///< The requested display type.
+ int m_targetScreen; ///< Index of the screen to be used
+ bool m_isFullscreen; ///< Whether to go fullscreen
+ double m_eyeSeparation; ///< Distance between eypoints in m.
+ double m_screenDistance; ///< Distance from user to screen in m.
+ double m_screenWidth; ///< Width of screen in m.
+ double m_screenHeight; ///< Height of screen in m.
+
+};
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_VIEW_H
diff --git a/SurgSim/Graphics/ViewElement.cpp b/SurgSim/Graphics/ViewElement.cpp
new file mode 100644
index 0000000..82bd544
--- /dev/null
+++ b/SurgSim/Graphics/ViewElement.cpp
@@ -0,0 +1,71 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/ViewElement.h"
+
+#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Graphics/View.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+ViewElement::ViewElement(const std::string& name) : BasicSceneElement(name)
+{
+}
+
+ViewElement::~ViewElement()
+{
+}
+
+bool ViewElement::setView(std::shared_ptr<View> view)
+{
+ m_view = view;
+ if (m_camera != nullptr)
+ {
+ m_view->setCamera(m_camera);
+ }
+ return true;
+}
+
+std::shared_ptr<View> ViewElement::getView()
+{
+ return m_view;
+}
+
+void ViewElement::setCamera(std::shared_ptr<SurgSim::Graphics::Camera> camera)
+{
+ m_camera = camera;
+}
+
+std::shared_ptr<Camera> ViewElement::getCamera()
+{
+ return m_camera;
+}
+
+bool ViewElement::doInitialize()
+{
+ SURGSIM_ASSERT(m_view != nullptr) << "ViewElements require a View";
+ SURGSIM_ASSERT(m_camera != nullptr) << "ViewElements require a Camera";
+
+ m_view->setCamera(m_camera);
+ addComponent(m_view);
+ addComponent(m_camera);
+ return true;
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/ViewElement.h b/SurgSim/Graphics/ViewElement.h
new file mode 100644
index 0000000..cc31899
--- /dev/null
+++ b/SurgSim/Graphics/ViewElement.h
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_GRAPHICS_VIEWELEMENT_H
+#define SURGSIM_GRAPHICS_VIEWELEMENT_H
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+
+namespace SurgSim
+{
+
+namespace Input
+{
+class CommonDevice;
+}
+
+namespace Graphics
+{
+
+class Camera;
+class View;
+
+/// Basic SceneElement that wraps a View so that it can be added to the Scene.
+///
+/// A Scene needs at least one Graphics::View component for any visualization of Graphics:Representation objects
+/// to be shown. A view needs a camera to do its' rendering, this component can connect all the pieces correctly.
+class ViewElement : public Framework::BasicSceneElement
+{
+public:
+ /// Constructor
+ /// \param name Name of the scene element
+ explicit ViewElement(const std::string& name);
+
+ /// Destructor
+ virtual ~ViewElement();
+
+ /// Sets the view component that provides the visualization of the graphics representations
+ /// \return True if setView() succeeds; Otherwise, false.
+ virtual bool setView(std::shared_ptr<View> view);
+
+ /// Returns the view component that provides the visualization of the graphics representations
+ /// \return A shared_ptr pointing to the View component
+ std::shared_ptr<View> getView();
+
+ /// Sets the camera for the view in this sceneelement
+ /// \param camera The camera to be used with the view
+ void setCamera(std::shared_ptr<Camera> camera);
+
+ /// Get the camera for the view in this sceneelement.
+ /// \return The camera used in the view of this sceneelement.
+ std::shared_ptr<Camera> getCamera();
+
+ /// Return the keyboard to be used with this view.
+ /// \return A keyboard device
+ virtual std::shared_ptr<SurgSim::Input::CommonDevice> getKeyboardDevice() = 0;
+
+ /// Turn on/off the keyboard device to be used.
+ /// \param val Indicate whether or not to use keyboard device
+ virtual void enableKeyboardDevice(bool val) = 0;
+
+ /// Return the mouse to be used with this view.
+ /// \return A mouse device
+ virtual std::shared_ptr<SurgSim::Input::CommonDevice> getMouseDevice() = 0;
+
+ /// Turn on/off the mouse device to be used.
+ /// \param val Indicate whether or not to use mouse device
+ virtual void enableMouseDevice(bool val) = 0;
+
+protected:
+ /// Initializes the scene element
+ /// \return True if it succeeds, false if it fails
+ virtual bool doInitialize() override;
+
+private:
+ /// View component that provides the visualization of the graphics representations
+ std::shared_ptr<View> m_view;
+
+ /// Camera component connected to the view
+ std::shared_ptr<Camera> m_camera;
+};
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_VIEWELEMENT_H
diff --git a/SurgSim/Input/CMakeLists.txt b/SurgSim/Input/CMakeLists.txt
new file mode 100644
index 0000000..30519ce
--- /dev/null
+++ b/SurgSim/Input/CMakeLists.txt
@@ -0,0 +1,54 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+set(SURGSIM_INPUT_SOURCES
+ CommonDevice.cpp
+ InputComponent.cpp
+ InputManager.cpp
+ OutputComponent.cpp
+)
+
+set(SURGSIM_INPUT_HEADERS
+ CommonDevice.h
+ DeviceInterface.h
+ InputComponent.h
+ InputConsumerInterface.h
+ InputManager.h
+ OutputComponent.h
+ OutputProducerInterface.h
+)
+
+surgsim_add_library(
+ SurgSimInput
+ "${SURGSIM_INPUT_SOURCES}"
+ "${SURGSIM_INPUT_HEADERS}"
+ "SurgSim/Input"
+)
+
+set(LIBS
+ SurgSimFramework
+ SurgSimDataStructures
+)
+
+target_link_libraries(SurgSimInput ${LIBS}
+)
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+endif()
+
+# Put SurgSimInput into folder "Input"
+set_target_properties(SurgSimInput PROPERTIES FOLDER "Input")
diff --git a/SurgSim/Input/CommonDevice.cpp b/SurgSim/Input/CommonDevice.cpp
new file mode 100644
index 0000000..920da61
--- /dev/null
+++ b/SurgSim/Input/CommonDevice.cpp
@@ -0,0 +1,215 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Framework/Log.h"
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+
+
+namespace SurgSim
+{
+namespace Input
+{
+
+
+struct CommonDevice::State
+{
+ /// Constructor.
+ State()
+ {
+ }
+
+ /// The list of input consumers.
+ std::vector<std::shared_ptr<InputConsumerInterface>> inputConsumerList;
+
+ /// The output producer, if any.
+ std::shared_ptr<OutputProducerInterface> outputProducer;
+
+ /// The mutex that protects the consumers and the producer.
+ boost::mutex consumerProducerMutex;
+};
+
+
+
+CommonDevice::CommonDevice(const std::string& name) :
+ m_name(name),
+ m_nameForCallback(name),
+ m_inputData(SurgSim::DataStructures::DataGroup()),
+ m_state(new State)
+{
+}
+
+CommonDevice::CommonDevice(const std::string& name, const SurgSim::DataStructures::DataGroup& inputData) :
+ m_name(name),
+ m_nameForCallback(name),
+ m_inputData(inputData),
+ m_state(new State)
+{
+}
+
+CommonDevice::CommonDevice(const std::string& name, SurgSim::DataStructures::DataGroup&& inputData) :
+ m_name(name),
+ m_nameForCallback(name),
+ m_inputData(std::move(inputData)),
+ m_state(new State)
+{
+}
+
+CommonDevice::~CommonDevice()
+{
+}
+
+std::string CommonDevice::getName() const
+{
+ return m_name;
+}
+
+void CommonDevice::setNameForCallback(const std::string& name)
+{
+ m_nameForCallback = name;
+}
+
+std::string CommonDevice::getNameForCallback() const
+{
+ return m_nameForCallback;
+}
+
+bool CommonDevice::addInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer)
+{
+ if (! inputConsumer)
+ {
+ return false;
+ }
+
+ boost::lock_guard<boost::mutex> lock(m_state->consumerProducerMutex);
+ auto it = std::find(m_state->inputConsumerList.begin(), m_state->inputConsumerList.end(), inputConsumer);
+ if (it != m_state->inputConsumerList.end())
+ {
+ return false;
+ }
+
+ // NB: callbacks are called with the local m_nameForCallback.
+ // This allows e.g. filters to call their callbacks with a name different from their "real" name.
+ inputConsumer->initializeInput(m_nameForCallback, m_inputData);
+ m_state->inputConsumerList.emplace_back(std::move(inputConsumer));
+ return true;
+}
+
+bool CommonDevice::removeInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer)
+{
+ if (! inputConsumer)
+ {
+ return false;
+ }
+
+ boost::lock_guard<boost::mutex> lock(m_state->consumerProducerMutex);
+ for (auto it = m_state->inputConsumerList.begin(); it != m_state->inputConsumerList.end(); ++it)
+ {
+ if (*it == inputConsumer)
+ {
+ m_state->inputConsumerList.erase(it);
+ // The iterator is now invalid.
+ return true;
+ }
+ }
+ return false;
+}
+
+bool CommonDevice::setOutputProducer(std::shared_ptr<OutputProducerInterface> outputProducer)
+{
+ if (! outputProducer)
+ {
+ return false;
+ }
+
+ boost::lock_guard<boost::mutex> lock(m_state->consumerProducerMutex);
+ if (m_state->outputProducer == outputProducer)
+ {
+ return false;
+ }
+ m_state->outputProducer = std::move(outputProducer);
+ return true;
+}
+
+bool CommonDevice::removeOutputProducer(std::shared_ptr<OutputProducerInterface> outputProducer)
+{
+ if (! outputProducer)
+ {
+ return false;
+ }
+
+ boost::lock_guard<boost::mutex> lock(m_state->consumerProducerMutex);
+ if (m_state->outputProducer == outputProducer)
+ {
+ m_state->outputProducer.reset();
+ return true;
+ }
+ return false;
+}
+
+
+bool CommonDevice::hasOutputProducer()
+{
+ return (m_state->outputProducer != nullptr);
+}
+
+void CommonDevice::pushInput()
+{
+ boost::lock_guard<boost::mutex> lock(m_state->consumerProducerMutex);
+ for (auto it = m_state->inputConsumerList.begin(); it != m_state->inputConsumerList.end(); ++it)
+ {
+ // NB: callbacks are called with the local m_nameForCallback.
+ // This allows e.g. filters to call their callbacks with a name different from their "real" name.
+ (*it)->handleInput(m_nameForCallback, m_inputData);
+ }
+}
+
+bool CommonDevice::pullOutput()
+{
+ boost::lock_guard<boost::mutex> lock(m_state->consumerProducerMutex);
+ if (m_state->outputProducer)
+ {
+ // NB: callbacks are called with the local m_nameForCallback.
+ // This allows e.g. filters to call their callbacks with a name different from their "real" name.
+ bool gotOutput = m_state->outputProducer->requestOutput(m_nameForCallback, &m_outputData);
+ if (gotOutput)
+ {
+ return true;
+ }
+
+ // If we're here, then the producer has refused to provide output.
+ }
+
+ // If we haven't received an update, the old data is meaningless.
+ m_outputData.resetAll();
+
+ return false;
+}
+
+SurgSim::DataStructures::DataGroup& CommonDevice::getInputData()
+{
+ return m_inputData;
+}
+
+const SurgSim::DataStructures::DataGroup& CommonDevice::getOutputData() const
+{
+ return m_outputData;
+}
+
+
+}; // namespace Input
+}; // namespace SurgSim
diff --git a/SurgSim/Input/CommonDevice.h b/SurgSim/Input/CommonDevice.h
new file mode 100644
index 0000000..ae81157
--- /dev/null
+++ b/SurgSim/Input/CommonDevice.h
@@ -0,0 +1,150 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_INPUT_COMMONDEVICE_H
+#define SURGSIM_INPUT_COMMONDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+
+
+/// A class that implements some common management code on top of the DeviceInterface.
+/// Practically every class that implements DeviceInterface will likely want to inherit from CommonDevice.
+class CommonDevice : public DeviceInterface
+{
+public:
+ /// Constructor. Sets the input data to an empty DataGroup.
+ /// \param name The name associated with the input device.
+ explicit CommonDevice(const std::string& name);
+
+ /// Constructor.
+ ///
+ /// \param name The name associated with the input device.
+ /// \param inputData An initial value for the application's input from the device (e.g. pose etc).
+ /// The concrete device implementation should pass in a DataGroup whose contents has been set up, e.g. by
+ /// using a DataGroupBuilder, to that device's supported values that it will push to the application.
+ CommonDevice(const std::string& name, const SurgSim::DataStructures::DataGroup& inputData);
+
+ /// Constructor.
+ ///
+ /// \param name The name associated with the input device.
+ /// \param inputData An initial value for the application's input from the device (e.g. pose etc).
+ /// The concrete device implementation should pass in a DataGroup whose contents has been set up, e.g. by
+ /// using a DataGroupBuilder, to that device's supported values that it will push to the application.
+ CommonDevice(const std::string& name, SurgSim::DataStructures::DataGroup&& inputData);
+
+ /// Destructor.
+ virtual ~CommonDevice();
+
+ /// Return a (hopefully unique) device name.
+ virtual std::string getName() const override;
+
+ /// Set the name used for calling the input consumers and output producer.
+ /// By default, this will be the same as the name of the device that was passed to the constructor.
+ /// \param name The name to be used.
+ void setNameForCallback(const std::string& name);
+
+ /// Get the name used for calling the input consumers and output producer.
+ /// By default, this will be the same as the name of the device that was passed to the constructor.
+ /// \return The name being used.
+ std::string getNameForCallback() const;
+
+ /// Connect this device to an InputConsumerInterface, which will receive the data that comes from this device.
+ /// \param inputConsumer The InputConsumerInterface to connect with.
+ /// \return true if successful
+ virtual bool addInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer) override;
+
+ /// Disconnect this device from an InputConsumerInterface, which will no longer receive data from this device.
+ /// \param inputConsumer The InputConsumerInterface to disconnect from.
+ /// \return true if successful
+ virtual bool removeInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer) override;
+
+ /// Connect this device to an OutputProducerInterface, which will send data to this device.
+ /// \param outputProducer The OutputProducerInterface to connect with.
+ /// \return true if successful
+ virtual bool setOutputProducer(std::shared_ptr<OutputProducerInterface> outputProducer) override;
+
+ /// Disconnect this device from an OutputProducerInterface, which will no longer send data to this device.
+ /// \param outputProducer The OutputProducerInterface to disconnect from.
+ /// \return true if successful
+ virtual bool removeOutputProducer(std::shared_ptr<OutputProducerInterface> outputProducer) override;
+
+ /// Getter for whether or not this device is connected with an OutputProducerInterface.
+ /// \return true if an OutputProducerInterface is connected.
+ virtual bool hasOutputProducer() override;
+
+protected:
+
+ /// Push application input to consumers.
+ virtual void pushInput();
+
+ /// Pull application output from a producer.
+ virtual bool pullOutput();
+
+ /// Getter for the input data \ref SurgSim::DataStructures::DataGroup "DataGroup". This function is typically
+ /// called by friend scaffolds, to get a DataGroup they can modify then set back to the device to send to the
+ /// device's input consumers.
+ /// \return A reference to the input data.
+ SurgSim::DataStructures::DataGroup& getInputData();
+
+ /// Getter for the output data \ref SurgSim::DataStructures::DataGroup "DataGroup". This function is typically
+ /// called by friend scaffolds, to get the data that the output producer wants to send to the device (and then send
+ /// that data through the device's SDK). Note that a writable variant is not provided, an output producer registered
+ /// via \ref setOutputProducer will set the output data.
+ /// \return A reference to the output data.
+ const SurgSim::DataStructures::DataGroup& getOutputData() const;
+
+private:
+ struct State;
+
+ const std::string m_name;
+
+ /// The name used for the callbacks, defaults to the device name.
+ std::string m_nameForCallback;
+
+ /// The data the device is providing to its input consumers.
+ SurgSim::DataStructures::DataGroup m_inputData;
+
+ /// The data the output producer (if any) is providing to the device.
+ SurgSim::DataStructures::DataGroup m_outputData;
+
+ /// Struct to hide some of the private member variables, PImpl (Pointer to Implementation).
+ /// For CommonDevice, we are hiding:
+ /// - The list of input consumers,
+ /// - The output producer, if any, and
+ /// - The mutex that protects the consumers and the producer.
+ /// The PImpl idiom is being used so that subclasses of CommonDevice will never store device-specific datatypes in
+ /// member variables. Instead they would store them in the PImpl object, so that the device-specific include
+ /// file(s) are only included by the subclass's .cpp file. A benefit of this idiom is that any change to the
+ /// device's API/SDK will not force a recompile of any file including the subclass's .h file. For historical
+ /// reasons we are not currently using the PImpl object to store all this class's private member variables, as is
+ /// commonly recommended.
+ std::unique_ptr<State> m_state;
+};
+
+
+}; // namespace Input
+}; // namespace SurgSim
+
+#endif // SURGSIM_INPUT_COMMONDEVICE_H
diff --git a/SurgSim/Input/DeviceInterface.h b/SurgSim/Input/DeviceInterface.h
new file mode 100644
index 0000000..2080d86
--- /dev/null
+++ b/SurgSim/Input/DeviceInterface.h
@@ -0,0 +1,90 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_INPUT_DEVICEINTERFACE_H
+#define SURGSIM_INPUT_DEVICEINTERFACE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+
+
+/// Interface used to communicate with user-interface hardware devices.
+///
+/// Classes that implement communication with a hardware device implement this interface. This includes
+/// input/output devices (haptic devices are the most obvious example, but many other devices can, for example,
+/// display visual indicators such as LEDs, etc.), as well as input-only and output-only devices.
+///
+/// Derived classes will likely want to hide their constructor and only
+/// allow creation through a manager object for that type of device.
+class DeviceInterface
+{
+public:
+ /// Virtual destructor (empty).
+ virtual ~DeviceInterface()
+ {
+ }
+
+ /// Return a (hopefully unique) device name.
+ virtual std::string getName() const = 0;
+
+ /// Fully initialize the device.
+ ///
+ /// When the manager object creates the device, the internal state of the device usually isn't fully
+ /// initialized yet. This method performs any needed initialization.
+ virtual bool initialize() = 0;
+
+ /// Adds an input consumer that will be notified when the application input state is updated.
+ ///
+ /// \param inputConsumer The input consumer to be added.
+ /// \return true on success, false on failure.
+ virtual bool addInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer) = 0;
+
+ /// Removes an input consumer previously added via \ref addInputConsumer.
+ /// \param inputConsumer The input consumer to be removed.
+ virtual bool removeInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer) = 0;
+
+ /// Sets an output producer that will be asked for application output state when the device needs it.
+ /// Any previously set output producer will be removed.
+ ///
+ /// \param outputProducer The output producer to be added.
+ /// \return true on success, false on failure.
+ virtual bool setOutputProducer(std::shared_ptr<OutputProducerInterface> outputProducer) = 0;
+
+ /// Removes an output producer previously added via \ref setOutputProducer.
+ /// \param outputProducer The output producer to be removed.
+ virtual bool removeOutputProducer(std::shared_ptr<OutputProducerInterface> outputProducer) = 0;
+
+ /// Query if this object has output producer.
+ /// \return true if there is an output producer, false if not.
+ virtual bool hasOutputProducer() = 0;
+
+protected:
+ /// Finalize (de-initialize) the device.
+ virtual bool finalize() = 0;
+};
+
+
+}; // namespace Input
+}; // namespace SurgSim
+
+#endif // SURGSIM_INPUT_DEVICEINTERFACE_H
diff --git a/SurgSim/Input/InputComponent.cpp b/SurgSim/Input/InputComponent.cpp
new file mode 100644
index 0000000..bb2c642
--- /dev/null
+++ b/SurgSim/Input/InputComponent.cpp
@@ -0,0 +1,133 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Input/InputComponent.h"
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/LockedContainer.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Input::InputComponent, InputComponent);
+
+/// An input consumer monitors device and signal state update
+class InputConsumer: public InputConsumerInterface
+{
+public:
+ /// Constructor
+ InputConsumer()
+ {
+ }
+ /// Destructor
+ virtual ~InputConsumer()
+ {
+ }
+
+ /// Handle the input coming from device.
+ /// \param device The name of the device that is producing the input.
+ /// \param inputData The input data coming from the device.
+ virtual void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override
+ {
+ m_lastInput.set(inputData);
+ }
+
+ /// Initialize the input data information stored in this input consumer.
+ /// \param device The name of the device that is producing the input.
+ /// \param initialData Initial input data of the device.
+ virtual void initializeInput(const std::string& device,
+ const SurgSim::DataStructures::DataGroup& initialData) override
+ {
+ m_lastInput.set(initialData);
+ }
+
+ /// Retrieve input data information stored in this input consumer
+ /// \param [out] dataGroup Used to accept the retrieved input data information
+ void getData(SurgSim::DataStructures::DataGroup* dataGroup)
+ {
+ m_lastInput.get(dataGroup);
+ }
+
+private:
+ /// Used to store input data information passed in from device
+ SurgSim::Framework::LockedContainer<SurgSim::DataStructures::DataGroup> m_lastInput;
+};
+
+
+InputComponent::InputComponent(const std::string& name) :
+ Component(name),
+ m_deviceName(),
+ m_deviceConnected(false),
+ m_input(std::make_shared<InputConsumer>())
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(InputComponent, std::string, DeviceName,
+ getDeviceName, setDeviceName);
+}
+
+InputComponent::~InputComponent()
+{
+}
+
+void InputComponent::setDeviceName(const std::string& deviceName)
+{
+ m_deviceName = deviceName;
+}
+
+bool InputComponent::isDeviceConnected()
+{
+ return m_deviceConnected;
+}
+
+void InputComponent::getData(SurgSim::DataStructures::DataGroup* dataGroup)
+{
+ SURGSIM_ASSERT(m_deviceConnected) << "No device connected to InputComponent named '" << getName() <<
+ "'. Unable to getData.";
+ m_input->getData(dataGroup);
+}
+
+bool InputComponent::doInitialize()
+{
+ return true;
+}
+
+bool InputComponent::doWakeUp()
+{
+ return true;
+}
+
+std::string InputComponent::getDeviceName() const
+{
+ return m_deviceName;
+}
+
+void InputComponent::connectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device)
+{
+ device->addInputConsumer(m_input);
+ m_deviceConnected = true;
+}
+
+void InputComponent::disconnectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device)
+{
+ device->removeInputConsumer(m_input);
+ m_deviceConnected = false;
+}
+
+}; // namespace Input
+}; // namespace SurgSim
+
diff --git a/SurgSim/Input/InputComponent.h b/SurgSim/Input/InputComponent.h
new file mode 100644
index 0000000..335a805
--- /dev/null
+++ b/SurgSim/Input/InputComponent.h
@@ -0,0 +1,99 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_INPUT_INPUTCOMPONENT_H
+#define SURGSIM_INPUT_INPUTCOMPONENT_H
+
+#include <string>
+#include <memory>
+#include "SurgSim/Framework/Component.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+class DataGroup;
+}
+
+namespace Input
+{
+class DeviceInterface;
+class InputConsumer;
+
+SURGSIM_STATIC_REGISTRATION(InputComponent);
+
+/// InputComponent combines the Component interface and the InputConsumerInterface so that input devices can
+/// provide input through the normal component interface. Multiple InputComponents can be added to
+/// the same device.
+class InputComponent : public SurgSim::Framework::Component
+{
+public:
+ /// Constructor
+ /// \param name Name of this input component
+ explicit InputComponent(const std::string& name);
+
+ /// Destructor
+ virtual ~InputComponent();
+
+ SURGSIM_CLASSNAME(SurgSim::Input::InputComponent);
+
+ /// Set name of the device this input component connects to.
+ /// \param deviceName Name of the device this input component connects
+ void setDeviceName(const std::string& deviceName);
+
+ /// Is a device connected
+ /// \return true if a device has been connected.
+ bool isDeviceConnected();
+
+ /// Connect to a device
+ /// This call will be made by the InputManager, and should generally not be called directly.
+ /// \param device The device to connect to.
+ void connectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device);
+
+ /// Disconnect from a device
+ /// This call will be made by the InputManager, and should generally not be called directly.
+ /// \param device The device to disconnect from.
+ void disconnectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device);
+
+ /// Gets the input data.
+ /// \param [out] dataGroup The location to write the data. The pointer must be non-null.
+ /// \exception Asserts if the InputComponent is not connected to a device.
+ void getData(SurgSim::DataStructures::DataGroup* dataGroup);
+
+ /// Overridden from Component, do nothing
+ virtual bool doInitialize() override;
+
+ /// Overridden from Component, do nothing
+ virtual bool doWakeUp() override;
+
+ /// Gets device name.
+ /// \return The device name.
+ std::string getDeviceName() const;
+
+private:
+ /// Name of the device to which this input component connects
+ std::string m_deviceName;
+ /// Indicates if this input component is connected to a device
+ bool m_deviceConnected;
+ /// Input consumer which brings in information from hardware device
+ std::shared_ptr<InputConsumer> m_input;
+};
+
+}; // namespace Input
+}; // namespace SurgSim
+
+
+#endif
diff --git a/SurgSim/Input/InputConsumerInterface.h b/SurgSim/Input/InputConsumerInterface.h
new file mode 100644
index 0000000..883b384
--- /dev/null
+++ b/SurgSim/Input/InputConsumerInterface.h
@@ -0,0 +1,76 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_INPUT_INPUTCONSUMERINTERFACE_H
+#define SURGSIM_INPUT_INPUTCONSUMERINTERFACE_H
+
+#include <string>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class DataGroup;
+}; // namespace DataStructures
+
+namespace Input
+{
+
+
+/// Interface for a consumer that monitors device and signal state updates (pose, buttons, etc).
+class InputConsumerInterface
+{
+public:
+ /// Virtual destructor (empty).
+ virtual ~InputConsumerInterface()
+ {
+ }
+
+ /// Notifies the consumer that the application input coming from the device has been updated.
+ ///
+ /// Typical input data contents (but note that individual devices may do things differently):
+ /// | type | name | |
+ /// | ---- | ---- | --- |
+ /// | pose | "pose" | %Device pose (units are meters). |
+ /// | bool | "button1" | State of the first device button. |
+ /// | bool | "button2" | State of the second device button (and so on). |
+ ///
+ /// Other possible contents includes:
+ /// | type | name | |
+ /// | ---- | ---- | |
+ /// | bool | "isHomed" | %Device homing status. |
+ /// | bool | "isHomedX" | Individual homing status for the X axis (and so on). |
+ /// | bool | "isHeld" | Safety sensor etc. status. |
+ /// | string | "model" | %Device model description. |
+ /// | string | "serial" | Serial number string. |
+ /// | (any) | "debug:*" | Various debugging information |
+ ///
+ /// \param device The name of the device that is producing the input. This should only be used to identify
+ /// the device (e.g. if the consumer is listening to several devices at once).
+ /// \param inputData The application input state coming from the device.
+ virtual void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) = 0;
+
+ /// Set the initial input data group.
+ /// \param device The name of the device that is producing the input. This should only be used to identify
+ /// the device (e.g. if the consumer is listening to several devices at once).
+ /// \param inputData The application input state coming from the device.
+ virtual void initializeInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) = 0;
+};
+
+
+}; // namespace Input
+}; // namespace SurgSim
+
+#endif // SURGSIM_INPUT_INPUTCONSUMERINTERFACE_H
diff --git a/SurgSim/Input/InputManager.cpp b/SurgSim/Input/InputManager.cpp
new file mode 100644
index 0000000..5bdd8db
--- /dev/null
+++ b/SurgSim/Input/InputManager.cpp
@@ -0,0 +1,188 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Input/InputManager.h"
+
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Input/OutputComponent.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+
+InputManager::InputManager() :
+ ComponentManager("Input Manager")
+{
+}
+
+InputManager::~InputManager()
+{
+}
+
+bool InputManager::doInitialize()
+{
+ return true;
+}
+
+bool InputManager::doStartUp()
+{
+ return true;
+}
+
+bool InputManager::doUpdate(double dt)
+{
+ // Add all components that came in before the last update
+ processComponents();
+
+ // Process specific behaviors belongs to this manager
+ processBehaviors(dt);
+
+ return true;
+}
+
+bool InputManager::executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component)
+{
+ auto input = tryAddComponent(component, &m_inputs);
+ if (input != nullptr)
+ {
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ // Early exit
+ return addInputComponent(input);
+ }
+
+ auto output = tryAddComponent(component, &m_outputs);
+ if (output != nullptr)
+ {
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ // Early exit
+ return addOutputComponent(output);
+ }
+
+ // If we got he the component was neither an Input nor and OutputComponent, no add was performed
+ // return false
+ return false;
+}
+
+bool InputManager::executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component)
+{
+
+ bool result = false;
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ if (tryRemoveComponent(component, &m_inputs))
+ {
+ auto input = std::static_pointer_cast<InputComponent>(component);
+ input->disconnectDevice(m_devices[input->getDeviceName()]);
+ result = true;
+ }
+ else if(tryRemoveComponent(component, &m_outputs))
+ {
+ auto output = std::dynamic_pointer_cast<OutputComponent>(component);
+ m_devices[output->getDeviceName()]->setOutputProducer(nullptr);
+ result = true;
+ }
+ return result;
+}
+
+bool InputManager::addInputComponent(const std::shared_ptr<InputComponent>& input)
+{
+ bool result = false;
+ if (m_devices.find(input->getDeviceName()) != m_devices.end())
+ {
+ input->connectDevice(m_devices[input->getDeviceName()]);
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added input component " << input->getName() <<
+ " connected to device " << input->getDeviceName();
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << __FUNCTION__ << " Could not find Device named '" << input->getDeviceName() <<
+ "' when adding input component named '" << input->getName() << "'.";
+ }
+ return result;
+}
+
+bool InputManager::addOutputComponent(const std::shared_ptr<OutputComponent>& output)
+{
+ bool result = false;
+ if (m_devices.find(output->getDeviceName()) != m_devices.end())
+ {
+ if (!m_devices[output->getDeviceName()]->hasOutputProducer())
+ {
+ output->connectDevice(m_devices[output->getDeviceName()]);
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added output component " << output->getName() <<
+ " connected to device " << output->getDeviceName();
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << __FUNCTION__ <<
+ " Trying to add OutputProducer " << output->getName() << " to device " << output->getDeviceName() <<
+ " but the device already has an OutputProducer assigned, this add will be ignored!";
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << __FUNCTION__ << " Could not find Device with name " <<
+ output->getDeviceName() << " when adding output component " << output->getName();
+ }
+ return result;
+}
+
+bool InputManager::addDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device)
+{
+ bool result = false;
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ if (m_devices.find(device->getName()) == m_devices.cend())
+ {
+ m_devices[device->getName()] = device;
+ SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added device " << device->getName();
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << __FUNCTION__ << " Device " << device->getName() <<
+ " is already available in Input Manager";
+ }
+ return result;
+}
+
+bool InputManager::removeDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device)
+{
+ bool result = false;
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ auto it = m_devices.find(device->getName());
+ if (it != m_devices.end())
+ {
+ m_devices.erase(it);
+ SURGSIM_LOG_DEBUG(m_logger) << __FUNCTION__ << " Removed device " << device->getName();
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << __FUNCTION__ << " Failed to remove device " << device->getName();
+ }
+ return result;
+}
+
+int InputManager::getType() const
+{
+ return SurgSim::Framework::MANAGER_TYPE_INPUT;
+}
+
+} // Input
+} // SurgSim
diff --git a/SurgSim/Input/InputManager.h b/SurgSim/Input/InputManager.h
new file mode 100644
index 0000000..f8f4ccc
--- /dev/null
+++ b/SurgSim/Input/InputManager.h
@@ -0,0 +1,101 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_INPUT_INPUTMANAGER_H
+#define SURGSIM_INPUT_INPUTMANAGER_H
+
+#include <boost/thread/mutex.hpp>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include "SurgSim/Framework/ComponentManager.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+class DeviceInterface;
+class InputComponent;
+class OutputComponent;
+
+/// Manager to handle InputComponent and OutputComponent, SceneElement can add these to
+/// get input from devices, or even write output to devices. The devices have to be added
+/// to this class before components can be added to it.
+class InputManager : public SurgSim::Framework::ComponentManager
+{
+public:
+ InputManager();
+ virtual ~InputManager();
+
+ friend class InputManagerTest;
+
+ /// Adds a device to the manager.
+ /// \param device The device.
+ /// \return true if it succeeds, false if the device already exists in the manager.
+ bool addDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device);
+
+ /// Removes the device described by device.
+ /// \param device The device.
+ /// \return true if it succeeds, false if the device is not in.
+ bool removeDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device);
+
+ virtual int getType() const override;
+
+private:
+ virtual bool doInitialize() override;
+ virtual bool doStartUp() override;
+ virtual bool doUpdate(double dt) override;
+
+ /// Adds a component, this can be either input or output, it will call the appropriate
+ /// function in the device. For an InputComonent this will succeed if the device name
+ /// inside the component is known to the InputManager and if the component has not
+ /// been added as an input yet. For an OutputComponent the call will fail if the device
+ /// does not exist or the device has already been assigned an output.
+ /// \param component The component.
+ /// \return true if it succeeds, it will fail if the device cannot be found to the component
+ /// has already been added to the manager, and return false.
+ virtual bool executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component) override;
+
+ /// Removes the component described by component.
+ /// \param component The component.
+ /// \return true if it succeeds, it will fail if the component cannot be found and return false.
+ virtual bool executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component) override;
+
+
+ /// Specific call for input components.
+ /// Link input consumer to input device
+ /// Data produced by device will then be consumed by input consumer
+ bool addInputComponent(const std::shared_ptr<InputComponent>& input);
+ /// Specific call for output components.
+ bool addOutputComponent(const std::shared_ptr<OutputComponent>& output);
+
+ /// Collection of all input components.
+ std::vector<std::shared_ptr<InputComponent>> m_inputs;
+ /// Collection of all output components.
+ std::vector<std::shared_ptr<OutputComponent>> m_outputs;
+
+ /// Collection of all devices that have been added to the input manager
+ /// key is the name, no two devices with the same name can be added to the
+ /// input manager
+ std::unordered_map<std::string, std::shared_ptr<SurgSim::Input::DeviceInterface>> m_devices;
+
+ /// Protect critical sections
+ boost::mutex m_mutex;
+};
+
+}; //namespace Input
+}; //namespace SurgSim
+#endif
diff --git a/SurgSim/Input/OutputComponent.cpp b/SurgSim/Input/OutputComponent.cpp
new file mode 100644
index 0000000..a046f60
--- /dev/null
+++ b/SurgSim/Input/OutputComponent.cpp
@@ -0,0 +1,130 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Input/OutputComponent.h"
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/Framework/LockedContainer.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Input::OutputComponent, OutputComponent);
+/// An output producer sends data to a device
+class OutputProducer: public OutputProducerInterface
+{
+public:
+ /// Constructor
+ OutputProducer() : m_haveData(false)
+ {
+ }
+ /// Destructor
+ virtual ~OutputProducer()
+ {
+ }
+
+ /// Send the output to the device.
+ /// \param device The name of the device to receive the output.
+ /// \param [out] outputData The output data going to the device.
+ /// \return true if outputData was provided.
+ virtual bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) override
+ {
+ bool result = false;
+ if (m_haveData && (outputData != nullptr))
+ {
+ m_lastOutput.get(outputData); // cannot get() until after the first call to setData
+ result = true;
+ }
+ return result;
+ }
+
+ /// Set the output data information stored in this output producer
+ /// \param dataGroup Data to be sent to the device
+ void setData(const SurgSim::DataStructures::DataGroup& dataGroup)
+ {
+ m_lastOutput.set(dataGroup);
+ m_haveData = true;
+ }
+
+private:
+ /// Used to store output data information to be passed out to device. The DataGroup in the LockedContainer is
+ /// default-constructed, so m_lastOutput.get will assert until after the first call to m_lastOutput.set in setData.
+ SurgSim::Framework::LockedContainer<SurgSim::DataStructures::DataGroup> m_lastOutput;
+
+ /// Has setData been called since construction?
+ bool m_haveData;
+};
+
+OutputComponent::OutputComponent(const std::string& name) :
+ Component(name),
+ m_deviceName(),
+ m_deviceConnected(false),
+ m_output(std::make_shared<OutputProducer>())
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OutputComponent, std::string, DeviceName,
+ getDeviceName, setDeviceName);
+}
+
+OutputComponent::~OutputComponent()
+{
+}
+
+void OutputComponent::setDeviceName(const std::string& deviceName)
+{
+ m_deviceName = deviceName;
+}
+
+bool OutputComponent::isDeviceConnected()
+{
+ return m_deviceConnected;
+}
+
+void OutputComponent::setData(const SurgSim::DataStructures::DataGroup& dataGroup)
+{
+ m_output->setData(dataGroup);
+}
+
+bool OutputComponent::doInitialize()
+{
+ return true;
+}
+
+bool OutputComponent::doWakeUp()
+{
+ return true;
+}
+
+std::string OutputComponent::getDeviceName() const
+{
+ return m_deviceName;
+}
+
+void OutputComponent::connectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device)
+{
+ device->setOutputProducer(m_output);
+ m_deviceConnected = true;
+}
+
+void OutputComponent::disconnectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device)
+{
+ device->removeOutputProducer(m_output);
+ m_deviceConnected = false;
+}
+
+}; // namespace Input
+}; // namespace SurgSim
diff --git a/SurgSim/Input/OutputComponent.h b/SurgSim/Input/OutputComponent.h
new file mode 100644
index 0000000..3098de3
--- /dev/null
+++ b/SurgSim/Input/OutputComponent.h
@@ -0,0 +1,96 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_INPUT_OUTPUTCOMPONENT_H
+#define SURGSIM_INPUT_OUTPUTCOMPONENT_H
+
+#include <string>
+#include <memory>
+#include "SurgSim/Framework/Component.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+class DataGroup;
+}
+
+namespace Input
+{
+class DeviceInterface;
+class OutputProducer;
+
+SURGSIM_STATIC_REGISTRATION(OutputComponent);
+
+/// OutputComponent is a Component that has an OutputProducer, a concrete instance of OutputProducerInterface, so that
+/// output devices can receive data through the normal component interface to SceneElements.
+class OutputComponent : public SurgSim::Framework::Component
+{
+public:
+ /// Constructor
+ /// \param name Name of this output component
+ explicit OutputComponent(const std::string& name);
+ /// Destructor
+ virtual ~OutputComponent();
+
+ SURGSIM_CLASSNAME(SurgSim::Input::OutputComponent);
+
+ /// Set name of the device of output component.
+ /// param deviceName The name of the device that will receive the output data.
+ void setDeviceName(const std::string& deviceName);
+
+ /// Is a device connected
+ /// \return true if a device has been connected.
+ bool isDeviceConnected();
+
+ /// Connect to a device
+ /// This call will be made by the InputManager, and should generally not be called directly.
+ /// \param device The device to connect to.
+ void connectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device);
+
+ /// Disconnect from a device
+ /// This call will be made by the InputManager, and should generally not be called directly.
+ /// \param device The device to disconnect from.
+ void disconnectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device);
+
+ /// Sets the output data.
+ /// \param dataGroup The data to output.
+ void setData(const SurgSim::DataStructures::DataGroup& dataGroup);
+
+ /// Overridden from Component, do nothing
+ virtual bool doInitialize();
+
+ /// Overridden from Component, do nothing
+ virtual bool doWakeUp();
+
+ /// Gets device name.
+ /// \return The device name.
+ std::string getDeviceName() const;
+
+private:
+ /// Name of the device to which this output component connects
+ std::string m_deviceName;
+ /// Indicates if this output component is connected to a device
+ bool m_deviceConnected;
+ /// Output producer which sends data to hardware device
+ std::shared_ptr<OutputProducer> m_output;
+};
+
+}; // namespace Input
+}; // namespace SurgSim
+
+
+#endif
diff --git a/SurgSim/Input/OutputProducerInterface.h b/SurgSim/Input/OutputProducerInterface.h
new file mode 100644
index 0000000..de2c6d0
--- /dev/null
+++ b/SurgSim/Input/OutputProducerInterface.h
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_INPUT_OUTPUTPRODUCERINTERFACE_H
+#define SURGSIM_INPUT_OUTPUTPRODUCERINTERFACE_H
+
+#include <string>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class DataGroup;
+}; // namespace DataStructures
+
+namespace Input
+{
+
+
+/// Interface for a producer that generates device output updates (forces, status LED state, etc).
+class OutputProducerInterface
+{
+public:
+ /// Virtual destructor (empty).
+ virtual ~OutputProducerInterface()
+ {
+ }
+
+ /// Asks the producer to provide output state to the device.
+ ///
+ /// Note that devices may never call this method, e.g. because the device doesn't actually have any
+ /// output capability.
+ ///
+ /// Typical output data contents (but note that individual devices may do things differently):
+ /// | type | name | |
+ /// | ---- | ---- | --- |
+ /// | vector | "force" | Commanded force for the device (units are newtons). |
+ /// | vector | "torque" | Commanded torque for the device (units are newton-meters). |
+ /// | bool | "isEnabled" | Safety switch input. |
+ ///
+ /// Other possible contents includes:
+ /// | type | name | |
+ /// | ---- | ---- | --- |
+ /// | bool | "led0" | Desired state for LED 0. |
+ /// | bool | "led1" | Desired state for LED 1. |
+ /// | string | "toolId" | Calibration ID to use, e.g. for camera devices. |
+ ///
+ /// \param device The name of the device that is requesting the output. This should only be used to identify
+ /// the device (e.g. if the producer is listening to several devices at once).
+ /// \param [out] outputData The application output state being fed into the device.
+ ///
+ /// \return true if the producer has provided some output, false if it refuses to do so. A producer
+ /// that returns false should leave outputData unmodified.
+ virtual bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) = 0;
+};
+
+
+}; // namespace Input
+}; // namespace SurgSim
+
+#endif // SURGSIM_INPUT_OUTPUTPRODUCERINTERFACE_H
diff --git a/SurgSim/Input/UnitTests/CMakeLists.txt b/SurgSim/Input/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..95f34e9
--- /dev/null
+++ b/SurgSim/Input/UnitTests/CMakeLists.txt
@@ -0,0 +1,38 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ CommonDeviceTests.cpp
+ InputComponentTest.cpp
+ InputManagerTest.cpp
+ OutputComponentTest.cpp
+ TestDevice.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ TestDevice.h
+)
+
+set(LIBS
+ SurgSimInput
+)
+
+surgsim_add_unit_tests(SurgSimInputTest)
+
+set_target_properties(SurgSimInputTest PROPERTIES FOLDER "Input")
diff --git a/SurgSim/Input/UnitTests/CommonDeviceTests.cpp b/SurgSim/Input/UnitTests/CommonDeviceTests.cpp
new file mode 100644
index 0000000..ef8679f
--- /dev/null
+++ b/SurgSim/Input/UnitTests/CommonDeviceTests.cpp
@@ -0,0 +1,248 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 CommonDevice class.
+
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+
+#include "SurgSim/Input/UnitTests/TestDevice.h"
+
+using SurgSim::Input::CommonDevice;
+using SurgSim::Input::InputConsumerInterface;
+using SurgSim::Input::OutputProducerInterface;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+
+TEST(CommonDeviceTests, CanConstruct)
+{
+ EXPECT_NO_THROW({TestDevice device("MyTestDevice");});
+}
+
+TEST(CommonDeviceTests, Name)
+{
+ TestDevice device("MyTestDevice");
+ EXPECT_EQ("MyTestDevice", device.getName());
+}
+
+TEST(CommonDeviceTests, AddInputConsumer)
+{
+ TestDevice device("MyTestDevice");
+ std::shared_ptr<TestInputConsumer> consumer = std::make_shared<TestInputConsumer>();
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device.addInputConsumer(consumer));
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device.addInputConsumer(consumer));
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ std::shared_ptr<TestInputConsumer> consumer2 = std::make_shared<TestInputConsumer>();
+ EXPECT_EQ(0, consumer2->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device.addInputConsumer(consumer2));
+ EXPECT_EQ(0, consumer2->m_numTimesReceivedInput);
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+}
+
+TEST(CommonDeviceTests, SetOutputProducer)
+{
+ TestDevice device("MyTestDevice");
+ std::shared_ptr<TestOutputProducer> producer = std::make_shared<TestOutputProducer>();
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_TRUE(device.setOutputProducer(producer));
+ EXPECT_TRUE(device.hasOutputProducer());
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ EXPECT_FALSE(device.setOutputProducer(producer));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ std::shared_ptr<TestOutputProducer> producer2 = std::make_shared<TestOutputProducer>();
+ EXPECT_EQ(0, producer2->m_numTimesRequestedOutput);
+
+ EXPECT_TRUE(device.setOutputProducer(producer2));
+ EXPECT_TRUE(device.hasOutputProducer());
+ EXPECT_EQ(0, producer2->m_numTimesRequestedOutput);
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+}
+
+TEST(CommonDeviceTests, PushInput)
+{
+ TestDevice device("MyTestDevice");
+ std::shared_ptr<TestInputConsumer> consumer1 = std::make_shared<TestInputConsumer>();
+ std::shared_ptr<TestInputConsumer> consumer2 = std::make_shared<TestInputConsumer>();
+ std::shared_ptr<TestOutputProducer> producer = std::make_shared<TestOutputProducer>();
+ EXPECT_TRUE(device.addInputConsumer(consumer1));
+ EXPECT_TRUE(device.addInputConsumer(consumer2));
+ EXPECT_TRUE(device.setOutputProducer(producer));
+ EXPECT_EQ(0, consumer1->m_numTimesReceivedInput);
+ EXPECT_EQ(0, consumer2->m_numTimesReceivedInput);
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ device.pushInput();
+ EXPECT_EQ(1, consumer1->m_numTimesReceivedInput);
+ EXPECT_EQ(1, consumer2->m_numTimesReceivedInput);
+ EXPECT_TRUE(consumer1->m_lastReceivedInput.strings().hasData("helloWorld"));
+ EXPECT_TRUE(consumer2->m_lastReceivedInput.strings().hasData("helloWorld"));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+
+ // invalidate the state
+ consumer1->m_lastReceivedInput.resetAll();
+ consumer2->m_lastReceivedInput.resetAll();
+ EXPECT_EQ(1, consumer1->m_numTimesReceivedInput);
+ EXPECT_EQ(1, consumer2->m_numTimesReceivedInput);
+ EXPECT_FALSE(consumer1->m_lastReceivedInput.strings().hasData("helloWorld"));
+ EXPECT_FALSE(consumer2->m_lastReceivedInput.strings().hasData("helloWorld"));
+
+ device.pushInput();
+ EXPECT_EQ(2, consumer1->m_numTimesReceivedInput);
+ EXPECT_EQ(2, consumer2->m_numTimesReceivedInput);
+ EXPECT_TRUE(consumer1->m_lastReceivedInput.strings().hasData("helloWorld"));
+ EXPECT_TRUE(consumer2->m_lastReceivedInput.strings().hasData("helloWorld"));
+ EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
+}
+
+TEST(CommonDeviceTests, PullOutput)
+{
+ TestDevice device("MyTestDevice");
+ std::shared_ptr<TestOutputProducer> producer1 = std::make_shared<TestOutputProducer>();
+ std::shared_ptr<TestOutputProducer> producer2 = std::make_shared<TestOutputProducer>();
+ std::shared_ptr<TestInputConsumer> consumer = std::make_shared<TestInputConsumer>();
+ EXPECT_TRUE(device.setOutputProducer(producer1));
+ EXPECT_TRUE(device.setOutputProducer(producer2));
+ EXPECT_TRUE(device.addInputConsumer(consumer));
+ EXPECT_EQ(0, producer1->m_numTimesRequestedOutput);
+ EXPECT_EQ(0, producer2->m_numTimesRequestedOutput);
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device.pullOutput());
+ EXPECT_EQ(0, producer1->m_numTimesRequestedOutput);
+ EXPECT_EQ(1, producer2->m_numTimesRequestedOutput);
+ EXPECT_TRUE(device.getOutputData().integers().hasData("value"));
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ EXPECT_TRUE(device.pullOutput());
+ EXPECT_EQ(0, producer1->m_numTimesRequestedOutput);
+ EXPECT_EQ(2, producer2->m_numTimesRequestedOutput);
+ EXPECT_TRUE(device.getOutputData().integers().hasData("value"));
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+
+ // Test what happens when the producer returns false.
+ producer2->m_refuseToProduce = true;
+ EXPECT_FALSE(device.pullOutput());
+ EXPECT_EQ(0, producer1->m_numTimesRequestedOutput);
+ EXPECT_EQ(3, producer2->m_numTimesRequestedOutput);
+ EXPECT_FALSE(device.getOutputData().integers().hasData("value"));
+ EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
+}
+
+TEST(CommonDeviceTests, RemoveInputConsumer)
+{
+ TestDevice device("MyTestDevice");
+ EXPECT_FALSE(device.removeInputConsumer(std::shared_ptr<TestInputConsumer>()));
+ EXPECT_FALSE(device.removeInputConsumer(std::shared_ptr<TestInputConsumer>(nullptr)));
+
+ std::shared_ptr<TestInputConsumer> consumer1 = std::make_shared<TestInputConsumer>();
+ std::shared_ptr<TestInputConsumer> consumer2 = std::make_shared<TestInputConsumer>();
+ EXPECT_FALSE(device.removeInputConsumer(consumer1));
+ EXPECT_TRUE(device.addInputConsumer(consumer1));
+ EXPECT_TRUE(device.addInputConsumer(consumer2));
+ EXPECT_EQ(0, consumer1->m_numTimesReceivedInput);
+ EXPECT_EQ(0, consumer2->m_numTimesReceivedInput);
+
+ EXPECT_FALSE(device.removeInputConsumer(std::shared_ptr<TestInputConsumer>()));
+ EXPECT_FALSE(device.removeInputConsumer(std::shared_ptr<TestInputConsumer>(nullptr)));
+
+ device.pushInput();
+ EXPECT_EQ(1, consumer1->m_numTimesReceivedInput);
+ EXPECT_EQ(1, consumer2->m_numTimesReceivedInput);
+ EXPECT_TRUE(consumer1->m_lastReceivedInput.strings().hasData("helloWorld"));
+ EXPECT_TRUE(consumer2->m_lastReceivedInput.strings().hasData("helloWorld"));
+
+ // invalidate the state
+ consumer1->m_lastReceivedInput.resetAll();
+ consumer2->m_lastReceivedInput.resetAll();
+ EXPECT_EQ(1, consumer1->m_numTimesReceivedInput);
+ EXPECT_EQ(1, consumer2->m_numTimesReceivedInput);
+ EXPECT_FALSE(consumer1->m_lastReceivedInput.strings().hasData("helloWorld"));
+ EXPECT_FALSE(consumer2->m_lastReceivedInput.strings().hasData("helloWorld"));
+
+ EXPECT_TRUE(device.removeInputConsumer(consumer1));
+ device.pushInput();
+ EXPECT_EQ(1, consumer1->m_numTimesReceivedInput);
+ EXPECT_EQ(2, consumer2->m_numTimesReceivedInput);
+ EXPECT_FALSE(consumer1->m_lastReceivedInput.strings().hasData("helloWorld"));
+ EXPECT_TRUE(consumer2->m_lastReceivedInput.strings().hasData("helloWorld"));
+
+ EXPECT_FALSE(device.removeInputConsumer(consumer1));
+ device.pushInput();
+ EXPECT_EQ(1, consumer1->m_numTimesReceivedInput);
+ EXPECT_EQ(3, consumer2->m_numTimesReceivedInput);
+ EXPECT_FALSE(consumer1->m_lastReceivedInput.strings().hasData("helloWorld"));
+ EXPECT_TRUE(consumer2->m_lastReceivedInput.strings().hasData("helloWorld"));
+}
+
+TEST(CommonDeviceTests, RemoveOutputProducer)
+{
+ TestDevice device("MyTestDevice");
+ EXPECT_FALSE(device.removeOutputProducer(std::shared_ptr<TestOutputProducer>()));
+ EXPECT_FALSE(device.removeOutputProducer(std::shared_ptr<TestOutputProducer>(nullptr)));
+
+ std::shared_ptr<TestOutputProducer> producer1 = std::make_shared<TestOutputProducer>();
+ std::shared_ptr<TestOutputProducer> producer2 = std::make_shared<TestOutputProducer>();
+ EXPECT_FALSE(device.removeOutputProducer(producer1));
+ EXPECT_TRUE(device.setOutputProducer(producer1));
+ EXPECT_TRUE(device.setOutputProducer(producer2));
+ EXPECT_EQ(0, producer1->m_numTimesRequestedOutput);
+ EXPECT_EQ(0, producer2->m_numTimesRequestedOutput);
+
+ EXPECT_FALSE(device.removeOutputProducer(std::shared_ptr<TestOutputProducer>()));
+ EXPECT_FALSE(device.removeOutputProducer(std::shared_ptr<TestOutputProducer>(nullptr)));
+
+ EXPECT_TRUE(device.pullOutput());
+ EXPECT_EQ(0, producer1->m_numTimesRequestedOutput);
+ EXPECT_EQ(1, producer2->m_numTimesRequestedOutput);
+ EXPECT_TRUE(device.getOutputData().integers().hasData("value"));
+
+ EXPECT_FALSE(device.removeOutputProducer(producer1));
+ EXPECT_TRUE(device.pullOutput());
+ EXPECT_EQ(0, producer1->m_numTimesRequestedOutput);
+ EXPECT_EQ(2, producer2->m_numTimesRequestedOutput);
+ EXPECT_TRUE(device.getOutputData().integers().hasData("value"));
+
+ EXPECT_TRUE(device.removeOutputProducer(producer2));
+ EXPECT_FALSE(device.pullOutput());
+ EXPECT_EQ(0, producer1->m_numTimesRequestedOutput);
+ EXPECT_EQ(2, producer2->m_numTimesRequestedOutput);
+ EXPECT_FALSE(device.getOutputData().integers().hasData("value"));
+
+ EXPECT_FALSE(device.removeOutputProducer(producer2));
+ EXPECT_FALSE(device.pullOutput());
+ EXPECT_EQ(0, producer1->m_numTimesRequestedOutput);
+ EXPECT_EQ(2, producer2->m_numTimesRequestedOutput);
+ EXPECT_FALSE(device.getOutputData().integers().hasData("value"));
+}
diff --git a/SurgSim/Input/UnitTests/InputComponentTest.cpp b/SurgSim/Input/UnitTests/InputComponentTest.cpp
new file mode 100644
index 0000000..2da5b97
--- /dev/null
+++ b/SurgSim/Input/UnitTests/InputComponentTest.cpp
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 InputComponent class.
+ */
+
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "yaml-cpp/yaml.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+using SurgSim::Input::InputComponent;
+using SurgSim::DataStructures::DataGroup;
+
+TEST(InputComponentTest, CanConstruct)
+{
+ EXPECT_NO_THROW(InputComponent input("Input"); input.setDeviceName("InputDevice"));
+}
+
+TEST(InputComponentTest, Accessors)
+{
+ InputComponent input("Input");
+ input.setDeviceName("InputDevice");
+ EXPECT_EQ("Input", input.getName());
+ EXPECT_EQ("InputDevice", input.getDeviceName());
+}
+
+TEST(InputComponentTest, NotConnected)
+{
+ InputComponent input("Input");
+ input.setDeviceName("InputDevice");
+ DataGroup dataGroup;
+ EXPECT_THROW(input.getData(&dataGroup), SurgSim::Framework::AssertionFailure);
+ EXPECT_FALSE(input.isDeviceConnected());
+}
+
+TEST(InputComponentTest, Serialization)
+{
+ auto input = std::make_shared<InputComponent>("Input");
+ input->setDeviceName("InputDevice");
+
+ // Encode
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*input););
+
+ // Decode
+ std::shared_ptr<InputComponent> newInput;
+ EXPECT_NO_THROW(newInput = std::dynamic_pointer_cast<InputComponent>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()););
+
+ // Verify
+ EXPECT_EQ(input->getDeviceName(), newInput->getDeviceName());
+}
+
diff --git a/SurgSim/Input/UnitTests/InputManagerTest.cpp b/SurgSim/Input/UnitTests/InputManagerTest.cpp
new file mode 100644
index 0000000..12b2078
--- /dev/null
+++ b/SurgSim/Input/UnitTests/InputManagerTest.cpp
@@ -0,0 +1,214 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 InputManaget class. Note that InputManagerTest, the test fixture
+/// is declared as a friend class in InputManager to make it easier to test the
+/// add and removal of components, for this to work correctly PhysicsManagerTest is required
+/// to be in the SurgSim::Physics namespace.
+
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/Input/InputManager.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Input/OutputComponent.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+
+#include "SurgSim/Input/UnitTests/TestDevice.h"
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Component;
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Input::CommonDevice;
+using SurgSim::Input::OutputProducerInterface;
+using SurgSim::Input::InputManager;
+using SurgSim::Input::InputComponent;
+using SurgSim::Input::OutputComponent;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+
+
+class MockComponent : public SurgSim::Framework::Component
+{
+public:
+ explicit MockComponent(const std::string& name = "MockComponent") : Component(name) {}
+ virtual ~MockComponent() {}
+
+protected:
+ virtual bool doInitialize() {return true;}
+ virtual bool doWakeUp() {return true;}
+};
+namespace SurgSim
+{
+namespace Input
+{
+
+class InputManagerTest : public ::testing::Test
+{
+public:
+ virtual void SetUp()
+ {
+ testDevice1 = std::make_shared<TestDevice>("TestDevice1");
+ testDevice2 = std::make_shared<TestDevice>("TestDevice2");
+
+ runtime = std::make_shared<Runtime>();
+ inputManager = std::make_shared<InputManager>();
+
+ runtime->addManager(inputManager);
+ runtime->start();
+
+ inputManager->addDevice(testDevice1);
+ inputManager->addDevice(testDevice2);
+ }
+
+ virtual void TearDown()
+ {
+ runtime->stop();
+ }
+
+ bool testDoAddComponent(const std::shared_ptr<Component>& component)
+ {
+ return inputManager->executeAdditions(component);
+ }
+
+ bool testDoRemoveComponent(const std::shared_ptr<Component>& component)
+ {
+ return inputManager->executeRemovals(component);
+ }
+
+ std::shared_ptr<TestDevice> testDevice1;
+ std::shared_ptr<TestDevice> testDevice2;
+
+ std::shared_ptr<Runtime> runtime;
+ std::shared_ptr<InputManager> inputManager;
+};
+
+TEST_F(InputManagerTest, DeviceAddRemove)
+{
+
+ std::shared_ptr<DeviceInterface> testDevice3 = std::make_shared<TestDevice>("TestDevice3");
+ std::shared_ptr<DeviceInterface> testDevice4 = std::make_shared<TestDevice>("TestDevice4");
+
+ EXPECT_TRUE(inputManager->addDevice(testDevice3));
+ EXPECT_TRUE(inputManager->addDevice(testDevice4));
+ EXPECT_FALSE(inputManager->addDevice(testDevice3));
+ EXPECT_TRUE(inputManager->removeDevice(testDevice4));
+ EXPECT_FALSE(inputManager->removeDevice(testDevice4));
+ EXPECT_TRUE(inputManager->addDevice(testDevice4));
+}
+
+TEST_F(InputManagerTest, InputAddRemove)
+{
+ std::shared_ptr<InputComponent> listener1 = std::make_shared<InputComponent>("Component1");
+ std::shared_ptr<InputComponent> listener2 = std::make_shared<InputComponent>("Component2");
+ std::shared_ptr<InputComponent> listener3 = std::make_shared<InputComponent>("Component3");
+ std::shared_ptr<InputComponent> notvalid = std::make_shared<InputComponent>("Component4");
+
+ listener1->setDeviceName("TestDevice1");
+ listener2->setDeviceName("TestDevice1");
+ listener3->setDeviceName("TestDevice2");
+ notvalid->setDeviceName("NonExistantDevice");
+
+ // Add various listeners to the input manager
+ EXPECT_TRUE(testDoAddComponent(listener1));
+ EXPECT_TRUE(testDoAddComponent(listener2));
+ EXPECT_TRUE(testDoAddComponent(listener3));
+ EXPECT_FALSE(testDoAddComponent(notvalid));
+
+ // Excercise adds and removes
+
+ // Duplicate false on duplicate will become deprecated
+ EXPECT_FALSE(testDoAddComponent(listener1));
+ EXPECT_TRUE(testDoRemoveComponent(listener1));
+ EXPECT_FALSE(testDoRemoveComponent(listener1));
+
+ // Should not be able to add random components
+ std::shared_ptr<MockComponent> component = std::make_shared<MockComponent>();
+ EXPECT_FALSE(testDoAddComponent(component));
+}
+
+TEST_F(InputManagerTest, InputfromDevice)
+{
+ std::string data;
+ SurgSim::DataStructures::DataGroup dataGroup;
+
+ std::shared_ptr<InputComponent> listener1 = std::make_shared<InputComponent>("Component1");
+ listener1->setDeviceName("TestDevice1");
+
+ testDoAddComponent(listener1);
+
+ EXPECT_TRUE(listener1->isDeviceConnected());
+ EXPECT_NO_THROW(listener1->getData(&dataGroup));
+
+ testDevice1->pushInput("avalue");
+ EXPECT_NO_THROW(listener1->getData(&dataGroup));
+ EXPECT_TRUE(dataGroup.strings().get("helloWorld",&data));
+ EXPECT_EQ("avalue",data);
+
+ testDevice1->pushInput("bvalue");
+ EXPECT_NO_THROW(listener1->getData(&dataGroup));
+ EXPECT_TRUE(dataGroup.strings().get("helloWorld",&data));
+ EXPECT_EQ("bvalue",data);
+}
+
+TEST_F(InputManagerTest, OutputAddRemove)
+{
+ std::shared_ptr<OutputComponent> output1 = std::make_shared<OutputComponent>("Component1");
+ std::shared_ptr<OutputComponent> output2 = std::make_shared<OutputComponent>("Component2");
+ std::shared_ptr<OutputComponent> output3 = std::make_shared<OutputComponent>("Component3");
+ std::shared_ptr<OutputComponent> invalid = std::make_shared<OutputComponent>("Component4");
+ output1->setDeviceName("TestDevice1");
+ output2->setDeviceName("TestDevice1");
+ output3->setDeviceName("TestDevice2");
+ invalid->setDeviceName("InvalidDevice");
+ EXPECT_TRUE(testDoAddComponent(output1));
+ EXPECT_FALSE(testDoAddComponent(output2)); // same device already attached to an OutputComponent
+ EXPECT_FALSE(testDoAddComponent(output2));
+ EXPECT_TRUE(testDoAddComponent(output3));
+ EXPECT_FALSE(testDoAddComponent(invalid));
+ EXPECT_TRUE(testDoRemoveComponent(output1));
+ EXPECT_FALSE(testDoRemoveComponent(output1));
+}
+
+TEST_F(InputManagerTest, OutputPush)
+{
+ std::shared_ptr<OutputComponent> output = std::make_shared<OutputComponent>("Component1");
+ output->setDeviceName("TestDevice1");
+ EXPECT_TRUE(testDoAddComponent(output));
+ DataGroupBuilder builder;
+ builder.addString("data");
+ DataGroup data = builder.createData();
+ data.strings().set("data", "outputdata");
+ output->setData(data);
+ EXPECT_TRUE(testDevice1->pullOutput());
+ EXPECT_EQ("outputdata", testDevice1->lastPulledData);
+}
+
+TEST_F(InputManagerTest, TypeTest)
+{
+ EXPECT_EQ(SurgSim::Framework::MANAGER_TYPE_INPUT, inputManager->getType());
+}
+
+}; // namespace Input
+}; // namespace SurgSim
+
diff --git a/SurgSim/Input/UnitTests/OutputComponentTest.cpp b/SurgSim/Input/UnitTests/OutputComponentTest.cpp
new file mode 100644
index 0000000..a6c90f2
--- /dev/null
+++ b/SurgSim/Input/UnitTests/OutputComponentTest.cpp
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 InputComponent class.
+ */
+
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+#include "SurgSim/Input/OutputComponent.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "yaml-cpp/yaml.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+using SurgSim::Input::OutputComponent;
+using SurgSim::DataStructures::DataGroup;
+
+TEST(OutputComponentTest, CanConstruct)
+{
+ EXPECT_NO_THROW(OutputComponent output("Output"); output.setDeviceName("OutputDevice"));
+}
+
+TEST(OutputComponentTest, Accessors)
+{
+ OutputComponent output("Output");
+ output.setDeviceName("OutputDevice");
+ EXPECT_EQ("Output", output.getName());
+ EXPECT_EQ("OutputDevice", output.getDeviceName());
+}
+
+TEST(OutputComponentTest, NotConnected)
+{
+ OutputComponent output("Output");
+ output.setDeviceName("OutputDevice");
+ DataGroup dataGroup;
+ EXPECT_THROW(output.setData(dataGroup), SurgSim::Framework::AssertionFailure);
+ EXPECT_FALSE(output.isDeviceConnected());
+}
+
+TEST(OutputComponentTest, Serialization)
+{
+ auto output = std::make_shared<OutputComponent>("Output");
+ output->setDeviceName("OutputDevice");
+
+ // Encode
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*output););
+
+ // Decode
+ std::shared_ptr<OutputComponent> newOutput;
+ EXPECT_NO_THROW(newOutput = std::dynamic_pointer_cast<OutputComponent>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()););
+
+ // Verify
+ EXPECT_EQ(output->getDeviceName(), newOutput->getDeviceName());
+}
+
diff --git a/SurgSim/Input/UnitTests/TestDevice.cpp b/SurgSim/Input/UnitTests/TestDevice.cpp
new file mode 100644
index 0000000..60c65ee
--- /dev/null
+++ b/SurgSim/Input/UnitTests/TestDevice.cpp
@@ -0,0 +1,93 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Input/UnitTests/TestDevice.h"
+
+TestDevice::TestDevice(const std::string& uniqueName) :
+ CommonDevice(uniqueName, buildInputData())
+{
+
+}
+
+// required by the DeviceInterface API
+bool TestDevice::initialize()
+{
+ return true;
+}
+
+// required by the DeviceInterface API
+bool TestDevice::finalize()
+{
+ return true;
+}
+
+// expose the pushInput method to the world
+void TestDevice::pushInput()
+{
+ CommonDevice::pushInput();
+}
+
+void TestDevice::pushInput(const std::string& data)
+{
+ getInputData().strings().set("helloWorld", data);
+ pushInput();
+}
+
+// expose the pullOutput method to the world
+bool TestDevice::pullOutput()
+{
+ bool result = CommonDevice::pullOutput();
+ getOutputData().strings().get("data", &lastPulledData);
+ return result;
+}
+
+// expose the getOutputData method to the world
+const DataGroup& TestDevice::getOutputData() const
+{
+ return CommonDevice::getOutputData();
+}
+
+DataGroup TestDevice::buildInputData()
+{
+ DataGroupBuilder builder;
+ builder.addString("helloWorld");
+ DataGroup data = builder.createData();
+ data.strings().set("helloWorld", "data");
+ return data;
+}
+
+// Consumer Class Callback Function
+void TestInputConsumer::handleInput(const std::string& device, const DataGroup& inputData)
+{
+ ++m_numTimesReceivedInput;
+ m_lastReceivedInput = inputData;
+}
+
+// Producer class Hook
+bool TestOutputProducer::requestOutput(const std::string& device, DataGroup* outputData)
+{
+ ++m_numTimesRequestedOutput;
+
+ if (m_refuseToProduce)
+ {
+ return false;
+ }
+ else
+ {
+ *outputData = m_nextSentOutput;
+ return true;
+ }
+}
+
diff --git a/SurgSim/Input/UnitTests/TestDevice.h b/SurgSim/Input/UnitTests/TestDevice.h
new file mode 100644
index 0000000..644b890
--- /dev/null
+++ b/SurgSim/Input/UnitTests/TestDevice.h
@@ -0,0 +1,95 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_INPUT_UNITTESTS_TESTDEVICE_H
+#define SURGSIM_INPUT_UNITTESTS_TESTDEVICE_H
+
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+
+using SurgSim::Input::CommonDevice;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Input::InputConsumerInterface;
+using SurgSim::Input::OutputProducerInterface;
+
+
+class TestDevice : public CommonDevice
+{
+public:
+ explicit TestDevice(const std::string& uniqueName);
+
+ virtual bool initialize();
+
+ virtual bool finalize();
+
+ virtual void pushInput();
+
+ // Send some data down the stream
+ void pushInput(const std::string& data);
+
+ virtual bool pullOutput();
+
+ const DataGroup& getOutputData() const;
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static DataGroup buildInputData();
+ DataGroup buildOutputData();
+
+ std::string lastPulledData;
+};
+
+
+struct TestInputConsumer : public InputConsumerInterface
+{
+public:
+ TestInputConsumer() :
+ m_numTimesReceivedInput(0)
+ {
+ }
+
+ virtual void initializeInput(const std::string& device, const DataGroup& initialInput)
+ {
+ }
+ virtual void handleInput(const std::string& device, const DataGroup& inputData);
+
+ int m_numTimesReceivedInput;
+ DataGroup m_lastReceivedInput;
+};
+
+struct TestOutputProducer : public OutputProducerInterface
+{
+public:
+ TestOutputProducer() :
+ m_numTimesRequestedOutput(0),
+ m_refuseToProduce(false)
+ {
+ DataGroupBuilder builder;
+ builder.addInteger("value");
+ m_nextSentOutput = builder.createData();
+ m_nextSentOutput.integers().set("value", 123);
+ }
+
+ virtual bool requestOutput(const std::string& device, DataGroup* outputData);
+
+ int m_numTimesRequestedOutput;
+ bool m_refuseToProduce;
+ DataGroup m_nextSentOutput;
+};
+
+#endif // SURGSIM_INPUT_UNITTESTS_TESTDEVICE_H
diff --git a/SurgSim/Math/Aabb.h b/SurgSim/Math/Aabb.h
new file mode 100644
index 0000000..8f034b4
--- /dev/null
+++ b/SurgSim/Math/Aabb.h
@@ -0,0 +1,84 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_AABB_H
+#define SURGSIM_MATH_AABB_H
+
+#include <Eigen/Geometry>
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// Wrapper around the Eigen type
+typedef Eigen::AlignedBox<float, 3> Aabbf;
+
+/// Wrapper around the Eigen type
+typedef Eigen::AlignedBox<double, 3> Aabbd;
+
+/// Determine whether two AABBs have an intersection with each other, for the calculation see
+/// http://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?page=3
+/// \tparam Scalar numeric type
+/// \tparam Dim dimension of the space to be used
+/// \param aabb0 first axis aligned bounding box
+/// \param aabb1 second axis aligned bounding box
+/// \param tolerance the bounding boxes will be considered bigger by this amount
+/// \return true if there is an overlap between the two boxes
+template <class Scalar, int Dim>
+bool doAabbIntersect(const Eigen::AlignedBox<Scalar, Dim>& aabb0,
+ const Eigen::AlignedBox<Scalar, Dim>& aabb1,
+ double tolerance)
+{
+ typedef typename Eigen::AlignedBox<Scalar, Dim>::VectorType VectorType;
+
+ VectorType vector = (aabb1.center() - aabb0.center()).array().abs();
+ VectorType totalSizes = ((aabb0.sizes() + aabb1.sizes()) * 0.5).array() + tolerance;
+
+ return (vector.array() <= totalSizes.array()).all();
+}
+
+/// Determine whether two AABBs overlap, using a minimal set of eigen calls, does not take a tolerance
+/// \tparam Scalar numeric type
+/// \tparam Dim dimension of the space to be used
+/// \param a first axis aligned bounding box
+/// \param b second axis aligned bounding box
+/// \return true if there is an overlap between the two boxes
+template <class Scalar, int Dim>
+bool doAabbIntersect(const Eigen::AlignedBox<Scalar, Dim>& a,
+ const Eigen::AlignedBox<Scalar, Dim>& b)
+{
+ return !a.intersection(b).isEmpty();
+}
+/// Convenience function for creating a bounding box from three vertices (e.g. the vertices of a triangle)
+/// \tparam Scalar numeric type
+/// \tparam Dim dimension of the space to be used
+/// \tparam MType the eigen type of the vectors
+/// \return an AABB containing all the points passed
+template <class Scalar, int Dim, int MType>
+Eigen::AlignedBox<Scalar, Dim> makeAabb(
+ const Eigen::Matrix<Scalar, Dim, 1, MType>& vector0,
+ const Eigen::Matrix<Scalar, Dim, 1, MType>& vector1,
+ const Eigen::Matrix<Scalar, Dim, 1, MType>& vector2)
+{
+ Eigen::AlignedBox<Scalar, Dim> result(vector0);
+ result.extend(vector1);
+ result.extend(vector2);
+ return std::move(result);
+}
+}
+}
+
+#endif
diff --git a/SurgSim/Math/BoxShape.cpp b/SurgSim/Math/BoxShape.cpp
new file mode 100644
index 0000000..6fccf1e
--- /dev/null
+++ b/SurgSim/Math/BoxShape.cpp
@@ -0,0 +1,129 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/BoxShape.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::BoxShape, BoxShape);
+
+BoxShape::BoxShape(double sizeX, double sizeY, double sizeZ) :
+ m_size(Vector3d(sizeX, sizeY, sizeZ))
+{
+ calculateVertices();
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoxShape, double, SizeX, getSizeX, setSizeX);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoxShape, double, SizeY, getSizeY, setSizeY);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoxShape, double, SizeZ, getSizeZ, setSizeZ);
+}
+
+
+int BoxShape::getType()
+{
+ return SHAPE_TYPE_BOX;
+}
+
+Vector3d BoxShape::getSize() const
+{
+ return m_size;
+}
+
+double BoxShape::getSizeX() const
+{
+ return m_size[0];
+}
+
+double BoxShape::getSizeY() const
+{
+ return m_size[1];
+}
+
+double BoxShape::getSizeZ() const
+{
+ return m_size[2];
+}
+
+void BoxShape::setSizeX(double sizeX)
+{
+ m_size[0] = sizeX;
+}
+
+void BoxShape::setSizeY(double sizeY)
+{
+ m_size[1] = sizeY;
+}
+
+void BoxShape::setSizeZ(double sizeZ)
+{
+ m_size[2] = sizeZ;
+}
+
+double BoxShape::getVolume() const
+{
+ return m_size[0] * m_size[1] * m_size[2];
+}
+
+SurgSim::Math::Vector3d BoxShape::getCenter() const
+{
+ return Vector3d(0.0, 0.0, 0.0);
+}
+
+SurgSim::Math::Matrix33d BoxShape::getSecondMomentOfVolume() const
+{
+ const double volume = getVolume();
+
+ const Vector3d sizeSquared = m_size.array() * m_size.array();
+ const double coef = 1.0 / 12.0 * volume;
+ Matrix33d inertia = Matrix33d::Zero();
+ inertia.diagonal() = coef * Vector3d(sizeSquared[1] + sizeSquared[2],
+ sizeSquared[0] + sizeSquared[2],
+ sizeSquared[0] + sizeSquared[1]);
+ return inertia;
+}
+
+SurgSim::Math::Vector3d BoxShape::getVertex(const int i) const
+{
+ return m_vertices[i];
+}
+
+const std::array<Vector3d, 8>& BoxShape::getVertices() const
+{
+ return m_vertices;
+}
+
+void BoxShape::calculateVertices()
+{
+ static const std::array<Vector3d, 8> multiplier = {{Vector3d(-0.5, -0.5, -0.5),
+ Vector3d(-0.5, -0.5, 0.5),
+ Vector3d(-0.5, 0.5, 0.5),
+ Vector3d(-0.5, 0.5, -0.5),
+ Vector3d( 0.5, -0.5, -0.5),
+ Vector3d( 0.5, -0.5, 0.5),
+ Vector3d( 0.5, 0.5, 0.5),
+ Vector3d( 0.5, 0.5, -0.5)}};
+ for(int i = 0; i < 8; ++i)
+ {
+ m_vertices[i] = m_size.array() * multiplier[i].array();
+ }
+}
+
+bool BoxShape::isValid() const
+{
+ return (m_size.minCoeff() >= 0);
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/BoxShape.h b/SurgSim/Math/BoxShape.h
new file mode 100644
index 0000000..ca6bb3c
--- /dev/null
+++ b/SurgSim/Math/BoxShape.h
@@ -0,0 +1,116 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_BOXSHAPE_H
+#define SURGSIM_MATH_BOXSHAPE_H
+
+#include <array>
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_STATIC_REGISTRATION(BoxShape);
+
+/// Box shape: box centered on (0 0 0), aligned with the axis
+/// with different sizes along X, Y and Z
+class BoxShape: public Shape
+{
+public:
+ /// Constructor
+ /// \param sizeX, sizeY, sizeZ the box sizes in all 3 directions (in m)
+ BoxShape(double sizeX = 0.0, double sizeY = 0.0, double sizeZ = 0.0);
+
+ SURGSIM_CLASSNAME(SurgSim::Math::BoxShape);
+
+ /// \return the type of the shape
+ virtual int getType() override;
+
+ /// Get size of the box
+ /// \return the size of the box (in m)
+ Vector3d getSize() const;
+
+ /// Get size in X direction
+ /// \return the size in the X direction (in m)
+ double getSizeX() const;
+
+ /// Get size in Y direction
+ /// \return the size in the Y direction (in m)
+ double getSizeY() const;
+
+ /// Get size in Z direction
+ /// \return the size in the Z direction (in m)
+ double getSizeZ() const;
+
+ /// Get the volume of the shape
+ /// \return The volume of the shape (in m-3)
+ virtual double getVolume() const override;
+
+ /// Get the volumetric center of the shape
+ /// \return The center of the shape
+ virtual Vector3d getCenter() const override;
+
+ /// Get the second central moment of the volume, commonly used
+ /// to calculate the moment of inertia matrix
+ /// \return The 3x3 symmetric second moment matrix
+ virtual Matrix33d getSecondMomentOfVolume() const override;
+
+ /// Function that returns the local vertex location, given an index.
+ /// \param i The vertex index.
+ /// \return The local vertex position.
+ Vector3d getVertex(const int i) const;
+
+ /// Function that returns the local vertices' location
+ /// \return All eight vertices of the box
+ const std::array<Vector3d, 8>& getVertices() const;
+
+ /// \return True if size along X, Y, Z are bigger than or equal to 0; Otherwise, false.
+ virtual bool isValid() const override;
+
+protected:
+ // Setters in 'protected' sections are for serialization purpose only.
+
+ /// Set size in X direction
+ /// \param sizeX the size in the X direction (in m)
+ void setSizeX(double sizeX);
+
+ /// Set size in Y direction
+ /// \param sizeY the size in the Y direction (in m)
+ void setSizeY(double sizeY);
+
+ /// Set size in Z direction
+ /// \param sizeZ the size in the Z direction (in m)
+ void setSizeZ(double sizeZ);
+
+private:
+ /// Function that calculates the box vertices.
+ void calculateVertices();
+
+ /// The box sizes along the 3 axis respectively {X,Y,Z}
+ Vector3d m_size;
+
+ /// The box vertices.
+ std::array<Vector3d,8> m_vertices;
+};
+
+}; // Math
+
+}; // SurgSim
+
+#endif // SURGSIM_MATH_BOXSHAPE_H
diff --git a/SurgSim/Math/CMakeLists.txt b/SurgSim/Math/CMakeLists.txt
new file mode 100644
index 0000000..510e3de
--- /dev/null
+++ b/SurgSim/Math/CMakeLists.txt
@@ -0,0 +1,123 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+
+set(SURGSIM_MATH_SOURCES
+ BoxShape.cpp
+ CapsuleShape.cpp
+ CylinderShape.cpp
+ DoubleSidedPlaneShape.cpp
+ GaussLegendreQuadrature.cpp
+ LinearSolveAndInverse.cpp
+ MathConvert.cpp
+ MeshShape.cpp
+ MlcpGaussSeidelSolver.cpp
+ MlcpProblem.cpp
+ OctreeShape.cpp
+ OdeEquation.cpp
+ OdeSolver.cpp
+ OdeSolverEulerExplicit.cpp
+ OdeSolverEulerExplicitModified.cpp
+ OdeSolverEulerImplicit.cpp
+ OdeSolverLinearEulerExplicit.cpp
+ OdeSolverLinearEulerExplicitModified.cpp
+ OdeSolverLinearEulerImplicit.cpp
+ OdeSolverLinearRungeKutta4.cpp
+ OdeSolverLinearStatic.cpp
+ OdeSolverRungeKutta4.cpp
+ OdeSolverStatic.cpp
+ OdeState.cpp
+ PlaneShape.cpp
+ Shape.cpp
+ SphereShape.cpp
+ SurfaceMeshShape.cpp
+)
+
+set(SURGSIM_MATH_HEADERS
+ Aabb.h
+ BoxShape.h
+ CapsuleShape.h
+ CylinderShape.h
+ DoubleSidedPlaneShape.h
+ GaussLegendreQuadrature.h
+ Geometry.h
+ LinearSolveAndInverse.h
+ LinearSolveAndInverse-inl.h
+ MathConvert.h
+ MathConvert-inl.h
+ Matrix.h
+ MeshShape.h
+ MeshShape-inl.h
+ MlcpConstraintType.h
+ MlcpConstraintTypeName.h
+ MlcpGaussSeidelSolver.h
+ MlcpProblem.h
+ MlcpSolution.h
+ MlcpSolver.h
+ OctreeShape.h
+ OctreeShape-inl.h
+ OdeEquation.h
+ OdeSolver.h
+ OdeSolverEulerExplicit.h
+ OdeSolverEulerExplicitModified.h
+ OdeSolverEulerImplicit.h
+ OdeSolverLinearEulerExplicit.h
+ OdeSolverLinearEulerExplicitModified.h
+ OdeSolverLinearEulerImplicit.h
+ OdeSolverLinearRungeKutta4.h
+ OdeSolverLinearStatic.h
+ OdeSolverRungeKutta4.h
+ OdeSolverStatic.h
+ OdeState.h
+ PlaneShape.h
+ Quaternion.h
+ RigidTransform.h
+ Shape.h
+ Shapes.h
+ SphereShape.h
+ SurfaceMeshShape.h
+ SurfaceMeshShape-inl.h
+ TriangleTriangleContactCalculation-inl.h
+ TriangleTriangleIntersection-inl.h
+ Valid.h
+ Valid-inl.h
+ Vector.h
+)
+
+surgsim_add_library(
+ SurgSimMath
+ "${SURGSIM_MATH_SOURCES}"
+ "${SURGSIM_MATH_HEADERS}"
+ "SurgSim/Math"
+)
+
+set(LIBS
+ SurgSimDataStructures
+ SurgSimFramework
+ ${YAML_CPP_LIBRARIES}
+)
+
+target_link_libraries(SurgSimMath ${LIBS}
+)
+add_dependencies(SurgSimMath yaml-cpp)
+
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+endif()
+
+# Put SurgSimMath into folder "Math"
+set_target_properties(SurgSimMath PROPERTIES FOLDER "Math")
diff --git a/SurgSim/Math/CapsuleShape.cpp b/SurgSim/Math/CapsuleShape.cpp
new file mode 100644
index 0000000..0911cbf
--- /dev/null
+++ b/SurgSim/Math/CapsuleShape.cpp
@@ -0,0 +1,125 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/CapsuleShape.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::CapsuleShape, CapsuleShape);
+
+CapsuleShape::CapsuleShape(double length, double radius) : m_length(length), m_radius(radius)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CapsuleShape, double, Radius, getRadius, setRadius);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CapsuleShape, double, Length, getLength, setLength);
+}
+
+int CapsuleShape::getType()
+{
+ return SHAPE_TYPE_CAPSULE;
+}
+
+double CapsuleShape::getLength() const
+{
+ return m_length;
+}
+
+double CapsuleShape::getRadius() const
+{
+ return m_radius;
+}
+
+void CapsuleShape::setLength(double length)
+{
+ m_length = length;
+}
+
+void CapsuleShape::setRadius(double radius)
+{
+ m_radius = radius;
+}
+
+double CapsuleShape::getVolume() const
+{
+ const double r2 = m_radius * m_radius;
+ const double localCylinderVolume = M_PI * (r2) * m_length;
+ const double localSphereVolume = 4.0 / 3.0 * M_PI * r2 * m_radius;
+
+ return localCylinderVolume + localSphereVolume;
+}
+
+SurgSim::Math::Vector3d CapsuleShape::getCenter() const
+{
+ return Vector3d(0.0, 0.0, 0.0);
+}
+
+SurgSim::Math::Vector3d CapsuleShape::topCenter() const
+{
+ return Vector3d(0.0, m_length / 2.0, 0.0);
+}
+
+SurgSim::Math::Vector3d CapsuleShape::bottomCenter() const
+{
+ return Vector3d(0.0, -m_length / 2.0, 0.0);
+}
+
+SurgSim::Math::Matrix33d CapsuleShape::getSecondMomentOfVolume() const
+{
+ const double &r = m_radius;
+ const double &l = m_length;
+ const double r2 = r * r;
+ const double l2 = l * l;
+ const double cylinderVolume = M_PI * (r2) * l;
+ const double sphereVolume = 4.0 / 3.0 * M_PI * r2 * r;
+
+ // The matrix is a combination of the cylinder and
+ // the 2 hemispheres:
+ //
+ // Second central moment of cylinder along the Y axis
+ // vc = PI.radius.radius.length (volume of the cylinder)
+ // a = 1/2.vc.r^2
+ // b = 1/12.vc.(3.r^2 + h^2)
+ // (b 0 0)
+ // I(cylinder) = (0 a 0)
+ // (0 0 b)
+ //
+ // Second central moment of the 2 hemispheres along the X axis (direction = 0)
+ // vs = 4/3 pi.radius.radius.radius (volume of the entire sphere)
+ // c = 2/5.vs.r^2
+ // d = 2/5.vs.r^2 + vs.h^2/4 + 3/8.vs.r.h
+ // (d 0 0)
+ // I(2 hemispheres) = (0 c 0)
+ // (0 0 d)
+ double a = 1.0 / 2.0 * cylinderVolume * r2;
+ double b = 1.0 / 12.0 * cylinderVolume * (3.0 * r2 + l2);
+ double c = 2.0 / 5.0 * sphereVolume * r2;
+ double d = c + sphereVolume * l * (l / 4.0 + 3.0 / 8.0 * r);
+
+ Matrix33d secondMoment;
+ secondMoment.setZero();
+ secondMoment.diagonal().setConstant(b + d);
+ secondMoment(1, 1) = a + c;
+
+ return secondMoment;
+}
+
+bool CapsuleShape::isValid() const
+{
+ return (m_length >= 0) && (m_radius >= 0);
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/CapsuleShape.h b/SurgSim/Math/CapsuleShape.h
new file mode 100644
index 0000000..745e3e1
--- /dev/null
+++ b/SurgSim/Math/CapsuleShape.h
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_CAPSULESHAPE_H
+#define SURGSIM_MATH_CAPSULESHAPE_H
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_STATIC_REGISTRATION(CapsuleShape);
+
+/// Capsule shape: centered on (0, 0, 0), aligned along Y,
+/// with length and radius
+class CapsuleShape: public Shape
+{
+public:
+ /// Constructor
+ /// \param length The capsule length (i.e. of the cylinder) (in m)
+ /// \param radius The capsule radius (i.e. of the cylinder/spheres) (in m)
+ CapsuleShape(double length = 0.0, double radius = 0.0);
+
+ SURGSIM_CLASSNAME(SurgSim::Math::CapsuleShape);
+
+ /// \return the type of the shape
+ virtual int getType() override;
+
+ /// Get the capsule length (i.e. cylinder length)
+ /// \return The capsule length (in m)
+ double getLength() const;
+
+ /// Get the capsule radius (i.e. cylinder/spheres radius)
+ /// \return The capsule radius (in m)
+ double getRadius() const;
+
+ /// Get the volume of the shape
+ /// \return The volume of the shape (in m-3)
+ virtual double getVolume() const override;
+
+ /// Get the volumetric center of the shape
+ /// \return The center of the shape
+ virtual Vector3d getCenter() const override;
+
+ /// Return the center of the top sphere of the internal cylinder
+ /// \return The top center of the sphere of the capsule
+ Vector3d topCenter() const;
+
+ /// Return the center of the bottom sphere of the internal cylinder
+ /// \return The bottom center of the sphere of the capsule
+ Vector3d bottomCenter() const;
+
+ /// Get the second central moment of the volume, commonly used
+ /// to calculate the moment of inertia matrix
+ /// \return The 3x3 symmetric second moment matrix
+ virtual Matrix33d getSecondMomentOfVolume() const override;
+
+ /// \return True if length and radius are bigger than or equal to 0; Otherwise, false.
+ virtual bool isValid() const override;
+
+protected:
+ // Setters in 'protected' sections are for serialization purpose only.
+
+ /// Set the capsule length (i.e. cylinder length)
+ /// \param length The capsule length (in m)
+ void setLength(double length);
+
+ /// Set the capsule radius (i.e. cylinder/spheres radius)
+ /// \param radius The capsule radius (in m)
+ void setRadius(double radius);
+
+private:
+ /// Capsule length
+ double m_length;
+
+ /// Capsule radius
+ double m_radius;
+};
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_CAPSULESHAPE_H
diff --git a/SurgSim/Math/CylinderShape.cpp b/SurgSim/Math/CylinderShape.cpp
new file mode 100644
index 0000000..056a15f
--- /dev/null
+++ b/SurgSim/Math/CylinderShape.cpp
@@ -0,0 +1,88 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/CylinderShape.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::CylinderShape, CylinderShape);
+
+CylinderShape::CylinderShape(double length, double radius) : m_length(length), m_radius(radius)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CylinderShape, double, Radius, getRadius, setRadius);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CylinderShape, double, Length, getLength, setLength);
+}
+
+int CylinderShape::getType()
+{
+ return SHAPE_TYPE_CYLINDER;
+}
+
+double CylinderShape::getLength() const
+{
+ return m_length;
+}
+
+double CylinderShape::getRadius() const
+{
+ return m_radius;
+}
+
+
+void CylinderShape::setLength(double length)
+{
+ m_length = length;
+}
+
+void CylinderShape::setRadius(double radius)
+{
+ m_radius = radius;
+}
+
+double CylinderShape::getVolume() const
+{
+ return M_PI * m_radius * m_radius * m_length;
+}
+
+SurgSim::Math::Vector3d CylinderShape::getCenter() const
+{
+ return Vector3d(0.0, 0.0, 0.0);
+}
+
+SurgSim::Math::Matrix33d CylinderShape::getSecondMomentOfVolume() const
+{
+ const double volume = getVolume();
+ const double coef = 1.0 / 12.0 * volume;
+ const double coefDir = 1.0 / 2.0 * volume;
+ const double squareL = m_length * m_length;
+ const double squareRadius = m_radius * m_radius;
+
+ Matrix33d secondMoment;
+ secondMoment.setZero();
+ secondMoment.diagonal().setConstant(coef * (3.0 * squareRadius + squareL));
+ secondMoment(1, 1) = coefDir * (squareRadius);
+
+ return secondMoment;
+}
+
+bool CylinderShape::isValid() const
+{
+ return (m_length >= 0) && (m_radius >= 0);
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/CylinderShape.h b/SurgSim/Math/CylinderShape.h
new file mode 100644
index 0000000..296b130
--- /dev/null
+++ b/SurgSim/Math/CylinderShape.h
@@ -0,0 +1,90 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_CYLINDERSHAPE_H
+#define SURGSIM_MATH_CYLINDERSHAPE_H
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_STATIC_REGISTRATION(CylinderShape);
+
+/// Cylinder shape: centered on (0 0 0), aligned along Y,
+/// defined with length and radius.
+class CylinderShape: public Shape
+{
+public:
+ /// Constructor
+ /// \param length The length of the cylinder (in m)
+ /// \param radius The cylinder radius (in m)
+ CylinderShape(double length = 0.0, double radius = 0.0);
+
+ SURGSIM_CLASSNAME(SurgSim::Math::CylinderShape);
+
+ /// \return the type of the shape
+ virtual int getType() override;
+
+ /// Get the cylinder length
+ /// \return The cylinder length (in m)
+ double getLength() const;
+
+ /// Get the cylinder radius
+ /// \return The cylinder radius (in m)
+ double getRadius() const;
+
+ /// Get the volume of the shape
+ /// \return The volume of the shape (in m-3)
+ virtual double getVolume() const override;
+
+ /// Get the volumetric center of the shape
+ /// \return The center of the shape
+ virtual Vector3d getCenter() const override;
+
+ /// Get the second central moment of the volume, commonly used
+ /// to calculate the moment of inertia matrix
+ /// \return The 3x3 symmetric second moment matrix
+ virtual Matrix33d getSecondMomentOfVolume() const override;
+
+ /// \return True if length and radius are bigger than or equal to 0; Otherwise, false.
+ virtual bool isValid() const override;
+
+protected:
+ // Setters in 'protected' sections are for serialization purpose only.
+
+ /// Set the cylinder length
+ /// \param length The capsule length (in m)
+ void setLength(double length);
+
+ /// Set the cylinder radius
+ /// \param radius The capsule radius (in m)
+ void setRadius(double radius);
+
+private:
+ /// The cylinder length
+ double m_length;
+
+ /// The cylinder radius
+ double m_radius;
+};
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_CYLINDERSHAPE_H
diff --git a/SurgSim/Math/DoubleSidedPlaneShape.cpp b/SurgSim/Math/DoubleSidedPlaneShape.cpp
new file mode 100644
index 0000000..1e75520
--- /dev/null
+++ b/SurgSim/Math/DoubleSidedPlaneShape.cpp
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::DoubleSidedPlaneShape, DoubleSidedPlaneShape);
+
+DoubleSidedPlaneShape::DoubleSidedPlaneShape()
+{
+}
+
+int DoubleSidedPlaneShape::getType()
+{
+ return SHAPE_TYPE_DOUBLESIDEDPLANE;
+}
+
+double DoubleSidedPlaneShape::getVolume() const
+{
+ return 0.0;
+}
+
+SurgSim::Math::Vector3d DoubleSidedPlaneShape::getCenter() const
+{
+ return Vector3d(0.0, 0.0, 0.0);
+}
+
+SurgSim::Math::Matrix33d DoubleSidedPlaneShape::getSecondMomentOfVolume() const
+{
+ return Matrix33d::Zero();
+}
+
+double DoubleSidedPlaneShape::getD() const
+{
+ return 0.0;
+}
+
+SurgSim::Math::Vector3d DoubleSidedPlaneShape::getNormal() const
+{
+ return Vector3d(0.0, 1.0, 0.0);
+}
+
+bool DoubleSidedPlaneShape::isValid() const
+{
+ return true;
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/DoubleSidedPlaneShape.h b/SurgSim/Math/DoubleSidedPlaneShape.h
new file mode 100644
index 0000000..fde853d
--- /dev/null
+++ b/SurgSim/Math/DoubleSidedPlaneShape.h
@@ -0,0 +1,71 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_DOUBLESIDEDPLANESHAPE_H
+#define SURGSIM_MATH_DOUBLESIDEDPLANESHAPE_H
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_STATIC_REGISTRATION(DoubleSidedPlaneShape);
+
+/// DoubleSidedPlaneShape: The XZ plane (d = 0) with normal pointing along
+/// positive Y axis.
+class DoubleSidedPlaneShape: public Shape
+{
+public:
+ /// Constructor
+ DoubleSidedPlaneShape();
+
+ SURGSIM_CLASSNAME(SurgSim::Math::DoubleSidedPlaneShape);
+
+ /// \return the type of the shape
+ virtual int getType() override;
+
+ /// Get the volume of the shape
+ /// \return The volume of the shape (in m-3)
+ virtual double getVolume() const override;
+
+ /// Get the volumetric center of the shape
+ /// \return The center of the shape
+ virtual Vector3d getCenter() const override;
+
+ /// Get the second central moment of the volume, commonly used
+ /// to calculate the moment of inertia matrix
+ /// \return The 3x3 symmetric second moment matrix
+ virtual Matrix33d getSecondMomentOfVolume() const override;
+
+ /// Gets the d of the plane equation.
+ /// \return The value of d (always 0).
+ double getD() const;
+
+ /// Gets the normal of the plane equation.
+ /// \return The value of the normal (always Y axis).
+ Vector3d getNormal() const;
+
+ /// A DoubleSidedPlaneShape is always valid.
+ /// \return True.
+ virtual bool isValid() const override;
+};
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_DOUBLESIDEDPLANESHAPE_H
diff --git a/SurgSim/Math/GaussLegendreQuadrature.cpp b/SurgSim/Math/GaussLegendreQuadrature.cpp
new file mode 100644
index 0000000..1c05251
--- /dev/null
+++ b/SurgSim/Math/GaussLegendreQuadrature.cpp
@@ -0,0 +1,165 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <math.h>
+
+#include "SurgSim/Math/GaussLegendreQuadrature.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+std::array<gaussQuadraturePoint, 1> gaussQuadrature1Point =
+{{
+ gaussQuadraturePoint(0.0, 2.0)
+}};
+
+std::array<gaussQuadraturePoint, 2> gaussQuadrature2Points =
+{{
+ gaussQuadraturePoint( 1.0 / sqrt(3.0), 1.0),
+ gaussQuadraturePoint(-1.0 / sqrt(3.0), 1.0)
+}};
+
+std::array<gaussQuadraturePoint, 3> gaussQuadrature3Points =
+{{
+ gaussQuadraturePoint( 0.0, 8.0 / 9.0),
+ gaussQuadraturePoint( sqrt(3.0 / 5.0), 5.0 / 9.0),
+ gaussQuadraturePoint(-sqrt(3.0 / 5.0), 5.0 / 9.0)
+}};
+
+std::array<gaussQuadraturePoint, 4> gaussQuadrature4Points =
+{{
+ gaussQuadraturePoint( sqrt((3.0 - 2.0 * sqrt(6.0 / 5.0)) / 7.0), (18.0 + sqrt(30.0)) / 36.0),
+ gaussQuadraturePoint(-sqrt((3.0 - 2.0 * sqrt(6.0 / 5.0)) / 7.0), (18.0 + sqrt(30.0)) / 36.0),
+ gaussQuadraturePoint( sqrt((3.0 + 2.0 * sqrt(6.0 / 5.0)) / 7.0), (18.0 - sqrt(30.0)) / 36.0),
+ gaussQuadraturePoint(-sqrt((3.0 + 2.0 * sqrt(6.0 / 5.0)) / 7.0), (18.0 - sqrt(30.0)) / 36.0)
+}};
+
+std::array<gaussQuadraturePoint, 5> gaussQuadrature5Points =
+{{
+ gaussQuadraturePoint( 0.0, 128.0 / 225.0),
+ gaussQuadraturePoint( sqrt(5.0 - 2.0 * sqrt(10.0 / 7.0)) / 3.0, (322.0 + 13.0 * sqrt(70.0)) / 900.0),
+ gaussQuadraturePoint(-sqrt(5.0 - 2.0 * sqrt(10.0 / 7.0)) / 3.0, (322.0 + 13.0 * sqrt(70.0)) / 900.0),
+ gaussQuadraturePoint( sqrt(5.0 + 2.0 * sqrt(10.0 / 7.0)) / 3.0, (322.0 - 13.0 * sqrt(70.0)) / 900.0),
+ gaussQuadraturePoint(-sqrt(5.0 + 2.0 * sqrt(10.0 / 7.0)) / 3.0, (322.0 - 13.0 * sqrt(70.0)) / 900.0)
+}};
+
+std::array<gaussQuadraturePoint, 100> gaussQuadrature100Points =
+{{
+ gaussQuadraturePoint( 0.0156289844215430828722167, 0.0312554234538633569476425),
+ gaussQuadraturePoint(-0.0156289844215430828722167, 0.0312554234538633569476425),
+ gaussQuadraturePoint( 0.0468716824215916316149239, 0.0312248842548493577323765),
+ gaussQuadraturePoint(-0.0468716824215916316149239, 0.0312248842548493577323765),
+ gaussQuadraturePoint( 0.0780685828134366366948174, 0.0311638356962099067838183),
+ gaussQuadraturePoint(-0.0780685828134366366948174, 0.0311638356962099067838183),
+ gaussQuadraturePoint( 0.1091892035800611150034260, 0.0310723374275665165878102),
+ gaussQuadraturePoint(-0.1091892035800611150034260, 0.0310723374275665165878102),
+ gaussQuadraturePoint( 0.1402031372361139732075146, 0.0309504788504909882340635),
+ gaussQuadraturePoint(-0.1402031372361139732075146, 0.0309504788504909882340635),
+ gaussQuadraturePoint( 0.1710800805386032748875324, 0.0307983790311525904277139),
+ gaussQuadraturePoint(-0.1710800805386032748875324, 0.0307983790311525904277139),
+ gaussQuadraturePoint( 0.2017898640957359972360489, 0.0306161865839804484964594),
+ gaussQuadraturePoint(-0.2017898640957359972360489, 0.0306161865839804484964594),
+ gaussQuadraturePoint( 0.2323024818449739696495100, 0.0304040795264548200165079),
+ gaussQuadraturePoint(-0.2323024818449739696495100, 0.0304040795264548200165079),
+ gaussQuadraturePoint( 0.2625881203715034791689293, 0.0301622651051691449190687),
+ gaussQuadraturePoint(-0.2625881203715034791689293, 0.0301622651051691449190687),
+ gaussQuadraturePoint( 0.2926171880384719647375559, 0.0298909795933328309168368),
+ gaussQuadraturePoint(-0.2926171880384719647375559, 0.0298909795933328309168368),
+ gaussQuadraturePoint( 0.3223603439005291517224766, 0.0295904880599126425117545),
+ gaussQuadraturePoint(-0.3223603439005291517224766, 0.0295904880599126425117545),
+ gaussQuadraturePoint( 0.3517885263724217209723438, 0.0292610841106382766201190),
+ gaussQuadraturePoint(-0.3517885263724217209723438, 0.0292610841106382766201190),
+ gaussQuadraturePoint( 0.3808729816246299567633625, 0.0289030896011252031348762),
+ gaussQuadraturePoint(-0.3808729816246299567633625, 0.0289030896011252031348762),
+ gaussQuadraturePoint( 0.4095852916783015425288684, 0.0285168543223950979909368),
+ gaussQuadraturePoint(-0.4095852916783015425288684, 0.0285168543223950979909368),
+ gaussQuadraturePoint( 0.4378974021720315131089780, 0.0281027556591011733176483),
+ gaussQuadraturePoint(-0.4378974021720315131089780, 0.0281027556591011733176483),
+ gaussQuadraturePoint( 0.4657816497733580422492166, 0.0276611982207923882942042),
+ gaussQuadraturePoint(-0.4657816497733580422492166, 0.0276611982207923882942042),
+ gaussQuadraturePoint( 0.4932107892081909335693088, 0.0271926134465768801364916),
+ gaussQuadraturePoint(-0.4932107892081909335693088, 0.0271926134465768801364916),
+ gaussQuadraturePoint( 0.5201580198817630566468157, 0.0266974591835709626603847),
+ gaussQuadraturePoint(-0.5201580198817630566468157, 0.0266974591835709626603847),
+ gaussQuadraturePoint( 0.5465970120650941674679943, 0.0261762192395456763423087),
+ gaussQuadraturePoint(-0.5465970120650941674679943, 0.0261762192395456763423087),
+ gaussQuadraturePoint( 0.5725019326213811913168704, 0.0256294029102081160756420),
+ gaussQuadraturePoint(-0.5725019326213811913168704, 0.0256294029102081160756420),
+ gaussQuadraturePoint( 0.5978474702471787212648065, 0.0250575444815795897037642),
+ gaussQuadraturePoint(-0.5978474702471787212648065, 0.0250575444815795897037642),
+ gaussQuadraturePoint( 0.6226088602037077716041908, 0.0244612027079570527199750),
+ gaussQuadraturePoint(-0.6226088602037077716041908, 0.0244612027079570527199750),
+ gaussQuadraturePoint( 0.6467619085141292798326303, 0.0238409602659682059625604),
+ gaussQuadraturePoint(-0.6467619085141292798326303, 0.0238409602659682059625604),
+ gaussQuadraturePoint( 0.6702830156031410158025870, 0.0231974231852541216224889),
+ gaussQuadraturePoint(-0.6702830156031410158025870, 0.0231974231852541216224889),
+ gaussQuadraturePoint( 0.6931491993558019659486479, 0.0225312202563362727017970),
+ gaussQuadraturePoint(-0.6931491993558019659486479, 0.0225312202563362727017970),
+ gaussQuadraturePoint( 0.7153381175730564464599671, 0.0218430024162473863139537),
+ gaussQuadraturePoint(-0.7153381175730564464599671, 0.0218430024162473863139537),
+ gaussQuadraturePoint( 0.7368280898020207055124277, 0.0211334421125276415426723),
+ gaussQuadraturePoint(-0.7368280898020207055124277, 0.0211334421125276415426723),
+ gaussQuadraturePoint( 0.7575981185197071760356680, 0.0204032326462094327668389),
+ gaussQuadraturePoint(-0.7575981185197071760356680, 0.0204032326462094327668389),
+ gaussQuadraturePoint( 0.7776279096494954756275514, 0.0196530874944353058653815),
+ gaussQuadraturePoint(-0.7776279096494954756275514, 0.0196530874944353058653815),
+ gaussQuadraturePoint( 0.7968978923903144763895729, 0.0188837396133749045529412),
+ gaussQuadraturePoint(-0.7968978923903144763895729, 0.0188837396133749045529412),
+ gaussQuadraturePoint( 0.8153892383391762543939888, 0.0180959407221281166643908),
+ gaussQuadraturePoint(-0.8153892383391762543939888, 0.0180959407221281166643908),
+ gaussQuadraturePoint( 0.8330838798884008235429158, 0.0172904605683235824393442),
+ gaussQuadraturePoint(-0.8330838798884008235429158, 0.0172904605683235824393442),
+ gaussQuadraturePoint( 0.8499645278795912842933626, 0.0164680861761452126431050),
+ gaussQuadraturePoint(-0.8499645278795912842933626, 0.0164680861761452126431050),
+ gaussQuadraturePoint( 0.8660146884971646234107400, 0.0156296210775460027239369),
+ gaussQuadraturePoint(-0.8660146884971646234107400, 0.0156296210775460027239369),
+ gaussQuadraturePoint( 0.8812186793850184155733168, 0.0147758845274413017688800),
+ gaussQuadraturePoint(-0.8812186793850184155733168, 0.0147758845274413017688800),
+ gaussQuadraturePoint( 0.8955616449707269866985210, 0.0139077107037187726879541),
+ gaussQuadraturePoint(-0.8955616449707269866985210, 0.0139077107037187726879541),
+ gaussQuadraturePoint( 0.9090295709825296904671263, 0.0130259478929715422855586),
+ gaussQuadraturePoint(-0.9090295709825296904671263, 0.0130259478929715422855586),
+ gaussQuadraturePoint( 0.9216092981453339526669513, 0.0121314576629794974077448),
+ gaussQuadraturePoint(-0.9216092981453339526669513, 0.0121314576629794974077448),
+ gaussQuadraturePoint( 0.9332885350430795459243337, 0.0112251140231859771172216),
+ gaussQuadraturePoint(-0.9332885350430795459243337, 0.0112251140231859771172216),
+ gaussQuadraturePoint( 0.9440558701362559779627747, 0.0103078025748689695857821),
+ gaussQuadraturePoint(-0.9440558701362559779627747, 0.0103078025748689695857821),
+ gaussQuadraturePoint( 0.9539007829254917428493369, 0.0093804196536944579514182),
+ gaussQuadraturePoint(-0.9539007829254917428493369, 0.0093804196536944579514182),
+ gaussQuadraturePoint( 0.9628136542558155272936593, 0.0084438714696689714026208),
+ gaussQuadraturePoint(-0.9628136542558155272936593, 0.0084438714696689714026208),
+ gaussQuadraturePoint( 0.9707857757637063319308979, 0.0074990732554647115788287),
+ gaussQuadraturePoint(-0.9707857757637063319308979, 0.0074990732554647115788287),
+ gaussQuadraturePoint( 0.9778093584869182885537811, 0.0065469484508453227641521),
+ gaussQuadraturePoint(-0.9778093584869182885537811, 0.0065469484508453227641521),
+ gaussQuadraturePoint( 0.9838775407060570154961002, 0.0055884280038655151572119),
+ gaussQuadraturePoint(-0.9838775407060570154961002, 0.0055884280038655151572119),
+ gaussQuadraturePoint( 0.9889843952429917480044187, 0.0046244500634221193510958),
+ gaussQuadraturePoint(-0.9889843952429917480044187, 0.0046244500634221193510958),
+ gaussQuadraturePoint( 0.9931249370374434596520099, 0.0036559612013263751823425),
+ gaussQuadraturePoint(-0.9931249370374434596520099, 0.0036559612013263751823425),
+ gaussQuadraturePoint( 0.9962951347331251491861317, 0.0026839253715534824194396),
+ gaussQuadraturePoint(-0.9962951347331251491861317, 0.0026839253715534824194396),
+ gaussQuadraturePoint( 0.9984919506395958184001634, 0.0017093926535181052395294),
+ gaussQuadraturePoint(-0.9984919506395958184001634, 0.0017093926535181052395294),
+ gaussQuadraturePoint( 0.9997137267734412336782285, 0.0007346344905056717304063),
+ gaussQuadraturePoint(-0.9997137267734412336782285, 0.0007346344905056717304063)
+}};
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/GaussLegendreQuadrature.h b/SurgSim/Math/GaussLegendreQuadrature.h
new file mode 100644
index 0000000..2199316
--- /dev/null
+++ b/SurgSim/Math/GaussLegendreQuadrature.h
@@ -0,0 +1,96 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 a n-point Gaussian quadrature rule (a.k.a. Gauss-Legendre quadrature rule)
+/// http://en.wikipedia.org/wiki/Gaussian_quadrature
+
+#ifndef SURGSIM_MATH_GAUSSLEGENDREQUADRATURE_H
+#define SURGSIM_MATH_GAUSSLEGENDREQUADRATURE_H
+
+#include <array>
+#include <utility>
+
+namespace SurgSim
+{
+namespace Math
+{
+
+struct gaussQuadraturePoint
+{
+ gaussQuadraturePoint(double p, double w) : point(p), weight(w){}
+
+ const double point;
+ const double weight;
+};
+
+/// 1-point Gauss-Legendre quadrature {<x_1, w_1>}
+/// \note Gauss-Legendre quadrature numerically evaluates the integral of a function \f$f\f$ with a finite sum
+/// using some weights and specific points of evaluation of the function \f$f\f$:
+/// \note \f$\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\f$
+/// \note n is the number of points used to discretized the integral
+/// \note \f$x_i\f$ is the point to evaluate the function \f$f\f$ with
+/// \note \f$w_i\f$ is the weight to assign to the function evaluation at the given point \f$x_i\f$
+extern std::array<gaussQuadraturePoint, 1> gaussQuadrature1Point;
+
+/// 2-points Gauss-Legendre quadrature {<x_1, w_1>, <x_2, w_2>}
+/// \note Gauss-Legendre quadrature numerically evaluates the integral of a function \f$f\f$ with a finite sum
+/// using some weights and specific points of evaluation of the function \f$f\f$:
+/// \note \f$\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\f$
+/// \note n is the number of points used to discretized the integral
+/// \note \f$x_i\f$ is the point to evaluate the function \f$f\f$ with
+/// \note \f$w_i\f$ is the weight to assign to the function evaluation at the given point \f$x_i\f$
+extern std::array<gaussQuadraturePoint, 2> gaussQuadrature2Points;
+
+/// 3-points Gauss-Legendre quadrature {<x_1, w_1>, <x_2, w_2>, <x_3, w_3>}
+/// \note Gauss-Legendre quadrature numerically evaluates the integral of a function \f$f\f$ with a finite sum
+/// using some weights and specific points of evaluation of the function \f$f\f$:
+/// \note \f$\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\f$
+/// \note n is the number of points used to discretized the integral
+/// \note \f$x_i\f$ is the point to evaluate the function \f$f\f$ with
+/// \note \f$w_i\f$ is the weight to assign to the function evaluation at the given point \f$x_i\f$
+extern std::array<gaussQuadraturePoint, 3> gaussQuadrature3Points;
+
+/// 4-points Gauss-Legendre quadrature {<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, <x_4, w_4>}
+/// \note Gauss-Legendre quadrature numerically evaluates the integral of a function \f$f\f$ with a finite sum
+/// using some weights and specific points of evaluation of the function \f$f\f$:
+/// \note \f$\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\f$
+/// \note n is the number of points used to discretized the integral
+/// \note \f$x_i\f$ is the point to evaluate the function \f$f\f$ with
+/// \note \f$w_i\f$ is the weight to assign to the function evaluation at the given point \f$x_i\f$
+extern std::array<gaussQuadraturePoint, 4> gaussQuadrature4Points;
+
+/// 5-points Gauss-Legendre quadrature {<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, <x_4, w_4>, <x_5, w_5>}
+/// \note Gauss-Legendre quadrature numerically evaluates the integral of a function \f$f\f$ with a finite sum
+/// using some weights and specific points of evaluation of the function \f$f\f$:
+/// \note \f$\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\f$
+/// \note n is the number of points used to discretized the integral
+/// \note \f$x_i\f$ is the point to evaluate the function \f$f\f$ with
+/// \note \f$w_i\f$ is the weight to assign to the function evaluation at the given point \f$x_i\f$
+extern std::array<gaussQuadraturePoint, 5> gaussQuadrature5Points;
+
+/// 100-points Gauss-Legendre quadrature {<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, ..., <x_100, w_100>}
+/// \note Gauss-Legendre quadrature numerically evaluates the integral of a function \f$f\f$ with a finite sum
+/// using some weights and specific points of evaluation of the function \f$f\f$:
+/// \note \f$\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\f$
+/// \note n is the number of points used to discretized the integral
+/// \note \f$x_i\f$ is the point to evaluate the function \f$f\f$ with
+/// \note \f$w_i\f$ is the weight to assign to the function evaluation at the given point \f$x_i\f$
+extern std::array<gaussQuadraturePoint, 100> gaussQuadrature100Points;
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_GAUSSLEGENDREQUADRATURE_H
diff --git a/SurgSim/Math/Geometry.h b/SurgSim/Math/Geometry.h
new file mode 100644
index 0000000..88313db
--- /dev/null
+++ b/SurgSim/Math/Geometry.h
@@ -0,0 +1,1619 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_GEOMETRY_H
+#define SURGSIM_MATH_GEOMETRY_H
+
+#include <boost/container/static_vector.hpp>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Vector.h"
+
+/// \file Geometry.h a collection of functions that calculation geometric properties of various basic geometric shapes.
+/// Point, LineSegment, Plane, Triangle. All functions are templated for the accuracy of the calculation
+/// (float/double). There are also three kinds of epsilon defined that are used on a case by case basis.
+/// In general all function here will return a floating point or boolean value and take a series of output
+/// parameters. When those outputs cannot be calculated their values will be set to NAN.
+/// This functions are meant as a basic layer that will be wrapped with calls from structures mainting more
+/// state information about the primitives they are handling.
+/// As a convention we are using a plane equation in the form nx + d = 0
+/// \note HS-2013-may-07 Even though some of the names in this file do not agree with the coding standards in
+/// regard to the use of verbs for functions it was determined that other phrasing would not necessarily
+/// improve the readability or expressiveness of the function names.
+
+namespace SurgSim
+{
+namespace Math
+{
+
+namespace Geometry
+{
+
+/// Used as epsilon for general distance calculations
+static const double DistanceEpsilon = 1e-10;
+
+/// Used as epsilon for general distance calculations with squared distances
+static const double SquaredDistanceEpsilon = 1e-10;
+
+/// Epsilon used in angular comparisons
+static const double AngularEpsilon = 1e-10;
+
+/// Used as epsilon for scalar comparisons
+static const double ScalarEpsilon = 1e-10;
+
+}
+
+/// Calculate the barycentric coordinates of a point with respect to a triangle.
+/// \pre The normal must be unit length
+/// \pre The triangle vertices must be in counter clockwise order in respect to the normal
+/// \tparam T Floating point type of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param pt Vertex of the point.
+/// \param tv0, tv1, tv2 Vertices of the triangle in counter clockwise order in respect to the normal.
+/// \param tn Normal of the triangle (yes must be of norm 1 and a,b,c CCW).
+/// \param [out] coordinates Barycentric coordinates.
+/// \return bool true on success, false if two or more if the triangle is considered degenerate
+template <class T,int MOpt> inline
+bool barycentricCoordinates(const Eigen::Matrix<T, 3, 1, MOpt>& pt,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tn,
+ Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
+{
+ const T signedTriAreaX2 = ((tv1-tv0).cross(tv2-tv0)).dot(tn);
+ if (signedTriAreaX2 < Geometry::SquaredDistanceEpsilon)
+ {
+ // SQ_ASSERT_WARNING(false, "Cannot compute barycentric coords (degenetrate triangle), assigning center");
+ coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
+ return false;
+ }
+ (*coordinates)[0] = ((tv1-pt).cross(tv2-pt)).dot(tn) / signedTriAreaX2;
+ (*coordinates)[1] = ((tv2-pt).cross(tv0-pt)).dot(tn) / signedTriAreaX2;
+ (*coordinates)[2] = 1 - (*coordinates)[0] - (*coordinates)[1];
+ return true;
+}
+
+/// Calculate the barycentric coordinates of a point with respect to a triangle.
+/// Please note that each time you use this call the normal of the triangle will be
+/// calculated, if you convert more than one coordinate against this triangle, precalculate
+/// the normal and use the other version of this function
+/// \tparam T Floating point type of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param pt Vertex of the point.
+/// \param tv0, tv1, tv2 Vertices of the triangle.
+/// \param [out] coordinates The Barycentric coordinates.
+/// \return bool true on success, false if two or more if the triangle is considered degenerate
+template <class T, int MOpt> inline
+bool barycentricCoordinates(
+ const Eigen::Matrix<T, 3, 1, MOpt>& pt,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
+ Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
+{
+ const Eigen::Matrix<T, 3, 1, MOpt> tn = (tv1-tv0).cross(tv2-tv0);
+ return barycentricCoordinates(pt, tv0, tv1, tv2, tn, coordinates);
+}
+
+/// Check if a point is inside a triangle
+/// \note Use barycentricCoordinates() if you need the coordinates
+/// \pre The normal must be unit length
+/// \pre The triangle vertices must be in counter clockwise order in respect to the normal
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param pt Vertex of the point.
+/// \param tv0, tv1, tv2 Vertices of the triangle, must be in CCW.
+/// \param tn Normal of the triangle (yes must be of norm 1 and a,b,c CCW).
+/// \return true if pt lies inside the triangle tv0, tv1, tv2, false otherwise.
+template <class T, int MOpt> inline
+bool isPointInsideTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& pt,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tn)
+{
+ Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
+ bool result = barycentricCoordinates(pt, tv0, tv1, tv2, tn, &baryCoords);
+ return (result &&
+ baryCoords[0] >= -Geometry::ScalarEpsilon &&
+ baryCoords[1] >= -Geometry::ScalarEpsilon &&
+ baryCoords[2] >= -Geometry::ScalarEpsilon);
+}
+
+/// Check if a point is inside a triangle.
+/// \note Use barycentricCoordinates() if you need the coordinates.
+/// Please note that the normal will be calculated each time you use this call, if you are doing more than one
+/// test with the same triangle, precalculate the normal and pass it. Into the other version of this function
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param pt Vertex of the point.
+/// \param tv0, tv1, tv2 Vertices of the triangle, must be in CCW.
+/// \return true if pt lies inside the triangle tv0, tv1, tv2, false otherwise.
+template <class T, int MOpt> inline
+bool isPointInsideTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& pt,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv2)
+{
+ Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
+ bool result = barycentricCoordinates(pt, tv0, tv1, tv2, &baryCoords);
+ return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
+ baryCoords[1] >= -Geometry::ScalarEpsilon &&
+ baryCoords[2] >= -Geometry::ScalarEpsilon);
+}
+
+/// Check whether the points are coplanar.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param a, b, c, d Points to check for coplanarity.
+/// \return true if the points are coplanar.
+template <class T, int MOpt> inline
+bool isCoplanar(
+ const Eigen::Matrix<T, 3, 1, MOpt>& a,
+ const Eigen::Matrix<T, 3, 1, MOpt>& b,
+ const Eigen::Matrix<T, 3, 1, MOpt>& c,
+ const Eigen::Matrix<T, 3, 1, MOpt>& d)
+{
+ return std::abs((c - a).dot((b - a).cross(d - c))) < Geometry::ScalarEpsilon;
+}
+
+/// Calculate the normal distance between a point and a line.
+/// \param pt The input point.
+/// \param v0,v1 Two vertices on the line.
+/// \param [out] result The point projected onto the line.
+/// \return The normal distance between the point and the line
+template <class T, int MOpt> inline
+T distancePointLine(
+ const Eigen::Matrix<T, 3, 1, MOpt>& pt,
+ const Eigen::Matrix<T, 3, 1, MOpt>& v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& v1,
+ Eigen::Matrix<T, 3, 1, MOpt>* result)
+{
+ // The lines is parametrized by:
+ // q = v0 + lambda0 * (v1-v0)
+ // and we solve for pq.v01 = 0;
+ Eigen::Matrix<T, 3, 1, MOpt> v01 = v1-v0;
+ T v01_norm2 = v01.squaredNorm();
+ if (v01_norm2 <= Geometry::SquaredDistanceEpsilon)
+ {
+ *result = v0; // closest point is either
+ T pv_norm2 = (pt-v0).squaredNorm();
+ return sqrt(pv_norm2);
+ }
+ T lambda = (v01).dot(pt-v0);
+ *result = v0 + lambda*v01/v01_norm2;
+ return (*result-pt).norm();
+}
+
+/// Point segment distance, if the projection of the closest point is not within the segments, the
+/// closest segment point is used for the distance calculation.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param pt The input point
+/// \param sv0,sv1 The segment extremities.
+/// \param [out] result Either the projection onto the segment or one of the 2 vertices.
+/// \return The distance of the point from the segment.
+template <class T, int MOpt> inline
+T distancePointSegment(
+ const Eigen::Matrix<T, 3, 1, MOpt>& pt,
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
+ Eigen::Matrix<T, 3, 1, MOpt>* result)
+{
+ Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1-sv0;
+ T v01Norm2 = v01.squaredNorm();
+ if (v01Norm2 <= Geometry::SquaredDistanceEpsilon)
+ {
+ *result = sv0; // closest point is either
+ return (pt-sv0).norm();
+ }
+ T lambda = v01.dot(pt-sv0);
+ if (lambda <= 0)
+ {
+ *result = sv0;
+ }
+ else if (lambda >= v01Norm2)
+ {
+ *result = sv1;
+ }
+ else
+ {
+ *result = sv0 + lambda*v01/v01Norm2;
+ }
+ return (*result-pt).norm();
+}
+
+/// Determine the distance between two lines
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param l0v0, l0v1 Points on Line 0.
+/// \param l1v0, l1v1 Points on Line 1.
+/// \param [out] pt0 The closest point on line 0.
+/// \param [out] pt1 The closest point on line 1.
+/// \return The normal distance between the two given lines i.e. (pt0 - pt1).norm()
+/// \note We are using distancePointSegment for the degenerate cases as opposed to
+/// distancePointLine, why is that ??? (HS-2013-apr-26)
+template <class T, int MOpt> inline
+T distanceLineLine(
+ const Eigen::Matrix<T, 3, 1, MOpt>& l0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& l0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& l1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& l1v1,
+ Eigen::Matrix<T, 3, 1, MOpt>* pt0,
+ Eigen::Matrix<T, 3, 1, MOpt>* pt1)
+{
+ // Based on the outline of http://www.geometrictools.com/Distance.html, also refer to
+ // http://geomalgorithms.com/a07-_distance.html for a geometric interpretation
+ // The lines are parametrized by:
+ // p0 = l0v0 + lambda0 * (l0v1-l0v0)
+ // p1 = l1v0 + lambda1 * (l1v1-l1v0)
+ // and we solve for p0p1 perpendicular to both lines
+ T lambda0, lambda1;
+ Eigen::Matrix<T, 3, 1, MOpt> l0v01 = l0v1-l0v0;
+ T a = l0v01.squaredNorm();
+ if (a <= Geometry::SquaredDistanceEpsilon)
+ {
+ // Degenerate line 0
+ *pt0 = l0v0;
+ return distancePointSegment(l0v0, l1v0, l1v1, pt1);
+ }
+ Eigen::Matrix<T, 3, 1, MOpt> l1v01 = l1v1-l1v0;
+ T b = -l0v01.dot(l1v01);
+ T c = l1v01.squaredNorm();
+ if (c <= Geometry::SquaredDistanceEpsilon)
+ {
+ // Degenerate line 1
+ *pt1 = l1v0;
+ return distancePointSegment(l1v0, l0v0, l0v1, pt0);
+ }
+ Eigen::Matrix<T, 3, 1, MOpt> l0v0_l1v0 = l0v0-l1v0;
+ T d = l0v01.dot(l0v0_l1v0);
+ T e = -l1v01.dot(l0v0_l1v0);
+ T ratio = a*c-b*b;
+ if (std::abs(ratio) <= Geometry::ScalarEpsilon)
+ {
+ // parallel case
+ lambda0 = 0;
+ lambda1 = e / c;
+ }
+ else
+ {
+ // non-parallel case
+ T inv_ratio = T(1) / ratio;
+ lambda0 = (b*e - c*d) * inv_ratio;
+ lambda1 = (b*d - a*e) * inv_ratio;
+ }
+ *pt0 = l0v0 + lambda0 * l0v01;
+ *pt1 = l1v0 + lambda1 * l1v01;
+ return ((*pt0)-(*pt1)).norm();
+}
+
+
+/// Distance between two segments, if the project of the closest point is not on the opposing segment,
+/// the segment endpoints will be used for the distance calculation
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param s0v0, s0v1 Segment 0 Extremities.
+/// \param s1v0, s1v1 Segment 1 Extremities.
+/// \param [out] pt0 Closest point on segment 0
+/// \param [out] pt1 Closest point on segment 1
+/// \param [out] s0t Abscissa at the point of intersection on Segment 0 (s0v0 + t * (s0v1 - s0v0)).
+/// \param [out] s1t Abscissa at the point of intersection on Segment 0 (s1v0 + t * (s1v1 - s1v0)).
+/// \return Distance between the segments, i.e. (pt0 - pt1).norm()
+template <class T, int MOpt>
+T distanceSegmentSegment(
+ const Eigen::Matrix<T, 3, 1, MOpt>& s0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& s0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& s1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& s1v1,
+ Eigen::Matrix<T, 3, 1, MOpt>* pt0,
+ Eigen::Matrix<T, 3, 1, MOpt>* pt1,
+ T *s0t = nullptr,
+ T *s1t = nullptr)
+{
+ // Based on the outline of http://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf, also refer to
+ // http://geomalgorithms.com/a07-_distance.html for a geometric interpretation
+ // The segments are parametrized by:
+ // p0 = l0v0 + s * (l0v1-l0v0), with s between 0 and 1
+ // p1 = l1v0 + t * (l1v1-l1v0), with t between 0 and 1
+ // We are minimizing Q(s, t) = as*as + 2bst + ct*ct + 2ds + 2et + f,
+ Eigen::Matrix<T, 3, 1, MOpt> s0v01 = s0v1-s0v0;
+ T a = s0v01.squaredNorm();
+ if (a <= Geometry::SquaredDistanceEpsilon)
+ {
+ // Degenerate segment 0
+ *pt0 = s0v0;
+ return distancePointSegment<T>(s0v0, s1v0, s1v1, pt1);
+ }
+ Eigen::Matrix<T, 3, 1, MOpt> s1v01 = s1v1-s1v0;
+ T b = -s0v01.dot(s1v01);
+ T c = s1v01.squaredNorm();
+ if (c <= Geometry::SquaredDistanceEpsilon)
+ {
+ // Degenerate segment 1
+ *pt1 = s1v1;
+ return distancePointSegment<T>(s1v0, s0v0, s0v1, pt0);
+ }
+ Eigen::Matrix<T, 3, 1, MOpt> tempLine = s0v0-s1v0;
+ T d = s0v01.dot(tempLine);
+ T e = -s1v01.dot(tempLine);
+ T ratio = a*c-b*b;
+ T s,t; // parametrization variables (do not initialize)
+ int region = -1;
+ T tmp;
+ // Non-parallel case
+ if (1.0 - std::abs(s0v01.normalized().dot(s1v01.normalized())) >= Geometry::SquaredDistanceEpsilon)
+ {
+ // Get the region of the global minimum in the s-t space based on the line-line solution
+ // s=0 s=1
+ // ^
+ // | |
+ // 4 | 3 | 2
+ // ----|-------|------- t=1
+ // | |
+ // 5 | 0 | 1
+ // | |
+ // ----|-------|-------> t=0
+ // | |
+ // 6 | 7 | 8
+ // | |
+ //
+ s = b*e-c*d;
+ t = b*d-a*e;
+ if (s >= 0)
+ {
+ if (s <= ratio)
+ {
+ if (t >= 0)
+ {
+ if (t <= ratio)
+ {
+ region = 0;
+ }
+ else
+ {
+ region = 3;
+ }
+ }
+ else
+ {
+ region = 7;
+ }
+ }
+ else
+ {
+ if (t >= 0)
+ {
+ if (t <= ratio)
+ {
+ region = 1;
+ }
+ else
+ {
+ region = 2;
+ }
+ }
+ else
+ {
+ region = 8;
+ }
+ }
+ }
+ else
+ {
+ if (t >= 0)
+ {
+ if (t <= ratio)
+ {
+ region = 5;
+ }
+ else
+ {
+ region = 4;
+ }
+ }
+ else
+ {
+ region = 6;
+ }
+ }
+ enum edge_type { s0, s1, t0, t1, edge_skip, edge_invalid };
+ edge_type edge = edge_invalid;
+ switch (region)
+ {
+ case 0:
+ // Global minimum inside [0,1] [0,1]
+ s /= ratio;
+ t /= ratio;
+ edge = edge_skip;
+ break;
+ case 1:
+ edge = s1;
+ break;
+ case 2:
+ // Q_s(1,1)/2 = a+b+d
+ if (a+b+d > 0)
+ {
+ edge = t1;
+ }
+ else
+ {
+ edge = s1;
+ }
+ break;
+ case 3:
+ edge = t1;
+ break;
+ case 4:
+ // Q_s(0,1)/2 = b+d
+ if (b+d > 0)
+ {
+ edge = s0;
+ }
+ else
+ {
+ edge = t1;
+ }
+ break;
+ case 5:
+ edge = s0;
+ break;
+ case 6:
+ // Q_s(0,0)/2 = d
+ if (d > 0)
+ {
+ edge = s0;
+ }
+ else
+ {
+ edge = t0;
+ }
+ break;
+ case 7:
+ edge = t0;
+ break;
+ case 8:
+ // Q_s(1,0)/2 = a+d
+ if (a+d > 0)
+ {
+ edge = t0;
+ }
+ else
+ {
+ edge = s1;
+ }
+ break;
+ default:
+ break;
+ }
+ switch (edge)
+ {
+ case s0:
+ // F(t) = Q(0,t), F?(t) = 2*(e+c*t)
+ // F?(T) = 0 when T = -e/c, then clamp between 0 and 1 (c always >= 0)
+ s = 0;
+ tmp = e;
+ if (tmp > 0)
+ {
+ t = 0;
+ }
+ else if (-tmp > c)
+ {
+ t = 1;
+ }
+ else
+ {
+ t = -tmp/c;
+ }
+ break;
+ case s1:
+ // F(t) = Q(1,t), F?(t) = 2*((b+e)+c*t)
+ // F?(T) = 0 when T = -(b+e)/c, then clamp between 0 and 1 (c always >= 0)
+ s = 1;
+ tmp = b+e;
+ if (tmp > 0)
+ {
+ t = 0;
+ }
+ else if (-tmp > c)
+ {
+ t = 1;
+ }
+ else
+ {
+ t = -tmp/c;
+ }
+ break;
+ case t0:
+ // F(s) = Q(s,0), F?(s) = 2*(d+a*s) =>
+ // F?(S) = 0 when S = -d/a, then clamp between 0 and 1 (a always >= 0)
+ t = 0;
+ tmp = d;
+ if (tmp > 0)
+ {
+ s = 0;
+ }
+ else if (-tmp > a)
+ {
+ s = 1;
+ }
+ else
+ {
+ s = -tmp/a;
+ }
+ break;
+ case t1:
+ // F(s) = Q(s,1), F?(s) = 2*(b+d+a*s) =>
+ // F?(S) = 0 when S = -(b+d)/a, then clamp between 0 and 1 (a always >= 0)
+ t = 1;
+ tmp = b+d;
+ if (tmp > 0)
+ {
+ s = 0;
+ }
+ else if (-tmp > a)
+ {
+ s = 1;
+ }
+ else
+ {
+ s = -tmp/a;
+ }
+ break;
+ case edge_skip:
+ break;
+ default:
+ break;
+ }
+ }
+ else
+ // Parallel case
+ {
+ if (b > 0)
+ {
+ // Segments have different directions
+ if (d >= 0)
+ {
+ // 0-0 end points since s-segment 0 less than t-segment 0
+ s = 0;
+ t = 0;
+ }
+ else if (-d <= a)
+ {
+ // s-segment 0 end-point in the middle of the t 0-1 segment, get distance
+ s = -d/a;
+ t = 0;
+ }
+ else
+ {
+ // s-segment 1 is definitely closer
+ s = 1;
+ tmp = a+d;
+ if (-tmp >= b)
+ {
+ t = 1;
+ }
+ else
+ {
+ t = -tmp/b;
+ }
+ }
+ }
+ else
+ {
+ // Both segments have the same dir
+ if (-d >= a)
+ {
+ // 1-0
+ s = 1;
+ t = 0;
+ }
+ else if (d <= 0)
+ {
+ // mid-0
+ s = -d/a;
+ t = 0;
+ }
+ else
+ {
+ s = 0;
+ // 1-mid
+ if (d >= -b)
+ {
+ t = 1;
+ }
+ else
+ {
+ t = -d/b;
+ }
+ }
+ }
+ }
+ *pt0 = s0v0 + s * (s0v01);
+ *pt1 = s1v0 + t * (s1v01);
+ if (s0t != nullptr && s1t != nullptr)
+ {
+ *s0t = s;
+ *s1t = t;
+ }
+ return ((*pt1)-(*pt0)).norm();
+}
+
+/// Calculate the normal distance of a point from a triangle, the resulting point will be on the edge of the triangle
+/// if the projection of the point is not inside the triangle.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param pt The point that is being measured.
+/// \param tv0, tv1, tv2 The vertices of the triangle.
+/// \param [out] result The point on the triangle that is closest to pt, if the projection of pt onto the triangle.
+/// plane is not inside the triangle the closest point on the edge will be used.
+/// \return The distance between the point and the triangle, i.e (result - pt).norm()
+template <class T, int MOpt> inline
+T distancePointTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& pt,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
+ Eigen::Matrix<T, 3, 1, MOpt>* result)
+{
+ // Based on the outline of http://www.geometrictools.com/Distance.html, also refer to
+ // http://softsurfer.com/Archive/algorithm_0106 for a geometric interpretation
+ // The triangle is parametrized by:
+ // t: tv0 + s * (tv1-tv0) + t * (tv2-tv0) , with s and t between 0 and 1
+ // We are minimizing Q(s, t) = as*as + 2bst + ct*ct + 2ds + 2et + f,
+ Eigen::Matrix<T, 3, 1, MOpt> tv01 = tv1-tv0;
+ Eigen::Matrix<T, 3, 1, MOpt> tv02 = tv2-tv0;
+ T a = tv01.squaredNorm();
+ if (a <= Geometry::SquaredDistanceEpsilon)
+ {
+ // Degenerate edge 1
+ return distancePointSegment<T>(pt, tv0, tv2, result);
+ }
+ T b = tv01.dot(tv02);
+ T tCross = tv01.cross(tv02).squaredNorm();
+ if (tCross <= Geometry::SquaredDistanceEpsilon)
+ {
+ // Degenerate edge 2
+ return distancePointSegment<T>(pt, tv0, tv1, result);
+ }
+ T c = tv02.squaredNorm();
+ if (c <= Geometry::SquaredDistanceEpsilon)
+ {
+ // Degenerate edge 3
+ return distancePointSegment<T>(pt, tv0, tv1, result);
+ }
+ Eigen::Matrix<T, 3, 1, MOpt> tv0pv0 = tv0-pt;
+ T d = tv01.dot(tv0pv0);
+ T e = tv02.dot(tv0pv0);
+ T ratio = a*c-b*b;
+ T s = b*e-c*d;
+ T t = b*d-a*e;
+ // Determine region (inside-outside triangle)
+ int region = -1;
+ if (s+t <= ratio)
+ {
+ if (s < 0)
+ {
+ if (t < 0)
+ {
+ region = 4;
+ }
+ else
+ {
+ region = 3;
+ }
+ }
+ else if (t < 0)
+ {
+ region = 5;
+ }
+ else
+ {
+ region = 0;
+ }
+ }
+ else
+ {
+ if (s < 0)
+ {
+ region = 2;
+ }
+ else if (t < 0)
+ {
+ region = 6;
+ }
+ else
+ {
+ region = 1;
+ }
+ }
+ // Regions: /
+ // ^ t=0 /
+ // \ 2| /
+ // \ | /
+ // \| /
+ // \ /
+ // |\ /
+ // | \ 1 /
+ // 3 | \ /
+ // | 0 \ /
+ // ----|----\-------> s=0 /
+ // | \ /
+ // 4 | 5 \ 6 /
+ // | \ /
+ // /
+ T numer, denom, tmp0, tmp1;
+ enum edge_type { s0, t0, s1t1, edge_skip, edge_invalid };
+ edge_type edge = edge_invalid;
+ switch (region)
+ {
+ case 0:
+ // Global minimum inside [0,1] [0,1]
+ numer = T(1) / ratio;
+ s *= numer;
+ t *= numer;
+ edge = edge_skip;
+ break;
+ case 1:
+ edge = s1t1;
+ break;
+ case 2:
+ // Grad(Q(0,1)).(0,-1)/2 = -c-e
+ // Grad(Q(0,1)).(1,-1)/2 = b=d-c-e
+ tmp0 = b+d;
+ tmp1 = c+e;
+ if (tmp1 > tmp0)
+ {
+ edge = s1t1;
+ }
+ else
+ {
+ edge = s0;
+ }
+ break;
+ case 3:
+ edge = s0;
+ break;
+ case 4:
+ // Grad(Q(0,0)).(0,1)/2 = e
+ // Grad(Q(0,0)).(1,0)/2 = d
+ if (e >= d)
+ {
+ edge = t0;
+ }
+ else
+ {
+ edge = s0;
+ }
+ break;
+ case 5:
+ edge = t0;
+ break;
+ case 6:
+ // Grad(Q(1,0)).(-1,0)/2 = -a-d
+ // Grad(Q(1,0)).(-1,1)/2 = -a-d+b+e
+ tmp0 = -a-d;
+ tmp1 = -a-d+b+e;
+ if (tmp1 > tmp0)
+ {
+ edge = t0;
+ }
+ else
+ {
+ edge = s1t1;
+ }
+ break;
+ default:
+ break;
+ }
+ switch (edge)
+ {
+ case s0:
+ // F(t) = Q(0, t), F'(t)=0 when -e/c = 0
+ s = 0;
+ if (e >= 0)
+ {
+ t = 0;
+ }
+ else
+ {
+ t = (-e >= c ? 1 : -e/c);
+ }
+ break;
+ case t0:
+ // F(s) = Q(s, 0), F'(s)=0 when -d/a = 0
+ t = 0;
+ if (d >= 0)
+ {
+ s = 0;
+ }
+ else
+ {
+ s = (-d >= a ? 1 : -d/a);
+ }
+ break;
+ case s1t1:
+ // F(s) = Q(s, 1-s), F'(s) = 0 when (c+e-b-d)/(a-2b+c) = 0 (denom = || tv01-tv02 ||^2 always > 0)
+ numer = c+e-b-d;
+ if (numer <= 0)
+ {
+ s = 0;
+ }
+ else
+ {
+ denom = a-2*b+c;
+ s = (numer >= denom ? 1 : numer / denom);
+ }
+ t = 1-s;
+ break;
+ case edge_skip:
+ break;
+ default:
+ break;
+ }
+ *result = tv0 + s * tv01 + t * tv02;
+ return ((*result)-pt).norm();
+}
+
+/// Calculate the intersection of a line segment with a triangle
+/// See http://geomalgorithms.com/a06-_intersect-2.html#intersect_RayTriangle for the algorithm
+/// \pre The normal must be unit length
+/// \pre The triangle vertices must be in counter clockwise order in respect to the normal
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param sv0,sv1 Extremities of the segment.
+/// \param tv0,tv1,tv2 The triangle vertices. CCW around the normal.
+/// \param tn The triangle normal, should be normalized.
+/// \param [out] result The point where the triangle and the line segment intersect, invalid if they don't intersect.
+/// \return true if the segment intersects with the triangle, false if it does not
+/// \note HS-2013-may-07 This is the only function that only checks for intersection rather than returning a distance
+/// if necessary this should be rewritten to do the distance calculation, doing so would necessitate to check
+/// against all the triangle edges in the non intersection cases.
+template <class T, int MOpt> inline
+bool doesCollideSegmentTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tn,
+ Eigen::Matrix<T, 3, 1, MOpt>* result)
+{
+ // Triangle edges vectors
+ Eigen::Matrix<T, 3, 1, MOpt> u = tv1-tv0;
+ Eigen::Matrix<T, 3, 1, MOpt> v = tv2-tv0;
+
+ // Ray direction vector
+ Eigen::Matrix<T, 3, 1, MOpt> dir = sv1-sv0;
+ Eigen::Matrix<T, 3, 1, MOpt> w0 = sv0-tv0;
+ T a = -tn.dot(w0);
+ T b = tn.dot(dir);
+
+ result->setConstant((std::numeric_limits<double>::quiet_NaN()));
+
+ // Ray is parallel to triangle plane
+ if (std::abs(b) <= Geometry::AngularEpsilon)
+ {
+ if (a == 0)
+ {
+ // Ray lies in triangle plane
+ Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
+ for (int i=0; i<2; ++i)
+ {
+ barycentricCoordinates((i==0?sv0:sv1), tv0, tv1, tv2, tn, &baryCoords);
+ if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
+ {
+ *result = (i==0)?sv0:sv1;
+ return true;
+ }
+ }
+ // All segment endpoints outside of triangle
+ return false;
+ }
+ else
+ {
+ // Segment parallel to triangle but not in same plane
+ return false;
+ }
+ }
+
+ // Get intersect point of ray with triangle plane
+ T r = a / b;
+ // Ray goes away from triangle
+ if (r < -Geometry::DistanceEpsilon)
+ {
+ return false;
+ }
+ //Ray comes toward triangle but isn't long enough to reach it
+ if (r > 1+Geometry::DistanceEpsilon)
+ {
+ return false;
+ }
+
+ // Intersect point of ray and plane
+ Eigen::Matrix<T, 3, 1, MOpt> presumedIntersection = sv0 + r * dir;
+ // Collision point inside T?
+ T uu = u.dot(u);
+ T uv = u.dot(v);
+ T vv = v.dot(v);
+ Eigen::Matrix<T, 3, 1, MOpt> w = presumedIntersection - tv0;
+ T wu = w.dot(u);
+ T wv = w.dot(v);
+ T D = uv * uv - uu * vv;
+ // Get and test parametric coords
+ T s = (uv * wv - vv * wu) / D;
+ // I is outside T
+ if (s < 0 || s > 1)
+ {
+ return false;
+ }
+ T t = (uv * wu - uu * wv) / D;
+ // I is outside T
+ if (t < 0 || (s + t) > 1)
+ {
+ return false;
+ }
+ // I is in T
+ *result = sv0 + r * dir;
+ return true;
+}
+
+
+/// Calculate the distance of a point to a plane
+/// \pre n needs to the normalized
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param pt The point to check.
+/// \param n The normal of the plane n (normalized).
+/// \param d Constant d for the plane equation as in n.x + d = 0.
+/// \param [out] result Projection of point p into the plane.
+/// \return The distance to the plane (negative if on the backside of the plane).
+template <class T, int MOpt> inline
+T distancePointPlane(
+ const Eigen::Matrix<T, 3, 1, MOpt>& pt,
+ const Eigen::Matrix<T, 3, 1, MOpt>& n,
+ T d,
+ Eigen::Matrix<T, 3, 1, MOpt>* result)
+{
+ T dist = n.dot(pt) + d;
+ *result = pt - n*dist;
+ return dist;
+}
+
+
+/// Calculate the distance between a segment and a plane.
+/// \pre n should be normalized
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param sv0,sv1 Endpoints of the segments.
+/// \param n Normal of the plane n (normalized).
+/// \param d Constant d in n.x + d = 0.
+/// \param [out] closestPointSegment Point closest to the plane, the midpoint of the segment (v0+v1)/2
+/// is being used if the segment is parallel to the plane. If the segment actually
+/// intersects the plane segmentIntersectionPoint will be equal to planeIntersectionPoint.
+/// \param [out] planeIntersectionPoint the point on the plane where the line defined by the segment
+/// intersects the plane.
+/// \return the distance of closest point of the segment to the plane, 0 if the segment intersects the plane,
+/// negative if the closest point is on the other side of the plane.
+template <class T, int MOpt> inline
+T distanceSegmentPlane(
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& n,
+ T d,
+ Eigen::Matrix<T, 3, 1, MOpt>* closestPointSegment,
+ Eigen::Matrix<T, 3, 1, MOpt>* planeIntersectionPoint)
+{
+ T dist0 = n.dot(sv0) + d;
+ T dist1 = n.dot(sv1) + d;
+ // Parallel case
+ Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
+ if (std::abs(n.dot(v01)) <= Geometry::AngularEpsilon)
+ {
+ *closestPointSegment = (sv0 + sv1)*T(0.5);
+ dist0 = n.dot(*closestPointSegment) + d;
+ *planeIntersectionPoint = *closestPointSegment - dist0*n;
+ return (std::abs(dist0) < Geometry::DistanceEpsilon ? 0 : dist0);
+ }
+ // Both on the same side
+ if ((dist0 > 0 && dist1 > 0) || (dist0 < 0 && dist1 < 0))
+ {
+ if (std::abs(dist0) < std::abs(dist1))
+ {
+ *closestPointSegment = sv0;
+ *planeIntersectionPoint = sv0 - dist0*n;
+ return dist0;
+ }
+ else
+ {
+ *closestPointSegment = sv1;
+ *planeIntersectionPoint = sv1 - dist1*n;
+ return dist1;
+ }
+ }
+ // Segment cutting through
+ else
+ {
+ Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1-sv0;
+ T lambda= (-d-sv0.dot(n)) / v01.dot(n);
+ *planeIntersectionPoint = sv0 + lambda * v01;
+ *closestPointSegment = *planeIntersectionPoint;
+ return 0;
+ }
+}
+
+
+/// Calculate the distance of a triangle to a plane.
+/// \pre n should be normalized.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param tv0,tv1,tv2 Points of the triangle.
+/// \param n Normal of the plane n (normalized).
+/// \param d Constant d in n.x + d = 0.
+/// \param closestPointTriangle Closest point on the triangle, when the triangle is coplanar to
+/// the plane (tv0+tv1+tv2)/3 is used, when the triangle intersects the plane the midpoint of
+/// the intersection segment is returned.
+/// \param planeProjectionPoint Projection of the closest point onto the plane, when the triangle intersects
+/// the plane the midpoint of the intersection segment is returned.
+/// \return The distance of the triangle to the plane.
+template <class T, int MOpt> inline
+T distanceTrianglePlane(
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& n,
+ T d,
+ Eigen::Matrix<T, 3, 1, MOpt>* closestPointTriangle,
+ Eigen::Matrix<T, 3, 1, MOpt>* planeProjectionPoint)
+{
+ Eigen::Matrix<T, 3, 1, MOpt> distances(n.dot(tv0) + d, n.dot(tv1) + d, n.dot(tv2) + d);
+ Eigen::Matrix<T, 3, 1, MOpt> t01 = tv1-tv0;
+ Eigen::Matrix<T, 3, 1, MOpt> t02 = tv2-tv0;
+ Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2-tv1;
+
+ closestPointTriangle->setConstant((std::numeric_limits<double>::quiet_NaN()));
+ planeProjectionPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
+
+ // HS-2013-may-09 Could there be a case where we fall into the wrong tree because of the checks against
+ // the various epsilon values all going against us ???
+ // Parallel case (including Coplanar)
+ if (std::abs(n.dot(t01)) <= Geometry::AngularEpsilon && std::abs(n.dot(t02)) <= Geometry::AngularEpsilon)
+ {
+ *closestPointTriangle = (tv0 + tv1 + tv2) / T(3);
+ *planeProjectionPoint = *closestPointTriangle - n * distances[0];
+ return distances[0];
+ }
+
+ // Is there an intersection
+ if ((distances.array() < -Geometry::DistanceEpsilon).any() &&
+ (distances.array() > Geometry::DistanceEpsilon).any())
+ {
+ if (distances[0] * distances[1] < 0)
+ {
+ *closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t01) * t01;
+ if (distances[0] * distances[2] < 0)
+ {
+ *planeProjectionPoint = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
+ }
+ else
+ {
+ Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2-tv1;
+ *planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
+ }
+ }
+ else
+ {
+ *closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
+ *planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
+ }
+
+ // Find the midpoint, take this out to return the segment endpoints
+ *closestPointTriangle = *planeProjectionPoint = (*closestPointTriangle + *planeProjectionPoint) * T(0.5);
+ return 0;
+ }
+
+ int index;
+ distances.cwiseAbs().minCoeff(&index);
+ switch (index)
+ {
+ case 0: //distances[0] is closest
+ *closestPointTriangle = tv0;
+ *planeProjectionPoint = tv0 - n * distances[0];
+ return distances[0];
+ case 1: //distances[1] is closest
+ *closestPointTriangle = tv1;
+ *planeProjectionPoint = tv1 - n * distances[1];
+ return distances[1];
+ case 2: //distances[2] is closest
+ *closestPointTriangle = tv2;
+ *planeProjectionPoint = tv2 - n * distances[2];
+ return distances[2];
+ }
+
+ return std::numeric_limits<T>::quiet_NaN();
+}
+
+/// Test if two planes are intersecting, if yes also calculate the intersection line.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param pn0,pd0 Normal and constant of the first plane, nx + d = 0.
+/// \param pn1,pd1 Normal and constant of the second plane, nx + d = 0.
+/// \param [out] pt0,pt1 Two points on the intersection line, not valid if there is no intersection.
+/// \return true when a unique line exists, false for disjoint or coinciding.
+template <class T, int MOpt> inline
+bool doesIntersectPlanePlane(
+ const Eigen::Matrix<T, 3, 1, MOpt>& pn0, T pd0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& pn1, T pd1,
+ Eigen::Matrix<T, 3, 1, MOpt>* pt0,
+ Eigen::Matrix<T, 3, 1, MOpt>* pt1)
+{
+ // Algorithm from real time collision detection - optimized version page 210 (with extra checks)
+ const Eigen::Matrix<T, 3, 1, MOpt> lineDir = pn0.cross(pn1);
+ const T lineDirNorm2 = lineDir.squaredNorm();
+
+ pt0->setConstant((std::numeric_limits<double>::quiet_NaN()));
+ pt1->setConstant((std::numeric_limits<double>::quiet_NaN()));
+
+ // Test if the two planes are parallel
+ if (lineDirNorm2 <= Geometry::SquaredDistanceEpsilon)
+ {
+ return false; // planes disjoint
+ }
+ // Compute common point
+ *pt0 = (pd1*pn0-pd0*pn1).cross(lineDir) / lineDirNorm2;
+ *pt1 = *pt0 + lineDir;
+ return true;
+}
+
+
+/// Calculate the distance of a line segment to a triangle.
+/// Note that this version will calculate the normal of the triangle,
+/// if the normal is known use the other version of this function.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param sv0,sv1 Extremities of the line segment.
+/// \param tv0, tv1, tv2 Triangle points.
+/// \param [out] segmentPoint Closest point on the segment.
+/// \param [out] trianglePoint Closest point on the triangle.
+/// \return the the distance between the two closest points, i.e. (trianglePoint - segmentPoint).norm().
+template <class T, int MOpt> inline
+T distanceSegmentTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
+ Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
+ Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
+{
+ Eigen::Matrix<T, 3, 1, MOpt> n = (tv1 - tv0).cross(tv2 - tv1);
+ n.normalize();
+ return distanceSegmentTriangle(sv0, sv1, tv0, tv1, tv2, n, segmentPoint, trianglePoint);
+}
+
+/// Calculate the distance of a line segment to a triangle.
+/// \pre n needs to be normalized.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param sv0,sv1 Extremities of the line segment.
+/// \param tv0, tv1, tv2 Points of the triangle.
+/// \param normal Normal of the triangle (Expected to be normalized)
+/// \param [out] segmentPoint Closest point on the segment.
+/// \param [out] trianglePoint Closest point on the triangle.
+/// \return the distance between the two closest points.
+template <class T, int MOpt> inline
+T distanceSegmentTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& normal,
+ Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
+ Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
+{
+ segmentPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
+ trianglePoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
+
+ // Setting up the plane that the triangle is in
+ const Eigen::Matrix<T, 3, 1, MOpt>& n = normal;
+ T d = -n.dot(tv0);
+ Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
+ // Degenerate case: Line and triangle plane parallel
+ const Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1-sv0;
+ const T v01DotTn = n.dot(v01);
+ if (std::abs(v01DotTn) <= Geometry::AngularEpsilon)
+ {
+ // Check if any of the points project onto the tri
+ // otherwise normal (non-parallel) processing will get the right result
+ T dst = std::abs(distancePointPlane(sv0, n, d, trianglePoint));
+ Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
+ barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
+ if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
+ {
+ *segmentPoint = sv0;
+ return dst;
+ }
+ dst = std::abs(distancePointPlane(sv1, n, d, trianglePoint));
+ barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
+ if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
+ {
+ *segmentPoint = sv1;
+ return dst;
+ }
+ }
+ // Line and triangle plane *not* parallel: check cut through case only, the rest will be check later
+ else
+ {
+ T lambda = -n.dot(sv0-tv0) / v01DotTn;
+ if (lambda >= 0 && lambda <= 1)
+ {
+ *segmentPoint = *trianglePoint = sv0 + lambda * v01;
+ barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
+ if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
+ {
+ // Segment goes through the triangle
+ return 0;
+ }
+ }
+ }
+ // At this point the segment is nearest point to one of the triangle edges or one of the end points
+ Eigen::Matrix<T, 3, 1, MOpt> segColPt01, segColPt02, segColPt12, triColPt01, triColPt02, triColPt12;
+ T dst01 = distanceSegmentSegment(sv0, sv1, tv0, tv1, &segColPt01, &triColPt01);
+ T dst02 = distanceSegmentSegment(sv0, sv1, tv0, tv2, &segColPt02, &triColPt02);
+ T dst12 = distanceSegmentSegment(sv0, sv1, tv1, tv2, &segColPt12, &triColPt12);
+ Eigen::Matrix<T, 3, 1, MOpt> ptTriCol0, ptTriCol1;
+ T dstPtTri0 = std::abs(distancePointPlane(sv0, n, d, &ptTriCol0));
+ barycentricCoordinates(ptTriCol0, tv0, tv1, tv2, normal, &baryCoords);
+ if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
+ {
+ dstPtTri0 = std::numeric_limits<T>::max();
+ }
+ T dstPtTri1 = std::abs(distancePointPlane(sv1, n, d, &ptTriCol1));
+ barycentricCoordinates(ptTriCol1, tv0, tv1, tv2, normal, &baryCoords);
+ if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
+ {
+ dstPtTri1 = std::numeric_limits<T>::max();
+ }
+
+ int minIndex;
+ Eigen::Matrix<double, 5, 1> distances;
+ (distances << dst01, dst02, dst12, dstPtTri0, dstPtTri1).finished().minCoeff(&minIndex);
+ switch (minIndex)
+ {
+ case 0:
+ *segmentPoint = segColPt01;
+ *trianglePoint = triColPt01;
+ return dst01;
+ case 1:
+ *segmentPoint = segColPt02;
+ *trianglePoint = triColPt02;
+ return dst02;
+ case 2:
+ *segmentPoint = segColPt12;
+ *trianglePoint = triColPt12;
+ return dst12;
+ case 3:
+ *segmentPoint = sv0;
+ *trianglePoint = ptTriCol0;
+ return dstPtTri0;
+ case 4:
+ *segmentPoint = sv1;
+ *trianglePoint = ptTriCol1;
+ return dstPtTri1;
+ }
+
+ // Invalid ...
+ return std::numeric_limits<T>::quiet_NaN();
+
+}
+
+
+/// Calculate the distance between two triangles
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param t0v0,t0v1,t0v2 Points of the first triangle.
+/// \param t1v0,t1v1,t1v2 Points of the second triangle.
+/// \param [out] closestPoint0 Closest point on the first triangle, unless penetrating,
+/// in which case it is the point along the edge that allows min separation.
+/// \param [out] closestPoint1 Closest point on the second triangle, unless penetrating,
+/// in which case it is the point along the edge that allows min separation.
+/// \return the distance between the two triangles.
+template <class T, int MOpt> inline
+T distanceTriangleTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
+ Eigen::Matrix<T, 3, 1, MOpt>* closestPoint0,
+ Eigen::Matrix<T, 3, 1, MOpt>* closestPoint1)
+{
+ // Check the segments of t0 against t1
+ T minDst = std::numeric_limits<T>::max();
+ T currDst = 0;
+ Eigen::Matrix<T, 3, 1, MOpt> segPt, triPt;
+ Eigen::Matrix<T, 3, 1, MOpt> n0 = (t0v1-t0v0).cross(t0v2-t0v0);
+ n0.normalize();
+ Eigen::Matrix<T, 3, 1, MOpt> n1 = (t1v1-t1v0).cross(t1v2-t1v0);
+ n1.normalize();
+ currDst = distanceSegmentTriangle(t0v0, t0v1, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
+ if (currDst < minDst)
+ {
+ minDst = currDst;
+ *closestPoint0 = segPt;
+ *closestPoint1 = triPt;
+ }
+ currDst = distanceSegmentTriangle(t0v1, t0v2, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
+ if (currDst < minDst)
+ {
+ minDst = currDst;
+ *closestPoint0 = segPt;
+ *closestPoint1 = triPt;
+ }
+ currDst = distanceSegmentTriangle(t0v2, t0v0, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
+ if (currDst < minDst)
+ {
+ minDst = currDst;
+ *closestPoint0 = segPt;
+ *closestPoint1 = triPt;
+ }
+ // Check the segments of t1 against t0
+ currDst = distanceSegmentTriangle(t1v0, t1v1, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
+ if (currDst < minDst)
+ {
+ minDst = currDst;
+ *closestPoint1 = segPt;
+ *closestPoint0 = triPt;
+ }
+ currDst = distanceSegmentTriangle(t1v1, t1v2, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
+ if (currDst < minDst)
+ {
+ minDst = currDst;
+ *closestPoint1 = segPt;
+ *closestPoint0 = triPt;
+ }
+ currDst = distanceSegmentTriangle(t1v2, t1v0, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
+ if (currDst < minDst)
+ {
+ minDst = currDst;
+ *closestPoint1 = segPt;
+ *closestPoint0 = triPt;
+ }
+ return (minDst);
+}
+
+/// Calculate the intersections between a line segment and an axis aligned box
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param sv0,sv1 Extremities of the line segment.
+/// \param box Axis aligned bounding box
+/// \param [out] intersections The points of intersection between the segment and the box
+template <class T, int MOpt>
+void intersectionsSegmentBox(
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
+ const Eigen::AlignedBox<T, 3>& box,
+ std::vector<Eigen::Matrix<T, 3, 1, MOpt> >* intersections)
+{
+ Eigen::Array<T, 3, 1, MOpt> v01 = sv1 - sv0;
+ Eigen::Array<bool, 3, 1, MOpt> parallelToPlane = (v01.cwiseAbs().array() < Geometry::DistanceEpsilon);
+ if (parallelToPlane.any())
+ {
+ Eigen::Array<bool, 3, 1, MOpt> beyondMinCorner = (sv0.array() < box.min().array());
+ Eigen::Array<bool, 3, 1, MOpt> beyondMaxCorner = (sv0.array() > box.max().array());
+ if ((parallelToPlane && (beyondMinCorner || beyondMaxCorner)).any())
+ {
+ return;
+ }
+ }
+
+ // Calculate the intersection of the segment with each of the 6 box planes.
+ // The intersection is calculated as the distance along the segment (abscissa)
+ // scaled from 0 to 1.
+ Eigen::Array<T, 3, 2, MOpt> planeIntersectionAbscissas;
+ planeIntersectionAbscissas.col(0) = (box.min().array() - sv0.array());
+ planeIntersectionAbscissas.col(1) = (box.max().array() - sv0.array());
+
+ // While we could be dividing by zero here, INF values are
+ // correctly handled by the rest of the function.
+ planeIntersectionAbscissas.colwise() /= v01;
+
+ T entranceAbscissa = planeIntersectionAbscissas.rowwise().minCoeff().maxCoeff();
+ T exitAbscissa = planeIntersectionAbscissas.rowwise().maxCoeff().minCoeff();
+ if (entranceAbscissa < exitAbscissa && exitAbscissa > T(0.0))
+ {
+ if (entranceAbscissa >= T(0.0) && entranceAbscissa <= T(1.0))
+ {
+ intersections->push_back(sv0 + v01.matrix() * entranceAbscissa);
+ }
+
+ if (exitAbscissa >= T(0.0) && exitAbscissa <= T(1.0))
+ {
+ intersections->push_back(sv0 + v01.matrix() * exitAbscissa);
+ }
+ }
+}
+
+/// Test if an axis aligned box intersects with a capsule
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param capsuleBottom Position of the capsule bottom
+/// \param capsuleTop Position of the capsule top
+/// \param capsuleRadius The capsule radius
+/// \param box Axis aligned bounding box
+/// \return True, if intersection is detected.
+template <class T, int MOpt>
+bool doesIntersectBoxCapsule(
+ const Eigen::Matrix<T, 3, 1, MOpt>& capsuleBottom,
+ const Eigen::Matrix<T, 3, 1, MOpt>& capsuleTop,
+ const T capsuleRadius,
+ const Eigen::AlignedBox<T, 3>& box)
+{
+ Eigen::AlignedBox<double, 3> dilatedBox(box.min().array() - capsuleRadius, box.max().array() + capsuleRadius);
+ std::vector<Vector3d> candidates;
+ intersectionsSegmentBox(capsuleBottom, capsuleTop, dilatedBox, &candidates);
+ if (dilatedBox.contains(capsuleBottom))
+ {
+ candidates.push_back(capsuleBottom);
+ }
+ if (dilatedBox.contains(capsuleTop))
+ {
+ candidates.push_back(capsuleTop);
+ }
+
+ bool doesIntersect = false;
+ ptrdiff_t dimensionsOutsideBox;
+ Vector3d clampedPosition, segmentPoint;
+ for (auto candidate = candidates.cbegin(); candidate != candidates.cend(); ++candidate)
+ {
+ // Collisions between a capsule and a box are the same as a segment and a dilated
+ // box with rounded corners. If the intersection occurs outside the original box
+ // in two dimensions (collision with an edge of the dilated box) or three
+ // dimensions (collision with the corner of the dilated box) dimensions, we need
+ // to check if it is inside these rounded corners.
+ dimensionsOutsideBox = (candidate->array() > box.max().array()).count();
+ dimensionsOutsideBox += (candidate->array() < box.min().array()).count();
+ if (dimensionsOutsideBox >= 2)
+ {
+ clampedPosition = (*candidate).array().min(box.max().array()).max(box.min().array());
+ if (distancePointSegment(clampedPosition, capsuleBottom, capsuleTop, &segmentPoint) > capsuleRadius)
+ {
+ // Doesn't intersect, try the next candidate.
+ continue;
+ }
+ }
+ doesIntersect = true;
+ break;
+ }
+ return doesIntersect;
+}
+
+/// Check if the two triangles intersect using separating axis test.
+/// Algorithm is implemented from http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/pubs/tritri.pdf
+///
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param t0v0,t0v1,t0v2 Vertices of the first triangle.
+/// \param t1v0,t1v1,t1v2 Vertices of the second triangle.
+/// \param t0n Normal of the first triangle, should be normalized.
+/// \param t1n Normal of the second triangle, should be normalized.
+/// \return True, if intersection is detected.
+template <class T, int MOpt> inline
+bool doesIntersectTriangleTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1n);
+
+/// Check if the two triangles intersect using separating axis test.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param t0v0,t0v1,t0v2 Vertices of the first triangle.
+/// \param t1v0,t1v1,t1v2 Vertices of the second triangle.
+/// \return True, if intersection is detected.
+template <class T, int MOpt> inline
+bool doesIntersectTriangleTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v2);
+
+/// Calculate the contact between two triangles.
+/// Algorithm presented in
+/// https://docs.google.com/a/simquest.com/document/d/11ajMD7QoTVelT2_szGPpeUEY0wHKKxW1TOgMe8k5Fsc/pub.
+/// If the triangle are known to intersect, the deepest penetration of the triangles into each other is calculated.
+/// The triangle which penetrates less into the other triangle is chosen as contact.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param t0v0,t0v1,t0v2 Vertices of the first triangle.
+/// \param t1v0,t1v1,t1v2 Vertices of the second triangle.
+/// \param t0n Unit length normal of the first triangle, should be normalized.
+/// \param t1n Unit length normal of the second triangle, should be normalized.
+/// \param [out] penetrationDepth The depth of penetration.
+/// \param [out] penetrationPoint0 The contact point on triangle0 (t0v0,t0v1,t0v2).
+/// \param [out] penetrationPoint1 The contact point on triangle1 (t1v0,t1v1,t1v2).
+/// \param [out] contactNormal The contact normal that points from triangle1 to triangle0.
+/// \return True, if intersection is detected.
+/// \note The [out] params are not modified if there is no intersection.
+/// \note If penetrationPoint0 is moved by (contactNormal*penetrationDepth*0.5) and penetrationPoint1
+/// is moved by -(contactNormal*penetrationDepth*0.5), the triangles will no longer be intersecting.
+template <class T, int MOpt> inline
+bool calculateContactTriangleTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
+ T* penetrationDepth,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
+ Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
+
+/// Calculate the contact between two triangles.
+/// Algorithm presented in
+/// https://docs.google.com/a/simquest.com/document/d/11ajMD7QoTVelT2_szGPpeUEY0wHKKxW1TOgMe8k5Fsc/pub.
+/// If the triangle are known to intersect, the deepest penetration of the triangles into each other is calculated.
+/// The triangle which penetrates less into the other triangle is chosen as contact.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param t0v0,t0v1,t0v2 Vertices of the first triangle, should be normalized.
+/// \param t1v0,t1v1,t1v2 Vertices of the second triangle, should be normalized.
+/// \param [out] penetrationDepth The depth of penetration.
+/// \param [out] penetrationPoint0 The contact point on triangle0 (t0v0,t0v1,t0v2).
+/// \param [out] penetrationPoint1 The contact point on triangle1 (t1v0,t1v1,t1v2).
+/// \param [out] contactNormal The contact normal that points from triangle1 to triangle0.
+/// \return True, if intersection is detected.
+/// \note The [out] params are not modified if there is no intersection.
+/// \note If penetrationPoint0 is moved by (contactNormal*penetrationDepth*0.5) and penetrationPoint1
+/// is moved by -(contactNormal*penetrationDepth*0.5), the triangles will no longer be intersecting.
+template <class T, int MOpt> inline
+bool calculateContactTriangleTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
+ T* penetrationDepth,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
+ Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
+
+
+}; // namespace Math
+}; // namespace SurgSim
+
+
+#include "SurgSim/Math/TriangleTriangleIntersection-inl.h"
+#include "SurgSim/Math/TriangleTriangleContactCalculation-inl.h"
+
+
+#endif
diff --git a/SurgSim/Math/LinearSolveAndInverse-inl.h b/SurgSim/Math/LinearSolveAndInverse-inl.h
new file mode 100644
index 0000000..786721a
--- /dev/null
+++ b/SurgSim/Math/LinearSolveAndInverse-inl.h
@@ -0,0 +1,201 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_LINEARSOLVEANDINVERSE_INL_H
+#define SURGSIM_MATH_LINEARSOLVEANDINVERSE_INL_H
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+template <int BlockSize>
+const Eigen::Block<const Matrix, BlockSize, BlockSize>
+ LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::minusAi(const SurgSim::Math::Matrix& A, size_t i) const
+{
+ return A.block<BlockSize, BlockSize>(BlockSize * i, BlockSize * (i - 1));
+}
+
+template <int BlockSize>
+const Eigen::Block<const Matrix, BlockSize, BlockSize>
+ LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::Bi(const SurgSim::Math::Matrix& A, size_t i) const
+{
+ return A.block<BlockSize, BlockSize>(BlockSize * i, BlockSize * i);
+}
+
+template <int BlockSize>
+const Eigen::Block<const Matrix, BlockSize, BlockSize>
+ LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::minusCi(const SurgSim::Math::Matrix& A, size_t i) const
+{
+ return A.block<BlockSize, BlockSize>(BlockSize * i, BlockSize * (i + 1));
+}
+
+template <int BlockSize>
+void LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::inverseTriDiagonalBlock(const SurgSim::Math::Matrix& A,
+ SurgSim::Math::Matrix* inverse,
+ bool isSymmetric)
+{
+ SURGSIM_ASSERT(inverse != nullptr) << "Null inverse matrix pointer";
+
+ SURGSIM_ASSERT(A.cols() == A.rows()) <<
+ "Cannot inverse a non square tri-diagonal block matrix ("<< A.rows() <<" x "<< A.cols() <<")";
+
+ const size_t size = A.rows();
+ const size_t numBlocks = size / BlockSize;
+
+ SURGSIM_ASSERT(numBlocks * BlockSize == size) <<
+ "Bad tri-diagonal block matrix structure, size = " << size << " block size = " << BlockSize <<
+ " and the number of blocks are " << numBlocks;
+
+ // If the matrix size is less or equal to 4 (Eigen inverse use co-factor for those), or the matrix is
+ // composed of an unique block, simply call the normal Eigen inverse method.
+ if (size <= 4 || numBlocks == static_cast<size_t>(1))
+ {
+ *inverse = A.inverse();
+ return;
+ }
+
+ if (inverse->rows() < 0 || static_cast<size_t>(inverse->rows()) != size
+ || inverse->cols() < 0 || static_cast<size_t>(inverse->cols()) != size)
+ {
+ inverse->resize(size, size);
+ }
+
+ m_Bi_AiDiminus1_inv.resize(numBlocks);
+ m_Di.resize(numBlocks - 1);
+ m_Ei.resize(numBlocks); // Should be of size m_numBlocks - 1 too, but index 0 is undefined and we
+ // decided to not change the indexing to not introduce any confusion
+
+ // Bi_AiDiminus1_inv[0] = (B0)^-1
+ // D [0] = (B0)^-1.C0
+ m_Bi_AiDiminus1_inv[0] = Bi(A, 0).inverse();
+ m_Di[0] = m_Bi_AiDiminus1_inv[0] * (-minusCi(A, 0));
+ // Bi_AiDiminus1_inv[i] = (Bi - Ai.D[i-1])^-1
+ // Di [i] = (Bi - Ai.D[i-1])^-1 . Ci
+ for(size_t i = 1; i < numBlocks - 1; ++i)
+ {
+ m_Bi_AiDiminus1_inv[i] = (Bi(A, i) - (-minusAi(A, i)) * m_Di[i - 1]).inverse();
+ m_Di[i] = m_Bi_AiDiminus1_inv[i] * (-minusCi(A, i));
+ }
+ // Bi_AiDiminus1_inv[nbBlock-1] = (B(nbBlock-1) - A(nbBlock-1).D(nbBlock-2))^-1
+ // D [nbBlock-1] = UNDEFINED because C(nbBlock-1) does not exist
+ m_Bi_AiDiminus1_inv[numBlocks - 1] =
+ (Bi(A, numBlocks - 1) - (-minusAi(A, numBlocks - 1)) * m_Di[numBlocks - 2]).inverse();
+
+ // E[nbBlock-1] = (B(nbBlock-1))^-1 . A(nbBlock-1)
+ // Ei = (Bi - Ci.E(i+1))^-1 . Ai
+ // E0 = UNDEFINED because A0 does not exist
+ m_Ei[numBlocks - 1] = Bi(A, numBlocks - 1).inverse() * (-minusAi(A, numBlocks - 1));
+ for(size_t i = numBlocks - 2; i > 0; --i)
+ {
+ m_Ei[i] = (Bi(A, i) - (-minusCi(A, i)) * m_Ei[i + 1]).inverse() * (-minusAi(A, i));
+ }
+
+ // Inverse diagonal blocks:
+ // inv(i,i) = (I - Di.E(i+1))^-1.Bi_AiDiminus1_inv[i]
+ for(size_t i = 0; i < numBlocks - 1; ++i)
+ {
+ inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * i) =
+ (Block::Identity() - m_Di[i] * m_Ei[i + 1]).inverse() * m_Bi_AiDiminus1_inv[i];
+ }
+ // inv(nbBlock-1,nbBlock-1) = Bi_AiDiminus1_inv[nbBlock-1]
+ inverse->block<BlockSize, BlockSize>(BlockSize * (numBlocks - 1), BlockSize * (numBlocks - 1)) =
+ m_Bi_AiDiminus1_inv[numBlocks - 1];
+
+ // Inverse off-diagonal blocks:
+ // inv(i,j) = Di.inv(i+1,j) for i<j
+ // inv(i,j) = Ei.inv(i-1,j) for i>j
+ if (isSymmetric)
+ {
+ for(size_t j = 1; j < numBlocks; ++j)
+ {
+ for(size_t i = j; i > 0; --i)
+ {
+ inverse->block<BlockSize, BlockSize>(BlockSize * (i - 1), BlockSize * j) =
+ m_Di[i - 1] * inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * j);
+ inverse->block<BlockSize, BlockSize>(BlockSize * j, BlockSize * (i - 1)) =
+ inverse->block<BlockSize, BlockSize>(BlockSize * (i - 1), BlockSize * j).transpose();
+ }
+ }
+ }
+ else
+ {
+ for(int j = 0; j < static_cast<int>(numBlocks); ++j)
+ {
+ for(int i = j - 1; i >= 0; --i)
+ {
+ inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * j) =
+ m_Di[i] * inverse->block<BlockSize, BlockSize>(BlockSize * (i + 1), BlockSize * j);
+ }
+ for(int i = j + 1; i < static_cast<int>(numBlocks); ++i)
+ {
+ inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * j) =
+ m_Ei[i] * inverse->block<BlockSize, BlockSize>(BlockSize * (i - 1), BlockSize * j);
+ }
+ }
+ }
+}
+
+template <int BlockSize>
+void LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::operator ()(const Matrix& A, const Vector& b, Vector* x,
+ Matrix* Ainv)
+{
+ SURGSIM_ASSERT(A.cols() == A.rows()) << "Cannot inverse a non square matrix";
+
+ if (Ainv != nullptr)
+ {
+ inverseTriDiagonalBlock(A, Ainv);
+ if (x != nullptr)
+ {
+ (*x) = (*Ainv) * b;
+ }
+ }
+ else if (x != nullptr)
+ {
+ inverseTriDiagonalBlock(A, &m_inverse);
+ (*x) = m_inverse * b;
+ }
+}
+
+template <int BlockSize>
+void LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<BlockSize>::operator ()(const Matrix& A, const Vector& b,
+ Vector* x,
+ Matrix* Ainv)
+{
+ SURGSIM_ASSERT(A.cols() == A.rows()) << "Cannot inverse a non square matrix";
+
+ if (Ainv != nullptr)
+ {
+ inverseTriDiagonalBlock(A, Ainv, true);
+ if (x != nullptr)
+ {
+ (*x) = (*Ainv) * b;
+ }
+ }
+ else if (x != nullptr)
+ {
+ inverseTriDiagonalBlock(A, &m_inverse, true);
+ (*x) = m_inverse * b;
+ }
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_LINEARSOLVEANDINVERSE_INL_H
diff --git a/SurgSim/Math/LinearSolveAndInverse.cpp b/SurgSim/Math/LinearSolveAndInverse.cpp
new file mode 100644
index 0000000..f5dc13a
--- /dev/null
+++ b/SurgSim/Math/LinearSolveAndInverse.cpp
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/LinearSolveAndInverse.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+void LinearSolveAndInverseDiagonalMatrix::operator ()(const Matrix& A, const Vector& b, Vector* x, Matrix* Ainv)
+{
+ SURGSIM_ASSERT(A.cols() == A.rows()) << "Cannot inverse a non square matrix";
+
+ if (Ainv != nullptr)
+ {
+ if (Ainv->cols() != A.cols() || Ainv->rows() != A.cols())
+ {
+ Ainv->resize(A.rows(), A.cols());
+ }
+ Ainv->setZero();
+ Ainv->diagonal() = A.diagonal().cwiseInverse();
+
+ if (x != nullptr)
+ {
+ (*x) = Ainv->diagonal().cwiseProduct(b);
+ }
+ }
+ else if (x != nullptr)
+ {
+ (*x) = A.diagonal().cwiseInverse().cwiseProduct(b);
+ }
+}
+
+void LinearSolveAndInverseDenseMatrix::operator ()(const Matrix& A, const Vector& b, Vector* x, Matrix* Ainv)
+{
+ SURGSIM_ASSERT(A.cols() == A.rows()) << "Cannot inverse a non square matrix";
+
+ if (x != nullptr || Ainv != nullptr)
+ {
+ const Eigen::PartialPivLU<typename Eigen::MatrixBase<Matrix>::PlainObject> lu = A.partialPivLu();
+ if (x != nullptr)
+ {
+ (*x) = lu.solve(b);
+ }
+ if (Ainv != nullptr)
+ {
+ (*Ainv) = lu.inverse();
+ }
+ }
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/LinearSolveAndInverse.h b/SurgSim/Math/LinearSolveAndInverse.h
new file mode 100644
index 0000000..1bf9030
--- /dev/null
+++ b/SurgSim/Math/LinearSolveAndInverse.h
@@ -0,0 +1,134 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_LINEARSOLVEANDINVERSE_H
+#define SURGSIM_MATH_LINEARSOLVEANDINVERSE_H
+
+#include "SurgSim/Framework/Assert.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+
+#include <Eigen/Core>
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// LinearSolveAndInverse aims at performing an efficient linear system resolution and
+/// calculating its inverse matrix at the same time.
+/// This class is very useful in the OdeSolver resolution to improve performance.
+/// \sa SurgSim::Math::OdeSolver
+class LinearSolveAndInverse
+{
+public:
+ virtual ~LinearSolveAndInverse(){}
+
+ /// Solve a linear system A.x=b and compute the matrix A^-1
+ /// \param A Linear system matrix
+ /// \param b Linear system right-hand-side
+ /// \param[out] x Linear system unknown (if requested)
+ /// \param[out] Ainv Linear system matrix inverse = A^-1 (if requested)
+ virtual void operator ()(const Matrix& A, const Vector& b, Vector* x = nullptr, Matrix* Ainv = nullptr) = 0;
+};
+
+/// Derivation for dense matrix type
+class LinearSolveAndInverseDenseMatrix : public LinearSolveAndInverse
+{
+public:
+ virtual void operator ()(const Matrix& A, const Vector& b, Vector* x = nullptr, Matrix* Ainv = nullptr) override;
+};
+
+/// Derivation for diagonal matrix type
+class LinearSolveAndInverseDiagonalMatrix : public LinearSolveAndInverse
+{
+public:
+ virtual void operator ()(const Matrix& A, const Vector& b, Vector* x = nullptr, Matrix* Ainv = nullptr) override;
+};
+
+/// Derivation for tri-diagonal block matrix type
+/// \tparam BlockSize Define the block size of the tri-diagonal block matrix
+template <int BlockSize>
+class LinearSolveAndInverseTriDiagonalBlockMatrix : public LinearSolveAndInverse
+{
+protected:
+ /// Computes the inverse matrix
+ /// \param A The matrix to inverse
+ /// \param[out] inv The inverse matrix
+ /// \param isSymmetric True if the matrix is symmetric, False otherwise
+ /// \note isSymmetric is only indicative and helps optimizing the computation when the matrix is symmetric.
+ /// \note On the other side, if the flag is true and the matrix is not symmetric, the result will be wrong.
+ /// \note Assert on inverse matrix pointer (inv), on the matrix being square and on
+ /// \note proper size matching between the matrix size and the blockSize
+ void inverseTriDiagonalBlock(const SurgSim::Math::Matrix& A, SurgSim::Math::Matrix* inv, bool isSymmetric = false);
+
+ /// Member variable to hold the inverse matrix in case only the solving is requested
+ Matrix m_inverse;
+
+private:
+ static_assert(BlockSize > 0,
+ "Cannot define a tri-diagonal block matrix with block size 0 or negative");
+
+ typedef Eigen::Matrix<Matrix::Scalar, BlockSize, BlockSize, Matrix::Options> Block;
+
+ /// Gets a lower-diagonal block element (named -Ai in the algorithm)
+ /// \param A The matrix on which to retrieve the lower-diagonal block element
+ /// \param i The line index on which to retrieve the lower-diagonal block element
+ /// \return The lower-diagonal block element requested (i.e. block (i, i-1))
+ const Eigen::Block<const Matrix, BlockSize, BlockSize> minusAi(const SurgSim::Math::Matrix& A, size_t i) const;
+
+ /// Gets a diagonal block element (named Bi in the algorithm)
+ /// \param A The matrix on which to retrieve the diagonal block element
+ /// \param i The line index on which to retrieve the diagonal block element
+ /// \return The diagonal block element requested (i.e. block (i, i))
+ const Eigen::Block<const Matrix, BlockSize, BlockSize> Bi(const SurgSim::Math::Matrix& A, size_t i) const;
+
+ /// Gets a upper-diagonal block element (named -Ci in the algorithm)
+ /// \param A The matrix on which to retrieve the upper-diagonal block element
+ /// \param i The line index on which to retrieve the upper-diagonal block element
+ /// \return The upper-diagonal block element requested (i.e. block (i, i+1))
+ const Eigen::Block<const Matrix, BlockSize, BlockSize> minusCi(const SurgSim::Math::Matrix& A, size_t i) const;
+
+ ///@{
+ /// Intermediate block matrices, helpful to construct the inverse matrix
+ std::vector<Block> m_Di, m_Ei, m_Bi_AiDiminus1_inv;
+ ///@}
+
+public:
+ virtual void operator ()(const Matrix& A, const Vector& b, Vector* x = nullptr, Matrix* Ainv = nullptr) override;
+};
+
+/// Derivation for symmetric tri-diagonal block matrix type
+/// \tparam BlockSize Define the block size of the tri-diagonal block matrix
+template <int BlockSize>
+class LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix :
+ public LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>
+{
+public:
+ virtual void operator ()(const Matrix& A, const Vector& b, Vector* x = nullptr, Matrix* Ainv = nullptr) override;
+
+ using LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::inverseTriDiagonalBlock;
+ using LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::m_inverse;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#include "SurgSim/Math/LinearSolveAndInverse-inl.h"
+
+#endif // SURGSIM_MATH_LINEARSOLVEANDINVERSE_H
diff --git a/SurgSim/Math/MathConvert-inl.h b/SurgSim/Math/MathConvert-inl.h
new file mode 100644
index 0000000..9135067
--- /dev/null
+++ b/SurgSim/Math/MathConvert-inl.h
@@ -0,0 +1,194 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_MATHCONVERT_INL_H
+#define SURGSIM_MATH_MATHCONVERT_INL_H
+
+#include <string>
+
+#include "SurgSim/Framework/Log.h"
+
+namespace
+{
+const std::string rotationPropertyName = "Quaternion";
+const std::string translationPropertyName = "Translation";
+const std::string serializeLogger = "Serialization";
+};
+
+SURGSIM_DOUBLE_SPECIALIZATION
+template <typename Type, int Rows, int MOpt>
+YAML::Node YAML::convert<typename Eigen::Matrix<Type, Rows, 1, MOpt>>::encode(
+ const typename Eigen::Matrix<Type, Rows, 1, MOpt>& rhs)
+{
+ Node node;
+ node.SetStyle(YAML::FlowStyle);
+ for (int i = 0; i < rhs.size(); ++i)
+ {
+ node.push_back(rhs[i]);
+ }
+
+ return node;
+}
+
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Type, int Rows, int MOpt>
+bool YAML::convert<typename Eigen::Matrix<Type, Rows, 1, MOpt>>::decode(
+ const Node& node, typename Eigen::Matrix<Type, Rows, 1, MOpt>& rhs)
+{
+ if (! node.IsSequence() || node.size() != Rows)
+ {
+ return false;
+ }
+
+ for (unsigned i = 0; i < node.size(); ++i)
+ {
+ try
+ {
+ rhs[i] = node[i].as<Type>();
+ }
+ catch (YAML::RepresentationException)
+ {
+ rhs[i] = std::numeric_limits<Type>::quiet_NaN();
+
+ auto logger = SurgSim::Framework::Logger::getLogger(serializeLogger);
+ SURGSIM_LOG(logger, WARNING) << "Bad conversion: #NaN value";
+ }
+ }
+ return true;
+}
+
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Type, int Rows, int Cols, int MOpt>
+YAML::Node YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::encode(
+ const typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs)
+{
+ YAML::Node node;
+ node.SetStyle(YAML::FlowStyle);
+ for (int row = 0; row < Rows; ++row)
+ {
+ YAML::Node rowNode;
+ for (int col = 0; col < Cols; ++col)
+ {
+ rowNode.push_back(rhs.row(row)[col]);
+ }
+ node.push_back(rowNode);
+ }
+ return node;
+}
+
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Type, int Rows, int Cols, int MOpt>
+bool YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::decode(
+ const Node& node,
+ typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs)
+{
+ if (! node.IsSequence() || node.size() != Rows)
+ {
+ return false;
+ }
+
+ for (size_t row = 0; row < node.size(); ++row)
+ {
+ YAML::Node rowNode = node[row];
+ if (!rowNode.IsSequence() || node.size() != Cols)
+ {
+ return false;
+ }
+ for (size_t col = 0; col < rowNode.size(); ++col)
+ {
+ try
+ {
+ rhs.row(row)[col] = rowNode[col].as<Type>();
+ }
+ catch (YAML::RepresentationException)
+ {
+ rhs.row(row)[col] = std::numeric_limits<Type>::quiet_NaN();
+ auto logger = SurgSim::Framework::Logger::getLogger(serializeLogger);
+ SURGSIM_LOG(logger, WARNING) << "Bad conversion: #NaN value";
+ }
+ }
+ }
+ return true;
+}
+
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Type, int QOpt>
+YAML::Node YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::encode(
+ const typename Eigen::Quaternion<Type, QOpt>& rhs)
+{
+ return Node(convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::encode(rhs.coeffs()));
+}
+
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Type, int QOpt>
+bool YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::decode(
+ const Node& node,
+ typename Eigen::Quaternion<Type, QOpt>& rhs)
+{
+ bool result = false;
+ if (node.IsSequence() && node.size() == 4)
+ {
+ result = convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::decode(node, rhs.coeffs());
+ }
+ return result;
+}
+
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Type, int Dim, int TMode, int TOptions>
+YAML::Node YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::encode(
+ const typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs)
+{
+ typedef typename Eigen::Transform<Type, Dim, TMode, TOptions>::LinearMatrixType LinearMatrixType;
+ LinearMatrixType linear(rhs.linear());
+ Eigen::Quaternion<Type, TOptions> quaternion(linear);
+ Eigen::Matrix<Type, Dim, 1, TOptions> translation(rhs.translation());
+
+ Node node;
+ node[rotationPropertyName] = quaternion;
+ node[translationPropertyName] = translation;
+ return node;
+}
+
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Type, int Dim, int TMode, int TOptions>
+bool YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::decode(
+ const Node& node,
+ typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs)
+{
+ bool result = false;
+
+
+ if (node.IsMap())
+ {
+ Eigen::Quaternion<Type, TOptions> rotation(Eigen::Quaternion<Type, TOptions>::Identity());
+ Eigen::Matrix<Type, Dim, 1, TOptions> translation(Eigen::Matrix<Type, Dim, 1, TOptions>::Zero());
+ if (node[rotationPropertyName].IsDefined())
+ {
+ rotation = node[rotationPropertyName].as<Eigen::Quaternion<Type, TOptions>>();
+ result = true;
+ }
+ if (node[translationPropertyName].IsDefined())
+ {
+ translation = node[translationPropertyName].as<Eigen::Matrix<Type, Dim, 1, TOptions>>();
+ result = true;
+ }
+ rhs.makeAffine();
+ rhs.linear() = rotation.matrix();
+ rhs.translation() = translation;
+ }
+ return result;
+}
+
+#endif // SURGSIM_MATH_MATHCONVERT_INL_H
diff --git a/SurgSim/Math/MathConvert.cpp b/SurgSim/Math/MathConvert.cpp
new file mode 100644
index 0000000..0a514fe
--- /dev/null
+++ b/SurgSim/Math/MathConvert.cpp
@@ -0,0 +1,117 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/MathConvert.h"
+
+#include <algorithm>
+
+#include "SurgSim/Math/Shape.h"
+
+namespace YAML
+{
+
+Node convert<std::shared_ptr<SurgSim::Math::Shape>>::encode(
+ const std::shared_ptr<SurgSim::Math::Shape>& rhs)
+{
+ SURGSIM_ASSERT(nullptr != rhs) << "Trying to encode nullptr SurgSim::Math::Shape";
+ Node result;
+ result[rhs->getClassName()] = rhs->encode();
+
+ return result;
+}
+
+bool convert<std::shared_ptr<SurgSim::Math::Shape>>::decode(
+ const Node& node,
+ std::shared_ptr<SurgSim::Math::Shape>& rhs)
+{
+ bool result = false;
+
+ if (node.IsMap())
+ {
+ if (nullptr == rhs)
+ {
+ std::string className = node.begin()->first.as<std::string>();
+ SurgSim::Math::Shape::FactoryType& factory = SurgSim::Math::Shape::getFactory();
+
+ if (factory.isRegistered(className))
+ {
+ rhs = factory.create(className);
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Class " << className << " is not registered in the factory.";
+ }
+ }
+
+ Node data = node.begin()->second;
+ if (data.IsMap())
+ {
+ rhs->decode(data);
+ }
+
+ result = true;
+ }
+
+ return result;
+}
+
+
+Node convert<SurgSim::Math::IntegrationScheme>::encode(
+ const SurgSim::Math::IntegrationScheme& rhs)
+{
+ Node result;
+
+ auto it = SurgSim::Math::IntegrationSchemeNames.find(rhs);
+ SURGSIM_ASSERT(it != std::end(SurgSim::Math::IntegrationSchemeNames)) << "Can not find the enum value in " <<
+ "SurgSim::Math::IntegrationSchemeNames. Is the enum value registered?";
+
+ result["SurgSim::Math::IntegrationScheme"] = it->second;
+
+ return result;
+}
+
+bool convert<SurgSim::Math::IntegrationScheme>::decode(
+ const Node& node,
+ SurgSim::Math::IntegrationScheme& rhs)
+{
+ bool result = false;
+
+ if (node.IsMap())
+ {
+ std::string className = node.begin()->first.as<std::string>();
+ SURGSIM_ASSERT("SurgSim::Math::IntegrationScheme" == className);
+
+ std::string schemeName = node.begin()->second.as<std::string>();
+ std::transform(schemeName.begin(), schemeName.end(), schemeName.begin(), ::toupper);
+
+ auto it = std::find_if(std::begin(SurgSim::Math::IntegrationSchemeNames),
+ std::end(SurgSim::Math::IntegrationSchemeNames),
+ [&schemeName](const std::pair<SurgSim::Math::IntegrationScheme, std::string>& pair)
+ {
+ return pair.second == schemeName;
+ }
+ );
+
+ SURGSIM_ASSERT (it != std::end(SurgSim::Math::IntegrationSchemeNames)) <<
+ "Unknown IntegrationScheme " << schemeName;
+
+ rhs = it->first;
+ result = true;
+ }
+
+ return result;
+}
+
+}; // namespace YAML
\ No newline at end of file
diff --git a/SurgSim/Math/MathConvert.h b/SurgSim/Math/MathConvert.h
new file mode 100644
index 0000000..618802b
--- /dev/null
+++ b/SurgSim/Math/MathConvert.h
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_MATHCONVERT_H
+#define SURGSIM_MATH_MATHCONVERT_H
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Math/OdeSolver.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+class Shape;
+}
+}
+
+/// \file MathConvert.h
+/// This contains a series of functions to encode and decode Eigen data structures to
+/// and from YAML nodes. These conversion functions will extinguish Eigen options, these
+/// are not serialized, the output is determined by the type as it is declared in the
+/// appropriate conversion function, with Eigen::Transform, this could lead to problems
+/// If the mode that is used for reading is different than the mode that was used while
+/// writing.
+
+namespace YAML
+{
+ /// Specialization of convert for fixed size Eigen::Matrix
+ SURGSIM_DOUBLE_SPECIALIZATION
+ template <typename Type, int Rows, int Cols, int MOpt>
+ struct convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>
+ {
+ static Node encode(const typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs);
+ static bool decode(const Node& node, typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs);
+ };
+
+ /// Specialization for Eigen Row Vectors, which are the type that Vector2x, Vector3x use
+ SURGSIM_DOUBLE_SPECIALIZATION
+ template <class Type, int Rows, int MOpt>
+ struct convert <typename Eigen::Matrix<Type,Rows,1,MOpt>>
+ {
+ static Node encode(const typename Eigen::Matrix<Type, Rows, 1, MOpt>& rhs);
+ static bool decode(const Node& node, typename Eigen::Matrix<Type, Rows, 1, MOpt>& rhs);
+ };
+
+ /// Specialization of convert for Eigen::Quaternion
+ SURGSIM_DOUBLE_SPECIALIZATION
+ template <class Type, int QOpt>
+ struct convert<typename Eigen::Quaternion<Type, QOpt>>
+ {
+ static Node encode(const typename Eigen::Quaternion<Type, QOpt>& rhs);
+ static bool decode(const Node& node, typename Eigen::Quaternion<Type, QOpt>& rhs);
+ };
+
+ /// Specialization of convert for Eigen::RigidTransform
+ SURGSIM_DOUBLE_SPECIALIZATION
+ template <class Type, int Dim, int TMode, int TOptions>
+ struct convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>
+ {
+ static Node encode(const typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs);
+ static bool decode(const Node& node, typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs);
+ };
+
+ template <>
+ struct convert<std::shared_ptr<SurgSim::Math::Shape>>
+ {
+ static Node encode(const std::shared_ptr<SurgSim::Math::Shape>& rhs);
+ static bool decode(const Node& node, std::shared_ptr<SurgSim::Math::Shape>& rhs);
+ };
+
+ template <>
+ struct convert<SurgSim::Math::IntegrationScheme>
+ {
+ static Node encode(const SurgSim::Math::IntegrationScheme& rhs);
+ static bool decode(const Node& node, SurgSim::Math::IntegrationScheme& rhs);
+ };
+};
+
+#include "SurgSim/Math/MathConvert-inl.h"
+
+#endif // SURGSIM_MATH_MATHCONVERT_H
\ No newline at end of file
diff --git a/SurgSim/Math/Matrix.h b/SurgSim/Math/Matrix.h
new file mode 100644
index 0000000..99f0a3f
--- /dev/null
+++ b/SurgSim/Math/Matrix.h
@@ -0,0 +1,223 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 small fixed-size square matrix types.
+
+#ifndef SURGSIM_MATH_MATRIX_H
+#define SURGSIM_MATH_MATRIX_H
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <Eigen/LU> // needed for determinant() and inverse()
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// A 2x2 matrix of floats.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<float, 2, 2, Eigen::RowMajor> Matrix22f;
+
+/// A 3x3 matrix of floats.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Matrix33f;
+
+/// A 4x4 matrix of floats.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor> Matrix44f;
+
+/// A 2x2 matrix of doubles.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<double, 2, 2, Eigen::RowMajor> Matrix22d;
+
+/// A 3x3 matrix of doubles.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Matrix33d;
+
+/// A 4x4 matrix of doubles.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<double, 4, 4, Eigen::RowMajor> Matrix44d;
+
+/// A 6x6 matrix of doubles.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Matrix66d;
+
+/// A dynamic size diagonal matrix
+typedef Eigen::DiagonalMatrix<double, Eigen::Dynamic> DiagonalMatrix;
+
+/// A dynamic size matrix
+typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
+
+/// Create a rotation matrix corresponding to the specified angle (in radians) and axis.
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam VOpt the option flags (alignment etc.) used for the axis vector argument. Can be deduced.
+/// \param angle the angle of the rotation, in radians.
+/// \param axis the axis of the rotation.
+/// \returns the rotation matrix.
+template <typename T, int VOpt>
+inline Eigen::Matrix<T, 3, 3> makeRotationMatrix(const T& angle, const Eigen::Matrix<T, 3, 1, VOpt>& axis)
+{
+ return Eigen::AngleAxis<T>(angle, axis).toRotationMatrix();
+}
+
+/// Create a skew-symmetric matrix corresponding to the specified vector. Skew-symmetric matrices are particularly
+/// useful for representing a portion of the vector cross-product.
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam VOpt the option flags (alignment etc.) used for the vector argument. Can be deduced.
+/// \param vector the vector to be transformed.
+/// \returns the skew-symmetric matrix corresponding with the vector argument.
+template <typename T, int VOpt>
+inline Eigen::Matrix<T, 3, 3> makeSkewSymmetricMatrix(const Eigen::Matrix<T, 3, 1, VOpt>& vector)
+{
+ Eigen::Matrix<T, 3, 3> result;
+
+ result(0, 0) = 0.0;
+ result(0, 1) = -vector(2);
+ result(0, 2) = vector(1);
+
+ result(1, 0) = vector(2);
+ result(1, 1) = 0.0;
+ result(1, 2) = -vector(0);
+
+ result(2, 0) = -vector(1);
+ result(2, 1) = vector(0);
+ result(2, 2) = 0.0;
+
+ return result;
+}
+
+/// Extract the unique vector from the skew-symmetric part of a given matrix.
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam MOpt the option flags (alignment etc.) used for the matrix argument. Can be deduced.
+/// \param matrix the matrix to compute the skew symmetric part from.
+/// \returns the unique vector defining the skew-symmetric part of the matrix.
+/// \note For any vector u, skew(makeSkewSymmetricMatrix(u)) = u
+/// \note In general, returns the vector of the skew symmetric part of matrix: (matrix - matrix^T)/2
+template <typename T, int MOpt>
+inline Eigen::Matrix<T, 3, 1> skew(const Eigen::Matrix<T, 3, 3, MOpt>& matrix)
+{
+ Eigen::Matrix<T, 3, 3, MOpt> skewSymmetricPart = (matrix - matrix.transpose()) / 2.0;
+ return Eigen::Matrix<T, 3, 1>(skewSymmetricPart(2, 1), skewSymmetricPart(0, 2), skewSymmetricPart(1, 0));
+}
+
+/// Get the angle (in radians) and axis corresponding to a rotation matrix.
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam MOpt the option flags (alignment etc.) used for the rotation matrix argument. Can be deduced.
+/// \tparam VOpt the option flags (alignment etc.) used for the axis vector argument. Can be deduced.
+/// \param matrix the rotation matrix to inspect.
+/// \param [out] angle the angle of the rotation, in radians.
+/// \param [out] axis the axis of the rotation.
+template <typename T, int MOpt, int VOpt>
+inline void computeAngleAndAxis(const Eigen::Matrix<T, 3, 3, MOpt>& matrix,
+ T* angle, Eigen::Matrix<T, 3, 1, VOpt>* axis)
+{
+ Eigen::AngleAxis<T> angleAxis(matrix);
+ *angle = angleAxis.angle();
+ *axis = angleAxis.axis();
+}
+
+/// Get the angle corresponding to a quaternion's rotation, in radians.
+/// If you don't care about the rotation axis, this is more efficient than computeAngleAndAxis().
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam MOpt the option flags (alignment etc.) used for the rotation matrix argument. Can be deduced.
+/// \param matrix the rotation matrix to inspect.
+/// \returns the angle of the rotation, in radians.
+template <typename T, int MOpt>
+inline T computeAngle(const Eigen::Matrix<T, 3, 3, MOpt>& matrix)
+{
+ // TODO(bert): there has to be a more efficient way...
+ Eigen::AngleAxis<T> angleAxis(matrix);
+ return angleAxis.angle();
+}
+
+/// Helper method to add a sub-matrix into a matrix, for the sake of clarity
+/// \tparam Matrix The matrix type
+/// \tparam SubMatrix The sub-matrix type
+/// \param subMatrix The sub-matrix
+/// \param blockIdRow, blockIdCol The block indices in matrix
+/// \param blockSizeRow, blockSizeCol The block size (size of the sub-matrix)
+/// \param[out] matrix The matrix to add the sub-matrix into
+template <class Matrix, class SubMatrix>
+void addSubMatrix(const SubMatrix& subMatrix, size_t blockIdRow, size_t blockIdCol,
+ size_t blockSizeRow, size_t blockSizeCol, Matrix* matrix)
+{
+ matrix->block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol, blockSizeRow, blockSizeCol) += subMatrix;
+}
+
+/// Helper method to add a sub-matrix made of squared-blocks into a matrix, for the sake of clarity
+/// \tparam Matrix The matrix type
+/// \tparam SubMatrix The sub-matrix type
+/// \param subMatrix The sub-matrix (containing all the squared-blocks)
+/// \param blockIds Vector of block indices (for accessing matrix) corresponding to the blocks in sub-matrix
+/// \param blockSize The blocks size
+/// \param[out] matrix The matrix to add the sub-matrix blocks into
+template <class Matrix, class SubMatrix>
+void addSubMatrix(const SubMatrix& subMatrix, const std::vector<size_t> blockIds, size_t blockSize, Matrix* matrix)
+{
+ const size_t numBlocks = blockIds.size();
+
+ for (size_t block0 = 0; block0 < numBlocks; block0++)
+ {
+ size_t blockId0 = blockIds[block0];
+
+ for (size_t block1 = 0; block1 < numBlocks; block1++)
+ {
+ size_t blockId1 = blockIds[block1];
+
+ matrix->block(blockSize * blockId0, blockSize * blockId1, blockSize, blockSize)
+ += subMatrix.block(blockSize * block0, blockSize * block1, blockSize, blockSize);
+ }
+ }
+}
+
+/// Helper method to set a sub-matrix into a matrix, for the sake of clarity
+/// \tparam Matrix The matrix type
+/// \tparam SubMatrix The sub-matrix type
+/// \param subMatrix The sub-matrix
+/// \param blockIdRow, blockIdCol The block indices for row and column in matrix
+/// \param blockSizeRow, blockSizeCol The size of the sub-matrix
+/// \param[out] matrix The matrix to set the sub-matrix into
+template <class Matrix, class SubMatrix>
+void setSubMatrix(const SubMatrix& subMatrix, size_t blockIdRow, size_t blockIdCol,
+ size_t blockSizeRow, size_t blockSizeCol, Matrix* matrix)
+{
+ matrix->block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol,
+ blockSizeRow, blockSizeCol) = subMatrix;
+}
+
+/// Helper method to access a sub-matrix from a matrix, for the sake of clarity
+/// \tparam Matrix The matrix type to get the sub-matrix from
+/// \param matrix The matrix to get the sub-matrix from
+/// \param blockIdRow, blockIdCol The block indices
+/// \param blockSizeRow, blockSizeCol The block size
+/// \return The requested sub-matrix
+/// \note Disable cpplint warnings for use of non-const reference
+/// \note Eigen has a specific type for Block that we want to return with read/write access
+/// \note therefore the Matrix from which the Block is built from must not be const
+template <class Matrix>
+Eigen::Block<Matrix> getSubMatrix(Matrix& matrix, size_t blockIdRow, size_t blockIdCol, // NOLINT
+ size_t blockSizeRow, size_t blockSizeCol)
+{
+ return matrix.block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol, blockSizeRow, blockSizeCol);
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_MATRIX_H
diff --git a/SurgSim/Math/MeshShape-inl.h b/SurgSim/Math/MeshShape-inl.h
new file mode 100644
index 0000000..9e6d769
--- /dev/null
+++ b/SurgSim/Math/MeshShape-inl.h
@@ -0,0 +1,42 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+
+#ifndef SURGSIM_MATH_MESHSHAPE_INL_H
+#define SURGSIM_MATH_MESHSHAPE_INL_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <class VertexData, class EdgeData, class TriangleData>
+MeshShape::MeshShape(const SurgSim::DataStructures::TriangleMeshBase<VertexData, EdgeData, TriangleData>& mesh) :
+ m_volume(0.0)
+{
+ SURGSIM_ASSERT(mesh.isValid()) << "Invalid mesh";
+
+ m_initialMesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(mesh);
+ m_mesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(*m_initialMesh);
+ updateAabbTree();
+
+ // Computes the geometric properties for the initial mesh
+ computeVolumeIntegrals();
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Math/MeshShape.cpp b/SurgSim/Math/MeshShape.cpp
new file mode 100644
index 0000000..4b7a48c
--- /dev/null
+++ b/SurgSim/Math/MeshShape.cpp
@@ -0,0 +1,212 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::MeshShape, MeshShape);
+
+MeshShape::MeshShape() : m_volume(0.0)
+{
+ serializeFileName(this);
+}
+
+int MeshShape::getType()
+{
+ return SHAPE_TYPE_MESH;
+}
+
+bool MeshShape::isValid() const
+{
+ return (nullptr != m_mesh) && (m_mesh->isValid());
+}
+
+bool MeshShape::doLoad(const std::string& filePath)
+{
+ m_initialMesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>();
+ SURGSIM_ASSERT(m_initialMesh->doLoad(filePath)) << "Failed to load file " << filePath;
+ SURGSIM_ASSERT(m_initialMesh->isValid()) << filePath << " contains an invalid mesh.";
+
+ m_mesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(*m_initialMesh);
+
+ updateAabbTree();
+ computeVolumeIntegrals();
+
+ return true;
+}
+
+std::shared_ptr<SurgSim::DataStructures::TriangleMesh> MeshShape::getInitialMesh()
+{
+ return m_initialMesh;
+}
+
+std::shared_ptr<SurgSim::DataStructures::TriangleMesh> MeshShape::getMesh()
+{
+ return m_mesh;
+}
+
+double MeshShape::getVolume() const
+{
+ if (nullptr == m_mesh)
+ {
+ SURGSIM_LOG_CRITICAL(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "No mesh set for MeshShape, so it cannot compute volume.";
+ }
+ return m_volume;
+}
+
+SurgSim::Math::Vector3d MeshShape::getCenter() const
+{
+ if (nullptr == m_mesh)
+ {
+ SURGSIM_LOG_CRITICAL(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "No mesh set for MeshShape, so it cannot compute center.";
+ }
+ return m_center;
+}
+
+SurgSim::Math::Matrix33d MeshShape::getSecondMomentOfVolume() const
+{
+ if (nullptr == m_mesh)
+ {
+ SURGSIM_LOG_CRITICAL(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "No mesh set for MeshShape, so it cannot compute SecondMomentOfVolume.";
+ }
+ return m_secondMomentOfVolume;
+}
+
+namespace
+{
+/// Compute various integral terms
+/// \note Please refer to http://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf
+/// \note for details about parameters
+void computeIntegralTerms(const double w0, const double w1, const double w2,
+ double* f1, double* f2, double* f3, double* g0, double* g1, double* g2)
+{
+ double temp0 = w0 + w1;
+ double temp1 = w0 * w0;
+ double temp2 = temp1 + w1 * temp0;
+ *f1 = temp0 + w2;
+ *f2 = temp2 + w2 * (*f1);
+ *f3 = w0 * temp1 + w1 * temp2 + w2 * (*f2);
+ *g0 = (*f2) + w0 * ((*f1) + w0);
+ *g1 = (*f2) + w1 * ((*f1) + w1);
+ *g2 = (*f2) + w2 * ((*f1) + w2);
+}
+};
+
+void MeshShape::computeVolumeIntegrals()
+{
+ Eigen::VectorXd multiplier(10);
+ multiplier << 1.0 / 6.0, 1.0 / 24.0, 1.0 / 24.0, 1.0 / 24.0, 1.0 / 60.0,
+ 1.0 / 60.0, 1.0 / 60.0, 1.0 / 120.0, 1.0 / 120.0, 1.0 / 120.0;
+
+ Eigen::VectorXd integral(10); // integral order: 1, x, y, z, x^2, y^2, z^2, xy, yz, zx
+ integral.setZero();
+
+ for (auto const& triangle : m_mesh->getTriangles())
+ {
+ if (! triangle.isValid)
+ {
+ continue;
+ }
+
+ const Vector3d& v0 = m_mesh->getVertexPosition(triangle.verticesId[0]);
+ const Vector3d& v1 = m_mesh->getVertexPosition(triangle.verticesId[1]);
+ const Vector3d& v2 = m_mesh->getVertexPosition(triangle.verticesId[2]);
+
+ // get edges and cross product of edges
+ Vector3d normal = (v1 - v0).cross(v2 - v0);
+
+ // compute integral terms
+ double f1x, f1y, f1z, f2x, f2y, f2z, f3x, f3y, f3z;
+ double g0x, g0y, g0z, g1x, g1y, g1z, g2x, g2y, g2z;
+ computeIntegralTerms(v0.x(), v1.x(), v2.x(), &f1x, &f2x, &f3x, &g0x, &g1x, &g2x);
+ computeIntegralTerms(v0.y(), v1.y(), v2.y(), &f1y, &f2y, &f3y, &g0y, &g1y, &g2y);
+ computeIntegralTerms(v0.z(), v1.z(), v2.z(), &f1z, &f2z, &f3z, &g0z, &g1z, &g2z);
+
+ // update integrals
+ integral[0] += normal[0] * f1x;
+ integral[1] += normal[0] * f2x;
+ integral[2] += normal[1] * f2y;
+ integral[3] += normal[2] * f2z;
+ integral[4] += normal[0] * f3x;
+ integral[5] += normal[1] * f3y;
+ integral[6] += normal[2] * f3z;
+ integral[7] += normal[0] * (v0.y() * g0x + v1.y() * g1x + v2.y() * g2x);
+ integral[8] += normal[1] * (v0.z() * g0y + v1.z() * g1y + v2.z() * g2y);
+ integral[9] += normal[2] * (v0.x() * g0z + v1.x() * g1z + v2.x() * g2z);
+ }
+ integral = integral.cwiseProduct(multiplier);
+
+ // volume
+ m_volume = integral[0];
+
+ // center of mass
+ if (m_volume != 0.0)
+ {
+ m_center = integral.segment(1, 3) / m_volume;
+ }
+ else
+ {
+ m_center.setZero();
+ }
+
+ // second moment of volume relative to center
+ Vector3d centerSquared = m_center.cwiseProduct(m_center);
+ m_secondMomentOfVolume(0, 0) = integral[5] + integral[6] - m_volume * (centerSquared.y() + centerSquared.z());
+ m_secondMomentOfVolume(1, 1) = integral[4] + integral[6] - m_volume * (centerSquared.z() + centerSquared.x());
+ m_secondMomentOfVolume(2, 2) = integral[4] + integral[5] - m_volume * (centerSquared.x() + centerSquared.y());
+ m_secondMomentOfVolume(0, 1) = -(integral[7] - m_volume * m_center.x() * m_center.y());
+ m_secondMomentOfVolume(1, 0) = m_secondMomentOfVolume(0, 1);
+ m_secondMomentOfVolume(1, 2) = -(integral[8] - m_volume * m_center.y() * m_center.z());
+ m_secondMomentOfVolume(2, 1) = m_secondMomentOfVolume(1, 2);
+ m_secondMomentOfVolume(0, 2) = -(integral[9] - m_volume * m_center.z() * m_center.x());
+ m_secondMomentOfVolume(2, 0) = m_secondMomentOfVolume(0, 2);
+}
+
+void MeshShape::setPose(const SurgSim::Math::RigidTransform3d& pose)
+{
+ m_mesh->copyWithTransform(pose, *m_initialMesh);
+ updateAabbTree();
+}
+
+std::shared_ptr<SurgSim::DataStructures::AabbTree> MeshShape::getAabbTree()
+{
+ return m_aabbTree;
+}
+
+void MeshShape::updateAabbTree()
+{
+ m_aabbTree = std::make_shared<SurgSim::DataStructures::AabbTree>();
+
+ auto const& triangles = m_mesh->getTriangles();
+ for (size_t id = 0; id < triangles.size(); ++id)
+ {
+ if (triangles[id].isValid)
+ {
+ auto vertices = m_mesh->getTrianglePositions(id);
+ m_aabbTree->add(SurgSim::Math::makeAabb(vertices[0], vertices[1], vertices[2]), id);
+ }
+ }
+}
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/MeshShape.h b/SurgSim/Math/MeshShape.h
new file mode 100644
index 0000000..70b7159
--- /dev/null
+++ b/SurgSim/Math/MeshShape.h
@@ -0,0 +1,142 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 code is based on David Eberly's paper:
+// http://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf
+// which is improving Brian Mirtich previous work (http://www.cs.berkeley.edu/~jfc/mirtich/massProps.html)
+// by making the assumption that the polyhedral mesh is composed of triangles.
+
+#ifndef SURGSIM_MATH_MESHSHAPE_H
+#define SURGSIM_MATH_MESHSHAPE_H
+
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_STATIC_REGISTRATION(MeshShape);
+
+
+/// Mesh shape: shape made of a triangle mesh
+/// The triangle mesh needs to be watertight to produce valid volume, center and second moment of
+/// volume. If it is not the case and you need valid geometric properties, use SurfaceMeshShape instead.
+/// Various geometrical properties (volume based) are computed from the triangle mesh using
+/// David Eberly's work:
+/// http://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf
+///
+/// \note The internal mesh should not be modified, otherwise the geometric properties will be invalid.
+/// \note Practical use cases:
+/// \note * Fixed/Rigid object, the mesh will not change anyway.
+/// \note * Deformable object, the mesh will be updated, but the geometric properties will not be used.
+///
+/// \sa SurfaceMeshShape
+class MeshShape : public Shape, public SurgSim::Framework::Asset
+{
+public:
+ /// Constructor
+ MeshShape();
+
+ /// Constructor
+ /// \param mesh The triangle mesh to build the shape from
+ /// \exception Raise an exception if the mesh is invalid
+ template <class VertexData, class EdgeData, class TriangleData>
+ explicit MeshShape(const SurgSim::DataStructures::TriangleMeshBase<VertexData, EdgeData, TriangleData>& mesh);
+
+ SURGSIM_CLASSNAME(SurgSim::Math::MeshShape);
+
+ /// \return the type of the shape
+ virtual int getType() override;
+
+ /// Gets the initial mesh
+ /// \return The collision mesh associated to this MeshShape
+ std::shared_ptr<SurgSim::DataStructures::TriangleMesh> getInitialMesh();
+
+ /// Get mesh
+ /// \return The collision mesh associated to this MeshShape
+ std::shared_ptr<SurgSim::DataStructures::TriangleMesh> getMesh();
+
+ /// Get the volume of the shape
+ /// \note this parameter is valid with respect to the initial mesh
+ /// \return The volume of the shape (in m-3)
+ virtual double getVolume() const override;
+
+ /// Get the volumetric center of the shape
+ /// \note this parameter is valid with respect to the initial mesh
+ /// \return The center of the shape
+ virtual Vector3d getCenter() const override;
+
+ /// Get the second central moment of the volume, commonly used
+ /// to calculate the moment of inertia matrix
+ /// \note this parameter is valid with respect to the initial mesh
+ /// \return The 3x3 symmetric second moment matrix
+ virtual Matrix33d getSecondMomentOfVolume() const override;
+
+ /// Set the object's global pose
+ /// \param pose the rigid transform to apply
+ void setPose(const SurgSim::Math::RigidTransform3d &pose);
+
+ /// Update the AabbTree, which is an axis-aligned bounding box r-tree used to accelerate spatial searches
+ void updateAabbTree();
+
+ /// Get the AabbTree
+ /// \return The object's associated AabbTree
+ std::shared_ptr<SurgSim::DataStructures::AabbTree> getAabbTree();
+
+ /// Check if this shape contains a valid mesh.
+ /// Equals 'MeshShape::getMesh() != nullptr && MeshShape::getMesh()->isValid()'
+ /// \return true if this shape contains a valid mesh; otherwise, false.
+ bool isValid() const;
+
+protected:
+ virtual bool doLoad(const std::string& filePath) override;
+
+private:
+ /// Compute useful volume integrals based on the triangle mesh, which
+ /// are used to get the volume , center and second moment of volume.
+ void computeVolumeIntegrals();
+
+ /// Center (considering a uniform distribution in the mesh volume)
+ SurgSim::Math::Vector3d m_center;
+
+ /// Volume (in m^-3)
+ double m_volume;
+
+ /// Second moment of volume
+ SurgSim::Math::Matrix33d m_secondMomentOfVolume;
+
+ /// The triangle mesh contained by this shape.
+ std::shared_ptr<SurgSim::DataStructures::TriangleMesh> m_mesh;
+
+ /// The initial triangle mesh contained by this shape.
+ std::shared_ptr<SurgSim::DataStructures::TriangleMesh> m_initialMesh;
+
+ /// The aabb tree used to accelerate collision detection against the mesh
+ std::shared_ptr<SurgSim::DataStructures::AabbTree> m_aabbTree;
+};
+
+}; // Math
+}; // SurgSim
+
+#include "SurgSim/Math/MeshShape-inl.h"
+
+#endif // SURGSIM_MATH_MESHSHAPE_H
diff --git a/SurgSim/Math/MlcpConstraintType.h b/SurgSim/Math/MlcpConstraintType.h
new file mode 100644
index 0000000..c3a508a
--- /dev/null
+++ b/SurgSim/Math/MlcpConstraintType.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_MLCPCONSTRAINTTYPE_H
+#define SURGSIM_MATH_MLCPCONSTRAINTTYPE_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+enum MlcpConstraintType
+{
+ // Invalid value -- not used for any constraint type
+ MLCP_INVALID_CONSTRAINT = -1,
+ // Fixing 1 DOF
+ MLCP_BILATERAL_1D_CONSTRAINT = 0,
+ // Fixing 2 DOF
+ MLCP_BILATERAL_2D_CONSTRAINT,
+ // Fixing 3 DOF (could be a fixed point in 3D space for example)
+ MLCP_BILATERAL_3D_CONSTRAINT,
+ // Fixing 4 DOF (could be a fixed point with twist included for the MechanicalSpline for example)
+ // MLCP_BILATERAL_4D_CONSTRAINT,
+ // Frictionless contact (could be in 2D or 3D => only 1 atomic unilateral constraint)
+ MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT,
+ // Frictional contact in 3D, using Coulomb friction => 1 entry in the friction coefficient array !
+ MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT,
+ // Frictionless suturing in 3D => 2 directional constraint = 2 bilateral constraints !
+ MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT,
+ // Frictional suturing in 3D => 2 directional constraint + 1 frictional tangent constraint linked by Coulomb's law
+ MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT,
+ MLCP_NUM_CONSTRAINT_TYPES
+};
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_MLCPCONSTRAINTTYPE_H
diff --git a/SurgSim/Math/MlcpConstraintTypeName.h b/SurgSim/Math/MlcpConstraintTypeName.h
new file mode 100644
index 0000000..b04e506
--- /dev/null
+++ b/SurgSim/Math/MlcpConstraintTypeName.h
@@ -0,0 +1,100 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_MLCPCONSTRAINTTYPENAME_H
+#define SURGSIM_MATH_MLCPCONSTRAINTTYPENAME_H
+
+#include <string>
+#include "SurgSim/Math/MlcpConstraintType.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+inline std::string getMlcpConstraintTypeName(MlcpConstraintType constraintType)
+{
+ switch (constraintType)
+ {
+ case MLCP_BILATERAL_1D_CONSTRAINT:
+ return "MLCP_BILATERAL_1D_CONSTRAINT";
+ case MLCP_BILATERAL_2D_CONSTRAINT:
+ return "MLCP_BILATERAL_2D_CONSTRAINT";
+ case MLCP_BILATERAL_3D_CONSTRAINT:
+ return "MLCP_BILATERAL_3D_CONSTRAINT";
+// case MLCP_BILATERAL_4D_CONSTRAINT:
+// return "MLCP_BILATERAL_4D_CONSTRAINT";
+ case MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT:
+ return "MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT";
+ case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT:
+ return "MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT";
+ case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
+ return "MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT";
+ case MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT:
+ return "MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT";
+ default:
+ SURGSIM_LOG_SEVERE(SURGSIM_ASSERT_LOGGER) << "bad MLCP constraint type value: " << constraintType;
+ return "";
+ }
+}
+
+inline MlcpConstraintType getMlcpConstraintTypeValue(const std::string& typeName)
+{
+
+ if (typeName == "MLCP_BILATERAL_1D_CONSTRAINT")
+ {
+ return MLCP_BILATERAL_1D_CONSTRAINT;
+ }
+ else if (typeName == "MLCP_BILATERAL_2D_CONSTRAINT")
+ {
+ return MLCP_BILATERAL_2D_CONSTRAINT;
+ }
+ else if (typeName == "MLCP_BILATERAL_3D_CONSTRAINT")
+ {
+ return MLCP_BILATERAL_3D_CONSTRAINT;
+ }
+// else if (typeName == "MLCP_BILATERAL_4D_CONSTRAINT")
+// {
+// return MLCP_BILATERAL_4D_CONSTRAINT;
+// }
+ else if (typeName == "MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT")
+ {
+ return MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
+ }
+ else if (typeName == "MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT")
+ {
+ return MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT;
+ }
+ else if (typeName == "MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT")
+ {
+ return MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT;
+ }
+ else if (typeName == "MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT")
+ {
+ return MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(SURGSIM_ASSERT_LOGGER) << "bad MLCP constraint type name: '" << typeName << "'";
+ return MLCP_INVALID_CONSTRAINT;
+ }
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_MLCPCONSTRAINTTYPENAME_H
diff --git a/SurgSim/Math/MlcpGaussSeidelSolver.cpp b/SurgSim/Math/MlcpGaussSeidelSolver.cpp
new file mode 100644
index 0000000..efaec33
--- /dev/null
+++ b/SurgSim/Math/MlcpGaussSeidelSolver.cpp
@@ -0,0 +1,1765 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/MlcpGaussSeidelSolver.h"
+
+#include <math.h>
+#include <stdio.h>
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+#include <Eigen/Dense>
+
+#include "SurgSim/Math/Valid.h"
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+namespace Math
+{
+
+// cf. Christian Duriez TVCG05 paper
+// Realistic Haptic Rendering of Interacting
+// Deformable Objects in Virtual Environments
+// Christian Duriez, Student Member, IEEE, Frederic Dubois,
+// Abderrahmane Kheddar, Member, IEEE, and Claude Andriot
+// +
+// recent work from Christian
+
+//! MLCP_GaussSeidel::solve Resolution of a given MLCP (Gauss Seidel iterative solver)
+/*!
+ at param problem The mlcp problem
+ at param solution The mlcp solution
+*/
+bool MlcpGaussSeidelSolver::solve(const MlcpProblem& problem, MlcpSolution* solution)
+{
+ // TODO(wschoen) 2014-06-04: Modify to word-sized types
+ int n = static_cast<int>(problem.getSize());
+ const MlcpProblem::Matrix& A = problem.A;
+ const int nbColumnInA = static_cast<int>(A.cols());
+ const MlcpProblem::Vector& b = problem.b;
+ MlcpSolution::Vector& initialGuess_and_solution = solution->x;
+ const MlcpProblem::Vector& frictionCoefs = problem.mu;
+ const std::vector<MlcpConstraintType>& constraintsType = problem.constraintTypes;
+ double subStep = 1.0;//XXX
+ int* MLCP_nbIterations = &solution->numIterations;
+ bool* validConvergence = &solution->validConvergence;
+ bool* validSignorini = &solution->validSignorini;
+ double* convergenceCriteria = &solution->convergenceCriteria;
+ double* initialConvergenceCriteria = &solution->initialConvergenceCriteria;
+ double* constraintConvergenceCriteria = solution->constraintConvergenceCriteria;
+ double* initialConstraintConvergenceCriteria = solution->initialConstraintConvergenceCriteria;
+ bool catchExplodingConvergenceCriteria = true;
+ bool verbose = true;
+
+ //int nbConstraints = static_cast<int>(constraintsType.size());
+ //cout << "======== MLCP ========" << endl;
+ //cout << "\tMLCP: nbAtomicEntry (nb Line In Matrix)="<<n<<" - nbConstraints=" << nbConstraints << endl;
+
+ // Loop until it converges or maxIterations are reached
+ unsigned int nbLoop=0;
+
+ double convergence_criteria;
+ double constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES];
+ bool signorini_verified;
+ bool signorini_valid;
+
+ double initial_convergence_criteria = 0.0;
+ double initial_constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES];
+ bool initialSignoriniVerified = true;
+ bool initialSignoriniValid = true;
+
+ calculateConvergenceCriteria(n, A, nbColumnInA, b,
+ initialGuess_and_solution, constraintsType, subStep,
+ initial_constraint_convergence_criteria, initial_convergence_criteria,
+ initialSignoriniVerified, initialSignoriniValid);
+
+ // If it is already converged, fill the output and return true.
+ if (initial_convergence_criteria <= m_epsilonConvergence && initialSignoriniVerified)
+ {
+ if (validSignorini)
+ {
+ *validSignorini = initialSignoriniVerified;
+ }
+ if (validConvergence)
+ {
+ *validConvergence = true;
+ }
+ if (initialConvergenceCriteria)
+ {
+ *initialConvergenceCriteria = initial_convergence_criteria;
+ }
+ if (convergenceCriteria)
+ {
+ *convergenceCriteria = initial_convergence_criteria;
+ }
+ if (MLCP_nbIterations)
+ {
+ *MLCP_nbIterations = 0;
+ }
+
+ if (initialConstraintConvergenceCriteria)
+ for (int i = 0; i < MLCP_NUM_CONSTRAINT_TYPES; i++)
+ {
+ initialConstraintConvergenceCriteria[i] = initial_constraint_convergence_criteria[i];
+ }
+
+ if (constraintConvergenceCriteria)
+ for (int i = 0; i < MLCP_NUM_CONSTRAINT_TYPES; i++)
+ {
+ initialConstraintConvergenceCriteria[i] = initial_constraint_convergence_criteria[i];
+ }
+
+ return true;
+ }
+
+ do
+ {
+ doOneIteration(n, A, nbColumnInA, b, &initialGuess_and_solution, frictionCoefs,
+ constraintsType, subStep, constraint_convergence_criteria, convergence_criteria,
+ signorini_verified);
+
+ calculateConvergenceCriteria(n, A, nbColumnInA, b,
+ initialGuess_and_solution, constraintsType, subStep,
+ constraint_convergence_criteria, convergence_criteria,
+ signorini_verified, signorini_valid);
+
+// printViolationsAndConvergence(n, A, nbColumnInA, b, initialGuess_and_solution, constraintsType, subStep,
+// convergence_criteria, signorini_verified, nbLoop);
+
+ nbLoop++;
+
+ if (catchExplodingConvergenceCriteria)
+ {
+ // If we have an incredibly high convergence criteria value, the displacements are going to be very large,
+ // causing problems in the next iteration, so we should break out here. The convergence_criteria should
+ // really only be a couple order of magnitudes higher than epsilon.
+ if (!SurgSim::Math::isValid(convergence_criteria) || convergence_criteria > 1.0)
+ {
+ printf("Convergence (%e) is NaN, infinite, or greater than 1.0! MLCP is exploding"
+ " after %d Gauss Seidel iterations!!\n", convergence_criteria, nbLoop);
+ break;
+ }
+ }
+
+ //printf(" Solver [loop %2d] convergence_criteria=%g<?%g\n",nbLoop,convergence_criteria,m_epsilonConvergence);
+
+ }
+ while ((!signorini_verified ||
+ (SurgSim::Math::isValid(convergence_criteria) && convergence_criteria>m_epsilonConvergence)) &&
+ nbLoop<m_maxIterations);
+
+ if (MLCP_nbIterations)
+ {
+ *MLCP_nbIterations=nbLoop;
+ }
+
+ if (validConvergence)
+ {
+ *validConvergence = true;
+
+ if (!SurgSim::Math::isValid(convergence_criteria) || convergence_criteria > 1.0)
+ {
+ *validConvergence = false;
+ }
+
+ if (!(convergence_criteria < sqrt(m_epsilonConvergence)))
+ {
+ if (verbose)
+ {
+ printf("Convergence criteria (%e) is greater than %e at end of %d Gauss Seidel iterations.\n",
+ convergence_criteria, sqrt(m_epsilonConvergence), nbLoop);
+ }
+ //*validConvergence = false;
+ }
+
+ if (!(convergence_criteria <= initial_convergence_criteria))
+ {
+ if (verbose)
+ {
+ printf("Convergence criteria (%e) is greater than before %d Gauss Seidel iterations (%e).\n",
+ convergence_criteria, nbLoop, initial_convergence_criteria);
+ }
+ // *validConvergence = false; // This is a bit strict but it is useful to know when diverging.
+ }
+ }
+
+ if (validSignorini)
+ {
+ *validSignorini = true;
+
+ if (!signorini_verified)
+ {
+ if (verbose)
+ {
+ printf("Signorini not verified after %d Gauss Seidel iterations.\n", nbLoop);
+ }
+ *validSignorini = false;
+ }
+ }
+
+ if (convergenceCriteria)
+ {
+ *convergenceCriteria = convergence_criteria;
+ }
+ if (initialConvergenceCriteria)
+ {
+ *initialConvergenceCriteria = initial_convergence_criteria;
+ }
+
+ if (convergence_criteria > m_epsilonConvergence || !SurgSim::Math::isValid(convergence_criteria) ||
+ !signorini_valid)
+ {
+//#ifdef PRINTOUT_TEST_APP
+// cout << "\tLCP_3DContactFriction_GaussSeidel_Christian::solve did <<<NOT>>> converge after " <<
+// nbLoop << " iterations" << endl;
+// cout << "\t SignoriniVerified = "<< SignoriniVerified <<" convergence_criteria = " <<
+// convergence_criteria << endl;
+//
+// printViolationsAndConvergence(n, A, nbColumnInA, b, initialGuess_and_solution, constraintsType, subStep,
+// convergence_criteria, signorini_verified);
+//#endif // PRINTOUT_TEST_APP
+
+ return false;
+ }
+#ifdef PRINTOUT_TEST_APP
+ else
+ {
+ std::cout << "LCP_3DContactFriction_GaussSeidel_Christian::solve converged after " <<
+ nbLoop << " iterations" << std::endl;
+ }
+#endif // PRINTOUT_TEST_APP
+
+ return true;
+}
+
+
+void MlcpGaussSeidelSolver::calculateConvergenceCriteria(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
+ const MlcpProblem::Vector& b,
+ const MlcpSolution::Vector& initialGuess_and_solution,
+ const std::vector<MlcpConstraintType>& constraintsType,
+ double subStep,
+ double constraint_convergence_criteria[],
+ double& convergence_criteria,
+ bool& signoriniVerified, bool& signoriniValid)
+{
+ // Calculate initial convergence criteria.
+
+ for (int i = 0; i < MLCP_NUM_CONSTRAINT_TYPES; i++)
+ {
+ constraint_convergence_criteria[i] = 0.0;
+ }
+ convergence_criteria = 0.0;
+ signoriniVerified = true;
+ signoriniValid = true;
+
+ int currentAtomicIndex=0;
+ int nbConstraints = static_cast<int>(constraintsType.size());
+
+ int nbNonContactConstraints = 0;
+
+ for (int i=0 ; i<nbConstraints ; i++)
+ {
+ switch (constraintsType[i])
+ {
+ case MLCP_BILATERAL_1D_CONSTRAINT:
+ {
+ double violation = b[currentAtomicIndex]*subStep;
+ //XXX REWRITE
+ for (int j=0 ; j<n ; j++)
+ {
+ violation += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ }
+ double criteria = sqrt(violation * violation);
+ convergence_criteria += criteria;
+ constraint_convergence_criteria[constraintsType[i]] += criteria;
+
+ nbNonContactConstraints++;
+ }
+ currentAtomicIndex+=1;
+ break;
+
+ case MLCP_BILATERAL_2D_CONSTRAINT:
+ {
+ // XXX REWRITE
+ double violation[2] = { b[currentAtomicIndex]* subStep , b[currentAtomicIndex+1]* subStep };
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ violation[1] += A(currentAtomicIndex+1, j) * initialGuess_and_solution[j];
+ }
+ double criteria = sqrt(violation[0]*violation[0] + violation[1]*violation[1]);
+ convergence_criteria += criteria;
+ constraint_convergence_criteria[constraintsType[i]] += criteria;
+
+ nbNonContactConstraints++;
+ }
+ currentAtomicIndex+=2;
+ break;
+
+ case MLCP_BILATERAL_3D_CONSTRAINT:
+ {
+ double violation[3] =
+ {
+ b[currentAtomicIndex]* subStep , b[currentAtomicIndex+1]* subStep , b[currentAtomicIndex+2]* subStep
+ };
+ // XXX REWRITE
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ violation[1] += A(currentAtomicIndex+1, j) * initialGuess_and_solution[j];
+ violation[2] += A(currentAtomicIndex+2, j) * initialGuess_and_solution[j];
+ }
+ double criteria = sqrt(violation[0]*violation[0] + violation[1]*violation[1] + violation[2]*violation[2]);
+ convergence_criteria += criteria;
+ constraint_convergence_criteria[constraintsType[i]] += criteria;
+
+ nbNonContactConstraints++;
+ }
+ currentAtomicIndex+=3;
+ break;
+
+ //case MLCP_BILATERAL_4D_CONSTRAINT:
+ //...
+
+ case MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT:
+ {
+ double violation = b[currentAtomicIndex]*subStep;
+ for (int j=0 ; j<n ; j++)
+ {
+ violation += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ }
+ // Enforce orthogonality condition
+ if (! SurgSim::Math::isValid(violation) || violation < -m_contactTolerance ||
+ (initialGuess_and_solution[currentAtomicIndex] > m_epsilonConvergence &&
+ violation > m_contactTolerance))
+ {
+ signoriniVerified=false;
+ }
+ }
+ currentAtomicIndex++;
+ break;
+
+ case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT:
+ {
+ double violation = b[currentAtomicIndex]*subStep;
+ for (int j=0 ; j<n ; j++)
+ {
+ violation += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ }
+ // Enforce orthogonality condition
+ if (! SurgSim::Math::isValid(violation) || violation < -m_contactTolerance ||
+ (initialGuess_and_solution[currentAtomicIndex] > m_epsilonConvergence &&
+ violation > m_contactTolerance))
+ {
+ signoriniVerified=false;
+ }
+ }
+ currentAtomicIndex+=3;
+ break;
+
+ case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
+ {
+ double violation[2] = { b[currentAtomicIndex] , b[currentAtomicIndex+1] };
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ violation[1] += A(currentAtomicIndex+1, j) * initialGuess_and_solution[j];
+ }
+ double criteria = sqrt(violation[0]*violation[0] + violation[1]*violation[1]);
+ convergence_criteria += criteria;
+ constraint_convergence_criteria[constraintsType[i]] += criteria;
+
+ nbNonContactConstraints++;
+ }
+ currentAtomicIndex+=2;
+ break;
+
+ case MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT:
+ {
+ // We verify that the sliding point is on the line...no matter what the friction violation is
+ // (3rd component)
+ double violation[2] = { b[currentAtomicIndex] , b[currentAtomicIndex+1] };
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ violation[1] += A(currentAtomicIndex+1, j) * initialGuess_and_solution[j];
+ }
+ double criteria = sqrt(violation[0]*violation[0] + violation[1]*violation[1]);
+ convergence_criteria += criteria;
+ constraint_convergence_criteria[constraintsType[i]] += criteria;
+
+ nbNonContactConstraints++;
+ }
+ currentAtomicIndex+=3;
+ break;
+
+ default:
+ //XXX
+ SURGSIM_FAILURE() << "unknown constraint type [" << constraintsType[i] << "]";
+ break;
+ }
+
+ }
+
+ if (nbNonContactConstraints)
+ {
+ convergence_criteria /= nbNonContactConstraints; // normalize if necessary
+ }
+}
+
+void MlcpGaussSeidelSolver::computeEnforcementSystem(
+ int n, const MlcpProblem::Matrix& A, int nbColumnInA, const MlcpProblem::Vector& b,
+ const MlcpSolution::Vector& initialGuess_and_solution,
+ const MlcpProblem::Vector& frictionCoefs,
+ const std::vector<MlcpConstraintType>& constraintsType, double subStep,
+ int constraintID, int matrixEntryForConstraintID)
+{
+ int nbConstraints = static_cast<int>(constraintsType.size());
+ int systemSize=0; // Total size of the system (number of line and column in the final matrix)
+ int systemSizeWithoutConstraintID=0; // Total size of the system counting only the {1D,2D,3D} bilateral constraints
+
+ // 1st) compute the size of the final system
+ // We suppose that the constraint to enforce are only 1D, 2D or 3D bilateral constraints
+ // and all appearing first in the list !
+ {
+ bool done=false;
+ for (int i=0 ; i<nbConstraints ; i++)
+ {
+ switch (constraintsType[i])
+ {
+ case MLCP_BILATERAL_1D_CONSTRAINT:
+ systemSize+=1;
+ systemSizeWithoutConstraintID+=1;
+ break;
+ case MLCP_BILATERAL_2D_CONSTRAINT:
+ systemSize+=2;
+ systemSizeWithoutConstraintID+=2;
+ break;
+ case MLCP_BILATERAL_3D_CONSTRAINT:
+ systemSize+=3;
+ systemSizeWithoutConstraintID+=3;
+ break;
+ default:
+ done=true;
+ break;
+ }
+ if (done)
+ {
+ break;
+ }
+ }
+
+ // We added all the constraints, now we need to add the constraintID (contact, sliding,...) to have the total
+ // system size!
+ switch (constraintsType[constraintID])
+ {
+ case MLCP_BILATERAL_1D_CONSTRAINT :
+ systemSize+=1;
+ break; // That should not be the case...
+ case MLCP_BILATERAL_2D_CONSTRAINT :
+ systemSize+=2;
+ break; // That should not be the case...
+ case MLCP_BILATERAL_3D_CONSTRAINT :
+ systemSize+=3;
+ break; // That should not be the case...
+ case MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT :
+ systemSize+=1;
+ break;
+ case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT :
+ systemSize+=1;
+ break; // The system will solve the normal contact, not the frictional parts !
+ case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
+ systemSize+=2;
+ break;
+ case MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT :
+ systemSize+=2;
+ break; // The system will solve the sliding case, not the frictional part !
+ default:
+ printf("MlcpGaussSeidelSolver::computeEnforcementSystem Unkown constraint !?\n");
+ break;
+ }
+ }
+ m_numEnforcedAtomicConstraints = systemSize;
+ m_rhsEnforcedLocalSystem.resize(systemSize);
+ m_lhsEnforcedLocalSystem.resize(systemSize, systemSize);
+
+ // 2nd) Fill up the system
+ // We suppose that the constraint to enforce are only 1D, 2D or 3D bilateral constraints
+ {
+ // Here we fill up the core part, compliance between all the constraints themselves !
+ for (int line=0 ; line<systemSizeWithoutConstraintID ; line++)
+ {
+ // At the same time, we compute the violation for the constraints
+ m_rhsEnforcedLocalSystem[line] = b[line] * subStep;
+ for (int column=0 ; column<systemSizeWithoutConstraintID ; column++)
+ {
+ m_lhsEnforcedLocalSystem(line, column) = A(line, column);
+ m_rhsEnforcedLocalSystem[line] += A(line, column)*initialGuess_and_solution[column];
+ }
+ // Now we complete the violation[line] computation by taking into account the effect of all remaining
+ // contacts/slidings/constraints
+ for (int column=systemSizeWithoutConstraintID; column<n ; column++)
+ {
+ m_rhsEnforcedLocalSystem[line] += A(line, column)*initialGuess_and_solution[column];
+ }
+ }
+
+ // Now we complete the contact matrix by adding the coupling constraint/{contact|sliding} and the compliance
+ // for {contact|sliding}
+ switch (constraintsType[constraintID])
+ {
+ case MLCP_BILATERAL_1D_CONSTRAINT:
+ {
+ // Coupling part (fill up LHS and RHS)
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID] = b[matrixEntryForConstraintID] * subStep;
+ for (int line=0 ; line<systemSizeWithoutConstraintID ; line++)
+ {
+ m_lhsEnforcedLocalSystem(line, systemSizeWithoutConstraintID) = A(line, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, line) = A(matrixEntryForConstraintID, line);
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID] +=
+ A(matrixEntryForConstraintID, line) * initialGuess_and_solution[line];
+ }
+ // Compliance part for the {contact|sliding}
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID) =
+ A(matrixEntryForConstraintID, matrixEntryForConstraintID);
+ //...and complete the violation
+ for (int column=systemSizeWithoutConstraintID; column<n ; column++)
+ {
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID] +=
+ A(matrixEntryForConstraintID, column)*initialGuess_and_solution[column];
+ }
+ }
+ break; // That should not be the case...
+
+ case MLCP_BILATERAL_2D_CONSTRAINT:
+ {
+ // Coupling part (fill up LHS and RHS)
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID ] = b[matrixEntryForConstraintID ] * subStep;
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+1] = b[matrixEntryForConstraintID+1] * subStep;
+ for (int line=0 ; line<systemSizeWithoutConstraintID ; line++)
+ {
+ m_lhsEnforcedLocalSystem(line, systemSizeWithoutConstraintID) = A(line, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(line, systemSizeWithoutConstraintID+1) = A(line, matrixEntryForConstraintID+1);
+
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, line) =
+ A(matrixEntryForConstraintID, line);
+ m_lhsEnforcedLocalSystem((systemSizeWithoutConstraintID+1), line) =
+ A(matrixEntryForConstraintID+1, line);
+
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID ] +=
+ A(matrixEntryForConstraintID, line) * initialGuess_and_solution[line];
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+1] +=
+ A(matrixEntryForConstraintID+1, line) * initialGuess_and_solution[line];
+ }
+ // Compliance part for the {contact|sliding}
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID) =
+ A(matrixEntryForConstraintID, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID+1) =
+ A(matrixEntryForConstraintID, matrixEntryForConstraintID+1);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+1, systemSizeWithoutConstraintID) =
+ A(matrixEntryForConstraintID+1, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+1, systemSizeWithoutConstraintID+1) =
+ A(matrixEntryForConstraintID+1, matrixEntryForConstraintID+1);
+ //...and complete the violation
+ for (int column=systemSizeWithoutConstraintID; column<n ; column++)
+ {
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID ] +=
+ A(matrixEntryForConstraintID, column)*initialGuess_and_solution[column];
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+1] +=
+ A(matrixEntryForConstraintID+1, column)*initialGuess_and_solution[column];
+ }
+ }
+ break; // That should not be the case...
+
+ case MLCP_BILATERAL_3D_CONSTRAINT:
+ {
+ // Coupling part (fill up LHS and RHS)
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID ] = b[matrixEntryForConstraintID ] * subStep;
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+1] = b[matrixEntryForConstraintID+1] * subStep;
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+2] = b[matrixEntryForConstraintID+2] * subStep;
+ for (int line=0 ; line<systemSizeWithoutConstraintID ; line++)
+ {
+ m_lhsEnforcedLocalSystem(line, systemSizeWithoutConstraintID) = A(line, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(line, systemSizeWithoutConstraintID+1) = A(line, matrixEntryForConstraintID+1);
+ m_lhsEnforcedLocalSystem(line, systemSizeWithoutConstraintID+2) = A(line, matrixEntryForConstraintID+2);
+
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, line) = A(matrixEntryForConstraintID, line);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+1, line) = A(matrixEntryForConstraintID+1, line);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+2, line) = A(matrixEntryForConstraintID+2, line);
+
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID ] +=
+ A(matrixEntryForConstraintID, line) * initialGuess_and_solution[line];
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+1] +=
+ A(matrixEntryForConstraintID+1, line) * initialGuess_and_solution[line];
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+2] +=
+ A(matrixEntryForConstraintID+2, line) * initialGuess_and_solution[line];
+ }
+ // Compliance part for the {contact|sliding}
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID) =
+ A(matrixEntryForConstraintID, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID+1) =
+ A(matrixEntryForConstraintID, matrixEntryForConstraintID+1);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID+2) =
+ A(matrixEntryForConstraintID, matrixEntryForConstraintID+2);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+1, systemSizeWithoutConstraintID) =
+ A(matrixEntryForConstraintID+1, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+1, systemSizeWithoutConstraintID+1) =
+ A(matrixEntryForConstraintID+1, matrixEntryForConstraintID+1);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+1, systemSizeWithoutConstraintID+2) =
+ A(matrixEntryForConstraintID+1, matrixEntryForConstraintID+2);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+2, systemSizeWithoutConstraintID) =
+ A(matrixEntryForConstraintID+2, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+2, systemSizeWithoutConstraintID+1) =
+ A(matrixEntryForConstraintID+2, matrixEntryForConstraintID+1);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+2, systemSizeWithoutConstraintID+2) =
+ A(matrixEntryForConstraintID+2, matrixEntryForConstraintID+2);
+ //...and complete the violation
+ for (int column=systemSizeWithoutConstraintID; column<n ; column++)
+ {
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID ] +=
+ A(matrixEntryForConstraintID, column)*initialGuess_and_solution[column];
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+1] +=
+ A(matrixEntryForConstraintID+1, column)*initialGuess_and_solution[column];
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+2] +=
+ A(matrixEntryForConstraintID+2, column)*initialGuess_and_solution[column];
+ }
+ }
+ break; // That should not be the case...
+
+ // In any case of contact, we only register the normal part...the friction part is computed afterward !
+ case MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT:
+ case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT:
+ {
+ // Coupling part (fill up LHS and RHS)
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID] = b[matrixEntryForConstraintID] * subStep;
+ for (int line=0 ; line<systemSizeWithoutConstraintID ; line++)
+ {
+ m_lhsEnforcedLocalSystem(line, systemSizeWithoutConstraintID) = A(line, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, line) = A(matrixEntryForConstraintID, line);
+
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID] +=
+ A(matrixEntryForConstraintID, line) * initialGuess_and_solution[line];
+ }
+
+ // Compliance part for the {contact|sliding}
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID) =
+ A(matrixEntryForConstraintID, matrixEntryForConstraintID);
+ //...and complete the violation for the normal contact constraint
+ for (int column=systemSizeWithoutConstraintID; column<n ; column++)
+ {
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID] +=
+ A(matrixEntryForConstraintID, column)*initialGuess_and_solution[column];
+ }
+ }
+ break;
+
+ // In any case of sliding, we only register the normals part...the friction part along the tangent is computed
+ // afterward !
+ case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
+ case MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT:
+ {
+ // Coupling part (fill up LHS and RHS)
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID ] = b[matrixEntryForConstraintID ] * subStep;
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+1] = b[matrixEntryForConstraintID+1] * subStep;
+ for (int line=0 ; line<systemSizeWithoutConstraintID ; line++)
+ {
+ m_lhsEnforcedLocalSystem(line, systemSizeWithoutConstraintID) = A(line, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(line, systemSizeWithoutConstraintID+1) = A(line, matrixEntryForConstraintID+1);
+
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, line) = A(matrixEntryForConstraintID, line);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+1, line) = A(matrixEntryForConstraintID+1, line);
+
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID ] +=
+ A(matrixEntryForConstraintID, line) * initialGuess_and_solution[line];
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+1] +=
+ A(matrixEntryForConstraintID+1, line) * initialGuess_and_solution[line];
+ }
+
+ // Compliance part for the {contact|sliding}
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID) =
+ A(matrixEntryForConstraintID, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID+1) =
+ A(matrixEntryForConstraintID, matrixEntryForConstraintID+1);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+1, systemSizeWithoutConstraintID) =
+ A(matrixEntryForConstraintID+1, matrixEntryForConstraintID);
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID+1, systemSizeWithoutConstraintID+1) =
+ A(matrixEntryForConstraintID+1, matrixEntryForConstraintID+1);
+ //...and complete the violation for the normal contact constraints
+ for (int column=systemSizeWithoutConstraintID; column<n ; column++)
+ {
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID ] +=
+ A(matrixEntryForConstraintID, column)*initialGuess_and_solution[column];
+ m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+1] +=
+ A(matrixEntryForConstraintID+1, column)*initialGuess_and_solution[column];
+ }
+
+ }
+ break;
+
+ default:
+ printf("MlcpGaussSeidelSolver::computeEnforcementSystem Unkown constraint !?\n");
+ break;
+ }
+ }
+
+}
+
+// Solve the system A x = b for x, with the assumption that the size is "size"
+static inline bool solveSystem(const MlcpProblem::Matrix& A, const MlcpProblem::Vector& b, int size,
+ MlcpSolution::Vector* x)
+{
+ MlcpProblem::Matrix AA = A.block(0, 0, size, size);
+ MlcpProblem::Vector bb = b.head(size);
+
+ MlcpSolution::Vector solution = AA.partialPivLu().solve(bb);
+ //MlcpSolution::Vector solution = AA.colPivHouseholderQr().solve(bb);
+ //MlcpSolution::Vector solution = AA.householderQr().solve(bb);
+ *x = solution;
+ return true;
+}
+
+void MlcpGaussSeidelSolver::doOneIteration(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
+ const MlcpProblem::Vector& b,
+ MlcpSolution::Vector* initialGuess_and_solution,
+ const MlcpProblem::Vector& frictionCoefs,
+ const std::vector<MlcpConstraintType>& constraintsType, double subStep,
+ double constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES],
+ double& convergence_criteria, bool& signoriniVerified)
+{
+ for (int i = 0; i < MLCP_NUM_CONSTRAINT_TYPES; i++)
+ {
+ constraint_convergence_criteria[i] = 0.0;
+ }
+ convergence_criteria = 0.0;
+ signoriniVerified=true;
+
+ int currentAtomicIndex=0;
+ int nbConstraints = static_cast<int>(constraintsType.size());
+
+ // For each constraint, we look if the constraint is violated or not !
+ for (int i=0 ; i<nbConstraints ; i++)
+ {
+ switch (constraintsType[i])
+ {
+
+ //####################################
+ //####################################
+ case MLCP_BILATERAL_1D_CONSTRAINT:
+ {
+ double& F = (*initialGuess_and_solution)[currentAtomicIndex];
+ double violation = b[currentAtomicIndex]*subStep;
+ for (int j=0 ; j<n ; j++)
+ {
+ violation += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+ }
+ F -= violation / A(currentAtomicIndex, currentAtomicIndex);
+ }
+ currentAtomicIndex++;
+ break;
+
+
+ //####################################
+ //####################################
+ case MLCP_BILATERAL_2D_CONSTRAINT:
+ {
+ double& F1 = (*initialGuess_and_solution)[currentAtomicIndex ];
+ double& F2 = (*initialGuess_and_solution)[currentAtomicIndex+1];
+ double violation[2] = { b[currentAtomicIndex]* subStep , b[currentAtomicIndex+1]* subStep };
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+ violation[1] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+ }
+ // det = ad-bc
+ // [ a b ] [ d -b ] [ 1 0 ]
+ // [ c d ] . [ -c a ]/det = [ 0 1 ]
+ double A_determinant =
+ A(currentAtomicIndex, currentAtomicIndex)*A(currentAtomicIndex+1, currentAtomicIndex+1) -
+ A(currentAtomicIndex, currentAtomicIndex+1)*A(currentAtomicIndex+1, currentAtomicIndex);
+ double Ainv[2][2] =
+ {
+ {
+ A(currentAtomicIndex+1, currentAtomicIndex+1)/A_determinant,
+ -A(currentAtomicIndex, currentAtomicIndex+1)/A_determinant
+ },
+ {
+ -A(currentAtomicIndex+1, currentAtomicIndex)/A_determinant,
+ A(currentAtomicIndex, currentAtomicIndex)/A_determinant
+ }
+ };
+ F1 -= (Ainv[0][0]*violation[0] + Ainv[0][1]*violation[1]);
+ F2 -= (Ainv[1][0]*violation[0] + Ainv[1][1]*violation[1]);
+ }
+ currentAtomicIndex+=2;
+ break;
+
+
+ //####################################
+ //####################################
+ case MLCP_BILATERAL_3D_CONSTRAINT:
+ {
+ double& F1 = (*initialGuess_and_solution)[currentAtomicIndex ];
+ double& F2 = (*initialGuess_and_solution)[currentAtomicIndex+1];
+ double& F3 = (*initialGuess_and_solution)[currentAtomicIndex+2];
+ double violation[3] = { b[currentAtomicIndex]* subStep , b[currentAtomicIndex+1]* subStep ,
+ b[currentAtomicIndex+2]* subStep
+ };
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+ violation[1] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+ violation[2] += A(currentAtomicIndex+2, j) * (*initialGuess_and_solution)[j];
+ }
+ Eigen::Matrix3d Ainv = A.block<3,3>(currentAtomicIndex, currentAtomicIndex).inverse();
+ F1 -= (Ainv(0, 0)*violation[0] + Ainv(0, 1)*violation[1] + Ainv(0, 2)*violation[2]);
+ F2 -= (Ainv(1, 0)*violation[0] + Ainv(1, 1)*violation[1] + Ainv(1, 2)*violation[2]);
+ F3 -= (Ainv(2, 0)*violation[0] + Ainv(2, 1)*violation[1] + Ainv(2, 2)*violation[2]);
+ }
+ currentAtomicIndex+=3;
+ break;
+
+
+ //####################################
+ //####################################
+ case MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT:
+ {
+ double& Fn = (*initialGuess_and_solution)[currentAtomicIndex];
+
+ // Form the local system
+ computeEnforcementSystem(n,A,nbColumnInA,b,(*initialGuess_and_solution),frictionCoefs,constraintsType,
+ subStep, i, currentAtomicIndex);
+
+ // Solve A.f = violation
+ if (! solveSystem(m_lhsEnforcedLocalSystem, m_rhsEnforcedLocalSystem, m_numEnforcedAtomicConstraints,
+ &m_rhsEnforcedLocalSystem))
+ {
+ return;
+ }
+
+ // Correct the forces accordingly
+ for (int i=0 ; i<m_numEnforcedAtomicConstraints-1 ; i++)
+ {
+ (*initialGuess_and_solution)[i] -= m_rhsEnforcedLocalSystem[i];
+ }
+ Fn -= m_rhsEnforcedLocalSystem[m_numEnforcedAtomicConstraints-1];
+
+ if (Fn<0.0)
+ {
+ Fn = 0; // inactive contact on normal
+ }
+ }
+ currentAtomicIndex++;
+ break;
+
+
+ //####################################
+ //####################################
+ case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT:
+ {
+ double local_mu = frictionCoefs[i];
+ double& Fn = (*initialGuess_and_solution)[currentAtomicIndex ];
+ double& Ft1 = (*initialGuess_and_solution)[currentAtomicIndex+1];
+ double& Ft2 = (*initialGuess_and_solution)[currentAtomicIndex+2];
+
+ // Form the local system
+ computeEnforcementSystem(n,A,nbColumnInA,b,(*initialGuess_and_solution),frictionCoefs,constraintsType,
+ subStep, i, currentAtomicIndex);
+
+ // Solve A.f = violation
+ if (! solveSystem(m_lhsEnforcedLocalSystem, m_rhsEnforcedLocalSystem, m_numEnforcedAtomicConstraints,
+ &m_rhsEnforcedLocalSystem))
+ {
+ return;
+ }
+
+ // Correct the forces accordingly
+ for (int i=0 ; i<m_numEnforcedAtomicConstraints-1 ; i++)
+ {
+ (*initialGuess_and_solution)[i] -= m_rhsEnforcedLocalSystem[i];
+ }
+ Fn -= m_rhsEnforcedLocalSystem[m_numEnforcedAtomicConstraints-1];
+
+ if (Fn>0.0)
+ {
+ // Compute the frictions violation
+ double violation[2]= { b[currentAtomicIndex+1]* subStep , b[currentAtomicIndex+2]* subStep };
+ for (int i=0 ; i<n ; i++)
+ {
+ violation[0] += A(currentAtomicIndex+1, i)*(*initialGuess_and_solution)[i];
+ violation[1] += A(currentAtomicIndex+2, i)*(*initialGuess_and_solution)[i];
+ }
+
+ Ft1 -= 2*violation[0]/(A(currentAtomicIndex+1, currentAtomicIndex+1) +
+ A(currentAtomicIndex+2, currentAtomicIndex+2));
+ Ft2 -= 2*violation[1]/(A(currentAtomicIndex+1, currentAtomicIndex+1) +
+ A(currentAtomicIndex+2, currentAtomicIndex+2));
+
+ double normFt = sqrt(Ft1 * Ft1 + Ft2 * Ft2);
+ if (normFt>local_mu*Fn)
+ {
+ // Here, the Friction is too strong, we keep the direction, but modulate its lenght
+ // to verify the Coulomb's law: |Ft| = mu |Fn|
+ Ft1 *= local_mu*Fn/normFt;
+ Ft2 *= local_mu*Fn/normFt;
+ }
+ }
+ else
+ {
+ Fn = 0; // inactive contact on normal
+ Ft1 = 0; // inactive contact on tangent
+ Ft2 = 0; // inactive contact on tangent
+ }
+ }
+ currentAtomicIndex+=3;
+ break;
+
+
+ //####################################
+ //####################################
+ case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
+ {
+ double& Fn1 = (*initialGuess_and_solution)[currentAtomicIndex ];
+ double& Fn2 = (*initialGuess_and_solution)[currentAtomicIndex+1];
+
+ // Form the local system
+ computeEnforcementSystem(n,A,nbColumnInA,b,(*initialGuess_and_solution),frictionCoefs,constraintsType,
+ subStep, i, currentAtomicIndex);
+
+ // Solve A.f = violation
+ if (! solveSystem(m_lhsEnforcedLocalSystem, m_rhsEnforcedLocalSystem, m_numEnforcedAtomicConstraints,
+ &m_rhsEnforcedLocalSystem))
+ {
+ return;
+ }
+
+ // Correct the forces accordingly
+ for (int i=0 ; i<m_numEnforcedAtomicConstraints-2 ; i++)
+ {
+ (*initialGuess_and_solution)[i] -= m_rhsEnforcedLocalSystem[i];
+ }
+ Fn1 -= m_rhsEnforcedLocalSystem[m_numEnforcedAtomicConstraints-2];
+ Fn2 -= m_rhsEnforcedLocalSystem[m_numEnforcedAtomicConstraints-1];
+
+// // 1st we analyze the constraints in the system to see if we should enforce any of them while solving
+// // the contact !
+// if(constraintsType[0]==MLCP_BILATERAL_3D_CONSTRAINT) // Bilateral 3D ?
+// {
+// if(constraintsType[1]==MLCP_BILATERAL_2D_CONSTRAINT) // Bilateral 3D+Directional constraints ?
+// {
+// if(constraintsType[2]==MLCP_BILATERAL_1D_CONSTRAINT) // Bilateral 3D+Directional+Axial constraints?
+// {
+// // HERE, we have:
+// // 1 bilateral 3D constraint store first in the system
+// // 1 directional 2D constraint store second in the system
+// // 1 axial 1D constraint store third in the system
+// // We want to enforce these constraints while solving the contact
+// // => solve 7x7 system, including the bilateral 3D constraint + directional 2D constraint +
+// // + axial 1D rotation + contact normal force (1D)
+// // => if the resulting contact normal force is positive, we will compute some frictional forces
+// double &FX = (*initialGuess_and_solution)[0];
+// double &FY = (*initialGuess_and_solution)[1];
+// double &FZ = (*initialGuess_and_solution)[2];
+//
+// double &FdirX = (*initialGuess_and_solution)[3];
+// double &FdirY = (*initialGuess_and_solution)[4];
+//
+// double &Faxial = (*initialGuess_and_solution)[5];
+//
+// double violation[8] = { b[0]*subStep , b[1]*subStep , b[2]*subStep ,
+// b[3]*subStep, b[4]*subStep, b[5]*subStep,
+// b[currentAtomicIndex]*subStep , b[currentAtomicIndex+1]*subStep };
+// for( int j=0 ; j<n ; j++ )
+// {
+// violation[0] += A(0, j) * (*initialGuess_and_solution)[j];
+// violation[1] += A(1, j) * (*initialGuess_and_solution)[j];
+// violation[2] += A(2, j) * (*initialGuess_and_solution)[j];
+// violation[3] += A(3, j) * (*initialGuess_and_solution)[j];
+// violation[4] += A(4, j) * (*initialGuess_and_solution)[j];
+// violation[5] += A(5, j) * (*initialGuess_and_solution)[j];
+// violation[6] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+// violation[7] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+// }
+// double localA[64]={
+// A(0, 0), A(0, 1), A(0, 2), A(0, 3), A(0, 4), A(0, 5),
+// A(0, currentAtomicIndex), A(0, currentAtomicIndex+1),
+// A(1, 0), A(1, 1), A(1, 2), A(1, 3), A(1, 4), A(1, 5),
+// A(1, currentAtomicIndex), A(1, currentAtomicIndex+1),
+// A(2, 0), A(2, 1), A(2, 2), A(2, 3), A(2, 4), A(2, 5),
+// A(2, currentAtomicIndex), A(2, currentAtomicIndex+1),
+// A(3, 0), A(3, 1), A(3, 2), A(3, 3), A(3, 4), A(3, 5),
+// A(3, currentAtomicIndex), A(3, currentAtomicIndex+1),
+// A(4, 0), A(4, 1), A(4, 2), A(4, 3), A(4, 4), A(4, 5),
+// A(4, currentAtomicIndex), A(4, currentAtomicIndex+1),
+// A(5, 0), A(5, 1), A(5, 2), A(5, 3), A(5, 4), A(5, 5),
+// A(5, currentAtomicIndex), A(5, currentAtomicIndex+1),
+// A(currentAtomicIndex, 0), A(currentAtomicIndex, 1), A(currentAtomicIndex, 2),
+// A(currentAtomicIndex, 3), A(currentAtomicIndex, 4), A(currentAtomicIndex, 5),
+// A(currentAtomicIndex, currentAtomicIndex),
+// A(currentAtomicIndex, currentAtomicIndex+1),
+// A(currentAtomicIndex+1, 0), A(currentAtomicIndex+1, 1), A(currentAtomicIndex+1, 2),
+// A(currentAtomicIndex+1, 3), A(currentAtomicIndex+1, 4), A(currentAtomicIndex+1, 5),
+// A(currentAtomicIndex+1, currentAtomicIndex),
+// A(currentAtomicIndex+1, currentAtomicIndex+1)
+// };
+// double Ainv[64];
+// // Try a block decomposition 7=5+3
+// if( !inverseMatrix_using_BlockDecomposition<double,8,5,3>(localA,Ainv) )
+// {
+// cerr << " MLCP could not inverse a local 6x6 matrix for contact + " <<
+// "(3D bilateral + 2D directional)" << endl;
+// cerr << " Press any key + [ENTER] to unlock the simulation" << endl;
+// char wait;
+// cin >> wait;
+// exit(0);
+// }
+// double F[8];
+// MatrixVectorProduct_nxm<double,8,8>((const double *)Ainv , (const double *)violation,
+// (double *)F);
+// FX -= F[0];
+// FY -= F[1];
+// FZ -= F[2];
+// FdirX -= F[3];
+// FdirY -= F[4];
+// Faxial-= F[5];
+// Fn1 -= F[6];
+// Fn2 -= F[7];
+// }
+// else // if(constraintsType[2]==MLCP_BILATERAL_1D_CONSTRAINT)
+// {
+// // HERE, we have:
+// // 1 bilateral 3D constraint store first in the system
+// // 1 directional 2D constraint store second in the system
+// // We want to enforce these constraints while solving the contact
+// // => solve 6x6 system, including the bilateral 3D constraint + directional 2D constraint +
+// // + contact normal force (1D)
+// // => if the resulting contact normal force is positive, we will compute some frictional forces
+// double &FX = (*initialGuess_and_solution)[0];
+// double &FY = (*initialGuess_and_solution)[1];
+// double &FZ = (*initialGuess_and_solution)[2];
+//
+// double &FdirX = (*initialGuess_and_solution)[3];
+// double &FdirY = (*initialGuess_and_solution)[4];
+//
+// double violation[7] = { b[0]*subStep , b[1]*subStep , b[2]*subStep , b[3]*subStep, b[4]*subStep,
+// b[currentAtomicIndex]*subStep , b[currentAtomicIndex+1]*subStep };
+// for( int j=0 ; j<n ; j++ )
+// {
+// violation[0] += A(0, j) * (*initialGuess_and_solution)[j];
+// violation[1] += A(1, j) * (*initialGuess_and_solution)[j];
+// violation[2] += A(2, j) * (*initialGuess_and_solution)[j];
+// violation[3] += A(3, j) * (*initialGuess_and_solution)[j];
+// violation[4] += A(4, j) * (*initialGuess_and_solution)[j];
+// violation[5] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+// violation[6] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+// }
+// double localA[49]={
+// A(0, 0), A(0, 1), A(0, 2), A(0, 3), A(0, 4),
+// A(0, currentAtomicIndex), A(0, currentAtomicIndex+1),
+// A(1, 0), A(1, 1), A(1, 2), A(1, 3), A(1, 4),
+// A(1, currentAtomicIndex), A(1, currentAtomicIndex+1),
+// A(2, 0), A(2, 1), A(2, 2), A(2, 3), A(2, 4),
+// A(2, currentAtomicIndex), A(2, currentAtomicIndex+1),
+// A(3, 0), A(3, 1), A(3, 2), A(3, 3), A(3, 4),
+// A(3, currentAtomicIndex), A(3, currentAtomicIndex+1),
+// A(4, 0), A(4, 1), A(4, 2), A(4, 3), A(4, 4),
+// A(4, currentAtomicIndex), A(4, currentAtomicIndex+1),
+// A(currentAtomicIndex, 0), A(currentAtomicIndex, 1), A(currentAtomicIndex, 2),
+// A(currentAtomicIndex, 3), A(currentAtomicIndex, 4),
+// A(currentAtomicIndex, currentAtomicIndex),
+// A(currentAtomicIndex, currentAtomicIndex+1),
+// A(currentAtomicIndex+1, 0), A(currentAtomicIndex+1, 1), A(currentAtomicIndex+1, 2),
+// A(currentAtomicIndex+1, 3), A(currentAtomicIndex+1, 4),
+// A(currentAtomicIndex+1, currentAtomicIndex),
+// A(currentAtomicIndex+1, currentAtomicIndex+1)
+// };
+// double Ainv[49];
+// if( !inverseMatrix_using_BlockDecomposition<double,7,4,3>(localA,Ainv) )
+// {
+// cerr << " MLCP could not inverse a local 6x6 matrix for contact + " <<
+// "(3D bilateral + 2D directional)" << endl;
+// cerr << " Press any key + [ENTER] to unlock the simulation" << endl;
+// char wait;
+// cin >> wait;
+// exit(0);
+// }
+// double F[7];
+// MatrixVectorProduct_nxm<double,7,7>((const double *)Ainv , (const double *)violation,
+// (double *)F);
+// FX -= F[0];
+// FY -= F[1];
+// FZ -= F[2];
+// FdirX -= F[3];
+// FdirY -= F[4];
+// Fn1 -= F[5];
+// Fn2 -= F[6];
+// }
+// }
+// else // if(constraintsType[1]==MLCP_BILATERAL_2D_CONSTRAINT)
+// {
+// // HERE, we have:
+// // 1 bilateral 3D constraint store first in the system
+// // We want to enforce this constraint while solving the contact
+// // => solve 4x4 system, including the bilateral 3D constraint + contact normal force (1D)
+// // => if the resulting contact normal force is positive, we will compute some frictional forces
+// double &FX = (*initialGuess_and_solution)[0];
+// double &FY = (*initialGuess_and_solution)[1];
+// double &FZ = (*initialGuess_and_solution)[2];
+// double violation[5] = { b[0]*subStep , b[1]*subStep , b[2]*subStep ,
+// b[currentAtomicIndex]*subStep , b[currentAtomicIndex+1]*subStep };
+// for( int j=0 ; j<n ; j++ )
+// {
+// violation[0] += A(0, j) * (*initialGuess_and_solution)[j];
+// violation[1] += A(1, j) * (*initialGuess_and_solution)[j];
+// violation[2] += A(2, j) * (*initialGuess_and_solution)[j];
+// violation[3] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+// violation[4] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+// }
+// double localA[25]={
+// A(0, 0), A(0, 1), A(0, 2), A(0, currentAtomicIndex), A(0, currentAtomicIndex+1),
+// A(1, 0), A(1, 1), A(1, 2), A(1, currentAtomicIndex), A(1, currentAtomicIndex+1),
+// A(2, 0), A(2, 1), A(2, 2), A(2, currentAtomicIndex), A(2, currentAtomicIndex+1),
+// A(currentAtomicIndex, 0), A(currentAtomicIndex, 1), A(currentAtomicIndex, 2),
+// A(currentAtomicIndex, currentAtomicIndex), A(currentAtomicIndex, currentAtomicIndex+1),
+// A(currentAtomicIndex+1, 0), A(currentAtomicIndex+1, 1), A(currentAtomicIndex+1, 2),
+// A(currentAtomicIndex+1, currentAtomicIndex), A(currentAtomicIndex+1, currentAtomicIndex+1)
+// };
+// double Ainv[25];
+// if( !inverseMatrix_using_BlockDecomposition<double,5,2,3>(localA,Ainv) )
+// {
+// cerr << " MLCP could not inverse a local 4x4 matrix for contact/bilateral" << endl;
+// cerr << " Press any key + [ENTER] to unlock the simulation" << endl;
+// char wait;
+// cin >> wait;
+// exit(0);
+// }
+// double F[5];
+// MatrixVectorProduct_nxm<double,5,5>(Ainv,violation,F);
+// FX -= F[0];
+// FY -= F[1];
+// FZ -= F[2];
+// Fn1-= F[3];
+// Fn2-= F[4];
+// }
+// }
+// else // if(constraintsType[0]==MLCP_BILATERAL_3D_CONSTRAINT)
+// {
+// // HERE, we do not have any expected constraints
+// // We treat this contact normally, without taking into account any other constraint than the current
+// // contact !
+// double violation[2] = { b[currentAtomicIndex]*subStep , b[currentAtomicIndex+1]*subStep };
+// for( int j=0 ; j<n ; j++ )
+// {
+// violation[0] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+// violation[1] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+// }
+//
+// // det = ad-bc
+// // [ a b ] [ d -b ] [ 1 0 ]
+// // [ c d ] . [ -c a ]/det = [ 0 1 ]
+// double A_determinant =
+// A(currentAtomicIndex, currentAtomicIndex)*A(currentAtomicIndex+1, currentAtomicIndex+1)-
+// A(currentAtomicIndex, currentAtomicIndex+1)*A(currentAtomicIndex+1, currentAtomicIndex);
+// double Ainv[2][2]={
+// { A(currentAtomicIndex+1, currentAtomicIndex+1)/A_determinant ,
+// -A(currentAtomicIndex, currentAtomicIndex+1)/A_determinant },
+// {-A(currentAtomicIndex+1, currentAtomicIndex )/A_determinant ,
+// A(currentAtomicIndex, currentAtomicIndex )/A_determinant }
+// } ;
+// Fn1 -= (Ainv[0][0]*violation[0] + Ainv[0][1]*violation[1]);
+// Fn2 -= (Ainv[1][0]*violation[0] + Ainv[1][1]*violation[1]);
+// }
+ }
+ currentAtomicIndex+=2;
+ break;
+
+
+ //####################################
+ //####################################
+ case MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT:
+ {
+ double local_mu = frictionCoefs[i];
+ double& Fn1 = (*initialGuess_and_solution)[currentAtomicIndex ];
+ double& Fn2 = (*initialGuess_and_solution)[currentAtomicIndex+1];
+ double& Ft = (*initialGuess_and_solution)[currentAtomicIndex+2];
+
+ // Form the local system
+ computeEnforcementSystem(n,A,nbColumnInA,b,(*initialGuess_and_solution),frictionCoefs,constraintsType,
+ subStep, i, currentAtomicIndex);
+
+ // Solve A.f = violation
+ if (! solveSystem(m_lhsEnforcedLocalSystem, m_rhsEnforcedLocalSystem, m_numEnforcedAtomicConstraints,
+ &m_rhsEnforcedLocalSystem))
+ {
+ return;
+ }
+
+ // Correct the forces accordingly
+ for (int i=0 ; i<m_numEnforcedAtomicConstraints-2 ; i++)
+ {
+ (*initialGuess_and_solution)[i] -= m_rhsEnforcedLocalSystem[i];
+ }
+ Fn1 -= m_rhsEnforcedLocalSystem[m_numEnforcedAtomicConstraints-2];
+ Fn2 -= m_rhsEnforcedLocalSystem[m_numEnforcedAtomicConstraints-1];
+
+ // No Signorini to verify here, it is NOT a unilateral constraint, but bilateral
+ //if(Fn>0.0)
+ {
+ // Complete the violation of the friction along t, with the missing terms...
+ double violation = b[currentAtomicIndex+2]*subStep;
+ for (int i=0 ; i<n ; i++)
+ {
+ violation += A(currentAtomicIndex+2, i)*(*initialGuess_and_solution)[i];
+ }
+
+ Ft -= violation/A(currentAtomicIndex+2, currentAtomicIndex+2);
+
+ double normFn = sqrt(Fn1*Fn1 + Fn2*Fn2);
+ double normFt = fabs(Ft);
+ if (normFt>local_mu*normFn)
+ {
+ // Here, the Friction is too strong, we keep the direction, but modulate its lenght
+ // to verify the Coulomb's law: |Ft| = mu |Fn|
+ Ft *= local_mu*normFn/normFt;
+ }
+ }
+
+// // 1st we analyze the constraints in the system to see if we should enforce any of them while solving
+// // the contact !
+// if(constraintsType[0]==MLCP_BILATERAL_3D_CONSTRAINT) // Bilateral 3D ?
+// {
+// if(constraintsType[1]==MLCP_BILATERAL_2D_CONSTRAINT) // Bilateral 3D+Directional constraints ?
+// {
+// if(constraintsType[2]==MLCP_BILATERAL_1D_CONSTRAINT) // Bilateral 3D+Directional+Axial constraints?
+// {
+// // HERE, we have:
+// // 1 bilateral 3D constraint store first in the system
+// // 1 directional 2D constraint store second in the system
+// // 1 axial 1D constraint store third in the system
+// // We want to enforce these constraints while solving the contact
+// // => solve 7x7 system, including the bilateral 3D constraint + directional 2D constraint +
+// // + axial 1D rotation + contact normal force (1D)
+// // => if the resulting contact normal force is positive, we will compute some frictional forces
+// double &FX = (*initialGuess_and_solution)[0];
+// double &FY = (*initialGuess_and_solution)[1];
+// double &FZ = (*initialGuess_and_solution)[2];
+//
+// double &FdirX = (*initialGuess_and_solution)[3];
+// double &FdirY = (*initialGuess_and_solution)[4];
+//
+// double &Faxial = (*initialGuess_and_solution)[5];
+//
+// double violation[9] = { b[0]*subStep , b[1]*subStep , b[2]*subStep ,
+// b[3]*subStep, b[4]*subStep, b[5]*subStep,
+// b[currentAtomicIndex]*subStep , b[currentAtomicIndex+1]*subStep ,
+// b[currentAtomicIndex+2]*subStep };
+// for( int j=0 ; j<n ; j++ )
+// {
+// violation[0] += A(0, j) * (*initialGuess_and_solution)[j];
+// violation[1] += A(1, j) * (*initialGuess_and_solution)[j];
+// violation[2] += A(2, j) * (*initialGuess_and_solution)[j];
+// violation[3] += A(3, j) * (*initialGuess_and_solution)[j];
+// violation[4] += A(4, j) * (*initialGuess_and_solution)[j];
+// violation[5] += A(5, j) * (*initialGuess_and_solution)[j];
+// violation[6] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+// violation[7] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+// if(j>=7 && j!=currentAtomicIndex)
+// {
+// violation[8] += A(currentAtomicIndex+2, j) * (*initialGuess_and_solution)[j];
+// }
+// }
+// double localA[64]={
+// A(0, 0), A(0, 1), A(0, 2), A(0, 3), A(0, 4), A(0, 5),
+// A(0, currentAtomicIndex), A(0, currentAtomicIndex+1),
+// A(1, 0), A(1, 1), A(1, 2), A(1, 3), A(1, 4), A(1, 5),
+// A(1, currentAtomicIndex), A(1, currentAtomicIndex+1),
+// A(2, 0), A(2, 1), A(2, 2), A(2, 3), A(2, 4), A(2, 5),
+// A(2, currentAtomicIndex), A(2, currentAtomicIndex+1),
+// A(3, 0), A(3, 1), A(3, 2), A(3, 3), A(3, 4), A(3, 5),
+// A(3, currentAtomicIndex), A(3, currentAtomicIndex+1),
+// A(4, 0), A(4, 1), A(4, 2), A(4, 3), A(4, 4), A(4, 5),
+// A(4, currentAtomicIndex), A(4, currentAtomicIndex+1),
+// A(5, 0), A(5, 1), A(5, 2), A(5, 3), A(5, 4), A(5, 5),
+// A(5, currentAtomicIndex), A(5, currentAtomicIndex+1),
+// A(currentAtomicIndex, 0), A(currentAtomicIndex, 1), A(currentAtomicIndex, 2),
+// A(currentAtomicIndex, 3), A(currentAtomicIndex, 4), A(currentAtomicIndex, 5),
+// A(currentAtomicIndex, currentAtomicIndex),
+// A(currentAtomicIndex, currentAtomicIndex+1),
+// A(currentAtomicIndex+1, 0), A(currentAtomicIndex+1, 1), A(currentAtomicIndex+1, 2),
+// A(currentAtomicIndex+1, 3), A(currentAtomicIndex+1, 4), A(currentAtomicIndex+1, 5),
+// A(currentAtomicIndex+1, currentAtomicIndex),
+// A(currentAtomicIndex+1, currentAtomicIndex+1)
+// };
+// double Ainv[64];
+// // Try a block decomposition 7=5+3
+// if( !inverseMatrix_using_BlockDecomposition<double,8,5,3>(localA,Ainv) )
+// {
+// cerr << " MLCP could not inverse a local 6x6 matrix for contact +" <<
+// " (3D bilateral + 2D directional)" << endl;
+// cerr << " Press any key + [ENTER] to unlock the simulation" << endl;
+// char wait;
+// cin >> wait;
+// exit(0);
+// }
+// double F[8];
+// MatrixVectorProduct_nxm<double,8,8>((const double *)Ainv , (const double *)violation,
+// (double *)F);
+// FX -= F[0];
+// FY -= F[1];
+// FZ -= F[2];
+// FdirX -= F[3];
+// FdirY -= F[4];
+// Faxial-= F[5];
+// Fn1 -= F[6];
+// Fn2 -= F[7];
+//
+// // No Signorini to verify here, it is NOT a unilateral constraint, but bilateral
+// //if(Fn>0.0)
+// {
+// // Complete the violation of the friction along t, with the missing terms...
+// violation[8] += A(currentAtomicIndex+2, 0)*FX +
+// A(currentAtomicIndex+2, 1)*FY +
+// A(currentAtomicIndex+2, 2)*FZ +
+// A(currentAtomicIndex+2, 3)*FdirX +
+// A(currentAtomicIndex+2, 4)*FdirY +
+// A(currentAtomicIndex+2, 5)*Faxial+
+// A(currentAtomicIndex+2, currentAtomicIndex )*Fn1 +
+// A(currentAtomicIndex+2, currentAtomicIndex+1)*Fn2;
+//
+// Ft -= violation[8]/A(currentAtomicIndex+2, currentAtomicIndex+2);
+//
+// double normFn = sqrt(Fn1*Fn1 + Fn2*Fn2);
+// double normFt = fabs(Ft);
+// if(normFt>local_mu*normFn)
+// {
+// // Here, the Friction is too strong, we keep the direction, but modulate its lenght
+// // to verify the Coulomb's law: |Ft| = mu |Fn|
+// Ft *= local_mu*normFn/normFt;
+// }
+// }
+// }
+// else // if(constraintsType[2]==MLCP_BILATERAL_1D_CONSTRAINT)
+// {
+// // HERE, we have:
+// // 1 bilateral 3D constraint store first in the system
+// // 1 directional 2D constraint store second in the system
+// // We want to enforce these constraints while solving the contact
+// // => solve 6x6 system, including the bilateral 3D constraint + directional 2D constraint +
+// // + contact normal force (1D)
+// // => if the resulting contact normal force is positive, we will compute some frictional forces
+// double &FX = (*initialGuess_and_solution)[0];
+// double &FY = (*initialGuess_and_solution)[1];
+// double &FZ = (*initialGuess_and_solution)[2];
+//
+// double &FdirX = (*initialGuess_and_solution)[3];
+// double &FdirY = (*initialGuess_and_solution)[4];
+//
+// double violation[8] = { b[0]*subStep , b[1]*subStep , b[2]*subStep , b[3]*subStep, b[4]*subStep,
+// b[currentAtomicIndex]*subStep , b[currentAtomicIndex+1]*subStep ,
+// b[currentAtomicIndex+2]*subStep };
+// for( int j=0 ; j<n ; j++ )
+// {
+// violation[0] += A(0, j) * (*initialGuess_and_solution)[j];
+// violation[1] += A(1, j) * (*initialGuess_and_solution)[j];
+// violation[2] += A(2, j) * (*initialGuess_and_solution)[j];
+// violation[3] += A(3, j) * (*initialGuess_and_solution)[j];
+// violation[4] += A(4, j) * (*initialGuess_and_solution)[j];
+// violation[5] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+// violation[6] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+// if(j>=6 && j!=currentAtomicIndex)
+// {
+// violation[7] += A(currentAtomicIndex+2, j) * (*initialGuess_and_solution)[j];
+// }
+// }
+// double localA[49]={
+// A(0, 0), A(0, 1), A(0, 2), A(0, 3), A(0, 4), A(0, currentAtomicIndex),
+// A(0, currentAtomicIndex+1),
+// A(1, 0), A(1, 1), A(1, 2), A(1, 3), A(1, 4), A(1, currentAtomicIndex),
+// A(1, currentAtomicIndex+1),
+// A(2, 0), A(2, 1), A(2, 2), A(2, 3), A(2, 4), A(2, currentAtomicIndex),
+// A(2, currentAtomicIndex+1),
+// A(3, 0), A(3, 1), A(3, 2), A(3, 3), A(3, 4), A(3, currentAtomicIndex),
+// A(3, currentAtomicIndex+1),
+// A(4, 0), A(4, 1), A(4, 2), A(4, 3), A(4, 4), A(4, currentAtomicIndex),
+// A(4, currentAtomicIndex+1),
+// A(currentAtomicIndex, 0), A(currentAtomicIndex, 1), A(currentAtomicIndex, 2),
+// A(currentAtomicIndex, 3), A(currentAtomicIndex, 4),
+// A(currentAtomicIndex, currentAtomicIndex),
+// A(currentAtomicIndex, currentAtomicIndex+1),
+// A(currentAtomicIndex+1, 0), A(currentAtomicIndex+1, 1), A(currentAtomicIndex+1, 2),
+// A(currentAtomicIndex+1, 3), A(currentAtomicIndex+1, 4),
+// A(currentAtomicIndex+1, currentAtomicIndex),
+// A(currentAtomicIndex+1, currentAtomicIndex+1)
+// };
+// double Ainv[49];
+// if( !inverseMatrix_using_BlockDecomposition<double,7,4,3>(localA,Ainv) )
+// {
+// cerr << " MLCP could not inverse a local 6x6 matrix for contact +" <<
+// " (3D bilateral + 2D directional)" << endl;
+// cerr << " Press any key + [ENTER] to unlock the simulation" << endl;
+// char wait;
+// cin >> wait;
+// exit(0);
+// }
+// double F[7];
+// MatrixVectorProduct_nxm<double,7,7>((const double *)Ainv , (const double *)violation,
+// (double *)F);
+// FX -= F[0];
+// FY -= F[1];
+// FZ -= F[2];
+// FdirX -= F[3];
+// FdirY -= F[4];
+// Fn1 -= F[5];
+// Fn2 -= F[6];
+//
+// // No Signorini to verify here, we have a bilateral constraint, not a unilateral one
+// //if(Fn>0.0)
+// {
+// // Complete the violation of the friction along t, with the missing terms...
+// violation[7] += A(currentAtomicIndex+2, 0)*FX +
+// A(currentAtomicIndex+2, 1)*FY +
+// A(currentAtomicIndex+2, 2)*FZ +
+// A(currentAtomicIndex+2, 3)*FdirX +
+// A(currentAtomicIndex+2, 4)*FdirY +
+// A(currentAtomicIndex+2, currentAtomicIndex )*Fn1+
+// A(currentAtomicIndex+2, currentAtomicIndex+1)*Fn2;
+//
+// Ft -= violation[7]/A(currentAtomicIndex+2, currentAtomicIndex+2);
+//
+// double normFn = sqrt(Fn1*Fn1 + Fn2*Fn2);
+// double normFt = fabs(Ft);
+// if(normFt>local_mu*normFn)
+// {
+// // Here, the Friction is too strong, we keep the direction, but modulate its lenght
+// // to verify the Coulomb's law: |Ft| = mu |Fn|
+// Ft *= local_mu*normFn/normFt;
+// }
+// }
+// }
+// }
+// else // if(constraintsType[1]==MLCP_BILATERAL_2D_CONSTRAINT)
+// {
+// // HERE, we have:
+// // 1 bilateral 3D constraint store first in the system
+// // We want to enforce this constraint while solving the contact
+// // => solve 4x4 system, including the bilateral 3D constraint + contact normal force (1D)
+// // => if the resulting contact normal force is positive, we will compute some frictional forces
+// double &FX = (*initialGuess_and_solution)[0];
+// double &FY = (*initialGuess_and_solution)[1];
+// double &FZ = (*initialGuess_and_solution)[2];
+// double violation[6] = { b[0]*subStep , b[1]*subStep , b[2]*subStep ,
+// b[currentAtomicIndex]*subStep , b[currentAtomicIndex+1]*subStep ,
+// b[currentAtomicIndex+2]*subStep };
+// for( int j=0 ; j<n ; j++ )
+// {
+// violation[0] += A(0, j) * (*initialGuess_and_solution)[j];
+// violation[1] += A(1, j) * (*initialGuess_and_solution)[j];
+// violation[2] += A(2, j) * (*initialGuess_and_solution)[j];
+// violation[3] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+// violation[4] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+// if(j>=4 && j!=currentAtomicIndex)
+// {
+// violation[5] += A(currentAtomicIndex+2, j) * (*initialGuess_and_solution)[j];
+// }
+// }
+// double localA[25]={
+// A(0, 0), A(0, 1), A(0, 2), A(0, currentAtomicIndex), A(0, currentAtomicIndex+1),
+// A(1, 0), A(1, 1), A(1, 2), A(1, currentAtomicIndex), A(1, currentAtomicIndex+1),
+// A(2, 0), A(2, 1), A(2, 2), A(2, currentAtomicIndex), A(2, currentAtomicIndex+1),
+// A(currentAtomicIndex, 0), A(currentAtomicIndex, 1), A(currentAtomicIndex, 2),
+// A(currentAtomicIndex, currentAtomicIndex), A(currentAtomicIndex, currentAtomicIndex+1),
+// A(currentAtomicIndex+1, 0), A(currentAtomicIndex+1, 1), A(currentAtomicIndex+1, 2),
+// A(currentAtomicIndex+1, currentAtomicIndex), A(currentAtomicIndex+1, currentAtomicIndex+1)
+// };
+// double Ainv[25];
+// if( !inverseMatrix_using_BlockDecomposition<double,5,2,3>(localA,Ainv) )
+// {
+// cerr << " MLCP could not inverse a local 4x4 matrix for contact/bilateral" << endl;
+// cerr << " Press any key + [ENTER] to unlock the simulation" << endl;
+// char wait;
+// cin >> wait;
+// exit(0);
+// }
+// double F[5];
+// MatrixVectorProduct_nxm<double,5,5>(Ainv,violation,F);
+// FX -= F[0];
+// FY -= F[1];
+// FZ -= F[2];
+// Fn1-= F[3];
+// Fn2-= F[4];
+//
+// // No Signorini to verify here, we have a bilateral constraint, not a unilateral one !
+// //if(Fn>0.0)
+// {
+// // Complete the violation of the friction along t, with the missing terms...
+// violation[5] += A(currentAtomicIndex+2, 0)*FX +
+// A(currentAtomicIndex+2, 1)*FY +
+// A(currentAtomicIndex+2, 2)*FZ +
+// A(currentAtomicIndex+2, currentAtomicIndex )*Fn1+
+// A(currentAtomicIndex+2, currentAtomicIndex+1)*Fn2;
+//
+// Ft -= violation[5]/A(currentAtomicIndex+2, currentAtomicIndex+2);
+//
+// double normFn = sqrt(Fn1*Fn1 + Fn2*Fn2);
+// double normFt = fabs(Ft);
+// if(normFt>local_mu*normFn)
+// {
+// // Here, the Friction is too strong, we keep the direction, but modulate its lenght
+// // to verify the Coulomb's law: |Ft| = mu |Fn|
+// Ft *= local_mu*normFn/normFt;
+// }
+// }
+// }
+// }
+// else // if(constraintsType[0]==MLCP_BILATERAL_3D_CONSTRAINT)
+// {
+// // HERE, we do not have any expected constraints
+// // We treat this contact normally, without taking into account any other constraint than the current
+// // contact !
+// double violation[3] = { b[currentAtomicIndex]*subStep , b[currentAtomicIndex+1]*subStep ,
+// b[currentAtomicIndex+2]*subStep };
+// for( int j=0 ; j<currentAtomicIndex ; j++ )
+// {
+// violation[0] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+// violation[1] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+// violation[2] += A(currentAtomicIndex+2, j) * (*initialGuess_and_solution)[j];
+// }
+// for( int j=currentAtomicIndex+3 ; j<n ; j++ )
+// {
+// violation[0] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+// violation[1] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+// violation[2] += A(currentAtomicIndex+2, j) * (*initialGuess_and_solution)[j];
+// }
+// violation[0] += A(currentAtomicIndex, currentAtomicIndex)*Fn1 +
+// A(currentAtomicIndex, currentAtomicIndex+1)*Fn2 +
+// A(currentAtomicIndex, currentAtomicIndex+2)*Ft;
+// violation[1] += A(currentAtomicIndex+1, currentAtomicIndex)*Fn1 +
+// A(currentAtomicIndex+1, currentAtomicIndex+1)*Fn2 +
+// A(currentAtomicIndex+1, currentAtomicIndex+2)*Ft;
+//
+// // det = ad-bc
+// // [ a b ] [ d -b ] [ 1 0 ]
+// // [ c d ] . [ -c a ]/det = [ 0 1 ]
+// double A_determinant =
+// A(currentAtomicIndex, currentAtomicIndex)*A(currentAtomicIndex+1, currentAtomicIndex+1)-
+// A(currentAtomicIndex, currentAtomicIndex+1)*A(currentAtomicIndex+1, currentAtomicIndex);
+// double Ainv[2][2]={
+// { A(currentAtomicIndex+1, currentAtomicIndex+1)/A_determinant ,
+// -A(currentAtomicIndex, currentAtomicIndex+1)/A_determinant },
+// {-A(currentAtomicIndex+1, currentAtomicIndex )/A_determinant ,
+// A(currentAtomicIndex, currentAtomicIndex )/A_determinant }
+// } ;
+// Fn1 -= (Ainv[0][0]*violation[0] + Ainv[0][1]*violation[1]);
+// Fn2 -= (Ainv[1][0]*violation[0] + Ainv[1][1]*violation[1]);
+//
+// // No Signorini to verify here, we have bilaterals constraints, not unilateral ones !
+// //if(Fn>0.0)
+// {
+// violation[2] += A(currentAtomicIndex+2, currentAtomicIndex)*Fn1
+// + A(currentAtomicIndex+2, currentAtomicIndex+1)*Fn2
+// + A(currentAtomicIndex+2, currentAtomicIndex+2)*Ft;
+//
+// Ft -= violation[2]/A(currentAtomicIndex+2, currentAtomicIndex+2);
+//
+// double normFt = fabs(Ft);
+// double normFn = sqrt(Fn1*Fn1 + Fn2*Fn2);
+// if(normFt>local_mu*normFn)
+// {
+// // Here, the Friction is too strong, we keep the direction, but modulate its lenght
+// // to verify the Coulomb's law: |Ft| = mu |Fn|
+// Ft *= local_mu*normFn/normFt;
+// }
+// }
+// }
+ }
+ currentAtomicIndex+=3;
+ break;
+
+ //####################################
+ //####################################
+ default:
+ //XXX
+ SURGSIM_FAILURE() << "unknown constraint type [" << constraintsType[i] << "]";
+ break;
+ }
+ }
+}
+
+void MlcpGaussSeidelSolver::printViolationsAndConvergence(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
+ const MlcpProblem::Vector& b,
+ const MlcpSolution::Vector& initialGuess_and_solution,
+ const std::vector<MlcpConstraintType>& constraintsType,
+ double subStep,
+ double convergence_criteria, bool signorini_verified,
+ int nbLoop)
+{
+ printf("MLCP at iteration %d =\n",nbLoop);
+
+ int currentAtomicIndex=0;
+ int nbConstraints = static_cast<int>(constraintsType.size());
+
+ for (int i=0 ; i<nbConstraints ; i++)
+ {
+ printf("Constraint [%2d] of type ",i);
+ switch (constraintsType[i])
+ {
+ case MLCP_BILATERAL_1D_CONSTRAINT:
+ {
+ printf("BILATERAL_1D_CONSTRAINT");
+ printf("\n\t with initial violation b=(%g) ",b[currentAtomicIndex]);
+ double violation = b[currentAtomicIndex];
+ for (int j=0 ; j<n ; j++)
+ {
+ violation += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ }
+ printf("\n\t with final violation b-Ax=(%g) ",violation);
+ printf("\n\t force=(%g) ",initialGuess_and_solution[currentAtomicIndex]);
+ currentAtomicIndex+=1;
+ }
+ break;
+ case MLCP_BILATERAL_2D_CONSTRAINT:
+ {
+ printf("BILATERAL_2D_CONSTRAINT");
+ printf("\n\t with initial violation b=(%g %g) ",b[currentAtomicIndex],b[currentAtomicIndex+1]);
+ double violation[2] = {b[currentAtomicIndex],b[currentAtomicIndex+1]};
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ violation[1] += A(currentAtomicIndex+1, j) * initialGuess_and_solution[j];
+ }
+ printf("\n\t with final violation b-Ax=(%g %g) ",violation[0],violation[1]);
+ printf("\n\t force=(%g %g) ",
+ initialGuess_and_solution[currentAtomicIndex],
+ initialGuess_and_solution[currentAtomicIndex+1]);
+ currentAtomicIndex+=2;
+ }
+ break;
+ case MLCP_BILATERAL_3D_CONSTRAINT:
+ {
+ printf("BILATERAL_3D_CONSTRAINT");
+ printf("\n\t with initial violation b=(%g %g %g) ",
+ b[currentAtomicIndex], b[currentAtomicIndex+1], b[currentAtomicIndex+2]);
+ double violation[3] = {b[currentAtomicIndex],b[currentAtomicIndex+1],b[currentAtomicIndex+2]};
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ violation[1] += A(currentAtomicIndex+1, j) * initialGuess_and_solution[j];
+ violation[2] += A(currentAtomicIndex+2, j) * initialGuess_and_solution[j];
+ }
+ printf("\n\t with final violation b-Ax=(%g %g %g) ",violation[0],violation[1],violation[2]);
+ printf("\n\t force=(%g %g %g) ",
+ initialGuess_and_solution[currentAtomicIndex],
+ initialGuess_and_solution[currentAtomicIndex+1],
+ initialGuess_and_solution[currentAtomicIndex+2]);
+ currentAtomicIndex+=3;
+ }
+ break;
+ case MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT:
+ {
+ printf("UNILATERAL_FRICTIONLESS_CONSTRAINT");
+ printf("\n\t with initial violation b=(%g) ",b[currentAtomicIndex]);
+ double violation = b[currentAtomicIndex];
+ for (int j=0 ; j<n ; j++)
+ {
+ violation += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ }
+ printf("\n\t with final violation b-Ax=(%g) ",violation);
+ if (violation < -m_contactTolerance)
+ {
+ printf("\n\t => normal violation = %g < -m_contactTolerance => Signorini not verified yet !",
+ violation);
+ }
+ printf("\n\t force=(%g) ",initialGuess_and_solution[currentAtomicIndex]);
+ currentAtomicIndex+=1;
+ }
+ break;
+ case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT:
+ {
+ printf("UNILATERAL_3D_FRICTIONAL_CONSTRAINT");
+ printf("\n\t with initial violation b=(%g %g %g) ",
+ b[currentAtomicIndex],
+ b[currentAtomicIndex+1],
+ b[currentAtomicIndex+2]);
+ double violation[3] = {b[currentAtomicIndex],b[currentAtomicIndex+1],b[currentAtomicIndex+2]};
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ violation[1] += A(currentAtomicIndex+1, j) * initialGuess_and_solution[j];
+ violation[2] += A(currentAtomicIndex+2, j) * initialGuess_and_solution[j];
+ }
+ printf("\n\t with final violation b-Ax=(%g %g %g) ",violation[0],violation[1],violation[2]);
+ if (violation[0] < -m_contactTolerance)
+ {
+ printf("\n\t => normal violation = %g < -contactTolerance => Signorini not verified yet !",
+ violation[0]);
+ }
+ printf("\n\t force=(%g %g %g) ",
+ initialGuess_and_solution[currentAtomicIndex],
+ initialGuess_and_solution[currentAtomicIndex+1],
+ initialGuess_and_solution[currentAtomicIndex+2]);
+ currentAtomicIndex+=3;
+ }
+ break;
+ case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
+ {
+ printf("UNILATERAL_3D_FRICTIONLESS_SUTURING");
+ printf("\n\t with initial violation b=(%g %g) ",b[currentAtomicIndex],b[currentAtomicIndex+1]);
+ double violation[2] = {b[currentAtomicIndex],b[currentAtomicIndex+1]};
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ violation[1] += A(currentAtomicIndex+1, j) * initialGuess_and_solution[j];
+ }
+ printf("\n\t with final violation b-Ax=(%g %g) ",violation[0],violation[1]);
+ printf("\n\t force=(%g %g) ",
+ initialGuess_and_solution[currentAtomicIndex],
+ initialGuess_and_solution[currentAtomicIndex+1]);
+ currentAtomicIndex+=2;
+ }
+ break;
+ case MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT:
+ {
+ printf("UNILATERAL_3D_FRICTIONAL_SUTURING");
+ printf("\n\t with initial violation b=(%g %g %g) ",
+ b[currentAtomicIndex],
+ b[currentAtomicIndex+1],
+ b[currentAtomicIndex+2]);
+ double violation[3] = {b[currentAtomicIndex],b[currentAtomicIndex+1],b[currentAtomicIndex+2]};
+ for (int j=0 ; j<n ; j++)
+ {
+ violation[0] += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ violation[1] += A(currentAtomicIndex+1, j) * initialGuess_and_solution[j];
+ violation[2] += A(currentAtomicIndex+2, j) * initialGuess_and_solution[j];
+ }
+ printf("\n\t with final violation b-Ax=(%g %g %g) ",violation[0],violation[1],violation[2]);
+ printf("\n\t force=(%g %g %g) ",
+ initialGuess_and_solution[currentAtomicIndex],
+ initialGuess_and_solution[currentAtomicIndex+1],
+ initialGuess_and_solution[currentAtomicIndex+2]);
+ currentAtomicIndex+=3;
+ }
+ break;
+ default:
+ break;
+ }
+ printf("\n");
+ }
+ printf("convergence_criteria=%g Signorini verified=%s\n",
+ convergence_criteria, (signorini_verified ? "yes" : "NO"));
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/MlcpGaussSeidelSolver.h b/SurgSim/Math/MlcpGaussSeidelSolver.h
new file mode 100644
index 0000000..b1b12a9
--- /dev/null
+++ b/SurgSim/Math/MlcpGaussSeidelSolver.h
@@ -0,0 +1,194 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_MLCPGAUSSSEIDELSOLVER_H
+#define SURGSIM_MATH_MLCPGAUSSSEIDELSOLVER_H
+
+#include "SurgSim/Math/MlcpSolver.h"
+#include <Eigen/Core>
+#include "SurgSim/Math/MlcpProblem.h"
+#include "SurgSim/Math/MlcpSolution.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// A solver for mixed LCP problems using the Gauss-Seidel iterative method.
+///
+/// \todo Clean this up more...
+///
+/// The problem can contain:
+/// - CONSTRAINT = Bilateral constraint (all atomic, a fixed 3D point=3 atomics independents constraints)
+/// - CONTACT = Unilateral constraint
+/// * frictionless => 1 atomic constraint per contact
+/// * frictional with Coulomb friction (1 mu parameter per contact) => 3 atomic dependent constraints per contact
+/// (1 directional + 2 tangentials)
+/// - SUTURING = Sliding constraint for suturing
+/// * Frictionless suturing constraint => 2 atomic constraints per sliding point
+/// * Frictional suturing constraint => 3 atomic constraints per sliding point (2 directional + 1 tangential with
+/// friction on it) => 1 mu parameter per frictional suturing
+///
+/// See e.g.: Duriez, Christian; Dubois, F.; Kheddar, A.; Andriot, C., "Realistic haptic rendering of interacting
+/// deformable objects in virtual environments," <i>IEEE Transactions on Visualization and Computer Graphics,</i>
+/// vol.12, no.1, pp.36,47, Jan.-Feb. 2006.
+class MlcpGaussSeidelSolver : public MlcpSolver
+{
+public:
+ MlcpGaussSeidelSolver() :
+ m_epsilonConvergence(defaultEpsilonConvergence()),
+ m_contactTolerance(defaultContactTolerance()),
+ m_substepRatio(1.0),
+ m_maxIterations(defaultMaxIterations()),
+ m_catchExplodingConvergenceCriteria(true),
+ m_verbose(false),
+ m_numEnforcedAtomicConstraints(-1)
+ {
+ }
+
+ MlcpGaussSeidelSolver(double epsilonConvergence, double contactTolerance, unsigned int maxIterations) :
+ m_epsilonConvergence(epsilonConvergence),
+ m_contactTolerance(contactTolerance),
+ m_substepRatio(1.0),
+ m_maxIterations(maxIterations),
+ m_catchExplodingConvergenceCriteria(true),
+ m_verbose(false),
+ m_numEnforcedAtomicConstraints(-1)
+ {
+ }
+
+ virtual ~MlcpGaussSeidelSolver()
+ {
+ }
+
+
+ bool solve(const MlcpProblem& problem, MlcpSolution* solution);
+
+
+ double getEpsilonConvergence() const
+ {
+ return m_epsilonConvergence;
+ }
+ void setEpsilonConvergence(double val)
+ {
+ m_epsilonConvergence = val;
+ }
+ double getContactTolerance() const
+ {
+ return m_contactTolerance;
+ }
+ void setContactTolerance(double val)
+ {
+ m_contactTolerance = val;
+ }
+ double getSubstepRatio() const
+ {
+ return m_substepRatio;
+ }
+ void setSubstepRatio(double val)
+ {
+ m_substepRatio = val;
+ }
+ unsigned int getMaxIterations() const
+ {
+ return m_maxIterations;
+ }
+ void setMaxIterations(unsigned int val)
+ {
+ m_maxIterations = val;
+ }
+ bool isCatchingExplodingConvergenceCriteria() const
+ {
+ return m_catchExplodingConvergenceCriteria;
+ }
+ void setCatchingExplodingConvergenceCriteria(bool val)
+ {
+ m_catchExplodingConvergenceCriteria = val;
+ }
+ bool isVerbose() const
+ {
+ return m_verbose;
+ }
+ void setVerbose(bool val)
+ {
+ m_verbose = val;
+ }
+
+
+ static double defaultEpsilonConvergence()
+ {
+ return 1e-4;
+ }
+ static double defaultContactTolerance()
+ {
+ return 2e-5;
+ }
+ static int defaultMaxIterations()
+ {
+ return 30;
+ }
+
+private:
+ void computeEnforcementSystem(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
+ const MlcpProblem::Vector& b,
+ const MlcpSolution::Vector& initialGuess_and_solution,
+ const MlcpProblem::Vector& frictionCoefs,
+ const std::vector<MlcpConstraintType>& constraintsType, double subStep,
+ int constraintID, int matrixEntryForConstraintID);
+
+ void calculateConvergenceCriteria(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
+ const MlcpProblem::Vector& b,
+ const MlcpSolution::Vector& initialGuess_and_solution,
+ const std::vector<MlcpConstraintType>& constraintsType,
+ double subStep,
+ double constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES],
+ double& convergence_criteria,
+ bool& signoriniVerified, bool& signoriniValid);
+
+ void doOneIteration(int n, const MlcpProblem::Matrix& A, int nbColumnInA, const MlcpProblem::Vector& b,
+ MlcpSolution::Vector* initialGuess_and_solution,
+ const MlcpProblem::Vector& frictionCoefs,
+ const std::vector<MlcpConstraintType>& constraintsType, double subStep,
+ double constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES], double& convergence_criteria,
+ bool& signoriniVerified);
+
+ void printViolationsAndConvergence(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
+ const MlcpProblem::Vector& b,
+ const MlcpSolution::Vector& initialGuess_and_solution,
+ const std::vector<MlcpConstraintType>& constraintsType,
+ double subStep, double convergence_criteria,
+ bool signorini_verified, int nbLoop);
+
+
+ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
+ typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
+
+
+ double m_epsilonConvergence;
+ double m_contactTolerance;
+ double m_substepRatio;
+ unsigned int m_maxIterations;
+ bool m_catchExplodingConvergenceCriteria;
+ bool m_verbose;
+
+ int m_numEnforcedAtomicConstraints;
+ Matrix m_lhsEnforcedLocalSystem;
+ Vector m_rhsEnforcedLocalSystem;
+};
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_MLCPGAUSSSEIDELSOLVER_H
diff --git a/SurgSim/Math/MlcpProblem.cpp b/SurgSim/Math/MlcpProblem.cpp
new file mode 100644
index 0000000..5549169
--- /dev/null
+++ b/SurgSim/Math/MlcpProblem.cpp
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/MlcpProblem.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+MlcpProblem::~MlcpProblem()
+{
+}
+
+void MlcpProblem::setZero(size_t numDof, size_t numConstraintDof, size_t numConstraints)
+{
+ A.resize(numConstraintDof, numConstraintDof);
+ A.setZero();
+ b.resize(numConstraintDof);
+ b.setZero();
+ mu.resize(numConstraints);
+ mu.setZero();
+
+ constraintTypes.clear();
+}
+
+MlcpProblem MlcpProblem::Zero(size_t numDof, size_t numConstraintDof, size_t numConstraints)
+{
+ MlcpProblem result;
+ result.setZero(numDof, numConstraintDof, numConstraints);
+
+ return result;
+
+}
+
+} // namespace SurgSim
+} // namespace Math
diff --git a/SurgSim/Math/MlcpProblem.h b/SurgSim/Math/MlcpProblem.h
new file mode 100644
index 0000000..29d9ad0
--- /dev/null
+++ b/SurgSim/Math/MlcpProblem.h
@@ -0,0 +1,119 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_MLCPPROBLEM_H
+#define SURGSIM_MATH_MLCPPROBLEM_H
+
+#include <vector>
+#include <Eigen/Core>
+#include "SurgSim/Math/MlcpConstraintType.h"
+
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// A description of an MLCP (mixed linear complementarity problem, or mixed LCP) system to be solved.
+///
+/// A traditional (not mixed!) LCP problem is expressed as \f$\mathbf{A}x + b = c\f$, where \f$x\f$ is the vector of
+/// variables of interest to be determined, \f$c\f$ is the vector of slack variables (transforming a system of
+/// inequalities into a system of equalities), and \f$x\f$ and \f$c\f$ are subject to the inequality conditions
+/// \f$x_i \ge 0\f$, \f$c_i \ge 0\f$, and \f$x \perp c\f$ (i.e., \f$x \cdot c = 0\f$).
+/// Thus for each row \f$i\f$, either
+/// * \f$(\mathbf{A}x+b)_i = \mathbf{A}_{i,*}\cdot x+b_i = c_i \ge 0\f$ <b>and</b> \f$x_i = 0\f$
+/// (the constraint is non-binding, so the variable of interest enforcing the constraint is zero);
+/// or
+/// * \f$(\mathbf{A}x+b)_i = \mathbf{A}_{i,*}\cdot x+b_i = c_i = 0\f$ <b>and</b> \f$x_i > 0\f$
+/// (the constraint is binding, so the variable of interest is nonzero to enforce the constraint).
+///
+/// Solving the problem produces the vector \f$x\f$, from which \f$c\f$ can also be computed if needed.
+///
+/// A mixed LCP problem is defined in the same way, except that for a certain subset of indices, the conditions are
+/// restricted further to require \f$c_i\f$ to be zero, \f$\mathbf{A}_{i,*}\cdot x+b_i = 0\f$. These are referred to as
+/// <i>bilateral</i> constraints, as opposed to the <i>unilateral</i> constraints in the LCP problem.
+///
+/// Friction is integrated directly into the problem, using the general approach described e.g. in:<br/>
+/// Duriez, Christian; Dubois, F.; Kheddar, A.; Andriot, C., "Realistic haptic rendering of interacting
+/// deformable objects in virtual environments," <i>IEEE Transactions on Visualization and Computer %Graphics,</i>
+/// vol.12, no.1, pp.36,47, Jan.-Feb. 2006.
+///
+/// \sa SurgSim::Physics::MlcpPhysicsProblem, MlcpSolution, MlcpSolver
+//
+// TODO(advornik): Describe the approach to friction in more detail.
+// TODO(advornik): Get rid of the constraint types and encode necessary info in other ways.
+struct MlcpProblem
+{
+ /// Destructor
+ virtual ~MlcpProblem();
+
+ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
+ typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
+
+ /// Matrix \f$\mathbf{A}\f$ used to describe the mixed LCP problem.
+ Matrix A;
+ /// Vector \f$b\f$ used to describe the mixed LCP problem.
+ Vector b;
+ /// A vector of friction coefficients used to describe the mixed LCP problem.
+ /// \todo This API will change in the future to something more independent of physics.
+ Vector mu;
+ /// A vector of constraint types used to describe the mixed LCP problem.
+ /// \todo This API will change in the future to something more independent of physics.
+ std::vector<MlcpConstraintType> constraintTypes;
+
+ // NB: We let the compiler generate the default code for the constructor, copy constructor and copy assignment,
+ // because we currently sometimes need to copy the problem (although we ought to minimize this).
+ // The C++11-ish way to indicate that explicitly would be to write code like this:
+ // MlcpProblem() = default;
+ // MlcpProblem(const MlcpProblem& other) = default;
+ // MlcpProblem& operator= (const MlcpProblem& other) = default;
+ // but I haven't yet tested that this works correctly on VS 2010, so I'm just putting in the comment.
+ // We may also want to add move construction and move assignment. --advornik 2013-06-24
+
+ /// Gets the size of the system.
+ /// \return the number of degrees of freedom of the system.
+ size_t getSize() const
+ {
+ return (b.rows() >= 0) ? static_cast<size_t>(b.rows()) : 0;
+ }
+
+ /// Checks if the sizes of various elements of the system are consistent with each other.
+ /// \return true if consistent, false otherwise.
+ bool isConsistent() const
+ {
+ size_t numConstraintTypes = constraintTypes.size();
+ return ((b.rows() >= 0) && (b.cols() == 1) && (A.rows() == b.rows()) && (A.cols() == A.rows())
+ && (numConstraintTypes <= static_cast<size_t>(b.rows())) && (mu.size() >= 0)
+ && (static_cast<size_t>(mu.size()) == numConstraintTypes));
+ }
+
+ /// Resize an MlcpProblem and set to zero.
+ /// \param numDof the total degrees of freedom.
+ /// \param numConstraintDof the total constrained degrees of freedom.
+ /// \param numConstraints the number of constraints.
+ virtual void setZero(size_t numDof, size_t numConstraintDof, size_t numConstraints);
+
+ /// Initialize an MlcpProblem with zero values.
+ /// \param numDof the total degrees of freedom for the MlcpProblem to be constructed.
+ /// \param numConstraintDof the total constrained degrees of freedom for the MlcpProblem to be constructed.
+ /// \param numConstraints the number of constraints for the MlcpProblem to be constructed.
+ /// \return An MlcpProblem appropriately sized and initialized to zero.
+ static MlcpProblem Zero(size_t numDof, size_t numConstraintDof, size_t numConstraints);
+};
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_MLCPPROBLEM_H
diff --git a/SurgSim/Math/MlcpSolution.h b/SurgSim/Math/MlcpSolution.h
new file mode 100644
index 0000000..a037325
--- /dev/null
+++ b/SurgSim/Math/MlcpSolution.h
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_MLCPSOLUTION_H
+#define SURGSIM_MATH_MLCPSOLUTION_H
+
+#include <Eigen/Core>
+#include "SurgSim/Math/MlcpConstraintType.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// The description of a solution to a \ref MlcpProblem "mixed linear complementarity problem".
+///
+/// The solution consists of the vector \f$x\f$ and various diagnostic parameters.
+/// If \f$c = \mathbf{A}x + b\f$ is also needed, it can be computed by the caller.
+///
+/// \sa SurgSim::Physics::MlcpPhysicsSolution, MlcpProblem, MlcpSolver
+struct MlcpSolution
+{
+ typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
+
+ /// Vector \f$x\f$ specifying a solution to the specified mixed LCP problem.
+ Vector x;
+
+ /// The number of iterations performed.
+ int numIterations;
+ /// True if the final value of the convergence criteria is valid.
+ bool validConvergence;
+ /// True if the final solution satisfies the Signorini conditions.
+ bool validSignorini;
+ /// The final value of the convergence criteria.
+ double convergenceCriteria;
+ /// The initial value of the convergence criteria, before the solver has done anything.
+ double initialConvergenceCriteria;
+ /// The final value of the convergence criteria for each of the constraint types.
+ double constraintConvergenceCriteria[MLCP_NUM_CONSTRAINT_TYPES];
+ /// The initial value of the convergence criteria for each of the constraint types.
+ double initialConstraintConvergenceCriteria[MLCP_NUM_CONSTRAINT_TYPES];
+
+ // NB: We let the compiler generate the default code for the constructor, copy constructor and copy assignment,
+ // because we currently sometimes need to copy the solution (although we ought to minimize this).
+ // The C++11-ish way to indicate that explicitly would be to write code like this:
+ // MlcpProblem() = default;
+ // MlcpProblem(const MlcpProblem& other) = default;
+ // MlcpProblem& operator= (const MlcpProblem& other) = default;
+ // but I haven't yet tested that this works correctly on VS 2010, so I'm just putting in the comment.
+ // We may also want to add move construction and move assignment. --advornik 2013-06-24
+};
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_MLCPSOLUTION_H
diff --git a/SurgSim/Math/MlcpSolver.h b/SurgSim/Math/MlcpSolver.h
new file mode 100644
index 0000000..c3a2ec9
--- /dev/null
+++ b/SurgSim/Math/MlcpSolver.h
@@ -0,0 +1,58 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_MLCPSOLVER_H
+#define SURGSIM_MATH_MLCPSOLVER_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+struct MlcpProblem;
+struct MlcpSolution;
+
+/// This class provides a solver interface for mixed linear complementarity problems.
+///
+/// \sa MlcpProblem
+class MlcpSolver
+{
+public:
+ /// Constructor.
+ MlcpSolver()
+ {
+ }
+
+ // Destructor.
+ virtual ~MlcpSolver()
+ {
+ }
+
+ /// Attempts to solve the specified MLCP problem.
+ /// \param problem the MLCP problem.
+ /// \param [out] solution the solution to the problem, if available.
+ /// \return true if solved (in which case solution will be set to the solution); false if failed.
+ virtual bool solve(const MlcpProblem& problem, MlcpSolution* solution) = 0;
+
+private:
+ /// Prevent copy construction and assignment.
+ MlcpSolver(const MlcpSolver&);
+ MlcpSolver& operator==(const MlcpSolver&);
+};
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_MLCPSOLVER_H
diff --git a/SurgSim/Math/OctreeShape-inl.h b/SurgSim/Math/OctreeShape-inl.h
new file mode 100644
index 0000000..c0dcd05
--- /dev/null
+++ b/SurgSim/Math/OctreeShape-inl.h
@@ -0,0 +1,34 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+
+#ifndef SURGSIM_MATH_OCTREESHAPE_INL_H
+#define SURGSIM_MATH_OCTREESHAPE_INL_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template<class T>
+OctreeShape::OctreeShape(const SurgSim::DataStructures::OctreeNode<T>& node) :
+ m_rootNode(std::make_shared<OctreeShape::NodeType>(node))
+{
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_OCTREESHAPE_INL_H
diff --git a/SurgSim/Math/OctreeShape.cpp b/SurgSim/Math/OctreeShape.cpp
new file mode 100644
index 0000000..80c1e2e
--- /dev/null
+++ b/SurgSim/Math/OctreeShape.cpp
@@ -0,0 +1,87 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OctreeShape.h"
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::OctreeShape, OctreeShape);
+
+OctreeShape::OctreeShape()
+{
+ serializeFileName(this);
+}
+
+OctreeShape::~OctreeShape()
+{
+}
+
+int OctreeShape::getType()
+{
+ return SHAPE_TYPE_OCTREE;
+}
+
+bool OctreeShape::doLoad(const std::string& filePath)
+{
+ m_rootNode = std::make_shared<NodeType>();
+ SURGSIM_ASSERT(m_rootNode->doLoad(filePath)) << "Failed to load file" << filePath;
+ SURGSIM_ASSERT(isValid()) << filePath << " contains an invalid octree.";
+ return true;
+}
+
+double OctreeShape::getVolume() const
+{
+ SURGSIM_FAILURE() << "OctreeShape::getVolume not implemented";
+ return 0.0;
+}
+
+Vector3d OctreeShape::getCenter() const
+{
+ return Vector3d::Zero();
+}
+
+Matrix33d OctreeShape::getSecondMomentOfVolume() const
+{
+ SURGSIM_FAILURE() << "OctreeShape::getSecondMomentOfVolume not implemented";
+ return Matrix33d::Zero();
+}
+
+std::shared_ptr<OctreeShape::NodeType> OctreeShape::getRootNode()
+{
+ return m_rootNode;
+}
+
+const std::shared_ptr<const OctreeShape::NodeType> OctreeShape::getRootNode() const
+{
+ return m_rootNode;
+}
+
+void OctreeShape::setRootNode(std::shared_ptr<OctreeShape::NodeType> node)
+{
+ m_rootNode = node;
+}
+
+bool OctreeShape::isValid() const
+{
+ return (nullptr != m_rootNode) && (m_rootNode->getBoundingBox().sizes().minCoeff() >= 0);
+}
+
+}; // namespace Math
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Math/OctreeShape.h b/SurgSim/Math/OctreeShape.h
new file mode 100644
index 0000000..c41163b
--- /dev/null
+++ b/SurgSim/Math/OctreeShape.h
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_OCTREESHAPE_H
+#define SURGSIM_MATH_OCTREESHAPE_H
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/OctreeNode.h"
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_STATIC_REGISTRATION(OctreeShape);
+
+/// Octree Shape
+/// A defined by an octree data structure
+class OctreeShape : public Shape, public SurgSim::Framework::Asset
+{
+public:
+ typedef SurgSim::DataStructures::OctreeNode<SurgSim::DataStructures::EmptyData> NodeType;
+
+ /// Constructor
+ OctreeShape();
+
+ SURGSIM_CLASSNAME(SurgSim::Math::OctreeShape);
+
+ /// Construct an OctreeShape by copying data from an OctreeNode
+ /// NOTE: The Data stored in the octree node will not be copied into the
+ /// OctreeShape.
+ /// \tparam T octree node data structure to build Octree Shape from
+ /// \param node octree node data structure to build Octree Shape from
+ template<class T>
+ explicit OctreeShape(const SurgSim::DataStructures::OctreeNode<T>& node);
+
+ /// Destructor
+ virtual ~OctreeShape();
+
+ /// \return the type of shape
+ virtual int getType() override;
+
+ /// Get the volume of the shape
+ /// \return The volume of the shape (in m-3)
+ virtual double getVolume() const override;
+
+ /// Get the volumetric center of the shape
+ /// \return The center of the shape
+ virtual Vector3d getCenter() const override;
+
+ /// Get the second central moment of the volume, commonly used
+ /// to calculate the moment of inertia matrix
+ /// \return The 3x3 symmetric second moment matrix
+ virtual Matrix33d getSecondMomentOfVolume() const override;
+
+ /// Get the root node
+ /// \return the octree root node of this shape
+ std::shared_ptr<NodeType> getRootNode();
+
+ /// const version to get the root node
+ /// \return A const reference of the shared pointer, which points to the octree root node of this shape.
+ const std::shared_ptr<const NodeType> getRootNode() const;
+
+ /// Set the root node
+ /// \param node the octree root node of this shape
+ void setRootNode(std::shared_ptr<NodeType> node);
+
+ /// \return True if the bounding box is bigger than or equal to 0; Otherwise, false.
+ virtual bool isValid() const override;
+
+ virtual bool doLoad(const std::string& filePath) override;
+
+private:
+ /// Root node of the octree datastructure
+ std::shared_ptr<NodeType> m_rootNode;
+};
+
+}; // Math
+}; // SurgSim
+
+#include "SurgSim/Math/OctreeShape-inl.h"
+
+#endif // SURGSIM_MATH_OCTREESHAPE_H
\ No newline at end of file
diff --git a/SurgSim/Math/OdeEquation.cpp b/SurgSim/Math/OdeEquation.cpp
new file mode 100644
index 0000000..8dd8a8e
--- /dev/null
+++ b/SurgSim/Math/OdeEquation.cpp
@@ -0,0 +1,32 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeEquation.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+const std::shared_ptr<OdeState> OdeEquation::getInitialState() const
+{
+ return m_initialState;
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeEquation.h b/SurgSim/Math/OdeEquation.h
new file mode 100644
index 0000000..9dab154
--- /dev/null
+++ b/SurgSim/Math/OdeEquation.h
@@ -0,0 +1,95 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODEEQUATION_H
+#define SURGSIM_MATH_ODEEQUATION_H
+
+#include <memory>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+class OdeState;
+
+/// Ode equation of 2nd order of the form M(x,v).a = F(x, v) with (x0, v0) for initial conditions
+/// and a set of boundary conditions. The problem is called a Boundary Value Problem (BVP).
+/// \note This ode equation is solved as an ode of order 1 by defining the state vector y = (x v)^t:
+/// \note y' = ( x' ) = ( dx/dt ) = ( v )
+/// \note ( v' ) = ( dv/dt ) = ( M(x, v)^{-1}.F(x, v) )
+/// \note To allow the use of explicit and implicit solver, we need to be able to evaluate
+/// \note M(x,v), F(x,v) but also K = -dF/dx(x,v), D = -dF/dv(x,v)
+/// \note Models wanting the use of implicit solvers will need to compute these Jacobian matrices.
+class OdeEquation
+{
+public:
+ /// Virtual destructor
+ virtual ~OdeEquation(){}
+
+ /// Retrieves the ode initial conditions (x0, v0) (i.e. the initial state)
+ /// \return The initial state
+ const std::shared_ptr<OdeState> getInitialState() const;
+
+ /// Evaluation of the RHS function f(x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the function f(x,v) with
+ /// \return The vector containing f(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeF() or computeFMDK()
+ virtual Vector& computeF(const OdeState& state) = 0;
+
+ /// Evaluation of the LHS matrix M(x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the matrix M(x,v) with
+ /// \return The matrix M(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeM() or computeFMDK()
+ virtual const Matrix& computeM(const OdeState& state) = 0;
+
+ /// Evaluation of D = -df/dv (x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the Jacobian matrix with
+ /// \return The matrix D = -df/dv(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeD() or computeFMDK()
+ virtual const Matrix& computeD(const OdeState& state) = 0;
+
+ /// Evaluation of K = -df/dx (x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the Jacobian matrix with
+ /// \return The matrix K = -df/dx(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeK() or computeFMDK()
+ virtual const Matrix& computeK(const OdeState& state) = 0;
+
+ /// Evaluation of f(x,v), M(x,v), D = -df/dv(x,v), K = -df/dx(x,v)
+ /// When all the terms are needed, this method can perform optimization in evaluating everything together
+ /// \param state (x, v) the current position and velocity to evaluate the various terms with
+ /// \param[out] f The RHS f(x,v)
+ /// \param[out] M The matrix M(x,v)
+ /// \param[out] D The matrix D = -df/dv(x,v)
+ /// \param[out] K The matrix K = -df/dx(x,v)
+ /// \note Returns pointers, the internal data will remain unchanged until the next call to computeFMDK() or
+ /// \note computeF(), computeM(), computeD(), computeK()
+ virtual void computeFMDK(const OdeState& state, Vector** f, Matrix** M, Matrix** D, Matrix** K) = 0;
+
+protected:
+ /// The initial state (which defines the ODE initial conditions (x0, v0))
+ /// \note MUST be set by the derived classes
+ std::shared_ptr<OdeState> m_initialState;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODEEQUATION_H
diff --git a/SurgSim/Math/OdeSolver.cpp b/SurgSim/Math/OdeSolver.cpp
new file mode 100644
index 0000000..e9c7343
--- /dev/null
+++ b/SurgSim/Math/OdeSolver.cpp
@@ -0,0 +1,66 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolver.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolver::OdeSolver(OdeEquation* equation) : m_equation(*equation)
+{
+ allocate(m_equation.getInitialState()->getPositions().size());
+
+ // Default linear solver
+ setLinearSolver(std::make_shared<LinearSolveAndInverseDenseMatrix>());
+}
+
+const std::string OdeSolver::getName() const
+{
+ return m_name;
+}
+
+void OdeSolver::setLinearSolver(std::shared_ptr<LinearSolveAndInverse> linearSolver)
+{
+ m_linearSolver = linearSolver;
+}
+
+std::shared_ptr<LinearSolveAndInverse> OdeSolver::getLinearSolver() const
+{
+ return m_linearSolver;
+}
+
+const Matrix& OdeSolver::getSystemMatrix() const
+{
+ return m_systemMatrix;
+}
+
+const Matrix& OdeSolver::getCompliance() const
+{
+ return m_compliance;
+}
+
+void OdeSolver::allocate(size_t size)
+{
+ m_systemMatrix.resize(size, size);
+ m_compliance.resize(size, size);
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolver.h b/SurgSim/Math/OdeSolver.h
new file mode 100644
index 0000000..448481f
--- /dev/null
+++ b/SurgSim/Math/OdeSolver.h
@@ -0,0 +1,139 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVER_H
+#define SURGSIM_MATH_ODESOLVER_H
+
+#include <functional>
+#include <unordered_map>
+
+#include <boost/assign/list_of.hpp> // for 'map_list_of()'
+
+#include "SurgSim/Math/LinearSolveAndInverse.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeEquation.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// The diverse numerical integration scheme supported
+/// Each Ode Solver should have its own entry in this enum
+enum IntegrationScheme {
+ INTEGRATIONSCHEME_STATIC = 0,
+ INTEGRATIONSCHEME_LINEAR_STATIC,
+ INTEGRATIONSCHEME_EXPLICIT_EULER,
+ INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER,
+ INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER,
+ INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER,
+ INTEGRATIONSCHEME_IMPLICIT_EULER,
+ INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER,
+ INTEGRATIONSCHEME_RUNGE_KUTTA_4,
+ INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4
+};
+
+const std::unordered_map<IntegrationScheme, std::string, std::hash<int>> IntegrationSchemeNames =
+ boost::assign::map_list_of
+ (INTEGRATIONSCHEME_STATIC, "INTEGRATIONSCHEME_STATIC")
+ (INTEGRATIONSCHEME_LINEAR_STATIC, "INTEGRATIONSCHEME_LINEAR_STATIC")
+ (INTEGRATIONSCHEME_EXPLICIT_EULER, "INTEGRATIONSCHEME_EXPLICIT_EULER")
+ (INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER, "INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER")
+ (INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER, "INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER")
+ (INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER, "INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER")
+ (INTEGRATIONSCHEME_IMPLICIT_EULER, "INTEGRATIONSCHEME_IMPLICIT_EULER")
+ (INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER, "INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER")
+ (INTEGRATIONSCHEME_RUNGE_KUTTA_4, "INTEGRATIONSCHEME_RUNGE_KUTTA_4")
+ (INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4, "INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4");
+
+/// Base class for all solvers of ode equation of order 2 of the form M(x(t), v(t)).a(t) = f(t, x(t), v(t))
+/// \note This ode equation is solved as an ode of order 1 by defining the state vector y = (x v)^t:
+/// \note y' = ( x' ) = ( dx/dt ) = ( v )
+/// \note ( v' ) = ( dv/dt ) = ( M(x, v)^{-1}.f(x, v) )
+/// \note To allow the use of explicit and implicit solver, we need to be able to evaluate
+/// \note M(x(t), v(t))
+/// \note f(t, x(t), v(t)) but also
+/// \note K = -df/dx(x(t), v(t))
+/// \note D = -df/dv(x(t), v(t))
+/// \note Models wanting the use of implicit solvers will need to compute these Jacobian matrices.
+/// \note Matrices all have dense storage, but a specialized linear solver can be set per solver.
+class OdeSolver
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolver(OdeEquation* equation);
+
+ /// Virtual destructor
+ virtual ~OdeSolver()
+ {}
+
+ /// Gets the solver's name
+ /// \return The solver name
+ const std::string getName() const;
+
+ /// Sets the specialized linear solver to use with this Ode solver
+ /// \param linearSolver the linear solver to use when solving the ode equation
+ void setLinearSolver(std::shared_ptr<LinearSolveAndInverse> linearSolver);
+
+ /// Gets the specialized linear solver used with this Ode solver
+ /// \return The linear solver used when solving the ode equation
+ std::shared_ptr<LinearSolveAndInverse> getLinearSolver() const;
+
+ /// Solves the equation
+ /// \param dt The time step
+ /// \param currentState State at time t
+ /// \param[out] newState State at time t+dt
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) = 0;
+
+ /// Queries the current system matrix
+ /// \return The latest system matrix calculated
+ const Matrix& getSystemMatrix() const;
+
+ /// Queries the current compliance matrix
+ /// \return The latest compliance matrix calculated
+ const Matrix& getCompliance() const;
+
+protected:
+ /// Allocates the system and compliance matrices
+ /// \param size The size to account for in the data structure
+ void allocate(size_t size);
+
+ /// Name for this solver
+ /// \note MUST be set by the derived classes
+ std::string m_name;
+
+ /// The ode equation (API providing the necessary evaluation methods and the initial state)
+ OdeEquation& m_equation;
+
+ /// The specialized linear solver to use when solving the ode equation
+ std::shared_ptr<LinearSolveAndInverse> m_linearSolver;
+
+ /// System matrix (can be M, K, combination of MDK depending on the solver)
+ /// A static solver will have K for system matrix
+ /// A dynamic explicit solver will have M for system matrix
+ /// A dynamic implicit solver will have a combination of M, D and K for system matrix
+ Matrix m_systemMatrix;
+
+ /// Compliance matrix which is the inverse of the system matrix
+ Matrix m_compliance;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVER_H
diff --git a/SurgSim/Math/OdeSolverEulerExplicit.cpp b/SurgSim/Math/OdeSolverEulerExplicit.cpp
new file mode 100644
index 0000000..9d1dd59
--- /dev/null
+++ b/SurgSim/Math/OdeSolverEulerExplicit.cpp
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolverEulerExplicit.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolverEulerExplicit::OdeSolverEulerExplicit(OdeEquation* equation)
+ : OdeSolver(equation)
+{
+ m_name = "Ode Solver Euler Explicit";
+}
+
+void OdeSolverEulerExplicit::solve(double dt, const OdeState& currentState, OdeState* newState)
+{
+ // General equation to solve:
+ // M.a(t) = f(t, x(t), v(t))
+ // System on the velocity level:
+ // (M/dt).deltaV = f(t, x(t), v(t))
+
+ // Computes f(t, x(t), v(t)) and M
+ Vector& f = m_equation.computeF(currentState);
+ const Matrix& M = m_equation.computeM(currentState);
+
+ // Computes the system matrix (left-hand-side matrix)
+ m_systemMatrix = M / dt;
+
+ // Apply boundary conditions to the linear system
+ currentState.applyBoundaryConditionsToVector(&f);
+ currentState.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+
+ // Computes deltaV (stored in the velocities) and m_compliance = 1/m_systemMatrix
+ Vector& deltaV = newState->getVelocities();
+ (*m_linearSolver)(m_systemMatrix, f, &deltaV, &m_compliance);
+
+ // Remove the boundary conditions compliance from the compliance matrix
+ // This helps to prevent potential exterior LCP type calculation to violates the boundary conditions
+ currentState.applyBoundaryConditionsToMatrix(&m_compliance, false);
+
+ // Compute the new state using the Euler Explicit scheme:
+ newState->getPositions() = currentState.getPositions() + dt * currentState.getVelocities();
+ newState->getVelocities() = currentState.getVelocities() + deltaV;
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolverEulerExplicit.h b/SurgSim/Math/OdeSolverEulerExplicit.h
new file mode 100644
index 0000000..a61614f
--- /dev/null
+++ b/SurgSim/Math/OdeSolverEulerExplicit.h
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVEREULEREXPLICIT_H
+#define SURGSIM_MATH_ODESOLVEREULEREXPLICIT_H
+
+#include "SurgSim/Math/OdeSolver.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Euler Explicit ode solver
+/// \note M(x(t), v(t)).a(t) = f(t, x(t), v(t))
+/// \note This ode equation is solved as an ode of order 1 by defining the state vector y = (x v)^t:
+/// \note y' = ( x' ) = ( dx/dt ) = ( v )
+/// \note ( v' ) = ( dv/dt ) = ( M(x, v)^{-1}.f(x, v) )
+/// \note y' = f(t, y)
+/// \note Euler Explicit is also called forward Euler as it solves this integral using a forward evaluation:
+/// \note y' = (y(t+dt) - y(t)) / dt
+/// \note which leads to the integration scheme:
+/// \note { x(t+dt) = x(t) + dt.v(t)
+/// \note { v(t+dt) = v(t) + dt.a(t)
+class OdeSolverEulerExplicit : public OdeSolver
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolverEulerExplicit(OdeEquation* equation);
+
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVEREULEREXPLICIT_H
diff --git a/SurgSim/Math/OdeSolverEulerExplicitModified.cpp b/SurgSim/Math/OdeSolverEulerExplicitModified.cpp
new file mode 100644
index 0000000..473a7fe
--- /dev/null
+++ b/SurgSim/Math/OdeSolverEulerExplicitModified.cpp
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolverEulerExplicitModified.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolverEulerExplicitModified::OdeSolverEulerExplicitModified(OdeEquation* equation)
+ : OdeSolver(equation)
+{
+ m_name = "Ode Solver Euler Explicit Modified";
+}
+
+void OdeSolverEulerExplicitModified::solve(double dt, const OdeState& currentState, OdeState* newState)
+{
+ // General equation to solve:
+ // M.a(t) = f(t, x(t), v(t))
+ // System on the velocity level:
+ // (M/dt).deltaV = f(t, x(t), v(t))
+
+ // Computes f(t, x(t), v(t)) and M
+ Vector& f = m_equation.computeF(currentState);
+ const Matrix& M = m_equation.computeM(currentState);
+
+ // Computes the system matrix (left-hand-side matrix)
+ m_systemMatrix = M / dt;
+
+ // Apply boundary conditions to the linear system
+ currentState.applyBoundaryConditionsToVector(&f);
+ currentState.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+
+ // Computes deltaV (stored in the velocities) and m_compliance = 1/m_systemMatrix
+ Vector& deltaV = newState->getVelocities();
+ (*m_linearSolver)(m_systemMatrix, f, &deltaV, &m_compliance);
+
+ // Remove the boundary conditions compliance from the compliance matrix
+ // This helps to prevent potential exterior LCP type calculation to violates the boundary conditions
+ currentState.applyBoundaryConditionsToMatrix(&m_compliance, false);
+
+ // Compute the new state using the Modified Euler Explicit scheme:
+ newState->getVelocities() = currentState.getVelocities() + deltaV;
+ newState->getPositions() = currentState.getPositions() + dt * newState->getVelocities();
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolverEulerExplicitModified.h b/SurgSim/Math/OdeSolverEulerExplicitModified.h
new file mode 100644
index 0000000..348d494
--- /dev/null
+++ b/SurgSim/Math/OdeSolverEulerExplicitModified.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVEREULEREXPLICITMODIFIED_H
+#define SURGSIM_MATH_ODESOLVEREULEREXPLICITMODIFIED_H
+
+#include "SurgSim/Math/OdeSolver.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Euler Explicit Modified ode solver
+/// \note M(x(t), v(t)).a(t) = f(t, x(t), v(t))
+/// \note This ode equation is solved as an ode of order 1 by defining the state vector y = (x v)^t:
+/// \note y' = ( x' ) = ( dx/dt ) = ( v )
+/// \note ( v' ) = ( dv/dt ) = ( M(x, v)^{-1}.f(x, v) )
+/// \note By simply using the newly computed velocity in the position update, the method gains in stability:
+/// \note { x(t+dt) = x(t) + dt.v(t+dt)
+/// \note { v(t+dt) = v(t) + dt.a(t)
+class OdeSolverEulerExplicitModified : public OdeSolver
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolverEulerExplicitModified(OdeEquation* equation);
+
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVEREULEREXPLICITMODIFIED_H
diff --git a/SurgSim/Math/OdeSolverEulerImplicit.cpp b/SurgSim/Math/OdeSolverEulerImplicit.cpp
new file mode 100644
index 0000000..0e0bc40
--- /dev/null
+++ b/SurgSim/Math/OdeSolverEulerImplicit.cpp
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolverEulerImplicit.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolverEulerImplicit::OdeSolverEulerImplicit(OdeEquation* equation)
+ : OdeSolver(equation)
+{
+ m_name = "Ode Solver Euler Implicit";
+}
+
+void OdeSolverEulerImplicit::solve(double dt, const OdeState& currentState, OdeState* newState)
+{
+ // General equation to solve:
+ // M.a(t+dt) = f(t+dt, x(t+dt), v(t+dt))
+ // M.a(t+dt) = f(t) + df/dx.deltaX + df/dv.deltaV
+ // Note that K = -df/dx and D = -df/dv
+ // Compliance matrix on the velocity level:
+ // (M.deltaV)/dt = f(t) - K.(dt.v(t) + dt.deltaV) - D.deltaV
+ // (M/dt + D + dt.K).deltaV = f(t) - dt.K.v(t)
+
+ // Computes f(t, x(t), v(t)), M, D, K all at the same time
+ Matrix* M;
+ Matrix* D;
+ Matrix* K;
+ Vector* f;
+ m_equation.computeFMDK(currentState, &f, &M, &D, &K);
+
+ // Adds the Euler Implicit terms on the right-hand-side
+ *f -= ((*K) * currentState.getVelocities()) * dt;
+
+ // Computes the system matrix (left-hand-side matrix)
+ m_systemMatrix = (*M) * (1.0 / dt);
+ m_systemMatrix += (*D);
+ m_systemMatrix += (*K) * dt;
+
+ // Apply boundary conditions to the linear system
+ currentState.applyBoundaryConditionsToVector(f);
+ currentState.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+
+ // Computes deltaV (stored in the velocities) and m_compliance = 1/m_systemMatrix
+ Vector& deltaV = newState->getVelocities();
+ (*m_linearSolver)(m_systemMatrix, *f, &deltaV, &m_compliance);
+
+ // Remove the boundary conditions compliance from the compliance matrix
+ // This helps to prevent potential exterior LCP type calculation to violates the boundary conditions
+ currentState.applyBoundaryConditionsToMatrix(&m_compliance, false);
+
+ // Compute the new state using the Euler Implicit scheme:
+ newState->getVelocities() = currentState.getVelocities() + deltaV;
+ newState->getPositions() = currentState.getPositions() + dt * newState->getVelocities();
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolverEulerImplicit.h b/SurgSim/Math/OdeSolverEulerImplicit.h
new file mode 100644
index 0000000..372ac90
--- /dev/null
+++ b/SurgSim/Math/OdeSolverEulerImplicit.h
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVEREULERIMPLICIT_H
+#define SURGSIM_MATH_ODESOLVEREULERIMPLICIT_H
+
+#include "SurgSim/Math/OdeSolver.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Euler Implicit ode solver
+/// \note M(x(t), v(t)).a(t) = f(t, x(t), v(t))
+/// \note This ode equation is solved as an ode of order 1 by defining the state vector y = (x v)^t:
+/// \note y' = ( x' ) = ( dx/dt ) = ( v )
+/// \note ( v' ) = ( dv/dt ) = ( M(x, v)^{-1}.f(x, v) )
+/// \note y' = f(t, y)
+/// \note Euler Implicit is also called backward Euler as it solves this integral using a backward evaluation:
+/// \note y' = (y(t) - y(t-dt)) / dt
+/// \note which leads to the integration scheme:
+/// \note { x(t+dt) = x(t) + dt.v(t+dt)
+/// \note { v(t+dt) = v(t) + dt.a(t+dt)
+class OdeSolverEulerImplicit : public OdeSolver
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolverEulerImplicit(OdeEquation* equation);
+
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVEREULERIMPLICIT_H
diff --git a/SurgSim/Math/OdeSolverLinearEulerExplicit.cpp b/SurgSim/Math/OdeSolverLinearEulerExplicit.cpp
new file mode 100644
index 0000000..5879330
--- /dev/null
+++ b/SurgSim/Math/OdeSolverLinearEulerExplicit.cpp
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolverLinearEulerExplicit.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolverLinearEulerExplicit::OdeSolverLinearEulerExplicit(OdeEquation* equation)
+ : OdeSolverEulerExplicit(equation), m_initialized(false)
+{
+ m_name = "Ode Solver Linear Euler Explicit";
+}
+
+void OdeSolverLinearEulerExplicit::solve(double dt, const OdeState& currentState, OdeState* newState)
+{
+ if (!m_initialized)
+ {
+ OdeSolverEulerExplicit::solve(dt, currentState, newState);
+ m_initialized = true;
+ }
+ else
+ {
+ Vector& f = m_equation.computeF(currentState);
+ currentState.applyBoundaryConditionsToVector(&f);
+ Vector deltaV = m_compliance * f;
+
+ newState->getPositions() = currentState.getPositions() + dt * currentState.getVelocities();
+ newState->getVelocities() = currentState.getVelocities() + deltaV;
+ }
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolverLinearEulerExplicit.h b/SurgSim/Math/OdeSolverLinearEulerExplicit.h
new file mode 100644
index 0000000..1a9bf15
--- /dev/null
+++ b/SurgSim/Math/OdeSolverLinearEulerExplicit.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVERLINEAREULEREXPLICIT_H
+#define SURGSIM_MATH_ODESOLVERLINEAREULEREXPLICIT_H
+
+#include "SurgSim/Math/OdeSolverEulerExplicit.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Linear Version of the Euler Explicit ode solver
+/// This solver assumes that the system is linear,
+/// ie that Mass, Damping, and Stiffness matrices do not change.
+class OdeSolverLinearEulerExplicit : public OdeSolverEulerExplicit
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolverLinearEulerExplicit(OdeEquation* equation);
+
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+
+private:
+ /// Has the solver been initialized
+ bool m_initialized;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVERLINEAREULEREXPLICIT_H
+
+
diff --git a/SurgSim/Math/OdeSolverLinearEulerExplicitModified.cpp b/SurgSim/Math/OdeSolverLinearEulerExplicitModified.cpp
new file mode 100644
index 0000000..bb54494
--- /dev/null
+++ b/SurgSim/Math/OdeSolverLinearEulerExplicitModified.cpp
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolverLinearEulerExplicitModified.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolverLinearEulerExplicitModified::OdeSolverLinearEulerExplicitModified(OdeEquation* equation)
+ : OdeSolverEulerExplicitModified(equation), m_initialized(false)
+{
+ m_name = "Ode Solver Linear Euler Explicit Modified";
+}
+
+void OdeSolverLinearEulerExplicitModified::solve(double dt, const OdeState& currentState, OdeState* newState)
+{
+ if (!m_initialized)
+ {
+ OdeSolverEulerExplicitModified::solve(dt, currentState, newState);
+ m_initialized = true;
+ }
+ else
+ {
+ Vector& f = m_equation.computeF(currentState);
+ currentState.applyBoundaryConditionsToVector(&f);
+ Vector deltaV = m_compliance * (f);
+
+ newState->getVelocities() = currentState.getVelocities() + deltaV;
+ newState->getPositions() = currentState.getPositions() + dt * newState->getVelocities();
+ }
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolverLinearEulerExplicitModified.h b/SurgSim/Math/OdeSolverLinearEulerExplicitModified.h
new file mode 100644
index 0000000..ddce1ca
--- /dev/null
+++ b/SurgSim/Math/OdeSolverLinearEulerExplicitModified.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVERLINEAREULEREXPLICITMODIFIED_H
+#define SURGSIM_MATH_ODESOLVERLINEAREULEREXPLICITMODIFIED_H
+
+#include "SurgSim/Math/OdeSolverEulerExplicitModified.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Linear Version of the Modified Euler Explicit ode solver
+/// This solver assumes that the system is linear,
+/// ie that Mass, Damping, and Stiffness matrices do not change.
+class OdeSolverLinearEulerExplicitModified : public OdeSolverEulerExplicitModified
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolverLinearEulerExplicitModified(OdeEquation* equation);
+
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+
+private:
+ /// Has the solver been initialized
+ bool m_initialized;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVERLINEAREULEREXPLICITMODIFIED_H
+
+
diff --git a/SurgSim/Math/OdeSolverLinearEulerImplicit.cpp b/SurgSim/Math/OdeSolverLinearEulerImplicit.cpp
new file mode 100644
index 0000000..281d6bb
--- /dev/null
+++ b/SurgSim/Math/OdeSolverLinearEulerImplicit.cpp
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolverLinearEulerImplicit.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolverLinearEulerImplicit::OdeSolverLinearEulerImplicit(OdeEquation* equation)
+ : OdeSolverEulerImplicit(equation), m_initialized(false)
+{
+ m_name = "Ode Solver Linear Euler Implicit";
+}
+
+void OdeSolverLinearEulerImplicit::solve(double dt, const OdeState& currentState, OdeState* newState)
+{
+ if (!m_initialized)
+ {
+ OdeSolverEulerImplicit::solve(dt, currentState, newState);
+ m_constantK = m_equation.computeK(currentState);
+ m_initialized = true;
+ }
+ else
+ {
+ Vector& f = m_equation.computeF(currentState);
+ f -= m_constantK * currentState.getVelocities() * dt;
+ currentState.applyBoundaryConditionsToVector(&f);
+ Vector deltaV = m_compliance * f;
+
+ newState->getVelocities() = currentState.getVelocities() + deltaV;
+ newState->getPositions() = currentState.getPositions() + dt * newState->getVelocities();
+ }
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolverLinearEulerImplicit.h b/SurgSim/Math/OdeSolverLinearEulerImplicit.h
new file mode 100644
index 0000000..9d9d346
--- /dev/null
+++ b/SurgSim/Math/OdeSolverLinearEulerImplicit.h
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVERLINEAREULERIMPLICIT_H
+#define SURGSIM_MATH_ODESOLVERLINEAREULERIMPLICIT_H
+
+#include "SurgSim/Math/OdeSolverEulerImplicit.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Linear Version of the Euler Implicit ode solver
+/// This solver assumes that the system is linear,
+/// ie that Mass, Damping, and Stiffness matrices do not change.
+class OdeSolverLinearEulerImplicit : public OdeSolverEulerImplicit
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolverLinearEulerImplicit(OdeEquation* equation);
+
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+
+private:
+ /// The constant stiffness matrix
+ Matrix m_constantK;
+
+ /// Has the solver been initialized
+ bool m_initialized;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVERLINEAREULERIMPLICIT_H
diff --git a/SurgSim/Math/OdeSolverLinearRungeKutta4.cpp b/SurgSim/Math/OdeSolverLinearRungeKutta4.cpp
new file mode 100644
index 0000000..52a7c8f
--- /dev/null
+++ b/SurgSim/Math/OdeSolverLinearRungeKutta4.cpp
@@ -0,0 +1,87 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolverLinearRungeKutta4.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolverLinearRungeKutta4::OdeSolverLinearRungeKutta4(OdeEquation* equation)
+ : OdeSolverRungeKutta4(equation),
+ m_initialized(false)
+{
+ m_name = "Ode Solver Linear Runge Kutta 4";
+}
+
+void OdeSolverLinearRungeKutta4::solve(double dt, const OdeState& currentState, OdeState* newState)
+{
+ if (!m_initialized)
+ {
+ OdeSolverRungeKutta4::solve(dt, currentState, newState);
+ m_initialized = true;
+ }
+ else
+ {
+ // General equation to solve:
+ // M.a(t) = F(t, x(t), v(t)), which is an ode of order 2 that can be reduced to an ode of order 1:
+ // y' = (x)' = ( v ) = f(t, y)
+ // (v) = (M^-1.F(t, x(t), v(t)))
+ // In terms of (x), f(t, (x)) = (v )
+ // (v) (v) (M^-1.F(t, x(t), v(t)))
+
+ // Runge Kutta 4 computes y(n+1) = y(n) + 1/6.dt.(k1 + 2 * k2 + 2 * k3 + k4)
+ // with k1 = f(t(n) , y(n) )
+ // with k2 = f(t(n) + dt/2, y(n) + k1 * dt/2)
+ // with k3 = f(t(n) + dt/2, y(n) + k2 * dt/2)
+ // with k4 = f(t(n) + dt , y(n) + k3 * dt )
+
+ // 1st evaluate k1 (note that y(n) is currentState)
+ m_k1.velocity = currentState.getVelocities();
+ // Reminder: m_compliance = dt.M^-1 (including 0 compliance for all boundary conditions)
+ m_k1.acceleration = m_compliance * m_equation.computeF(currentState) / dt;
+
+ // 2nd evaluate k2
+ newState->getPositions() = currentState.getPositions() + m_k1.velocity * dt / 2.0;
+ newState->getVelocities() = currentState.getVelocities() + m_k1.acceleration * dt / 2.0;
+ m_k2.velocity = newState->getVelocities();
+ m_k2.acceleration = m_compliance * m_equation.computeF(*newState) / dt;
+
+ // 3rd evaluate k3
+ newState->getPositions() = currentState.getPositions() + m_k2.velocity * dt / 2.0;
+ newState->getVelocities() = currentState.getVelocities() + m_k2.acceleration * dt / 2.0;
+ m_k3.velocity = newState->getVelocities();
+ m_k3.acceleration = m_compliance * m_equation.computeF(*newState) / dt;
+
+ // 4th evaluate k4
+ newState->getPositions() = currentState.getPositions() + m_k3.velocity * dt;
+ newState->getVelocities() = currentState.getVelocities() + m_k3.acceleration * dt;
+ m_k4.velocity = newState->getVelocities();
+ m_k4.acceleration = m_compliance * m_equation.computeF(*newState) / dt;
+
+ // Compute the new state using Runge Kutta 4 integration scheme:
+ newState->getPositions() = currentState.getPositions() +
+ (m_k1.velocity + m_k4.velocity + 2.0 * (m_k2.velocity + m_k3.velocity)) * dt / 6.0;
+ newState->getVelocities() = currentState.getVelocities() +
+ (m_k1.acceleration + m_k4.acceleration + 2.0 * (m_k2.acceleration + m_k3.acceleration)) * dt / 6.0;
+ }
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolverLinearRungeKutta4.h b/SurgSim/Math/OdeSolverLinearRungeKutta4.h
new file mode 100644
index 0000000..8a18088
--- /dev/null
+++ b/SurgSim/Math/OdeSolverLinearRungeKutta4.h
@@ -0,0 +1,47 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVERLINEARRUNGEKUTTA4_H
+#define SURGSIM_MATH_ODESOLVERLINEARRUNGEKUTTA4_H
+
+#include "SurgSim/Math/OdeSolverRungeKutta4.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Linear Version of the Runge Kutta 4 ode solver
+/// This solver assumes that the system is linear
+/// ie that Mass, Damping, and Stiffness matrices do not change.
+class OdeSolverLinearRungeKutta4 : public OdeSolverRungeKutta4
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolverLinearRungeKutta4(OdeEquation* equation);
+
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+
+private:
+ bool m_initialized;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVERLINEARRUNGEKUTTA4_H
diff --git a/SurgSim/Math/OdeSolverLinearStatic.cpp b/SurgSim/Math/OdeSolverLinearStatic.cpp
new file mode 100644
index 0000000..cae441d
--- /dev/null
+++ b/SurgSim/Math/OdeSolverLinearStatic.cpp
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolverLinearStatic.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolverLinearStatic::OdeSolverLinearStatic(OdeEquation* equation)
+ : OdeSolverStatic(equation), m_initialized(false)
+{
+ m_name = "Ode Solver Linear Static";
+}
+
+void OdeSolverLinearStatic::solve(double dt, const OdeState& currentState, OdeState* newState)
+{
+ if (!m_initialized)
+ {
+ OdeSolverStatic::solve(dt, currentState, newState);
+ m_initialized = true;
+ }
+ else
+ {
+ Vector& f = m_equation.computeF(currentState);
+ currentState.applyBoundaryConditionsToVector(&f);
+ Vector deltaX = m_compliance * f;
+
+ // Compute the new state using the static scheme:
+ newState->getPositions() = currentState.getPositions() + deltaX;
+ // Velocities are null in static mode (no time dependency)
+ newState->getVelocities().setZero();
+ }
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolverLinearStatic.h b/SurgSim/Math/OdeSolverLinearStatic.h
new file mode 100644
index 0000000..086f73e
--- /dev/null
+++ b/SurgSim/Math/OdeSolverLinearStatic.h
@@ -0,0 +1,47 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVERLINEARSTATIC_H
+#define SURGSIM_MATH_ODESOLVERLINEARSTATIC_H
+
+#include "SurgSim/Math/OdeSolverStatic.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Linear version of the static ode solver
+/// This solver assumes that the system is linear, ie that Stiffness matrix does not change.
+class OdeSolverLinearStatic : public OdeSolverStatic
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolverLinearStatic(OdeEquation* equation);
+
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+
+private:
+ /// Has the solver been initialized
+ bool m_initialized;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVERLINEARSTATIC_H
diff --git a/SurgSim/Math/OdeSolverRungeKutta4.cpp b/SurgSim/Math/OdeSolverRungeKutta4.cpp
new file mode 100644
index 0000000..6d2e917
--- /dev/null
+++ b/SurgSim/Math/OdeSolverRungeKutta4.cpp
@@ -0,0 +1,95 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolverRungeKutta4.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolverRungeKutta4::OdeSolverRungeKutta4(OdeEquation* equation)
+ : OdeSolver(equation)
+{
+ m_name = "Ode Solver Runge Kutta 4";
+}
+
+void OdeSolverRungeKutta4::solve(double dt, const OdeState& currentState, OdeState* newState)
+{
+ // General equation to solve:
+ // M.a(t) = F(t, x(t), v(t)), which is an ode of order 2 that can be reduced to an ode of order 1:
+ // y' = (x)' = ( v ) = f(t, y)
+ // (v) = (M^-1.F(t, x(t), v(t)))
+ // In terms of (x), f(t, (x)) = (v )
+ // (v) (v) (M^-1.F(t, x(t), v(t)))
+
+ // Runge Kutta 4 computes y(n+1) = y(n) + 1/6.dt.(k1 + 2 * k2 + 2 * k3 + k4)
+ // with k1 = f(t(n) , y(n) )
+ // with k2 = f(t(n) + dt/2, y(n) + k1 * dt/2)
+ // with k3 = f(t(n) + dt/2, y(n) + k2 * dt/2)
+ // with k4 = f(t(n) + dt , y(n) + k3 * dt )
+
+ // Computes M (stores it in m_systemMatrix to avoid dynamic re-allocation)
+ Matrix &M = (m_systemMatrix = m_equation.computeM(currentState));
+
+ // Apply the boundary conditions to the mass matrix
+ currentState.applyBoundaryConditionsToMatrix(&M);
+
+ // 1st evaluate k1 (note that y(n) is currentState)
+ m_k1.velocity = currentState.getVelocities();
+ (*m_linearSolver)(M, *currentState.applyBoundaryConditionsToVector(&m_equation.computeF(currentState)),
+ &m_k1.acceleration, &(m_compliance));
+
+ // Remove the boundary conditions compliance from the compliance matrix
+ // This helps to prevent potential exterior LCP type calculation to violates the boundary conditions
+ currentState.applyBoundaryConditionsToMatrix(&m_compliance, false);
+ // Note: no need to apply the boundary conditions to any computed forces as of now because m_compliance
+ // has entire lines of zero for all fixed dof. So m_compliance * F will produce the proper displacement
+ // without violating any boundary conditions.
+
+ // 2nd evaluate k2
+ newState->getPositions() = currentState.getPositions() + m_k1.velocity * dt / 2.0;
+ newState->getVelocities() = currentState.getVelocities() + m_k1.acceleration * dt / 2.0;
+ m_k2.velocity = newState->getVelocities();
+ m_k2.acceleration = m_compliance * m_equation.computeF(*newState);
+
+ // 3rd evaluate k3
+ newState->getPositions() = currentState.getPositions() + m_k2.velocity * dt / 2.0;
+ newState->getVelocities() = currentState.getVelocities() + m_k2.acceleration * dt / 2.0;
+ m_k3.velocity = newState->getVelocities();
+ m_k3.acceleration = m_compliance * m_equation.computeF(*newState);
+
+ // 4th evaluate k4
+ newState->getPositions() = currentState.getPositions() + m_k3.velocity * dt;
+ newState->getVelocities() = currentState.getVelocities() + m_k3.acceleration * dt;
+ m_k4.velocity = newState->getVelocities();
+ m_k4.acceleration = m_compliance * m_equation.computeF(*newState);
+
+ // Compute the new state using Runge Kutta 4 integration scheme:
+ newState->getPositions() = currentState.getPositions() +
+ (m_k1.velocity + m_k4.velocity + 2.0 * (m_k2.velocity + m_k3.velocity)) * dt / 6.0;
+ newState->getVelocities() = currentState.getVelocities() +
+ (m_k1.acceleration + m_k4.acceleration + 2.0 * (m_k2.acceleration + m_k3.acceleration)) * dt / 6.0;
+
+ // Computes the system matrix and compliance matrix
+ m_systemMatrix = M / dt;
+ m_compliance *= dt;
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolverRungeKutta4.h b/SurgSim/Math/OdeSolverRungeKutta4.h
new file mode 100644
index 0000000..4c88427
--- /dev/null
+++ b/SurgSim/Math/OdeSolverRungeKutta4.h
@@ -0,0 +1,76 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVERRUNGEKUTTA4_H
+#define SURGSIM_MATH_ODESOLVERRUNGEKUTTA4_H
+
+#include <array>
+
+#include "SurgSim/Math/OdeSolver.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Runge Kutta 4 ode solver
+/// See http://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
+/// \note M(x(t), v(t)).a(t) = f(t, x(t), v(t))
+/// \note This ode equation is solved as an ode of order 1 by defining the state vector y = (x v)^t:
+/// \note y' = ( x' ) = ( dx/dt ) = ( v )
+/// \note ( v' ) = ( dv/dt ) = ( M(x, v)^{-1}.f(x, v) )
+/// \note y' = f(t, y)
+/// \note Runge Kutta 4 solves it via 4 dependents evaluation of f at different times
+/// \note y(n+1) = y(n) + 1/6.dt (k1 + 2*k2 + 2*k3 + k4)
+/// \note with:
+/// \note k1 = f(t , y(n))
+/// \note k2 = f(t+dt/2, y(n) + k1.dt/2)
+/// \note k3 = f(t+dt/2, y(n) + k2.dt/2)
+/// \note k4 = f(t+dt , y(n) + k3.dt)
+class OdeSolverRungeKutta4 : public OdeSolver
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolverRungeKutta4(OdeEquation* equation);
+
+ /// Solves the equation
+ /// \param dt The time step
+ /// \param currentState State at time t
+ /// \param[out] newState State at time t+dt
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+
+protected:
+ /// Temporary vectors to store the 4 intermediates evaluations
+ Vector m_force;
+
+ /// Internal structure to hold the 4 temporary evaluations
+ struct RungeKuttaDerivedState
+ {
+ RungeKuttaDerivedState(){}
+ RungeKuttaDerivedState(const Vector& v, const Vector& a) : velocity(v), acceleration(a) {}
+ Vector velocity;
+ Vector acceleration;
+ };
+ /// Runge kutta 4 intermediate system evaluations
+ RungeKuttaDerivedState m_k1, m_k2, m_k3, m_k4;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVERRUNGEKUTTA4_H
diff --git a/SurgSim/Math/OdeSolverStatic.cpp b/SurgSim/Math/OdeSolverStatic.cpp
new file mode 100644
index 0000000..0fdf074
--- /dev/null
+++ b/SurgSim/Math/OdeSolverStatic.cpp
@@ -0,0 +1,65 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeSolverStatic.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeSolverStatic::OdeSolverStatic(OdeEquation* equation)
+ : OdeSolver(equation)
+{
+ m_name = "Ode Solver Static";
+}
+
+void OdeSolverStatic::solve(double dt, const OdeState& currentState, OdeState* newState)
+{
+ // General equation to solve:
+ // K.deltaX = Fext + Fint(t)
+ // which in the case of a linear model will derive in the expected equation:
+ // K.(x(t+dt) - x(t)) = Fext - K.(x(t) - x(0))
+ // K.(x(t+dt) - x(0)) = Fext
+
+ // Computes f(t, x(t), v(t)) and K
+ const Matrix& K = m_equation.computeK(currentState);
+ Vector& f = m_equation.computeF(currentState);
+
+ m_systemMatrix = K;
+
+ // Apply boundary conditions to the linear system
+ currentState.applyBoundaryConditionsToVector(&f);
+ currentState.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+
+ // Computes deltaX (stored in the positions) and m_compliance = 1/m_systemMatrix
+ Vector& deltaX = newState->getPositions();
+ (*m_linearSolver)(m_systemMatrix, f, &deltaX, &m_compliance);
+
+ // Remove the boundary conditions compliance from the compliance matrix
+ // This helps to prevent potential exterior LCP type calculation to violates the boundary conditions
+ currentState.applyBoundaryConditionsToMatrix(&m_compliance, false);
+
+ // Compute the new state using the static scheme:
+ newState->getPositions() = currentState.getPositions() + deltaX;
+ // Velocities are null in static mode (no time dependency)
+ newState->getVelocities().setZero();
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeSolverStatic.h b/SurgSim/Math/OdeSolverStatic.h
new file mode 100644
index 0000000..5f18acf
--- /dev/null
+++ b/SurgSim/Math/OdeSolverStatic.h
@@ -0,0 +1,46 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESOLVERSTATIC_H
+#define SURGSIM_MATH_ODESOLVERSTATIC_H
+
+#include "SurgSim/Math/OdeSolver.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Static ode solver
+/// \note M(x(t), v(t)).a(t) = f(t, x(t), v(t))
+/// \note This ode equation is solved w.r.t. x, by discarding all time derived variables (i.e. v, a)
+/// \note reducing the equation to solve to:
+/// \note 0 = f(t, x(t)) = Fext + Fint(t, x(t)) = Fext - K.(x - x0)
+class OdeSolverStatic : public OdeSolver
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit OdeSolverStatic(OdeEquation* equation);
+
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESOLVERSTATIC_H
diff --git a/SurgSim/Math/OdeState.cpp b/SurgSim/Math/OdeState.cpp
new file mode 100644
index 0000000..3cb743d
--- /dev/null
+++ b/SurgSim/Math/OdeState.cpp
@@ -0,0 +1,201 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/OdeState.h"
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/Valid.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+OdeState::OdeState() : m_numDofPerNode(0u), m_numNodes(0u)
+{
+}
+
+OdeState::~OdeState()
+{
+}
+
+bool OdeState::operator ==(const OdeState& state) const
+{
+ return m_x == state.m_x && m_v == state.m_v && m_boundaryConditionsPerDof == state.m_boundaryConditionsPerDof;
+}
+
+bool OdeState::operator !=(const OdeState& state) const
+{
+ return ! ((*this) == state);
+}
+
+void OdeState::reset()
+{
+ m_x.setZero();
+ m_v.setZero();
+ m_boundaryConditionsPerDof.setConstant(false);
+ m_boundaryConditionsAsDofIds.clear();
+}
+
+void OdeState::setNumDof(size_t numDofPerNode, size_t numNodes)
+{
+ const size_t numDof = numDofPerNode * numNodes;
+
+ m_numDofPerNode = numDofPerNode;
+ m_numNodes = numNodes;
+
+ m_x.resize(numDof);
+ m_v.resize(numDof);
+ m_boundaryConditionsPerDof.resize(numDof);
+
+ // Zero-out everything
+ reset();
+}
+
+size_t OdeState::getNumDof() const
+{
+ const size_t numDof = m_numDofPerNode * m_numNodes;
+
+ SURGSIM_ASSERT(m_x.size() == m_v.size() && m_x.size() == m_boundaryConditionsPerDof.size() && m_x.size() >= 0
+ && static_cast<size_t>(m_x.size()) == numDof);
+
+ return numDof;
+}
+
+size_t OdeState::getNumNodes() const
+{
+ return m_numNodes;
+}
+
+SurgSim::Math::Vector& OdeState::getPositions()
+{
+ return m_x;
+}
+
+const SurgSim::Math::Vector& OdeState::getPositions() const
+{
+ return m_x;
+}
+
+const SurgSim::Math::Vector3d OdeState::getPosition(size_t nodeId) const
+{
+ return SurgSim::Math::getSubVector(m_x, nodeId, m_numDofPerNode).segment(0, 3);
+}
+
+SurgSim::Math::Vector& OdeState::getVelocities()
+{
+ return m_v;
+}
+
+const SurgSim::Math::Vector& OdeState::getVelocities() const
+{
+ return m_v;
+}
+
+const SurgSim::Math::Vector3d OdeState::getVelocity(size_t nodeId) const
+{
+ return SurgSim::Math::getSubVector(m_v, nodeId, m_numDofPerNode).segment(0, 3);
+}
+
+void OdeState::addBoundaryCondition(size_t nodeId)
+{
+ SURGSIM_ASSERT(m_numDofPerNode != 0u) <<
+ "Number of dof per node = 0. Make sure to call setNumDof() prior to adding boundary conditions.";
+
+ for (size_t nodeDofId = 0; nodeDofId < m_numDofPerNode; ++nodeDofId)
+ {
+ addBoundaryCondition(nodeId, nodeDofId);
+ }
+}
+
+void OdeState::addBoundaryCondition(size_t nodeId, size_t nodeDofId)
+{
+ SURGSIM_ASSERT(m_numDofPerNode != 0u) <<
+ "Number of dof per node = 0. Make sure to call setNumDof() prior to adding boundary conditions.";
+ SURGSIM_ASSERT(nodeId < m_numNodes) << "Invalid nodeId " << nodeId << " number of nodes is " << m_numNodes;
+ SURGSIM_ASSERT(nodeDofId < m_numDofPerNode) <<
+ "Invalid nodeDofId " << nodeDofId << " number of dof per node is " << m_numDofPerNode;
+
+ size_t globalDofId = nodeId * m_numDofPerNode + nodeDofId;
+ if (! m_boundaryConditionsPerDof[globalDofId])
+ {
+ m_boundaryConditionsPerDof[globalDofId] = true;
+ m_boundaryConditionsAsDofIds.push_back(globalDofId);
+ }
+}
+
+size_t OdeState::getNumBoundaryConditions() const
+{
+ return m_boundaryConditionsAsDofIds.size();
+}
+
+const std::vector<size_t>& OdeState::getBoundaryConditions() const
+{
+ return m_boundaryConditionsAsDofIds;
+}
+
+bool OdeState::isBoundaryCondition(size_t dof) const
+{
+ return m_boundaryConditionsPerDof[dof];
+}
+
+Vector* OdeState::applyBoundaryConditionsToVector(Vector* vector) const
+{
+ SURGSIM_ASSERT(vector != nullptr && vector->size() >= 0 && static_cast<size_t>(vector->size()) == getNumDof())
+ << "Invalid vector to apply boundary conditions on";
+
+ for (std::vector<size_t>::const_iterator it = getBoundaryConditions().cbegin();
+ it != getBoundaryConditions().cend();
+ ++it)
+ {
+ (*vector)[*it] = 0.0;
+ }
+
+ return vector;
+}
+
+void OdeState::applyBoundaryConditionsToMatrix(Matrix* matrix, bool hasCompliance) const
+{
+ SURGSIM_ASSERT(matrix != nullptr && matrix->rows() >= 0 && matrix->cols() >= 0
+ && static_cast<size_t>(matrix->rows()) == getNumDof()
+ && static_cast<size_t>(matrix->cols()) == getNumDof())
+ << "Invalid matrix to apply boundary conditions on";
+
+ double complianceValue = 0.0;
+
+ if (hasCompliance)
+ {
+ complianceValue = 1.0;
+ }
+
+ for (std::vector<size_t>::const_iterator it = getBoundaryConditions().cbegin();
+ it != getBoundaryConditions().cend();
+ ++it)
+ {
+ (*matrix).middleRows(*it, 1).setZero();
+ (*matrix).middleCols(*it, 1).setZero();
+ (*matrix)(*it, *it) = complianceValue;
+ }
+}
+
+bool OdeState::isValid() const
+{
+ return SurgSim::Math::isValid(getPositions()) && SurgSim::Math::isValid(getVelocities());
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeState.h b/SurgSim/Math/OdeState.h
new file mode 100644
index 0000000..b3e7547
--- /dev/null
+++ b/SurgSim/Math/OdeState.h
@@ -0,0 +1,162 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_ODESTATE_H
+#define SURGSIM_MATH_ODESTATE_H
+
+#include <memory>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// OdeState defines the state y of an ode of 2nd order of the form M(x,v).a = F(x, v) with boundary conditions
+/// \note This ode equation is solved as an ode of order 1 by defining the state vector y = (x v)^t:
+/// \note y' = ( x' ) = ( dx/dt ) = ( v )
+/// \note ( v' ) = ( dv/dt ) = ( M(x, v)^{-1}.F(x, v) )
+class OdeState
+{
+public:
+ /// Default constructor
+ OdeState();
+
+ /// Destructor
+ ~OdeState();
+
+ /// Comparison operator (equality test)
+ /// \param state The state to compare it to
+ /// \return True if the 2 states are equal, False otherwise
+ bool operator ==(const OdeState& state) const;
+
+ /// Comparison operator (difference test)
+ /// \param state The state to compare it to
+ /// \return False if the 2 states are equal, True otherwise
+ bool operator !=(const OdeState& state) const;
+
+ /// Resets the state
+ /// \note Simply set all positions/velocities to 0 and remove all boundary conditions
+ void reset();
+
+ /// Allocates the state for a given number of degrees of freedom
+ /// \param numDofPerNode The number of degrees of freedom per node to account for
+ /// \param numNodes The number of nodes to account for
+ /// \note This method clears all the data structures and remove all existing boundary conditions
+ void setNumDof(size_t numDofPerNode, size_t numNodes);
+
+ /// Retrieves the number of degrees of freedom
+ /// \return The number of DOF for this representation
+ size_t getNumDof() const;
+
+ /// Retrieves the number of nodes
+ /// \return The number of nodes for this representation
+ size_t getNumNodes() const;
+
+ /// Retrieves all degrees of freedom's position (non-const version)
+ /// \return Vector of collected DOF's position
+ SurgSim::Math::Vector& getPositions();
+
+ /// Retrieves all degrees of freedom's position (const version)
+ /// \return Vector of collected DOF's position
+ const SurgSim::Math::Vector& getPositions() const;
+
+ /// Retrieves the position of a given node (const version)
+ /// \param nodeId The desired node id for which the position is requested (must be a valid id)
+ /// \return The position of the node nodeId
+ /// \note Behavior undefined if the nodeId is not in the correct range [0 getNumNodes()-1]
+ const SurgSim::Math::Vector3d getPosition(size_t nodeId) const;
+
+ /// Retrieves all degrees of freedom's velocity (non-const version)
+ /// \return Vector of collected DOF's velocity
+ SurgSim::Math::Vector& getVelocities();
+
+ /// Retrieves all degrees of freedom's velocity (const version)
+ /// \return Vector of collected DOF's velocity
+ const SurgSim::Math::Vector& getVelocities() const;
+
+ /// Retrieves the velocity of a given node (const version)
+ /// \param nodeId The desired node id for which the velocity is requested (must be a valid id)
+ /// \return The velocity of the node nodeId
+ /// \note Behavior undefined if the nodeId is not in the correct range [0 getNumNodes()-1]
+ const SurgSim::Math::Vector3d getVelocity(size_t nodeId) const;
+
+ /// Adds boundary conditions for a given node (fixes all the dof for this node)
+ /// \param nodeId The node to set the boundary conditions on
+ void addBoundaryCondition(size_t nodeId);
+
+ /// Adds a boundary condition on a given dof of a given node (only 1 dof is fixed)
+ /// \param nodeId The node on which the boundary condition needs to be set
+ /// \param nodeDofId The dof of the node to set as boundary condition
+ void addBoundaryCondition(size_t nodeId, size_t nodeDofId);
+
+ /// Retrieves the number of boundary conditions
+ /// \return The number of boundary conditions
+ size_t getNumBoundaryConditions() const;
+
+ /// Retrieves all boundary conditions
+ /// \return All boundary conditions as a vector of dof ids
+ const std::vector<size_t>& getBoundaryConditions() const;
+
+ /// Queries if a specific dof is a boundary condition or not
+ /// \param dof The requested dof
+ /// \return True if dof is a boundary condition, False otherwise
+ /// \note The behavior is undefined when dof is out of range [0 getNumBoundaryConditions()-1]
+ bool isBoundaryCondition(size_t dof) const;
+
+ /// Apply boundary conditions to a given vector
+ /// \param vector The vector to apply the boundary conditions on
+ /// \return vector. This enables chained use like the pseudo-code U = K^1 * applyBoundaryConditionsToVector(x)
+ Vector* applyBoundaryConditionsToVector(Vector* vector) const;
+
+ /// Apply boundary conditions to a given matrix
+ /// \param matrix The matrix to apply the boundary conditions on
+ /// \param hasCompliance True if the fixed dofs should have a compliance of 1 with themselves in the matrix or not.
+ /// \note hasCompliance is practical to remove all compliance, which is helpful when the compliance matrix is used
+ /// \note in an architecture of type LCP. It ensures that a separate constraint resolution will never violates the
+ /// \note boundary conditions.
+ void applyBoundaryConditionsToMatrix(Matrix* matrix, bool hasCompliance = true) const;
+
+ /// Check if this state is numerically valid
+ /// \return True if all positions and velocities are valid numerical values, False otherwise
+ bool isValid() const;
+
+private:
+ /// Default public copy constructor and assignment operator are being used on purpose
+
+ /// Keep track of the number of degrees of freedom per node and the number of nodes
+ size_t m_numDofPerNode, m_numNodes;
+
+ /// Degrees of freedom position
+ SurgSim::Math::Vector m_x;
+
+ /// Degrees of freedom velocity (m_x 1st derivative w.r.t. time)
+ SurgSim::Math::Vector m_v;
+
+ /// Boundary conditions stored as a list of dof ids
+ std::vector<size_t> m_boundaryConditionsAsDofIds;
+
+ /// Boundary conditions stored per dof (True indicates a boundary condition, False does not)
+ Eigen::Matrix<bool, Eigen::Dynamic, 1> m_boundaryConditionsPerDof;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_ODESTATE_H
diff --git a/SurgSim/Math/PlaneShape.cpp b/SurgSim/Math/PlaneShape.cpp
new file mode 100644
index 0000000..0b1ec1c
--- /dev/null
+++ b/SurgSim/Math/PlaneShape.cpp
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/PlaneShape.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::PlaneShape, PlaneShape);
+
+PlaneShape::PlaneShape()
+{
+}
+
+int PlaneShape::getType()
+{
+ return SHAPE_TYPE_PLANE;
+}
+
+double PlaneShape::getVolume() const
+{
+ return 0.0;
+}
+
+SurgSim::Math::Vector3d PlaneShape::getCenter() const
+{
+ return Vector3d(0.0, 0.0, 0.0);
+}
+
+SurgSim::Math::Matrix33d PlaneShape::getSecondMomentOfVolume() const
+{
+ return Matrix33d::Zero();
+}
+
+double PlaneShape::getD() const
+{
+ return 0.0;
+}
+
+SurgSim::Math::Vector3d PlaneShape::getNormal() const
+{
+ return Vector3d(0.0, 1.0, 0.0);
+}
+
+bool PlaneShape::isValid() const
+{
+ return true;
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/PlaneShape.h b/SurgSim/Math/PlaneShape.h
new file mode 100644
index 0000000..0178985
--- /dev/null
+++ b/SurgSim/Math/PlaneShape.h
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_PLANESHAPE_H
+#define SURGSIM_MATH_PLANESHAPE_H
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_STATIC_REGISTRATION(PlaneShape);
+
+/// The XZ plane (d = 0) with normal pointing along positive Y axis.
+/// The difference between PlaneShape and DoubleSidedPlane shape is in the way the Physics scene
+/// handles these in terms of collision detection. While the DoubleSidedPlane is handled as a thin
+/// solid of (essentially) zero thickness and space on either side it, the PlaneShape is considered
+/// to be a entirely solid on the side of the plane which is opposite to the normal. It is made of
+/// space only on the positive side of the plane normal. This results in having a robust collision
+/// object which does not let any objects through, and could be useful to define the scene floor.
+class PlaneShape: public Shape
+{
+public:
+
+ /// Constructor: No members to initialize.
+ PlaneShape();
+
+ SURGSIM_CLASSNAME(SurgSim::Math::PlaneShape);
+
+ /// \return the type of the shape
+ virtual int getType() override;
+
+ /// Get the volume of the shape
+ /// \return The volume of the shape (in m-3)
+ virtual double getVolume() const override;
+
+ /// Get the volumetric center of the shape
+ /// \return The center of the shape
+ virtual Vector3d getCenter() const override;
+
+ /// Get the second central moment of the volume, commonly used
+ /// to calculate the moment of inertia matrix
+ /// \return The 3x3 symmetric second moment matrix
+ virtual Matrix33d getSecondMomentOfVolume() const override;
+
+ /// Gets the d of the plane equation.
+ /// \return The value of d (always 0).
+ double getD() const;
+
+ /// Gets the normal of the plane equation.
+ /// \return The value of the normal (always Y axis).
+ Vector3d getNormal() const;
+
+ /// A PlaneShape is always valid.
+ /// \return True.
+ virtual bool isValid() const override;
+};
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_PLANESHAPE_H
diff --git a/SurgSim/Math/Quaternion.h b/SurgSim/Math/Quaternion.h
new file mode 100644
index 0000000..2ff4466
--- /dev/null
+++ b/SurgSim/Math/Quaternion.h
@@ -0,0 +1,162 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 quaternion types.
+
+#ifndef SURGSIM_MATH_QUATERNION_H
+#define SURGSIM_MATH_QUATERNION_H
+
+#include <math.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// A quaternion of floats.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Quaternion<float> Quaternionf;
+
+/// A quaternion of doubles.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Quaternion<double> Quaterniond;
+
+
+
+/// Create a quaternion rotation corresponding to the specified angle (in radians) and axis.
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam VOpt the option flags (alignment etc.) used for the axis vector argument. Can be deduced.
+/// \param angle the angle of the rotation, in radians.
+/// \param axis the axis of the rotation.
+/// \returns the rotation quaternion.
+template <typename T, int VOpt>
+inline Eigen::Quaternion<T> makeRotationQuaternion(const T& angle, const Eigen::Matrix<T, 3, 1, VOpt>& axis)
+{
+ return Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, axis));
+}
+
+/// Quaternion negation (i.e. unary operator -)
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam QOpt the option flags (alignment etc.) used for the quaternion arguments. Can be deduced.
+/// \param q The quaternion to negate
+/// \returns the negation of q (i.e. -q)
+template <typename T, int QOpt>
+inline Eigen::Quaternion<T, QOpt> negate(const Eigen::Quaternion<T, QOpt>& q)
+{
+ return Eigen::Quaternion<T, QOpt>(q.coeffs() * -1.0);
+}
+
+/// Get the angle (in radians) and axis corresponding to a quaternion's rotation.
+/// \note Unit quaternions cover the unit sphere twice (q=-q). To make sure that the same rotation (q or -q)
+/// \note returns the same Axis/Angle, we need a pi range for the angle.
+/// \note We choose to enforce half the angle in the quadrant [0 +pi/2], which leads to an output angle in [0 +pi].
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam QOpt the option flags (alignment etc.) used for the quaternion argument. Can be deduced.
+/// \tparam VOpt the option flags (alignment etc.) used for the axis vector argument. Can be deduced.
+/// \param quaternion the rotation quaternion to inspect.
+/// \param [out] angle the angle of the rotation in [0 +pi], in radians.
+/// \param [out] axis the axis of the rotation.
+template <typename T, int QOpt, int VOpt>
+inline void computeAngleAndAxis(const Eigen::Quaternion<T, QOpt>& quaternion,
+ T* angle, Eigen::Matrix<T, 3, 1, VOpt>* axis)
+{
+ using SurgSim::Math::negate;
+
+ if (quaternion.w() >= T(0))
+ {
+ Eigen::AngleAxis<T> angleAxis(quaternion);
+ *angle = angleAxis.angle();
+ *axis = angleAxis.axis();
+ }
+ else
+ {
+ Eigen::AngleAxis<T> angleAxis(negate(quaternion));
+ *angle = angleAxis.angle();
+ *axis = angleAxis.axis();
+ }
+}
+
+/// Get the angle corresponding to a quaternion's rotation, in radians.
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam QOpt the option flags (alignment etc.) used for the quaternion argument. Can be deduced.
+/// \param quaternion the rotation quaternion to inspect.
+/// \returns the angle of the rotation within [0 +pi], in radians.
+template <typename T, int QOpt>
+inline T computeAngle(const Eigen::Quaternion<T, QOpt>& quaternion)
+{
+ using SurgSim::Math::negate;
+
+ if (quaternion.w() >= T(0))
+ {
+ Eigen::AngleAxis<T> angleAxis(quaternion);
+ return angleAxis.angle();
+ }
+ else
+ {
+ Eigen::AngleAxis<T> angleAxis(negate(quaternion));
+ return angleAxis.angle();
+ }
+}
+
+/// Get the vector corresponding to the rotation between transforms.
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam TOpt the option flags (alignment etc.) used for the transform arguments. Can be deduced.
+/// \tparam VOpt the option flags (alignment etc.) used for the vector argument. Can be deduced.
+/// \param end the transform after the rotation.
+/// \param start the transform before the rotation.
+/// \param[out] rotationVector a vector describing the rotation.
+template <typename T, int TOpt, int VOpt>
+void computeRotationVector(const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& end,
+ const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& start,
+ Eigen::Matrix<T, 3, 1, VOpt>* rotationVector)
+{
+ Eigen::Quaternion<T> q1(end.linear());
+ Eigen::Quaternion<T> q2(start.linear());
+ double angle;
+ Eigen::Matrix<T, 3, 1, VOpt> axis;
+ computeAngleAndAxis((q1 * q2.inverse()).normalized(), &angle, &axis);
+ *rotationVector = angle * axis;
+}
+
+/// Interpolate (slerp) between 2 quaternions
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam QOpt the option flags (alignment etc.) used for the quaternion arguments. Can be deduced.
+/// \param q0 The start quaternion (at time 0.0).
+/// \param q1 The end quaternion (at time 1.0).
+/// \param t The interpolation time requested. Within [0..1].
+/// \returns the quaternion resulting in the slerp interpolation at time t, between q0 and q1.
+/// \note t=0 => returns either q0 or -q0
+/// \note t=1 => returns either q1 or -q1
+/// \note 'Interpolate' has been created because slerp might not be enough in certain cases.
+/// This gives room for correction and special future treatment
+template <typename T, int QOpt>
+inline Eigen::Quaternion<T, QOpt> interpolate(const Eigen::Quaternion<T, QOpt>& q0,
+ const Eigen::Quaternion<T, QOpt>& q1, T t)
+{
+ Eigen::Quaternion<T, QOpt> result;
+
+ result = q0.slerp(t, q1);
+
+ return result;
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_QUATERNION_H
diff --git a/SurgSim/Math/RigidTransform.h b/SurgSim/Math/RigidTransform.h
new file mode 100644
index 0000000..484fb64
--- /dev/null
+++ b/SurgSim/Math/RigidTransform.h
@@ -0,0 +1,165 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 2x2 and 3x3 rigid (isometric) transforms.
+
+#ifndef SURGSIM_MATH_RIGIDTRANSFORM_H
+#define SURGSIM_MATH_RIGIDTRANSFORM_H
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "SurgSim/Math/Quaternion.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// A 2D rigid (isometric) transform, represented as floats.
+/// This type (and any struct that contain it) can be safely allocated via new.
+typedef Eigen::Transform<float, 2, Eigen::Isometry> RigidTransform2f;
+
+/// A 3D rigid (isometric) transform, represented as floats.
+/// This type (and any struct that contain it) can be safely allocated via new.
+typedef Eigen::Transform<float, 3, Eigen::Isometry> RigidTransform3f;
+
+/// A 2D rigid (isometric) transform, represented as doubles.
+/// This type (and any struct that contain it) can be safely allocated via new.
+typedef Eigen::Transform<double, 2, Eigen::Isometry> RigidTransform2d;
+
+/// A 3D rigid (isometric) transform, represented as doubles.
+/// This type (and any struct that contain it) can be safely allocated via new.
+typedef Eigen::Transform<double, 3, Eigen::Isometry> RigidTransform3d;
+
+
+/// Create a rigid transform using the specified rotation matrix and translation.
+/// \tparam M the type used to describe the rotation matrix. Can usually be deduced.
+/// \tparam V the type used to describe the translation vector. Can usually be deduced.
+/// \param rotation the rotation matrix.
+/// \param translation the translation vector.
+/// \returns the transform with the specified rotation and translation.
+template <typename M, typename V>
+inline Eigen::Transform<typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry> makeRigidTransform(
+ const Eigen::MatrixBase<M>& rotation, const Eigen::MatrixBase<V>& translation)
+{
+ Eigen::Transform<typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry> rigid;
+ rigid.makeAffine();
+ rigid.linear() = rotation;
+ rigid.translation() = translation;
+ return rigid;
+}
+
+/// Create a rigid transform using the specified rotation quaternion and translation.
+/// \tparam Q the type used to describe the rotation quaternion. Can usually be deduced.
+/// \tparam V the type used to describe the translation vector. Can usually be deduced.
+/// \param rotation the rotation quaternion.
+/// \param translation the translation vector.
+/// \returns the transform with the specified rotation and translation.
+template <typename Q, typename V>
+inline Eigen::Transform<typename Q::Scalar, 3, Eigen::Isometry> makeRigidTransform(
+ const Eigen::QuaternionBase<Q>& rotation, const Eigen::MatrixBase<V>& translation)
+{
+ Eigen::Transform<typename Q::Scalar, 3, Eigen::Isometry> rigid;
+ rigid.makeAffine();
+ rigid.linear() = rotation.matrix();
+ rigid.translation() = translation;
+ return rigid;
+}
+
+/// Make a rigid transform from a eye point a center view point and an up direction, does not check whether up is
+/// colinear with eye-center
+/// The original formula can be found at
+/// http://www.opengl.org/sdk/docs/man2/xhtml/gluLookAt.xml
+/// \tparam typename T T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam int VOpt VOpt the option flags (alignment etc.) used for the axis vector argument. Can be deduced.
+/// \param position The position of the object.
+/// \param center The point to which the object should point.
+/// \param up The up vector to be used for this calculation.
+/// \return a RigidTransform that locates the object at position rotated into the direction of center.
+template <typename T, int VOpt>
+inline Eigen::Transform<T, 3, Eigen::Isometry> makeRigidTransform(
+ const Eigen::Matrix<T, 3, 1, VOpt>& position,
+ const Eigen::Matrix<T, 3, 1, VOpt>& center,
+ const Eigen::Matrix<T, 3, 1, VOpt>& up)
+{
+
+ Eigen::Transform<T, 3, Eigen::Isometry> rigid;
+ rigid.makeAffine();
+
+ Eigen::Matrix<T, 3, 1, VOpt> forward = (center - position).normalized();
+ Eigen::Matrix<T, 3, 1, VOpt> side = (forward.cross(up)).normalized();
+ Eigen::Matrix<T, 3, 1, VOpt> actualUp = side.cross(forward).normalized();
+
+ typename Eigen::Transform<T, 3, Eigen::Isometry>::LinearMatrixType rotation;
+ rotation << side[0], actualUp[0], -forward[0],
+ side[1], actualUp[1], -forward[1],
+ side[2], actualUp[2], -forward[2];
+
+ rigid.linear() = rotation;
+ rigid.translation() = position;
+
+ return rigid;
+}
+
+/// Create a rigid transform using the identity rotation and the specified translation.
+/// \tparam V the type used to describe the translation vector. Can usually be deduced.
+/// \param translation the translation vector.
+/// \returns the transform with the identity rotation and the specified translation.
+template <typename V>
+inline Eigen::Transform<typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry> makeRigidTranslation(
+ const Eigen::MatrixBase<V>& translation)
+{
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<V>);
+ Eigen::Transform<typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry> rigid;
+ rigid.makeAffine();
+ rigid.linear().setIdentity();
+ rigid.translation() = translation;
+ return rigid;
+}
+
+/// Interpolate (slerp) between 2 rigid transformations
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam TOpt the option flags (alignment etc.) used for the Transform arguments. Can be deduced.
+/// \param t0 The start transform (at time 0.0).
+/// \param t1 The end transform (at time 1.0).
+/// \param t The interpolation time requested. Within [0..1].
+/// \returns the transform resulting in the slerp interpolation at time t, between t0 and t1.
+/// \note t=0 => returns t0
+/// \note t=1 => returns t1
+template <typename T, int TOpt>
+inline Eigen::Transform<T, 3, Eigen::Isometry> interpolate(
+ const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& t0,
+ const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& t1,
+ T t)
+{
+ Eigen::Transform<T, 3, Eigen::Isometry> transform;
+ transform.makeAffine();
+ transform.translation() = t0.translation() * (static_cast<T>(1.0) - t) + t1.translation() * t;
+ {
+ Eigen::Quaternion<T> q0(t0.linear());
+ Eigen::Quaternion<T> q1(t1.linear());
+ q0.normalize();
+ q1.normalize();
+ transform.linear() = interpolate(q0, q1, t).matrix();
+ }
+ return transform;
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_RIGIDTRANSFORM_H
diff --git a/SurgSim/Math/Shape.cpp b/SurgSim/Math/Shape.cpp
new file mode 100644
index 0000000..f1b5ddd
--- /dev/null
+++ b/SurgSim/Math/Shape.cpp
@@ -0,0 +1,46 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+Shape::~Shape()
+{
+}
+
+Shape::FactoryType& Shape::getFactory()
+{
+ static FactoryType factory;
+ return factory;
+}
+
+/// Get class name
+std::string Shape::getClassName() const
+{
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "getClassName() called on Math::Shape base class, this is wrong" <<
+ " in almost all cases, this means there is a class that does not have getClassName() defined.";
+ return "SurgSim::Math::Shape";
+}
+
+} // namespace Math
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Math/Shape.h b/SurgSim/Math/Shape.h
new file mode 100644
index 0000000..36c94d9
--- /dev/null
+++ b/SurgSim/Math/Shape.h
@@ -0,0 +1,100 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_SHAPE_H
+#define SURGSIM_MATH_SHAPE_H
+
+#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Type defining the shape direction for certain templatized shape
+/// (i.e. CylinderShape and CapsuleShape)
+typedef enum
+{
+ SHAPE_DIRECTION_AXIS_X = 0,
+ SHAPE_DIRECTION_AXIS_Y = 1,
+ SHAPE_DIRECTION_AXIS_Z = 2
+} ShapeDirection;
+
+/// Fixed List of enums for the available Shape types, do not explicitly assign values, ShapeCount is
+/// used to determine the number of actual shape types
+typedef enum
+{
+ SHAPE_TYPE_NONE = -1,
+ SHAPE_TYPE_BOX,
+ SHAPE_TYPE_CAPSULE,
+ SHAPE_TYPE_CYLINDER,
+ SHAPE_TYPE_DOUBLESIDEDPLANE,
+ SHAPE_TYPE_MESH,
+ SHAPE_TYPE_OCTREE,
+ SHAPE_TYPE_PLANE,
+ SHAPE_TYPE_SPHERE,
+ SHAPE_TYPE_SURFACEMESH,
+ SHAPE_TYPE_COUNT
+} ShapeType;
+
+/// Generic rigid shape class defining a shape
+/// \note This class gives the ability to analyze the shape and compute
+/// \note physical information (volume, mass, mass center, inertia)
+class Shape : public SurgSim::Framework::Accessible
+{
+public:
+ typedef ::SurgSim::Math::Vector3d Vector3d;
+ typedef ::SurgSim::Math::Matrix33d Matrix33d;
+
+ // Destructor
+ virtual ~Shape();
+
+ /// \return the type of shape
+ virtual int getType() = 0;
+
+ /// Get the volume of the shape
+ /// \return The volume of the shape (in m-3)
+ virtual double getVolume() const = 0;
+
+ /// Get the volumetric center of the shape
+ /// \return The center of the shape
+ virtual Vector3d getCenter() const = 0;
+
+ /// Get the second central moment of the volume, commonly used
+ /// to calculate the moment of inertia matrix
+ /// \return The 3x3 symmetric second moment matrix
+ virtual Matrix33d getSecondMomentOfVolume() const = 0;
+
+ typedef SurgSim::Framework::ObjectFactory<SurgSim::Math::Shape> FactoryType;
+
+ /// \return The static class factory that is being used in the conversion.
+ static FactoryType& getFactory();
+
+ /// Get class name
+ virtual std::string getClassName() const;
+
+ /// Check if the shape is valid
+ /// \return True if shape is valid; Otherwise, false.
+ virtual bool isValid() const = 0;
+};
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_SHAPE_H
diff --git a/SurgSim/Math/Shapes.h b/SurgSim/Math/Shapes.h
new file mode 100644
index 0000000..1835586
--- /dev/null
+++ b/SurgSim/Math/Shapes.h
@@ -0,0 +1,30 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_SHAPES_H
+#define SURGSIM_MATH_SHAPES_H
+
+/// This file includes all the shapes
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/CapsuleShape.h"
+#include "SurgSim/Math/CylinderShape.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/OctreeShape.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/SurfaceMeshShape.h"
+
+#endif // SURGSIM_MATH_SHAPES_H
diff --git a/SurgSim/Math/SphereShape.cpp b/SurgSim/Math/SphereShape.cpp
new file mode 100644
index 0000000..1000cb6
--- /dev/null
+++ b/SurgSim/Math/SphereShape.cpp
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/SphereShape.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::SphereShape, SphereShape);
+
+SphereShape::SphereShape(double radius) : m_radius(radius)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphereShape, double, Radius, getRadius, setRadius);
+}
+
+int SphereShape::getType()
+{
+ return SHAPE_TYPE_SPHERE;
+}
+
+double SphereShape::getRadius() const
+{
+ return m_radius;
+}
+
+void SphereShape::setRadius(double radius)
+{
+ m_radius = radius;
+}
+
+double SphereShape::getVolume() const
+{
+ return 4.0 / 3.0 * M_PI * m_radius * m_radius * m_radius;
+}
+
+SurgSim::Math::Vector3d SphereShape::getCenter() const
+{
+ return Vector3d(0.0, 0.0, 0.0);
+}
+
+SurgSim::Math::Matrix33d SphereShape::getSecondMomentOfVolume() const
+{
+ const double volume = getVolume();
+
+ double diagonalCoefficient = 2.0 / 5.0 * volume * m_radius * m_radius;
+
+ Matrix33d secondMoment;
+ secondMoment.setZero();
+ secondMoment.diagonal().setConstant(diagonalCoefficient);
+
+ return secondMoment;
+}
+
+bool SphereShape::isValid() const
+{
+ return m_radius >= 0;
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/SphereShape.h b/SurgSim/Math/SphereShape.h
new file mode 100644
index 0000000..61d89f8
--- /dev/null
+++ b/SurgSim/Math/SphereShape.h
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_SPHERESHAPE_H
+#define SURGSIM_MATH_SPHERESHAPE_H
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_STATIC_REGISTRATION(SphereShape);
+
+/// Sphere shape: sphere centered on (0 0 0), defined with radius
+class SphereShape: public Shape
+{
+public:
+ /// Constructor
+ /// \param radius The sphere radius (in m)
+ explicit SphereShape(double radius = 0.0);
+
+ SURGSIM_CLASSNAME(SurgSim::Math::SphereShape);
+
+ /// \return the type of the shape
+ virtual int getType() override;
+
+ /// Get the sphere radius
+ /// \return The sphere radius
+ double getRadius() const;
+
+ /// Get the volume of the shape
+ /// \return The volume of the shape (in m-3)
+ virtual double getVolume() const override;
+
+ /// Get the volumetric center of the shape
+ /// \return The center of the shape
+ virtual Vector3d getCenter() const override;
+
+ /// Get the second central moment of the volume, commonly used
+ /// to calculate the moment of inertia matrix
+ /// \return The 3x3 symmetric second moment matrix
+ virtual Matrix33d getSecondMomentOfVolume() const override;
+
+ /// \return True if radius is bigger than or equal to 0; Otherwise, false.
+ virtual bool isValid() const override;
+
+protected:
+ // Setters in 'protected' sections are for serialization purpose only.
+
+ /// Set the sphere radius
+ /// \param radius The sphere radius
+ void setRadius(double radius);
+
+private:
+ /// Sphere radius
+ double m_radius;
+};
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_SPHERESHAPE_H
diff --git a/SurgSim/Math/SurfaceMeshShape-inl.h b/SurgSim/Math/SurfaceMeshShape-inl.h
new file mode 100644
index 0000000..2720129
--- /dev/null
+++ b/SurgSim/Math/SurfaceMeshShape-inl.h
@@ -0,0 +1,41 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+
+#ifndef SURGSIM_MATH_SURFACEMESHSHAPE_INL_H
+#define SURGSIM_MATH_SURFACEMESHSHAPE_INL_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <class VertexData, class EdgeData, class TriangleData>
+SurfaceMeshShape::SurfaceMeshShape(
+ const SurgSim::DataStructures::TriangleMeshBase<VertexData, EdgeData, TriangleData>& mesh,
+ double thickness) : m_thickness(thickness)
+{
+ SURGSIM_ASSERT(mesh.isValid()) << "Invalid mesh";
+
+ m_mesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(mesh);
+
+ // Computes the geometric properties for the initial mesh
+ computeVolumeIntegrals();
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_SURFACEMESHSHAPE_INL_H
diff --git a/SurgSim/Math/SurfaceMeshShape.cpp b/SurgSim/Math/SurfaceMeshShape.cpp
new file mode 100644
index 0000000..5cd1122
--- /dev/null
+++ b/SurgSim/Math/SurfaceMeshShape.cpp
@@ -0,0 +1,197 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+
+#include "SurgSim/Math/SurfaceMeshShape.h"
+#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace
+{
+const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Math
+{
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::SurfaceMeshShape, SurfaceMeshShape);
+
+SurfaceMeshShape::SurfaceMeshShape() : m_volume(0.0), m_thickness(1e-2)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SurfaceMeshShape, std::string, FileName, getFileName, setFileName);
+}
+
+int SurfaceMeshShape::getType()
+{
+ return SHAPE_TYPE_SURFACEMESH;
+}
+
+
+void SurfaceMeshShape::setFileName(const std::string& fileName)
+{
+ using SurgSim::DataStructures::TriangleMesh;
+ m_fileName = fileName;
+ m_mesh = SurgSim::DataStructures::loadTriangleMesh<SurgSim::DataStructures::TriangleMesh>(fileName);
+}
+
+std::string SurfaceMeshShape::getFileName() const
+{
+ return m_fileName;
+}
+
+std::shared_ptr<SurgSim::DataStructures::TriangleMesh> SurfaceMeshShape::getMesh()
+{
+ return m_mesh;
+}
+
+double SurfaceMeshShape::getVolume() const
+{
+ if (nullptr == m_mesh)
+ {
+ SURGSIM_LOG_CRITICAL(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "No mesh set for SurfaceMeshShape, so it cannot compute volume.";
+ }
+ return m_volume;
+}
+
+SurgSim::Math::Vector3d SurfaceMeshShape::getCenter() const
+{
+ if (nullptr == m_mesh)
+ {
+ SURGSIM_LOG_CRITICAL(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "No mesh set for SurfaceMeshShape, so it cannot compute center.";
+ }
+ return m_center;
+}
+
+SurgSim::Math::Matrix33d SurfaceMeshShape::getSecondMomentOfVolume() const
+{
+ if (nullptr == m_mesh)
+ {
+ SURGSIM_LOG_CRITICAL(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "No mesh set for SurfaceMeshShape, so it cannot compute second moment of volume.";
+ }
+ return m_secondMomentOfVolume;
+}
+
+void SurfaceMeshShape::computeVolumeIntegrals()
+{
+ // Considering a surface mesh with thickness, the goal is to compute the following properties:
+ // 1) volume = integral(over volume) dx dy dz
+ // 2) center = integral(over volume) (x y z)^T dx dy dz / volume
+ // 3) the inertia matrix without the mass density:
+ // I = (Ixx Ixy Ixz) = integral(over volume) (y^2+z^2 -xy -xz ) dx dy dz
+ // (Iyx Iyy Iyz) ( -yx x^2+z^2 -yz )
+ // (Izx Izy Izz) ( -zx -zy x^2+y^2)
+ // Each term of the matrix I can be evaluated independently and each monome can also be evaluated
+ // independently and summed up after: integral(V) y^2+z^2 dV = integral(V) y^2 dV + integral(V) z^2 dV
+ //
+ // Therefore, we simply need to compute 10 different volume integral:
+ // {1, x, y, z, x^2, y^2, z^2, xy, yz, zx}
+ //
+ // Example for the monome 'xy':
+ // = integral(over volume) x.y dx dy dz = integral(over volume) x.y dV
+ // = integral(over area) x.y dS * thickness
+ // = [sum(over all triangles) integral(over triangle) x.y dS] * thickness
+ //
+ // Change the integration variables from cartesian coordinates to a parametrization of the
+ // triangle ABC = {P | P = A + a.u + b.v}
+ // with
+ // { u=AB the 1st triangle edge supporting the parametrization
+ // { v=AC the 2nd triangle edge supporting the parametrization
+ // { 0<=a<=1 the triangle 1st coordinate in the new coordinate system (parametrized)
+ // { 0<=b<=1-a the triangle 2nd coordinate in the new coordinate system (parametrized)
+ // => dS = |dP/db x dP/da|.db.da = |vxu|.db.da = |uxv|.db.da
+ // => x = Px
+ // => y = Py
+ //
+ // integral(over volume) x.y dx dy dz =
+ // [sum(over all triangles) integral(over triangle) x.y dS] * thickness =
+ // [sum(over all triangles) integral(from 0 to 1) integral(from 0 to 1-a) Px.Py db.da |uxv|] * thickness
+ //
+ // From here, the various integral terms can be found related to A, u and v using any formal integration tool.
+
+ // Order: 1, x, y, z, x^2, y^2, z^2, xy, yz, zx
+ Eigen::VectorXd integral(10);
+ integral.setZero();
+
+ for (auto const& triangle : m_mesh->getTriangles())
+ {
+ if (!triangle.isValid)
+ {
+ continue;
+ }
+
+ auto A = m_mesh->getVertexPosition(triangle.verticesId[0]);
+ auto B = m_mesh->getVertexPosition(triangle.verticesId[1]);
+ auto C = m_mesh->getVertexPosition(triangle.verticesId[2]);
+
+ // Triangle parametrization P(a, b) = A + u.a + v.b with u=AB and v=AC
+ const Vector3d u = B - A;
+ const Vector3d v = C - A;
+ const double area = u.cross(v).norm() / 2.0; // Triangle area
+
+ integral[0] += area;
+ integral[1] += (A[0] + (u[0] + v[0]) / 3.0) * area;
+ integral[2] += (A[1] + (u[1] + v[1]) / 3.0) * area;
+ integral[3] += (A[2] + (u[2] + v[2]) / 3.0) * area;
+ integral[4] +=
+ (A[0] * (A[0] + 2.0 * (u[0] + v[0]) / 3.0) + (u[0] * v[0] + v[0] * v[0] + u[0] * u[0]) / 6.0) * area;
+ integral[5] +=
+ (A[1] * (A[1] + 2.0 * (u[1] + v[1]) / 3.0) + (u[1] * v[1] + v[1] * v[1] + u[1] * u[1]) / 6.0) * area;
+ integral[6] +=
+ (A[2] * (A[2] + 2.0 * (u[2] + v[2]) / 3.0) + (u[2] * v[2] + v[2] * v[2] + u[2] * u[2]) / 6.0) * area;
+ integral[7] += (A[0] * A[1] + (A[0] * (u[1] + v[1]) + A[1] * (u[0] + v[0])) / 3.0) * area;
+ integral[7] += ((u[0] * u[1] + v[0] * v[1]) / 6.0 + (u[0] * v[1] + u[1] * v[0]) / 12.0) * area;
+ integral[8] += (A[1] * A[2] + (A[1] * (u[2] + v[2]) + A[2] * (u[1] + v[1])) / 3.0) * area;
+ integral[8] += ((u[1] * u[2] + v[1] * v[2]) / 6.0 + (u[1] * v[2] + u[2] * v[1]) / 12.0) * area;
+ integral[9] += (A[2] * A[0] + (A[2] * (u[0] + v[0]) + A[0] * (u[2] + v[2])) / 3.0) * area;
+ integral[9] += ((u[2] * u[0] + v[2] * v[0]) / 6.0 + (u[2] * v[0] + u[0] * v[2]) / 12.0) * area;
+ }
+
+ // integral[0] is the sum of all triangle's area
+ double area = integral[0];
+
+ // Center of mass
+ m_center = integral.segment(1, 3);
+ if (area > epsilon)
+ {
+ m_center /= area;
+ }
+
+ // second moment of volume relative to center
+ Vector3d centerSquared = m_center.cwiseProduct(m_center);
+ m_secondMomentOfVolume(0, 0) = integral[5] + integral[6] - area * (centerSquared.y() + centerSquared.z());
+ m_secondMomentOfVolume(1, 1) = integral[4] + integral[6] - area * (centerSquared.z() + centerSquared.x());
+ m_secondMomentOfVolume(2, 2) = integral[4] + integral[5] - area * (centerSquared.x() + centerSquared.y());
+ m_secondMomentOfVolume(0, 1) = -(integral[7] - area * m_center.x() * m_center.y());
+ m_secondMomentOfVolume(1, 0) = m_secondMomentOfVolume(0, 1);
+ m_secondMomentOfVolume(1, 2) = -(integral[8] - area * m_center.y() * m_center.z());
+ m_secondMomentOfVolume(2, 1) = m_secondMomentOfVolume(1, 2);
+ m_secondMomentOfVolume(0, 2) = -(integral[9] - area * m_center.z() * m_center.x());
+ m_secondMomentOfVolume(2, 0) = m_secondMomentOfVolume(0, 2);
+
+ m_secondMomentOfVolume *= m_thickness;
+ m_volume = (area * m_thickness);
+}
+
+bool SurfaceMeshShape::isValid() const
+{
+ return (nullptr != m_mesh) && (m_thickness > 1e-5) && (m_mesh->isValid());
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/SurfaceMeshShape.h b/SurgSim/Math/SurfaceMeshShape.h
new file mode 100644
index 0000000..ccf5b4d
--- /dev/null
+++ b/SurgSim/Math/SurfaceMeshShape.h
@@ -0,0 +1,137 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_SURFACEMESHSHAPE_H
+#define SURGSIM_MATH_SURFACEMESHSHAPE_H
+
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_STATIC_REGISTRATION(SurfaceMeshShape);
+
+/// SurfaceMeshShape defines a shape based on a mesh, like MeshShape.
+/// But, unlike MeshShape, the mesh does not need to be watertight to produce valid volume, center and second moment of
+/// volume. In the MeshShape case, these quantities are based on the notion of volume and are therefore undefined
+/// if no volume if properly defined. In the SurfaceMeshShape case, these quantities are based on a surface
+/// mesh, with a thickness (which is considered constant over the surface and should be multiple orders of magnitude
+/// smaller than the two other dimesions, i.e. 1e-3 in practice). If the mesh is not closed or has holes, the class
+/// will still produce valid geometric properties.
+///
+/// \note If not used in physics, there is no differences between using a SurfaceMeshShape or a MeshShape.
+///
+/// \note Any change on the mesh will invalidate the geometric properties.
+/// \note Practical use cases:
+/// \note * Fixed/Rigid object, the mesh will not change anyway.
+/// \note * Deformable object, the mesh will be updated, but the geometric properties will not be used.
+///
+/// \note The thickness should be multiple order of magnitude smaller than the other 2 dimensions of the mesh.
+/// \note It should also not be smaller than 1e-5 to avoid formal and numerical issues when getting close to 0.
+///
+/// \note SurfaceMeshShape does not have any collision algorithm associated with it in SurgSim::Collision and
+/// \note SurgSim::Physics::DcdCollision so far.
+///
+/// \sa MeshShape
+class SurfaceMeshShape : public Shape
+{
+public:
+ /// Constructor
+ SurfaceMeshShape();
+
+ /// Constructor
+ /// \param mesh The triangle mesh to build the shape from
+ /// \param thickness The thickness associated to this surface mesh
+ /// \exception Raise an exception if the mesh is invalid
+ template <class VertexData, class EdgeData, class TriangleData>
+ SurfaceMeshShape(
+ const SurgSim::DataStructures::TriangleMeshBase<VertexData, EdgeData, TriangleData>& mesh,
+ double thickness = 1e-2);
+
+ SURGSIM_CLASSNAME(SurgSim::Math::SurfaceMeshShape);
+
+ /// \return the type of the shape
+ virtual int getType() override;
+
+ /// Get mesh
+ /// \return The collision mesh associated to this MeshShape
+ std::shared_ptr<SurgSim::DataStructures::TriangleMesh> getMesh();
+
+ /// Get the volume of the shape
+ /// \return The volume of the shape (in m-3)
+ virtual double getVolume() const override;
+
+ /// Get the volumetric center of the shape
+ /// \return The center of the shape
+ virtual Vector3d getCenter() const override;
+
+ /// Get the second central moment of the volume, commonly used
+ /// to calculate the moment of inertia matrix
+ /// \return The 3x3 symmetric second moment matrix
+ virtual Matrix33d getSecondMomentOfVolume() const override;
+
+ /// Set loading filename
+ /// \param fileName The filename to load
+ /// \note The mesh will be loaded right after the file name is set,
+ /// if 'fileName' indicates a file containing a valid mesh.
+ /// \note If the valid file contains an empty mesh, i.e. no vertex is specified in that file,
+ /// a empty mesh will be held by this mesh shape.
+ void setFileName(const std::string& fileName);
+
+ /// Get the file name of the external file which contains the triangle mesh.
+ /// \return File name of the external file which contains the triangle mesh.
+ std::string getFileName() const;
+
+ /// Check if this shape contains a valid mesh and the thickness is at least 1e-5 (in meter,
+ /// to avoid formal and numerical issues).
+ /// \return True if this shape contains a valid mesh and thickness is at least 1e-5; Otherwise, false.
+ virtual bool isValid() const override;
+
+private:
+
+ /// Compute useful volume integrals based on the triangle mesh, which
+ /// are used to get the volume , center and second moment of volume.
+ void computeVolumeIntegrals();
+
+ /// Center (considering a uniform distribution in the mesh volume)
+ SurgSim::Math::Vector3d m_center;
+
+ /// Volume (in m-3)
+ double m_volume;
+
+ /// Second moment of volume
+ SurgSim::Math::Matrix33d m_secondMomentOfVolume;
+
+ /// Collision mesh associated to this MeshShape
+ std::shared_ptr<SurgSim::DataStructures::TriangleMesh> m_mesh;
+
+ /// Surface mesh thickness
+ double m_thickness;
+
+ /// File name of the external file which contains the triangle mesh.
+ std::string m_fileName;
+};
+
+}; // Math
+}; // SurgSim
+
+#include "SurgSim/Math/SurfaceMeshShape-inl.h"
+
+#endif // SURGSIM_MATH_SURFACEMESHSHAPE_H
diff --git a/SurgSim/Math/TriangleTriangleContactCalculation-inl.h b/SurgSim/Math/TriangleTriangleContactCalculation-inl.h
new file mode 100644
index 0000000..cafb69b
--- /dev/null
+++ b/SurgSim/Math/TriangleTriangleContactCalculation-inl.h
@@ -0,0 +1,306 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
+#define SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// A helper class for a triangle, used for the following two purposes:
+/// - Clip against a given plane.
+/// - Find the deepest point given a plane.
+/// Created as a triangle and can become 4 or more sided polygon when clipped.
+/// \note The vertices are stored in order, so that the edges of the polygon run between adjacent vertices
+/// (and from the last vertex to the first).
+/// \tparam T Accuracy of the calculation.
+/// \tparam MOpt Eigen Matrix options.
+template <class T, int MOpt>
+class TriangleHelper
+{
+ static const size_t CAPACITY = 10;
+ typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
+ typedef boost::container::static_vector<Vector3, CAPACITY> Vertices;
+
+public:
+ /// Constructor using the triangle data to initialize.
+ /// \param v0, v1, v2 The vertices of the triangle.
+ /// \param n The normal of the triangle.
+ TriangleHelper(const Vector3& v0, const Vector3& v1, const Vector3& v2, const Vector3& n)
+ : m_normal(n), m_receiverBufferIndex(0)
+ {
+ m_vertices[0] = &v0;
+ m_vertices[1] = &v1;
+ m_vertices[2] = &v2;
+ m_planeD = -m_vertices[0]->dot(m_normal);
+ }
+
+ /// Given a triangle, find the deepest vertex in the swept volume of that triangle.
+ /// \param triangle The triangle against which the penetration is checked.
+ /// \param [out] penetrationDepth The depth of the deepest point in this triangle to the triangle sent in.
+ /// \param [out] penetrationPoint0 The penetration point on this triangle.
+ /// \param [out] penetrationPoint1 The penetration point on the triangle sent in.
+ void findDeepestPenetrationWithTriangle(const TriangleHelper& triangle, T* penetrationDepth,
+ Vector3* penetrationPoint0, Vector3* penetrationPoint1)
+ {
+ m_clippedVerticesBuffer[0].push_back(*m_vertices[0]);
+ m_clippedVerticesBuffer[0].push_back(*m_vertices[1]);
+ m_clippedVerticesBuffer[0].push_back(*m_vertices[2]);
+ m_receiverBufferIndex = 1;
+
+ Vector3 clipPlaneNormal;
+ T clipPlaneD;
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ triangle.getPrismPlane(i, &clipPlaneNormal, &clipPlaneD);
+ clipAgainstPlane(clipPlaneNormal, clipPlaneD);
+ }
+
+ findDeepestVertexUnderPlane(triangle.m_normal, triangle.m_planeD, penetrationDepth, penetrationPoint0);
+
+ SURGSIM_ASSERT(*penetrationDepth <= T(0))
+ << "The distance from triangle is calculated as " << *penetrationDepth << ". At this point in the"
+ << " algorithm, the depth is expected to be negative.";
+
+ *penetrationPoint1 = *penetrationPoint0 - (triangle.m_normal * (*penetrationDepth));
+ *penetrationDepth = -(*penetrationDepth);
+ }
+
+private:
+ /// Get the bounding plane of the swept volume of this triangle.
+ /// The swept volume of a triangle is an infinitely long prism.
+ /// \param index There are three prism sides, the index indicates which one is to be calculated.
+ /// \param planeNormal The outward facing normal of the prism plane.
+ /// \param planeD d from the plane equation (n.x + d = 0) of the prism plane.
+ void getPrismPlane(size_t index, Vector3* planeNormal, T* planeD) const
+ {
+ *planeNormal = *m_vertices[(index + 1) % 3] - *m_vertices[index];
+ *planeNormal = planeNormal->cross(m_normal);
+ planeNormal->normalize();
+ *planeD = -m_vertices[index]->dot(*planeNormal);
+ }
+
+ /// Clip the polygon given a plane. Any part of the polygon above this plane is clipped.
+ /// \note This may alter the number of vertices in this polygon.
+ /// \param planeN The normal of the clipping plane.
+ /// \param planeD The d from plane eqn (nx + d) of the clipping plane.
+ void clipAgainstPlane(const Vector3& planeN, T planeD)
+ {
+ // Loop through the edges starting from (m_vertices[0]->m_vertices[1]) to
+ // (m_vertices[m_numVertices - 1]->m_vertices[0]).
+ // The start vertex and end vertex can be either under/on/over the clipping plane.
+ // start | end | action
+ // ----------------------------------------------
+ // under | under | add start to clipped vertices
+ // under | on | add start to clipped vertices
+ // under | over | add start to clipped vertices, clip the edge to the plane (creating a vertex)
+ // on | under | add start to clipped vertices
+ // on | on | add start to clipped vertices
+ // on | over | add start to clipped vertices
+ // over | under | clip the edge to the plane (creating a vertex)
+ // over | on | none
+ // over | over | none
+
+ Vertices& clippedVertices = m_clippedVerticesBuffer[m_receiverBufferIndex];
+ m_receiverBufferIndex = (m_receiverBufferIndex + 1) % 2;
+ Vertices& originalVertices = m_clippedVerticesBuffer[m_receiverBufferIndex];
+ clippedVertices.clear();
+
+ static const T EPSILON = T(Geometry::DistanceEpsilon);
+
+ // Calculate the signed distance of the vertices from the clipping plane.
+ boost::container::static_vector<T, CAPACITY> signedDistanceFromPlane;
+ for (auto it = originalVertices.cbegin(); it != originalVertices.cend(); ++it)
+ {
+ signedDistanceFromPlane.push_back((*it).dot(planeN) + planeD);
+ }
+
+ // Temp variable.
+ T ratio;
+
+ // Iterators for the end vertices of an edge.
+ typename boost::container::static_vector<Vector3, CAPACITY>::iterator end;
+
+ // Iterators for the signed distance from plane of the start and end vertices of an edge.
+ auto startSignedDistance = signedDistanceFromPlane.begin();
+ typename boost::container::static_vector<T, CAPACITY>::iterator endSignedDistance;
+
+ // Iterate over the edges of the current polygon.
+ for (auto start = originalVertices.begin(); start != originalVertices.end(); ++start, ++startSignedDistance)
+ {
+ // If the end has reached the end of list, point it back to the front of list.
+ end = start + 1;
+ endSignedDistance = startSignedDistance + 1;
+ if (end == originalVertices.end())
+ {
+ end = originalVertices.begin();
+ endSignedDistance = signedDistanceFromPlane.begin();
+ }
+
+ // If the vertex is under or on the plane, add to the clippedVertices.
+ if (*startSignedDistance <= EPSILON)
+ {
+ clippedVertices.push_back(*start);
+ }
+
+ // If the edge runs from one side of the plane to another. Clip it.
+ if ((*startSignedDistance < -EPSILON && *endSignedDistance > EPSILON) ||
+ (*startSignedDistance > EPSILON && *endSignedDistance < -EPSILON))
+ {
+ ratio = *startSignedDistance / (*startSignedDistance - *endSignedDistance);
+ clippedVertices.push_back(*start + (*end - *start) * ratio);
+ }
+ }
+ }
+
+ /// Find the deepest vertex of this polygon under the plane.
+ /// \note Asserts if there are no vertices in the polygon.
+ /// \param planeN The normal of the plane.
+ /// \param planeD The distance from origin of the plane.
+ /// \param [out] depth The depth of the deepest point in the polygon from the given plane.
+ /// \param [out] point The deepest point in the polgon from the given plane.
+ void findDeepestVertexUnderPlane(const Vector3& planeN, T planeD, T* depth, Vector3* point) const
+ {
+ const Vertices& originalVertices = m_clippedVerticesBuffer[(m_receiverBufferIndex + 1) % 2];
+
+ SURGSIM_ASSERT(originalVertices.size() > 0)
+ << "There are no vertices under the plane. This scenario should not arise according to the"
+ << " Triangle-Triangle Contact Calculation algorithm, because of the circumstances under which"
+ << " this function is set to be called.";
+
+ T signedDistanceFromPlane;
+ *depth = T(0);
+ for (auto it = originalVertices.cbegin(); it != originalVertices.cend(); ++it)
+ {
+ signedDistanceFromPlane = (*it).dot(planeN) + planeD;
+ if (signedDistanceFromPlane < *depth)
+ {
+ *depth = signedDistanceFromPlane;
+ *point = *it;
+ }
+ }
+ }
+
+ /// Original vertices of the triangle.
+ const Vector3* m_vertices[3];
+
+ /// Normal of the triangle.
+ const Vector3& m_normal;
+
+ /// d from the plane equation (n.x + d = 0) for the plane of this triangle.
+ T m_planeD;
+
+ /// The buffers for the clipped vertices of the triangle.
+ Vertices m_clippedVerticesBuffer[2];
+ size_t m_receiverBufferIndex;
+};
+
+
+template <class T, int MOpt> inline
+bool calculateContactTriangleTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
+ T* penetrationDepth,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
+ Eigen::Matrix<T, 3, 1, MOpt>* contactNormal)
+{
+ typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
+
+ // Check if the triangles intersect.
+ if (!doesIntersectTriangleTriangle(t0v0, t0v1, t0v2, t1v0, t1v1, t1v2, t0n, t1n))
+ {
+ return false;
+ }
+
+ // When control reaches here, the two triangles are definitely intersecting.
+ // Calculate the deepest penetration along each of the triangle normals.
+
+ TriangleHelper<T, MOpt> triangle1(t0v0, t0v1, t0v2, t0n);
+ TriangleHelper<T, MOpt> triangle2(t1v0, t1v1, t1v2, t1n);
+
+ // Penetration info to be calculated.
+ T penetrationDepths[2] = {T(0), T(0)};
+ Vector3 penetrationPoints[2][2];
+
+ // Calculate deepest penetration for each of the triangle.
+ triangle1.findDeepestPenetrationWithTriangle(
+ triangle2, &penetrationDepths[0], &penetrationPoints[0][0], &penetrationPoints[0][1]);
+
+ triangle2.findDeepestPenetrationWithTriangle(
+ triangle1, &penetrationDepths[1], &penetrationPoints[1][1], &penetrationPoints[1][0]);
+
+ // Choose the lower penetration of the two as the contact.
+ if (penetrationDepths[0] < penetrationDepths[1])
+ {
+ *penetrationDepth = penetrationDepths[0];
+ *contactNormal = t1n;
+ *penetrationPoint0 = penetrationPoints[0][0];
+ *penetrationPoint1 = penetrationPoints[0][1];
+ }
+ else
+ {
+ *penetrationDepth = penetrationDepths[1];
+ *contactNormal = -t0n;
+ *penetrationPoint0 = penetrationPoints[1][0];
+ *penetrationPoint1 = penetrationPoints[1][1];
+ }
+
+ return true;
+}
+
+
+template <class T, int MOpt> inline
+bool calculateContactTriangleTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
+ T* penetrationDepth,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
+ Eigen::Matrix<T, 3, 1, MOpt>* contactNormal)
+{
+ Eigen::Matrix<T, 3, 1, MOpt> t0n = (t0v1 - t0v0).cross(t0v2 - t0v0);
+ Eigen::Matrix<T, 3, 1, MOpt> t1n = (t1v1 - t1v0).cross(t1v2 - t1v0);
+ if (t0n.isZero() || t1n.isZero())
+ {
+ // Degenerate triangle(s) passed to calculateContactTriangleTriangle
+ return false;
+ }
+ t0n.normalize();
+ t1n.normalize();
+ return calculateContactTriangleTriangle(t0v0, t0v1, t0v2, t1v0, t1v1, t1v2, t0n, t1n, penetrationDepth,
+ penetrationPoint0, penetrationPoint1, contactNormal);
+}
+
+
+} // namespace Math
+
+} // namespace SurgSim
+
+#endif // SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
diff --git a/SurgSim/Math/TriangleTriangleIntersection-inl.h b/SurgSim/Math/TriangleTriangleIntersection-inl.h
new file mode 100644
index 0000000..da5e2e8
--- /dev/null
+++ b/SurgSim/Math/TriangleTriangleIntersection-inl.h
@@ -0,0 +1,185 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H
+#define SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H
+
+
+namespace
+{
+static const double EPSILOND = 1e-12;
+}
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+
+/// Two ends of the triangle edge are given in terms of the following vertex properties.
+/// - Signed distance from the colliding triangle.
+/// - Projection on the separating axis.
+/// Get the intersection of this edge and the plane in terms of the projection on the separating axis.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \param dStart Signed distance of the start of edge from the plane of the colliding triangle.
+/// \param dEnd Signed distance of the end of edge from the plane of the colliding triangle.
+/// \param pvStart Projection of the start of edge from the plane of the colliding triangle.
+/// \param pvEnd Projection of the end of edge from the plane of the colliding triangle.
+/// \param parametricIntersection Parametric representation of the intersection between the triangle edge
+/// and the plane in terms of the projection on the separating axis.
+/// \param parametricIntersectionIndex The array index of parametricIntersection.
+template<class T>
+void edgeIntersection(T dStart, T dEnd, T pvStart, T pvEnd, T* parametricIntersection,
+ size_t* parametricIntersectionIndex)
+{
+ // Epsilon used in this function.
+ static const T EPSILON = static_cast<T>(EPSILOND);
+
+ bool edgeFromUnderToAbove = dStart < 0.0 && dEnd >= 0.0;
+ bool edgeFromAboveToUnder = dStart > 0.0 && dEnd <= 0.0;
+
+ if (edgeFromUnderToAbove || edgeFromAboveToUnder)
+ {
+ if (std::abs(dStart - dEnd) < EPSILON)
+ {
+ // Start and End are really close. Pick start.
+ parametricIntersection[(*parametricIntersectionIndex)++] = pvStart;
+ }
+ else
+ {
+ // Clip to the point in the intersection of Start->End and plane of the colliding triangle.
+ parametricIntersection[(*parametricIntersectionIndex)++] =
+ pvStart + (pvEnd - pvStart) * (dStart / (dStart - dEnd));
+ }
+ }
+}
+
+template <class T, int MOpt> inline
+bool doesIntersectTriangleTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1n)
+{
+ typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
+
+ if (t0n.isZero() || t1n.isZero())
+ {
+ // Degenerate triangle(s) passed to checkTriangleTriangleIntersection.
+ return false;
+ }
+
+ // Variable names mentioned here are the notations used in the paper:
+ // T1 - Triangle with vertices (t0v0, t0v1, t0v2).
+ // T2 - Triangle with vertices (t1v0, t1v1, t1v2).
+ // d1[3] - Signed distances of the vertices of T1 from the plane of T2.
+ // d2[3] - Signed distances of the vertices of T2 from the plane of T1.
+ // D - Separating axis used for the test. This is calculated as the cross products of the triangle normals.
+ // pv1[3] - Projection of the vertices of T1 onto the separating axis (D).
+ // pv2[3] - Projection of the vertices of T2 onto the separating axis (D).
+ // s1[2] - The intersection between T1 and D is a line segment.
+ // s1[0] and s1[1] are the parametric representation of the ends of this line segment.
+ // s2[2] - The intersection between T2 and D is a line segment.
+ // s2[0] and s2[1] are the parametric representation of the ends of this line segment.
+
+ // Early Rejection test:
+ // If all the vertices of one triangle are on one side of the plane of the other triangle,
+ // there is no intersection.
+
+ // Check if all the vertices of T2 are on one side of p1.
+ // Plane eqn of T1: DotProduct(t0n, X) + distanceFromOrigin = 0
+ // where distanceFromOrigin = -DotProduct(t0n, t0v0)
+ // So, plane eqn of T1: DotProduct(t0n, X - t0v0) = 0
+ // Distance of first vertex of T2 from the plane of T1 is: DotProduct(t0n, t1v0 - t0v0)
+ Vector3 d2(t0n.dot(t1v0 - t0v0), t0n.dot(t1v1 - t0v0), t0n.dot(t1v2 - t0v0));
+
+ if ((d2.array() <= 0.0).all() || (d2.array() >= 0.0).all())
+ {
+ return false;
+ }
+
+ // Check if all the vertices of T1 are on one side of p2.
+ Vector3 d1(t1n.dot(t0v0 - t1v0), t1n.dot(t0v1 - t1v0), t1n.dot(t0v2 - t1v0));
+
+ if ((d1.array() <= 0.0).all() || (d1.array() >= 0.0).all())
+ {
+ return false;
+ }
+
+ // The separating axis.
+ Vector3 D = t0n.cross(t1n);
+
+ // Projection of the triangle vertices on the separating axis.
+ Vector3 pv1(D.dot(t0v0), D.dot(t0v1), D.dot(t0v2));
+ Vector3 pv2(D.dot(t1v0), D.dot(t1v1), D.dot(t1v2));
+
+ // The intersection of the triangles with the separating axis (D).
+ T s1[3];
+ T s2[3];
+ size_t s1Index = 0;
+ size_t s2Index = 0;
+
+ // Loop through the edges of each triangle and find the intersectio of these edges onto
+ // the plane of the other triangle.
+ for (int i = 0; i < 3; ++i)
+ {
+ int j = (i + 1) % 3;
+
+ edgeIntersection(d1[i], d1[j], pv1[i], pv1[j], s1, &s1Index);
+ edgeIntersection(d2[i], d2[j], pv2[i], pv2[j], s2, &s2Index);
+ }
+
+ SURGSIM_ASSERT(s1Index == 2 && s2Index == 2)
+ << "The intersection between the triangle and the separating axis is not a line segment."
+ << " This scenario cannot happen, at this point in the algorithm.";
+
+ return !(s1[0] <= s2[0] && s1[0] <= s2[1] && s1[1] <= s2[0] && s1[1] <= s2[1]) &&
+ !(s1[0] >= s2[0] && s1[0] >= s2[1] && s1[1] >= s2[0] && s1[1] >= s2[1]);
+}
+
+
+template <class T, int MOpt> inline
+bool doesIntersectTriangleTriangle(
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
+ const Eigen::Matrix<T, 3, 1, MOpt>& t1v2)
+{
+ Eigen::Matrix<T, 3, 1, MOpt> t0n = (t0v1 - t0v0).cross(t0v2 - t0v0);
+ Eigen::Matrix<T, 3, 1, MOpt> t1n = (t1v1 - t1v0).cross(t1v2 - t1v0);
+ if (t0n.isZero() || t1n.isZero())
+ {
+ // Degenerate triangle(s) passed to checkTriangleTriangleIntersection.
+ return false;
+ }
+ t0n.normalize();
+ t1n.normalize();
+ return doesIntersectTriangleTriangle(t0v0, t0v1, t0v2, t1v0, t1v1, t1v2, t0n, t1n);
+}
+
+
+} // namespace Math
+
+} // namespace SurgSim
+
+
+#endif // SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H
diff --git a/SurgSim/Math/UnitTests/AabbTests.cpp b/SurgSim/Math/UnitTests/AabbTests.cpp
new file mode 100644
index 0000000..cb8679f
--- /dev/null
+++ b/SurgSim/Math/UnitTests/AabbTests.cpp
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/Aabb.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+TEST(AabbTests, DoIntersectTest)
+{
+ Aabbd center(Vector3d(-1.0, -1.0, -1.0), Vector3d(1.0, 1.0, 1.0));
+
+ // Move the sampler around in all directions only when one of the factors is greater than 2
+ // Should the intersection fail
+
+ for (double i = -3.0; i <= 3.0; i += 1)
+ {
+ for (double j = -3.0; j <= 3.0; j += 1)
+ {
+ for (double k = -3.0; k <= 3.0; k += 1)
+ {
+ Vector3d translation(i, j, k);
+ /// Whenever all of the coordinates are <= 2.0 the result should be true
+ bool expected = (translation.cwiseAbs().array().abs() <= Vector3d(2.0, 2.0, 2.0).array()).all();
+ Aabbd sampler = Aabbd(center).translate(translation);
+
+ EXPECT_EQ(expected, doAabbIntersect(center, sampler)) << "Error For " << translation.transpose();
+ EXPECT_EQ(expected, doAabbIntersect(sampler, center)) << "Error For " << translation.transpose();
+
+ EXPECT_EQ(expected, doAabbIntersect(center, sampler, 0.0)) << "Error For " << translation.transpose();
+ EXPECT_EQ(expected, doAabbIntersect(sampler, center, 0.0)) << "Error For " << translation.transpose();
+
+ double tolerance = 0.01;
+ expected = (translation.cwiseAbs().array().abs() <=
+ Vector3d(2.0 + tolerance, 2.0 + tolerance, 2.0 + tolerance).array()).all();
+
+ EXPECT_EQ(expected, doAabbIntersect(center, sampler, tolerance))
+ << "Error For " << tolerance << " " << translation.transpose();
+ EXPECT_EQ(expected, doAabbIntersect(sampler, center, tolerance))
+ << "Error For " << tolerance << " " << translation.transpose();
+ }
+ }
+ }
+}
+
+TEST(AabbdTests, makeAabb)
+{
+ Vector3d one(0.0, 0.0, 0.0);
+ Vector3d two(-1.0, -1.0, -1.0);
+ Vector3d three(1.0, 1.0, 1.0);
+ Aabbd aabb(makeAabb(one, two, three));
+ Aabbd expected(Vector3d(-1.0, -1.0, -1.0), Vector3d(1.0, 1.0, 1.0));
+ EXPECT_TRUE(expected.isApprox(aabb));
+}
+
+}
+}
+
diff --git a/SurgSim/Math/UnitTests/CMakeLists.txt b/SurgSim/Math/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..07abb38
--- /dev/null
+++ b/SurgSim/Math/UnitTests/CMakeLists.txt
@@ -0,0 +1,89 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ AabbTests.cpp
+ GeometryTests.cpp
+ MakeRigidTransformTests.cpp
+ MeshShapeTests.cpp
+ MlcpGaussSeidelSolverTests.cpp
+ OdeEquationTests.cpp
+ OdeSolverEulerExplicitModifiedTests.cpp
+ OdeSolverEulerExplicitTests.cpp
+ OdeSolverEulerImplicitTests.cpp
+ OdeSolverRungeKutta4Tests.cpp
+ OdeSolverStaticTests.cpp
+ OdeSolverTests.cpp
+ OdeStateTests.cpp
+ ShapeTests.cpp
+ SurfaceMeshShapeTests.cpp
+ TriangleTriangleContactCalculationTests.cpp
+ TriangleTriangleIntersectionTests.cpp
+ ValidTests.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ MockObject.h
+ MockTriangle.h
+ TriangleTriangleTestParameters.h
+)
+
+set(UNIT_TEST_EIGEN_TYPE_SOURCES
+ LinearSolveAndInverseTests.cpp
+ MatrixTests.cpp
+ QuaternionTests.cpp
+ RigidTransformTests.cpp
+ VectorTests.cpp
+)
+
+set(UNIT_TEST_EIGEN_TYPE_HEADERS
+)
+
+option(BUILD_TESTING_EIGEN
+ "Include basic tests for Eigen math class typedefs, which build very slowly."
+ OFF)
+if(BUILD_TESTING_EIGEN)
+ set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} ${UNIT_TEST_EIGEN_TYPE_SOURCES})
+ set(UNIT_TEST_HEADERS ${UNIT_TEST_HEADERS} ${UNIT_TEST_EIGEN_TYPE_HEADERS})
+
+ # Fix a problem with 64-bit builds running out of sections.
+ if(MSVC AND CMAKE_CL_64)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj")
+ endif(MSVC AND CMAKE_CL_64)
+endif(BUILD_TESTING_EIGEN)
+
+set(LIBS
+ SurgSimMath
+ MlcpTestIO
+)
+
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Math/UnitTests/MlcpTestData DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/MeshShapeData DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Data)
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/OctreeShapeData DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Data)
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+surgsim_add_unit_tests(SurgSimMathTest)
+
+set_target_properties(SurgSimMathTest PROPERTIES FOLDER "Math")
diff --git a/SurgSim/Math/UnitTests/GeometryTests.cpp b/SurgSim/Math/UnitTests/GeometryTests.cpp
new file mode 100644
index 0000000..1fd511d
--- /dev/null
+++ b/SurgSim/Math/UnitTests/GeometryTests.cpp
@@ -0,0 +1,1563 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Geometry.cpp functions.
+///
+
+
+#include <gtest/gtest.h>
+#include <array>
+#include <numeric>
+#include <cmath>
+
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/UnitTests/MockTriangle.h"
+#include <boost/math/special_functions/fpclassify.hpp>
+
+namespace SurgSim
+{
+namespace Math
+{
+
+typedef double SizeType;
+typedef Eigen::Matrix<SizeType, 3, 1> VectorType;
+
+::std::ostream& operator <<(std::ostream& stream, const VectorType& vector)
+{
+ stream << "(" << vector[0] << ", " << vector[1] << ", " << vector[2] << ")";
+ return stream;
+}
+
+bool near(double val1, double val2, double abs_error)
+{
+ const double diff = std::abs(val1 - val2);
+ if (diff <= abs_error)
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+::testing::AssertionResult eigenEqual(const VectorType& expected, const VectorType& actual)
+{
+ double precision = 1e-4;
+ if (expected.isApprox(actual, precision))
+ {
+ return ::testing::AssertionSuccess();
+ }
+ else
+ {
+ return ::testing::AssertionFailure() << "Eigen Matrices not the same " << std::endl <<
+ "expected: " << expected << std::endl << "actual: " << actual << std::endl;
+ }
+}
+
+::testing::AssertionResult eigenAllNan(const VectorType& actual)
+{
+ if (boost::math::isnan(actual[0]) && boost::math::isnan(actual[1]) && boost::math::isnan(actual[2]))
+ {
+ return ::testing::AssertionSuccess();
+ }
+ else
+ {
+ return ::testing::AssertionFailure() << "Not all elements are NAN";
+ }
+}
+
+class Segment
+{
+public:
+ VectorType a;
+ VectorType b;
+ VectorType ab;
+
+ Segment() {}
+ Segment(const VectorType& pointA, const VectorType& pointB) :
+ a(pointA), b(pointB), ab(pointB - pointA) {}
+ ~Segment() {}
+ /// Point on the line that the segment is on, s =< 1 and s >= 0 will give you a point on the segment
+ VectorType pointOnLine(double s) const
+ {
+ return a + ab * s;
+ }
+};
+
+
+namespace
+{
+SizeType epsilon = 1e-10;
+}
+
+class GeometryTest : public ::testing::Test
+{
+protected:
+ virtual void SetUp()
+ {
+ plainPoint = VectorType(45, 20, 10);
+ plainSegment = Segment(VectorType(1.1, 2.2, 3.3), VectorType(6.6, 5.5, 4.4));
+ plainNormal = plainSegment.ab.cross(plainPoint); // Normal to segment
+ plainNormal.normalize();
+
+ degenerateSegment.a = plainSegment.a;
+ degenerateSegment.b = degenerateSegment.a + (plainSegment.ab) * 1e-9;
+ degenerateSegment.ab = degenerateSegment.b - degenerateSegment.a;
+
+ plainLine = Segment(VectorType(-10.0, 10, 10), VectorType(10.0, 10.0, 10.0));
+ parallelLine = Segment(VectorType(-100.0, 5.0, 5.0), VectorType(-90.0, 5.0, 5.0));
+ intersectingLine = Segment(VectorType(0, 0, 0), VectorType(20, 20, 20));
+ nonIntersectingLine = Segment(VectorType(5, 5, -5), VectorType(5, 5, 5));
+
+ tri = MockTriangle(VectorType(5, 0, 0), VectorType(0, -5, -5), VectorType(0, 5, 5));
+ }
+
+ virtual void TearDown()
+ {
+ }
+ VectorType plainPoint;
+ Vector3d plainNormal;
+ Segment plainSegment;
+ Segment degenerateSegment;
+
+ Segment plainLine;
+ Segment parallelLine;
+ Segment intersectingLine;
+ Segment nonIntersectingLine;
+
+ MockTriangle tri;
+};
+
+
+TEST_F(GeometryTest, BaryCentricWithNormal)
+{
+ // Order of Points is v0,v1,v2
+ //Check Edges first
+ VectorType outputPoint;
+ EXPECT_TRUE(barycentricCoordinates(tri.v0, tri.v0, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(1, 0, 0), outputPoint));
+
+ EXPECT_TRUE(barycentricCoordinates(tri.v1, tri.v0, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(0, 1, 0), outputPoint));
+
+ EXPECT_TRUE(barycentricCoordinates(tri.v2, tri.v0, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(0, 0, 1), outputPoint));
+
+ // Halfway points
+ EXPECT_TRUE(barycentricCoordinates<double>(tri.pointInTriangle(0.5, 0),
+ tri.v0, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(0.5, 0.5, 0), outputPoint));
+
+ EXPECT_TRUE(barycentricCoordinates<double>(tri.pointInTriangle(0, 0.5),
+ tri.v0, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(0.5, 0.0, 0.5), outputPoint));
+
+ // Center Point
+ VectorType inputPoint;
+ inputPoint = (tri.v0 + tri.v1 + tri.v2) / 3;
+ EXPECT_TRUE(barycentricCoordinates(inputPoint, tri.v0, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0), outputPoint));
+
+ // random Point
+ inputPoint = tri.v0 * 0.2 + tri.v1 * 0.25 + tri.v2 * 0.55;
+ EXPECT_TRUE(barycentricCoordinates(inputPoint, tri.v0, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(0.2, 0.25, 0.55), outputPoint));
+
+ // Degenerate
+ EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v1, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenAllNan(outputPoint));
+
+ EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v0, tri.v0, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenAllNan(outputPoint));
+
+ EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v2, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenAllNan(outputPoint));
+}
+
+TEST_F(GeometryTest, BaryCentricWithoutNormal)
+{
+ // Order of Points is v0,v1,v2
+ //Check Edges first
+ VectorType outputPoint;
+ EXPECT_TRUE(barycentricCoordinates(tri.v0, tri.v0, tri.v1, tri.v2, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(1, 0, 0), outputPoint));
+
+ EXPECT_TRUE(barycentricCoordinates(tri.v1, tri.v0, tri.v1, tri.v2, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(0, 1, 0), outputPoint));
+
+ EXPECT_TRUE(barycentricCoordinates(tri.v2, tri.v0, tri.v1, tri.v2, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(0, 0, 1), outputPoint));
+
+ // Halfway points
+ EXPECT_TRUE(barycentricCoordinates<double>(tri.pointInTriangle(0.5, 0),
+ tri.v0, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(0.5, 0.5, 0), outputPoint));
+
+ EXPECT_TRUE(barycentricCoordinates<double>(tri.pointInTriangle(0, 0.5),
+ tri.v0, tri.v1, tri.v2, tri.n, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(0.5, 0.0, 0.5), outputPoint));
+
+ // Center Point
+ VectorType inputPoint;
+ inputPoint = (tri.v0 + tri.v1 + tri.v2) / 3;
+ EXPECT_TRUE(barycentricCoordinates(inputPoint, tri.v0, tri.v1, tri.v2, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0), outputPoint));
+
+ // random Point
+ inputPoint = tri.v0 * 0.2 + tri.v1 * 0.25 + tri.v2 * 0.55;
+ EXPECT_TRUE(barycentricCoordinates(inputPoint, tri.v0, tri.v1, tri.v2, &outputPoint));
+ EXPECT_TRUE(eigenEqual(VectorType(0.2, 0.25, 0.55), outputPoint));
+
+ // Degenerate
+ EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v1, tri.v1, tri.v2, &outputPoint));
+ EXPECT_TRUE(eigenAllNan(outputPoint));
+
+ EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v0, tri.v0, tri.v2, &outputPoint));
+ EXPECT_TRUE(eigenAllNan(outputPoint));
+
+ EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v2, tri.v1, tri.v2, &outputPoint));
+ EXPECT_TRUE(eigenAllNan(outputPoint));
+}
+
+TEST_F(GeometryTest, DistancePointLine)
+{
+ SizeType distance;
+ Vector3d result;
+
+ // Trivial point lies on the line
+ distance = distancePointLine<SizeType>(plainSegment.pointOnLine(0.5), plainSegment.a, plainSegment.b, &result);
+ EXPECT_NEAR(0.0, distance, epsilon);
+ EXPECT_EQ(plainSegment.pointOnLine(0.5), result);
+
+ // Point is away from the line
+ Vector3d offLinePoint = plainSegment.a + (plainNormal * 1.5);
+ distance = distancePointLine(offLinePoint, plainSegment.a, plainSegment.b, &result);
+ EXPECT_NEAR(1.5, distance, epsilon);
+ EXPECT_EQ(plainSegment.a, result);
+
+ // Degenerate line, just do plain distance
+ offLinePoint = plainSegment.a + plainNormal * 1.5;
+ distance = distancePointLine(offLinePoint, plainSegment.a, plainSegment.a, &result);
+ EXPECT_NEAR(1.5, distance, epsilon);
+ EXPECT_EQ(plainSegment.a, result);
+}
+
+TEST_F(GeometryTest, DistancePointSegment)
+{
+ SizeType distance;
+ Vector3d result;
+
+ // Trivial point lies on the line
+ distance = distancePointSegment(plainSegment.pointOnLine(0.5), plainSegment.a, plainSegment.b, &result);
+ EXPECT_NEAR(0.0, distance, epsilon);
+ EXPECT_EQ(plainSegment.pointOnLine(0.5), result);
+
+ // Point On the line but outside the segment
+ VectorType point = plainSegment.pointOnLine(1.5);
+ distance = distancePointSegment(point, plainSegment.a, plainSegment.b, &result);
+ EXPECT_NEAR((plainSegment.ab.norm() * 0.5), distance, epsilon);
+ EXPECT_TRUE(eigenEqual(plainSegment.b, result));
+
+ // Point projection is on the segment
+ VectorType resultPoint = plainSegment.a + plainSegment.ab * 0.25;
+ VectorType offLinePoint = resultPoint + (plainNormal * 1.5);
+ distance = distancePointSegment(offLinePoint, plainSegment.a, plainSegment.b, &result);
+ EXPECT_NEAR(1.5, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(resultPoint, result));
+
+ // Point projection is away from the segment, distance is to the closest segment point
+ resultPoint = plainSegment.a;
+ offLinePoint = plainSegment.a - plainSegment.ab + (plainNormal * 1.5);
+ distance = distancePointSegment(offLinePoint, plainSegment.a, plainSegment.b, &result);
+ EXPECT_NEAR((offLinePoint - resultPoint).norm(), distance, epsilon);
+ EXPECT_TRUE(eigenEqual(resultPoint, result));
+
+ // Other Side of the above case
+ resultPoint = plainSegment.b;
+ offLinePoint = plainSegment.b + plainSegment.ab * 0.01 + plainNormal * 0.1;
+ distance = distancePointSegment(offLinePoint, plainSegment.a, plainSegment.b, &result);
+ EXPECT_NEAR((offLinePoint - resultPoint).norm(), distance, epsilon);
+ EXPECT_TRUE(eigenEqual(resultPoint, result));
+
+
+ // Degenerated Segment
+ distance = distancePointSegment(offLinePoint, plainSegment.a, plainSegment.a, &result);
+ EXPECT_NEAR((offLinePoint - plainSegment.a).norm(), distance, epsilon);
+ EXPECT_TRUE(eigenEqual(plainSegment.a, result));
+}
+
+typedef std::tuple<Segment, Segment, VectorType, VectorType> LineLineCheckData;
+void checkLineLineDistance(const LineLineCheckData& data)
+{
+ Segment line0 = std::get<0>(data);
+ Segment line1 = std::get<1>(data);
+ VectorType expectedResult0 = std::get<2>(data);
+ VectorType expectedResult1 = std::get<3>(data);
+ VectorType result0, result1;
+ double distance;
+
+ {
+ SCOPED_TRACE("Forward Case");
+ distance = distanceLineLine(line0.a, line0.b, line1.a, line1.b, &result0, &result1);
+ EXPECT_NEAR((expectedResult1 - expectedResult0).norm(), distance, epsilon);
+ EXPECT_TRUE(expectedResult0.isApprox(result0));
+ EXPECT_TRUE(expectedResult1.isApprox(result1));
+ }
+
+ {
+ SCOPED_TRACE("Backward Case");
+ distance = distanceLineLine(line1.a, line1.b, line0.a, line0.b, &result1, &result0);
+ EXPECT_NEAR((expectedResult1 - expectedResult0).norm(), distance, epsilon);
+ EXPECT_TRUE(expectedResult0.isApprox(result0));
+ EXPECT_TRUE(expectedResult1.isApprox(result1));
+ }
+
+ {
+ SCOPED_TRACE("Turn around the first line");
+ distance = distanceLineLine(line0.b, line0.a, line1.a, line1.b, &result0, &result1);
+ EXPECT_NEAR((expectedResult1 - expectedResult0).norm(), distance, epsilon);
+ EXPECT_TRUE(expectedResult0.isApprox(result0));
+ EXPECT_TRUE(expectedResult1.isApprox(result1));
+ }
+
+ {
+ SCOPED_TRACE("And switch lines again");
+ distance = distanceLineLine(line0.b, line0.a, line1.a, line1.b, &result0, &result1);
+ EXPECT_NEAR((expectedResult1 - expectedResult0).norm(), distance, epsilon);
+ EXPECT_TRUE(expectedResult0.isApprox(result0));
+ EXPECT_TRUE(expectedResult1.isApprox(result1));
+ }
+
+
+
+}
+
+TEST_F(GeometryTest, DistanceLineLine)
+{
+ SizeType distance;
+ VectorType p0, p1;
+
+ // Trivial the same line compared against itself
+ distance = distanceLineLine(plainSegment.a, plainSegment.b, plainSegment.a, plainSegment.b, &p0, &p1);
+ EXPECT_NEAR(0.0, distance, epsilon);
+
+ // Parallel Line
+ Segment parallel = Segment(plainSegment.a + plainNormal * 2, plainSegment.b + plainNormal * 2);
+ distance = distanceLineLine(plainSegment.a, plainSegment.b, parallel.a, parallel.b, &p0, &p1);
+ EXPECT_NEAR(2.0, distance, epsilon);
+
+ // Not quite parallel, trying to get below epsilon
+ parallel = Segment(plainSegment.a + plainNormal * 2 , plainSegment.b + plainNormal * 2 - plainNormal * 1.0e-10);
+ distance = distanceLineLine(plainSegment.a, plainSegment.b, parallel.a, parallel.b, &p0, &p1);
+ EXPECT_NEAR(2.0, distance, epsilon);
+
+ {
+ SCOPED_TRACE("Intersecting Lines");
+ LineLineCheckData data(plainLine, intersectingLine, plainLine.b, plainLine.b);
+ checkLineLineDistance(data);
+ }
+
+ {
+ SCOPED_TRACE("Non-intersecting Lines");
+ // Non Intersecting Line, don't know a better way to design this case besides reimplementing line/line distance
+ Segment line0(VectorType(0, -5, 0), VectorType(0, 5, 0));
+ Segment line1(VectorType(-5, 5, 5), VectorType(5, 5, 5));
+ checkLineLineDistance(LineLineCheckData(line0, line1, VectorType(0, 5, 0), VectorType(0, 5, 5)));
+ }
+
+
+ // Degenerate Cases
+ {
+ SCOPED_TRACE("Both lines degenerate");
+ Segment line0(VectorType(0, -5, 0), VectorType(0, -5, 0));
+ Segment line1(VectorType(5, 5, 5), VectorType(5, 5, 5));
+ checkLineLineDistance(LineLineCheckData(line0, line1, line0.a, line1.a));
+ }
+
+ {
+ SCOPED_TRACE("Only one line degenerate");
+ VectorType offLinePoint = plainSegment.a + plainSegment.ab * 0.5 + plainNormal * 1.5;
+ LineLineCheckData data(plainSegment, Segment(offLinePoint, offLinePoint),
+ plainSegment.pointOnLine(0.5), offLinePoint);
+ checkLineLineDistance(data);
+ }
+
+ {
+ SCOPED_TRACE("Orthogonal Lines intersecting");
+ Segment line0(VectorType(0, -5, 0), VectorType(0, 5, 0));
+ VectorType v(3, 4, 5);
+ VectorType n = line0.ab.cross(v);
+ n.normalize();
+ Segment line1(line0.a + n, line0.a - n);
+ checkLineLineDistance(LineLineCheckData(line0, line1, line0.a, line0.a));
+ }
+
+ {
+ SCOPED_TRACE("Orthogonal Lines non intersecting");
+ Segment line0(VectorType(0, -5, 0), VectorType(0, 5, 0));
+ VectorType v(3, 4, 5);
+ VectorType n = line0.ab.cross(v);
+ n.normalize();
+ VectorType n2 = line0.ab.cross(n);
+ n2.normalize();
+ Segment line1(line0.a + n + n2, line0.a - n + n2);
+ checkLineLineDistance(LineLineCheckData(line0, line1, line0.a, line0.a + n2));
+ }
+
+ {
+ SCOPED_TRACE("not quite orthogonal Lines non intersecting ");
+ Segment line0(VectorType(0, -5, 0), VectorType(0, 5, 0));
+ VectorType v(3, 4, 5);
+ VectorType n = line0.ab.cross(v);
+ n.normalize();
+ VectorType n2 = line0.ab.cross(n);
+ n2.normalize();
+ Segment line1(line0.a + n + n2 + line0.ab * 0.01, line0.a - n + n2 - line0.ab * 0.01);
+ checkLineLineDistance(LineLineCheckData(line0, line1, line0.a, line0.a + n2));
+ }
+
+}
+
+
+
+struct SegmentData
+{
+ Segment segment0;
+ Segment segment1;
+ VectorType p0;
+ VectorType p1;
+ SegmentData() {}
+ SegmentData(Segment seg0, Segment seg1, VectorType vec0, VectorType vec1) :
+ segment0(seg0), segment1(seg1), p0(vec0), p1(vec1) {}
+};
+
+
+void testSegmentDistance(const SegmentData& segmentData, const std::string& info, size_t i)
+{
+ SizeType distance;
+ VectorType p0, p1;
+
+ // The expected distance should be the distance between the two points that were
+ // reported as being the closes ones
+ SizeType expectedDistance = (segmentData.p1 - segmentData.p0).norm();
+
+ distance = distanceSegmentSegment(segmentData.segment0.a, segmentData.segment0.b,
+ segmentData.segment1.a, segmentData.segment1.b, &p0, &p1);
+ EXPECT_NEAR(expectedDistance, distance, 1e-8) << "for " << info << " at index " << i;
+ EXPECT_TRUE(eigenEqual(segmentData.p0, p0)) << "for " << info << " at index " << i;
+ EXPECT_TRUE(eigenEqual(segmentData.p1, p1)) << "for " << info << " at index " << i;
+
+ distance = distanceSegmentSegment(segmentData.segment1.a, segmentData.segment1.b,
+ segmentData.segment0.a, segmentData.segment0.b, &p0, &p1);
+ EXPECT_NEAR(expectedDistance, distance, 1e-8) << "for " << info << " at index " << i;
+ EXPECT_TRUE(eigenEqual(segmentData.p1, p0)) << "for " << info << " at index " << i;
+ EXPECT_TRUE(eigenEqual(segmentData.p0, p1)) << "for " << info << " at index " << i;
+}
+
+
+
+TEST_F(GeometryTest, DistanceSegmentSegment)
+{
+ SizeType distance;
+ VectorType p0, p1;
+
+ // Intersecting segments
+ // Intersecting inside
+ VectorType closestPoint = plainSegment.pointOnLine(0.5);
+ Segment otherSegment(closestPoint + plainNormal, closestPoint - plainNormal);
+
+ distance = distanceSegmentSegment(plainSegment.a, plainSegment.b, otherSegment.a, otherSegment.b, &p0, &p1);
+ EXPECT_NEAR(0.0, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(closestPoint, p0));
+ EXPECT_TRUE(eigenEqual(closestPoint, p1));
+
+ // Explode the cases
+ std::vector<SegmentData> segments;
+
+ // The following series test the segment to segment distance for
+ // a) coplanar segments
+ // b) non coplanar segments
+ // for each of the groups multiple pairs of segments are testes in various configurations, where the
+ // projections either intersect or don't, this should cover all the segments that are used in the
+ // algorithm, inside testSegmentDistance the segments are also swapped around and tested against each other
+
+ // <0> Intersecting outside past b with segment straddling the line
+ closestPoint = plainSegment.pointOnLine(1.5);
+ otherSegment = Segment(closestPoint + plainNormal, closestPoint - plainNormal);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.b, closestPoint));
+
+ // <1> segment not straddling, the correct points on the edges of the segments should get picked
+ otherSegment = Segment(closestPoint + plainNormal, closestPoint + plainNormal * 2);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.b, otherSegment.a));
+
+ // <2> segment not straddling, reverse the order of the points, reverse the side where the other segments falls
+ otherSegment = Segment(closestPoint - plainNormal * 2, closestPoint - plainNormal);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.b, otherSegment.b));
+
+ // Go to the other side of the segment
+ closestPoint = plainSegment.pointOnLine(-0.5);
+ // <3> Straddling, there is actual an intersection
+ otherSegment = Segment(closestPoint + plainNormal, closestPoint - plainNormal);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.a, closestPoint));
+
+ // <4> segment not straddling, the correct points on the edges of the segments should get picked
+ otherSegment = Segment(closestPoint + plainNormal, closestPoint + plainNormal * 2);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.a, otherSegment.a));
+
+
+ // <5> segment not straddling, reverse the order of the points, reverse the side where the other segments falls
+ otherSegment = Segment(closestPoint - plainNormal * 2, closestPoint - plainNormal);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.a, otherSegment.b));
+
+
+ // Repeat the same sequence for the segments as they are not touching
+ VectorType otherNormal = plainSegment.ab.cross(plainNormal);
+
+ // <6> segment projections intersect
+ closestPoint = plainSegment.pointOnLine(0.5) + plainNormal * 3;
+ otherSegment = Segment(closestPoint + otherNormal, closestPoint - otherNormal);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.pointOnLine(0.5), closestPoint));
+
+ // <7> go past the end of the segment but straddle the line (T intersection)
+ closestPoint = plainSegment.pointOnLine(1.5) + plainNormal * 3;
+ otherSegment = Segment(closestPoint + otherNormal, closestPoint - otherNormal);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.b, closestPoint));
+
+ // <8> go past the end of the segment not straddling the line anymore
+ otherSegment = Segment(closestPoint + otherNormal, closestPoint + otherNormal * 2);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.b, otherSegment.a));
+
+ // <9> go past the end of the on the other side, switching up endpoints
+ otherSegment = Segment(closestPoint - otherNormal * 2, closestPoint - otherNormal);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.b, otherSegment.b));
+
+ // Repeat for the other end of the base segment
+ // <10> go past the end of the segment but straddle the line (T intersection)
+ closestPoint = plainSegment.pointOnLine(-2.0) + plainNormal * 3;
+ otherSegment = Segment(closestPoint + otherNormal, closestPoint - otherNormal);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.a, closestPoint));
+
+ // <11> go past the end of the segment not straddling the line anymore
+ otherSegment = Segment(closestPoint + otherNormal, closestPoint + otherNormal * 2);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.a, otherSegment.a));
+
+ // <12> go past the end of the on the other side, switching up endpoints
+ otherSegment = Segment(closestPoint - otherNormal * 2, closestPoint - otherNormal);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.a, otherSegment.b));
+
+ // <13> projections intersect, short segments
+ const VectorType aPoint = VectorType(1, 0, 0);
+ const SizeType shortLength = 0.0001;
+ const Segment shortSegment = Segment(VectorType(0, -shortLength/2, 0), VectorType(0, shortLength/2, 0));
+ const VectorType shortSegmentNormal = shortSegment.ab.cross(aPoint).normalized();
+ const Segment otherShortSegment = Segment(aPoint, aPoint + shortLength * shortSegmentNormal);
+ segments.push_back(SegmentData(shortSegment, otherShortSegment,
+ shortSegment.pointOnLine(0.5), otherShortSegment.a));
+
+ for (size_t i = 0; i < segments.size(); ++i)
+ {
+ testSegmentDistance(segments[i], "basic cases", i);
+ }
+
+ // Parallel Segments
+ otherSegment = Segment(plainSegment.a + plainNormal * 4, plainSegment.b + plainNormal * 4);
+ distance = distanceSegmentSegment(plainSegment.a, plainSegment.b, otherSegment.a, otherSegment.b, &p0, &p1);
+ EXPECT_NEAR(4.0, distance, epsilon);
+ // What should the points be here ?
+
+ segments.clear();
+
+ // <0> parallel, non-overlapping
+ closestPoint = plainSegment.a;
+ const Vector3d segmentDirection = plainSegment.a - plainSegment.b;
+ otherSegment = Segment(closestPoint + plainNormal * 4 + 2 * segmentDirection,
+ closestPoint + plainNormal * 4 + 4 * segmentDirection);
+ distance = distanceSegmentSegment(plainSegment.a, plainSegment.b, otherSegment.a, otherSegment.b, &p0, &p1);
+ segments.push_back(SegmentData(plainSegment, otherSegment, closestPoint, otherSegment.a));
+
+ // Anti-parallel Segments
+ otherSegment = Segment(plainSegment.b + plainNormal * 4, plainSegment.a + plainNormal * 4);
+ distance = distanceSegmentSegment(plainSegment.a, plainSegment.b, otherSegment.a, otherSegment.b, &p0, &p1);
+ EXPECT_NEAR(4.0, distance, epsilon);
+
+ // <1> anti-parallel, non-overlapping
+ closestPoint = plainSegment.a;
+ otherSegment = Segment(closestPoint + plainNormal * 4 + 4 * segmentDirection,
+ closestPoint + plainNormal * 4 + 2 * segmentDirection);
+ distance = distanceSegmentSegment(plainSegment.a, plainSegment.b, otherSegment.a, otherSegment.b, &p0, &p1);
+ segments.push_back(SegmentData(plainSegment, otherSegment, closestPoint, otherSegment.b));
+
+ for (size_t i = 0; i < segments.size(); ++i)
+ {
+ testSegmentDistance(segments[i], "parallel cases", i);
+ }
+
+ segments.clear();
+
+ // The closest points are some assumptions, it looks like the algorithm is slanted towards
+ // <0> the beginning points of the segments for this
+ closestPoint = plainSegment.pointOnLine(0.5);
+ otherSegment = Segment(closestPoint + plainNormal * 4, closestPoint + plainNormal * 8);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.pointOnLine(0.5), otherSegment.a));
+
+ // <1> Move past the end of the segment on the far end
+ closestPoint = plainSegment.pointOnLine(1.5);
+ otherSegment = Segment(closestPoint + plainNormal * 4, closestPoint + plainNormal * 8);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.b, otherSegment.a));
+
+ // <2> Move past the end of the segment on the near end
+ closestPoint = plainSegment.pointOnLine(-2.0);
+ otherSegment = Segment(closestPoint - plainNormal * 8, closestPoint - plainNormal * 4);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.a, otherSegment.b));
+
+
+ // Degenerate cases delegate to PointSegDistance, just some spotChecks
+ // <3> On the segment
+ closestPoint = plainSegment.pointOnLine(0.5);
+ otherSegment = Segment(closestPoint, closestPoint);
+ segments.push_back(SegmentData(plainSegment, otherSegment, closestPoint, closestPoint));
+
+ // <4> off the segment
+ closestPoint = plainSegment.pointOnLine(1.5) + plainNormal * 4 + otherNormal * 10;
+ otherSegment = Segment(closestPoint, closestPoint);
+ segments.push_back(SegmentData(plainSegment, otherSegment, plainSegment.b, closestPoint));
+
+
+ for (size_t i = 0; i < segments.size(); ++i)
+ {
+ testSegmentDistance(segments[i], "other cases", i);
+ }
+}
+
+TEST_F(GeometryTest, DistancePointTriangle)
+{
+ double distance;
+ VectorType closestPoint;
+ VectorType result;
+ VectorType inputPoint;
+
+ // Trivial, point on triangle
+ inputPoint = VectorType(0, 0, 0);
+ distance = distancePointTriangle(inputPoint, tri.v0, tri.v1, tri.v2, &result);
+ EXPECT_NEAR(0.0, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(inputPoint, result));
+
+ distance = distancePointTriangle(tri.v1, tri.v0, tri.v1, tri.v2, &result);
+ EXPECT_NEAR(0.0, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(tri.v1, result));
+
+ // Closest Point is inside Triangle
+ closestPoint = tri.v0 + tri.v0v1 * 0.3 + tri.v0v2 * 0.7;
+ inputPoint = closestPoint + tri.n * 2.5;
+ distance = distancePointTriangle(inputPoint, tri.v0, tri.v1, tri.v2, &result);
+ EXPECT_NEAR(2.5, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(closestPoint, result));
+
+ // other side
+ inputPoint = closestPoint - tri.n * 3.5;
+ distance = distancePointTriangle(inputPoint, tri.v0, tri.v1, tri.v2, &result);
+ EXPECT_NEAR(3.5, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(closestPoint, result));
+
+ // Test the Point close to a triangle Edge cases
+ // Point closest to edge v0v1
+ double expectedDistance;
+ inputPoint = tri.v0 + tri.v0v1 * 0.5 - tri.v0v2 + tri.n;
+ distance = distancePointTriangle(inputPoint, tri.v0, tri.v1, tri.v2, &result);
+ expectedDistance = distancePointSegment(inputPoint, tri.v0, tri.v1, &closestPoint);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(closestPoint, result));
+
+ // Point closest to edge v0v2
+ inputPoint = tri.v0 + tri.v0v2 * 0.3 - tri.v0v1 + tri.n * 2;
+ distance = distancePointTriangle(inputPoint, tri.v0, tri.v1, tri.v2, &result);
+ expectedDistance = distancePointSegment(inputPoint, tri.v0, tri.v2, &closestPoint);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(closestPoint, result));
+
+ // Point closest to edge v1v2
+ inputPoint = tri.v1 + (tri.v2 - tri.v1) * .75 + tri.v0v1 * 0.2 + tri.n;
+ distance = distancePointTriangle(inputPoint, tri.v0, tri.v1, tri.v2, &result);
+ expectedDistance = distancePointSegment(inputPoint, tri.v1, tri.v2, &closestPoint);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(closestPoint, result));
+
+ // Point closest to point v0
+ inputPoint = tri.v0 - tri.v0v1 - tri.v0v2 * 0.5 - tri.n;
+ distance = distancePointTriangle(inputPoint, tri.v0, tri.v1, tri.v2, &result);
+ expectedDistance = (tri.v0 - inputPoint).norm();
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(tri.v0, result));
+
+ // Point closest to point v1
+ inputPoint = tri.v1 + tri.v0v1 + (tri.v1 - tri.v2) * 2.0 - tri.n * 2.0;
+ distance = distancePointTriangle(inputPoint, tri.v0, tri.v1, tri.v2, &result);
+ expectedDistance = (tri.v1 - inputPoint).norm();
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(tri.v1, result));
+
+ // Point closest to point v2
+ inputPoint = tri.v2 + tri.v0v2 + (tri.v2 - tri.v1) * 3.0 - tri.n * 1.5;
+ distance = distancePointTriangle(inputPoint, tri.v0, tri.v1, tri.v2, &result);
+ expectedDistance = (tri.v2 - inputPoint).norm();
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(tri.v2, result));
+
+ // Degenerate Edges
+ // Edge v0v1
+ distance = distancePointTriangle(inputPoint,
+ tri.v0, (tri.v0 + tri.v0v1 * epsilon * 0.01).eval(), tri.v2,
+ &result);
+ expectedDistance = distancePointSegment(inputPoint, tri.v0, tri.v2, &closestPoint);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(closestPoint, result));
+
+ // Edge v0v2
+ distance = distancePointTriangle(inputPoint,
+ (tri.v2 - tri.v0v2 * epsilon * 0.01).eval(), tri.v1 , tri.v2,
+ &result);
+ expectedDistance = distancePointSegment(inputPoint, tri.v1, tri.v2, &closestPoint);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(closestPoint, result));
+
+ // Edge v1v2
+ distance = distancePointTriangle(inputPoint, tri.v0, tri.v1 , tri.v1, &result);
+ expectedDistance = distancePointSegment(inputPoint, tri.v1, tri.v0, &closestPoint);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(eigenEqual(closestPoint, result));
+}
+
+TEST_F(GeometryTest, PointInsideTriangleWithNormal)
+{
+ EXPECT_TRUE(isPointInsideTriangle(tri.v0, tri.v0, tri.v1, tri.v2, tri.n));
+ EXPECT_TRUE(isPointInsideTriangle(tri.v1, tri.v0, tri.v1, tri.v2, tri.n));
+ EXPECT_TRUE(isPointInsideTriangle(tri.v2, tri.v0, tri.v1, tri.v2, tri.n));
+
+ VectorType inputPoint = tri.v0 + tri.v0v1 * 0.2;
+ EXPECT_TRUE(isPointInsideTriangle(inputPoint, tri.v0, tri.v1, tri.v2, tri.n));
+ inputPoint += tri.v0v2 * 0.5;
+ EXPECT_TRUE(isPointInsideTriangle(inputPoint, tri.v0, tri.v1, tri.v2, tri.n));
+
+ inputPoint = tri.v0 + tri.v0v1 * 1.5;
+ EXPECT_FALSE(isPointInsideTriangle(inputPoint, tri.v0, tri.v1, tri.v2, tri.n));
+ EXPECT_FALSE(isPointInsideTriangle(inputPoint, tri.v1, tri.v1, tri.v2, tri.n));
+
+ inputPoint = tri.v0 + tri.v0v2 * 2 + tri.v0v1 * 2;
+ EXPECT_FALSE(isPointInsideTriangle(inputPoint, tri.v0, tri.v1, tri.v2, tri.n));
+
+}
+
+TEST_F(GeometryTest, PointInsideTriangleWithoutNormal)
+{
+ EXPECT_TRUE(isPointInsideTriangle(tri.v0, tri.v0, tri.v1, tri.v2));
+ EXPECT_TRUE(isPointInsideTriangle(tri.v1, tri.v0, tri.v1, tri.v2));
+ EXPECT_TRUE(isPointInsideTriangle(tri.v2, tri.v0, tri.v1, tri.v2));
+
+ VectorType inputPoint = tri.v0 + tri.v0v1 * 0.2;
+ EXPECT_TRUE(isPointInsideTriangle(inputPoint, tri.v0, tri.v1, tri.v2));
+ inputPoint += tri.v0v2 * 0.5;
+ EXPECT_TRUE(isPointInsideTriangle(inputPoint, tri.v0, tri.v1, tri.v2));
+
+ inputPoint = tri.v0 + tri.v0v1 * 1.5;
+ EXPECT_FALSE(isPointInsideTriangle(inputPoint, tri.v0, tri.v1, tri.v2));
+ EXPECT_FALSE(isPointInsideTriangle(inputPoint, tri.v1, tri.v1, tri.v2));
+
+ inputPoint = tri.v0 + tri.v0v2 * 2 + tri.v0v1 * 2;
+ EXPECT_FALSE(isPointInsideTriangle(inputPoint, tri.v0, tri.v1, tri.v2));
+}
+
+TEST_F(GeometryTest, Coplanarity)
+{
+ struct CoplanarityTestCandidate
+ {
+ bool expected;
+ std::array<Vector3d, 4> points;
+ };
+
+ CoplanarityTestCandidate candidates[] =
+ {
+ {true, Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)},
+ {true, Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0), Vector3d(3.7, 0.0, 0.0)},
+ {true, Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0), Vector3d(2.3, 0.0, 0.0), Vector3d(3.7, 0.0, 0.0)},
+ {true, Vector3d(0.0, 0.0, 0.0), Vector3d(1.1, 0.0, 0.0), Vector3d(2.3, 0.0, 0.0), Vector3d(3.7, 0.0, 0.0)},
+ {true, Vector3d(0.0, 0.0, 0.0), Vector3d(1.1, 1.5, 0.0), Vector3d(2.3, 0.0, 0.0), Vector3d(3.7, 0.0, 0.0)},
+ {true, Vector3d(0.0, 0.0, 0.0), Vector3d(1.1, 1.5, 0.0), Vector3d(2.3, 0.0, 0.0), Vector3d(3.7, 3.0, 0.0)},
+ {false, Vector3d(0.0, 0.0, 1.0), Vector3d(1.1, 1.5, 0.0), Vector3d(2.3, 0.0, 0.0), Vector3d(3.7, 3.0, 0.0)},
+ {false, Vector3d(0.0, 0.0, 0.0), Vector3d(1.1, 1.5, 1.1), Vector3d(2.3, 0.0, 0.0), Vector3d(3.7, 3.0, 0.0)},
+ {false, Vector3d(0.0, 0.0, 0.0), Vector3d(1.1, 1.5, 0.0), Vector3d(2.3, 0.0, 7.7), Vector3d(3.7, 3.0, 0.0)},
+ {false, Vector3d(0.0, 0.0, 0.0), Vector3d(1.1, 1.5, 0.0), Vector3d(2.3, 0.0, 0.0), Vector3d(3.7, 3.0, -9.6)},
+
+ {true, Vector3d(0.0, 0.0, 0.0), Vector3d(1.0, 0.0, 0.0), Vector3d(0.0, 1.0, 0.0), Vector3d(12.3, -41.3, 0.0)},
+ {false, Vector3d(0.0, 0.0, 0.0), Vector3d(1.0, 0.0, 0.0), Vector3d(0.0, 1.0, 0.0), Vector3d(12.3, -41.3, 4.0)},
+
+ {false, Vector3d(10932.645, 43.1987, -0.009874245),
+ Vector3d(53432.4, -9.87243, 654.31),
+ Vector3d(28.71, 0.005483927, 2.34515),
+ Vector3d(5897.1, -5.432, 512152.7654)}
+ };
+
+ for (auto candidate = std::begin(candidates); candidate != std::end(candidates); ++candidate)
+ {
+ EXPECT_EQ(candidate->expected, isCoplanar(candidate->points[0],
+ candidate->points[1],
+ candidate->points[2],
+ candidate->points[3]))
+ << "Candidate points were [" << candidate->points[0].transpose() << "], ["
+ << candidate->points[1].transpose() << "], ["
+ << candidate->points[2].transpose() << "], ["
+ << candidate->points[3].transpose() << "]";
+ }
+}
+
+typedef std::tuple<Segment, MockTriangle, VectorType, bool> SegTriIntersectionData;
+::testing::AssertionResult checkSegTriIntersection(const SegTriIntersectionData& data)
+{
+ std::stringstream errorMessage;
+ Segment segment = std::get<0>(data);
+ MockTriangle tri = std::get<1>(data);
+ VectorType expectedClosestPoint = std::get<2>(data);
+ bool expectedResult = std::get<3>(data);
+ VectorType closestPoint;
+
+ bool result = doesCollideSegmentTriangle(segment.a, segment.b, tri.v0, tri.v1, tri.v2, tri.n, &closestPoint);
+ if (result != expectedResult)
+ {
+ errorMessage << "Intersection result does not match should be: " << expectedResult << " but got " <<
+ result << std::endl;
+ };
+ if (expectedResult)
+ {
+ if (! expectedClosestPoint.isApprox(closestPoint))
+ {
+ errorMessage << "Closest Point was expected to be " << expectedClosestPoint << " but is " <<
+ closestPoint << std::endl;
+ }
+ }
+ else
+ {
+ errorMessage << eigenAllNan(closestPoint).message();
+ }
+
+ if (errorMessage.str() == "")
+ {
+ return ::testing::AssertionSuccess();
+ }
+ else
+ {
+ return ::testing::AssertionFailure() << errorMessage.str();
+ }
+}
+
+TEST_F(GeometryTest, SegmentTriangleIntersection)
+{
+ VectorType closestPoint;
+ VectorType intersectionPoint = tri.pointInTriangle(0.2, 0.7);
+ Segment intersecting(intersectionPoint - tri.n * 2, intersectionPoint + tri.n * 2);
+
+ SegTriIntersectionData data;
+
+ data = SegTriIntersectionData(intersecting, tri, intersectionPoint, true);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ intersecting.a = intersectionPoint + tri.n * 4;
+ data = SegTriIntersectionData(intersecting, tri, intersectionPoint, false);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ // in the plane of the triangle
+ intersecting = Segment(intersectionPoint, intersectionPoint + tri.v0v1 + tri.v1v2);
+ data = SegTriIntersectionData(intersecting, tri, intersectionPoint, true);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ intersecting = Segment(intersectionPoint + tri.v0v1 + tri.v1v2, intersectionPoint);
+ data = SegTriIntersectionData(intersecting, tri, intersectionPoint, true);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ intersecting = Segment(intersectionPoint + tri.v0v1 + tri.v1v2, intersectionPoint + 2 * tri.v0v1 + tri.v1v2);
+ data = SegTriIntersectionData(intersecting, tri, intersectionPoint, false);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ // Slanting but intersecting
+ // Point On triangle
+ intersecting = Segment(intersectionPoint, intersectionPoint + tri.n * 2 + tri.v0v1 * 2);
+ data = SegTriIntersectionData(intersecting, tri, intersectionPoint, true);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ // Intersection in Triangle
+ intersecting = Segment(intersectionPoint - tri.n * 2 - tri.v1v2 * 2, intersectionPoint + 2 * tri.n + tri.v1v2 * 2);
+ data = SegTriIntersectionData(intersecting, tri, intersectionPoint, true);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ // Intersection not on Segment
+ intersecting = Segment(intersectionPoint + tri.n * 4 + tri.v1v2 * 4, intersectionPoint + 2 * tri.n + tri.v1v2 * 2);
+ data = SegTriIntersectionData(intersecting, tri, intersectionPoint, false);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ // Normal segment through one edge
+ VectorType pointOnEdge = tri.v0 + tri.v0v1 * 0.5;
+ intersecting = Segment(pointOnEdge + tri.n, pointOnEdge - tri.n);
+ data = SegTriIntersectionData(intersecting, tri, pointOnEdge, true);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ intersecting = Segment(pointOnEdge + tri.n, pointOnEdge);
+ data = SegTriIntersectionData(intersecting, tri, pointOnEdge, true);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ intersecting = Segment(pointOnEdge + tri.n * 3, pointOnEdge + tri.n * 4);
+ data = SegTriIntersectionData(intersecting, tri, pointOnEdge, false);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ intersecting = Segment(pointOnEdge + tri.n + tri.v0v1 * 0.5, pointOnEdge);
+ data = SegTriIntersectionData(intersecting, tri, pointOnEdge, true);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+
+ // Segment away from the triangle
+ intersecting = Segment(tri.v0 - tri.v0v1 - tri.v1v2, tri.v0 - tri.n * 3.0 - tri.v0v1 - tri.v1v2);
+ data = SegTriIntersectionData(intersecting, tri, pointOnEdge, false);
+ EXPECT_TRUE(checkSegTriIntersection(data));
+}
+
+TEST_F(GeometryTest, distancePointPlane)
+{
+ MockTriangle triangle(VectorType(3, 4, 5), VectorType(5, 5, 5), VectorType(10, 5, 2));
+ VectorType pointInTriangle = triangle.v0 + triangle.v0v1 * 0.4;
+ double d = -triangle.n.dot(triangle.v0);
+ VectorType point = pointInTriangle;
+ VectorType projectionPoint;
+ double distance = distancePointPlane(point, triangle.n, d, &projectionPoint);
+ EXPECT_NEAR(0.0, distance, epsilon);
+ EXPECT_TRUE(pointInTriangle.isApprox(projectionPoint));
+
+ point = pointInTriangle + triangle.n * 2;
+ distance = distancePointPlane(point, triangle.n, d, &projectionPoint);
+ EXPECT_NEAR(2.0, distance, epsilon);
+ EXPECT_TRUE(pointInTriangle.isApprox(projectionPoint));
+
+ point = pointInTriangle - triangle.n * 3;
+ distance = distancePointPlane(point, triangle.n, d, &projectionPoint);
+ EXPECT_NEAR(-3.0, distance, epsilon);
+ EXPECT_TRUE(pointInTriangle.isApprox(projectionPoint));
+}
+
+typedef std::tuple<Segment, VectorType, double, VectorType, VectorType, int> SegmentPlaneData;
+void checkSegmentPlanDistance(const SegmentPlaneData& data)
+{
+ Segment seg = std::get<0>(data);
+ VectorType n = std::get<1>(data);
+ double d = std::get<2>(data);
+ VectorType expectedSegmentPoint = std::get<3>(data);
+ VectorType expectedPlanePoint = std::get<4>(data);
+ // The sign of the expected distance [1|-1|0], you must use 0 for expected 0
+ int sign = std::get<5>(data);
+ double distance;
+ VectorType segResultPoint, planeResultPoint;
+ distance = distanceSegmentPlane(seg. a, seg.b, n, d, &segResultPoint, &planeResultPoint);
+ EXPECT_NEAR((planeResultPoint - segResultPoint).norm(), std::abs(distance), epsilon);
+ EXPECT_TRUE(distance * sign > 0 || distance == static_cast<double>(sign));
+ EXPECT_TRUE(expectedSegmentPoint.isApprox(segResultPoint));
+ EXPECT_TRUE(expectedPlanePoint.isApprox(planeResultPoint));
+}
+
+TEST_F(GeometryTest, SegmentPlaneDistance)
+{
+ MockTriangle triangle(VectorType(3, 4, 5), VectorType(5, 5, 5), VectorType(10, 5, 2));
+ double d = -triangle.n.dot(triangle.v0);
+ VectorType intersectionPoint = triangle.pointInTriangle(0.2, 0.7);
+ Segment seg(intersectionPoint - triangle.n * 2, intersectionPoint + triangle.n * 2);
+
+ VectorType segResultPoint, planeResultPoint;
+
+ {
+ SCOPED_TRACE("Segment intersects Plane");
+ checkSegmentPlanDistance(SegmentPlaneData(seg, triangle.n, d, intersectionPoint, intersectionPoint, 0));
+ }
+
+ {
+ SCOPED_TRACE("Segment above plane, segment intersection should be point a");
+ seg = Segment(intersectionPoint + triangle.n * 2, intersectionPoint + triangle.n * 3);
+ distanceSegmentPlane(seg.a, seg.b, triangle.n, d, &segResultPoint, &planeResultPoint);
+ checkSegmentPlanDistance(SegmentPlaneData(seg, triangle.n, d, seg.a, intersectionPoint, 1));
+ }
+
+ {
+ SCOPED_TRACE("Segment below plane, segment intersection should be point a");
+ seg = Segment(intersectionPoint - triangle.n * 3, intersectionPoint - triangle.n * 2);
+ distanceSegmentPlane(seg.a, seg.b, triangle.n, d, &segResultPoint, &planeResultPoint);
+ checkSegmentPlanDistance(SegmentPlaneData(seg, triangle.n, d, seg.b, intersectionPoint, -1));
+ }
+
+ {
+ SCOPED_TRACE("Segment below plane, segment intersection should be point a, reverse case from above");
+ seg = Segment(intersectionPoint - triangle.n * 2, intersectionPoint - triangle.n * 3);
+ checkSegmentPlanDistance(SegmentPlaneData(seg, triangle.n, d, seg.a, intersectionPoint, -1));
+ }
+
+ {
+ SCOPED_TRACE("Segment coplanar with plane");
+ seg = Segment(intersectionPoint - triangle.v0v1, intersectionPoint + triangle.v0v1);
+ checkSegmentPlanDistance(SegmentPlaneData(seg, triangle.n, d, intersectionPoint, intersectionPoint, 0));
+ }
+
+ {
+ SCOPED_TRACE("Segment parallel with plane");
+ seg = Segment(intersectionPoint - triangle.v0v1 + triangle.n * 2.0,
+ intersectionPoint + triangle.v0v1 + triangle.n * 2.0);
+ checkSegmentPlanDistance(SegmentPlaneData(seg, triangle.n, d, seg.pointOnLine(0.5), intersectionPoint, 1));
+ }
+
+ {
+ SCOPED_TRACE("Segment parallel with plane but on the other side");
+ seg = Segment(intersectionPoint - triangle.v0v1 - triangle.n * 2.0,
+ intersectionPoint + triangle.v0v1 - triangle.n * 2.0);
+ checkSegmentPlanDistance(SegmentPlaneData(seg, triangle.n, d, seg.pointOnLine(0.5), intersectionPoint, -1));
+ }
+}
+
+typedef std::tuple<MockTriangle, VectorType, double, VectorType, VectorType, int> TriPlaneData;
+void checkTriPlaneDistance(const TriPlaneData& data)
+{
+ MockTriangle tri = std::get<0>(data);
+ VectorType n = std::get<1>(data);
+ double d = std::get<2>(data);
+ VectorType expectedTrianglePoint = std::get<3>(data);
+ VectorType expectedPlanePoint = std::get<4>(data);
+ int sign = std::get<5>(data);
+ VectorType triangleResultPoint, planeResultPoint;
+ double distance;
+
+ distance = distanceTrianglePlane(tri.v0, tri.v1, tri.v2, n, d, &triangleResultPoint, &planeResultPoint);
+ EXPECT_NEAR((planeResultPoint - triangleResultPoint).norm(), std::abs(distance), epsilon);
+ EXPECT_TRUE(distance * sign > 0 || distance == static_cast<double>(sign));
+ EXPECT_TRUE(expectedTrianglePoint.isApprox(triangleResultPoint));
+ EXPECT_TRUE(expectedPlanePoint.isApprox(planeResultPoint));
+}
+
+TEST_F(GeometryTest, TrianglePlaneTest)
+{
+ MockTriangle triangle(VectorType(3, 4, 5), VectorType(5, 5, 5), VectorType(10, 5, 2));
+ // Start with the coplanar case
+ double d = -triangle.n.dot(triangle.v0);
+ double distance;
+ VectorType intersectionPoint0;
+ VectorType intersectionPoint1;
+
+ // Coplanar
+ VectorType third = (triangle.v0 + triangle.v1 + triangle.v2) / 3.0;
+ distance = distanceTrianglePlane(triangle.v0, triangle.v1, triangle.v2, triangle.n, d,
+ &intersectionPoint0, &intersectionPoint1);
+ EXPECT_NEAR(0.0, distance, epsilon);
+ EXPECT_TRUE(third.isApprox(intersectionPoint0));
+ EXPECT_TRUE(third.isApprox(intersectionPoint1));
+
+ VectorType pointOnPlane = (triangle.v0 + triangle.v1 + triangle.v2) / 3.0;
+
+ {
+ SCOPED_TRACE("Coplanar Case");
+ MockTriangle target(triangle.v0 , triangle.v1 , triangle.v2);
+ checkTriPlaneDistance(TriPlaneData(target, triangle.n, d, pointOnPlane, pointOnPlane, 0));
+ }
+
+ {
+ SCOPED_TRACE("Parallel, below the plane");
+ MockTriangle target(triangle.v0 - triangle.n * 3, triangle.v1 - triangle.n * 3, triangle.v2 - triangle.n * 3);
+ checkTriPlaneDistance(TriPlaneData(target, triangle.n, d, pointOnPlane - triangle.n * 3, pointOnPlane, -1));
+ }
+
+ {
+ SCOPED_TRACE("Parallel, above the plane");
+ MockTriangle target(triangle.v0 + triangle.n, triangle.v1 + triangle.n, triangle.v2 + triangle.n);
+ checkTriPlaneDistance(TriPlaneData(target, triangle.n, d, pointOnPlane + triangle.n, pointOnPlane, 1));
+ }
+
+ {
+ SCOPED_TRACE("Not Intersecting, triangle.v0 is closest above the plane");
+ MockTriangle target(triangle.v0 + triangle.n * 2, triangle.v1 + triangle.n * 3, triangle.v2 + triangle.n * 3);
+ checkTriPlaneDistance(TriPlaneData(target, triangle.n, d, target.v0, triangle.v0, 1));
+ }
+
+ {
+ SCOPED_TRACE("Not Intersecting, triangle.v1 is closest above the plane");
+ MockTriangle target(triangle.v0 + triangle.n * 3, triangle.v1 + triangle.n * 2, triangle.v2 + triangle.n * 3);
+ checkTriPlaneDistance(TriPlaneData(target, triangle.n, d, target.v1, triangle.v1, 1));
+ }
+
+ {
+ SCOPED_TRACE("Not Intersecting, triangle.v2 is closest above the plane");
+ MockTriangle target(triangle.v0 + triangle.n * 4, triangle.v1 + triangle.n * 3, triangle.v2 + triangle.n * 2);
+ checkTriPlaneDistance(TriPlaneData(target, triangle.n, d, target.v2, triangle.v2, 1));
+ }
+
+ {
+ SCOPED_TRACE("Not Intersecting, triangle.v0 is closest below the plane");
+ MockTriangle target(triangle.v0 - triangle.n * 2, triangle.v1 - triangle.n * 3, triangle.v2 - triangle.n * 3);
+ checkTriPlaneDistance(TriPlaneData(target, triangle.n, d, target.v0, triangle.v0, -1));
+ }
+
+ {
+ SCOPED_TRACE("Not Intersecting, triangle.v1 is closest below the plane");
+ MockTriangle target(triangle.v0 - triangle.n * 4, triangle.v1 - triangle.n * 2, triangle.v2 - triangle.n * 3);
+ checkTriPlaneDistance(TriPlaneData(target, triangle.n, d, target.v1, triangle.v1, -1));
+ }
+
+ {
+ SCOPED_TRACE("Not Intersecting, triangle.v2 is closest below the plane");
+ MockTriangle target(triangle.v0 - triangle.n * 4, triangle.v1 - triangle.n * 3, triangle.v2 - triangle.n * 2);
+ checkTriPlaneDistance(TriPlaneData(target, triangle.n, d, target.v2, triangle.v2, -1));
+ }
+
+ {
+ SCOPED_TRACE("Triangle point on the plane");
+ // Need to change the order of points for this to work ... strange ...
+ MockTriangle target(triangle.v0 + triangle.n * 3, triangle.v2 + triangle.n * 3, triangle.v1);
+ checkTriPlaneDistance(TriPlaneData(target, triangle.n, d, target.v2, target.v2, 0));
+ }
+
+ {
+ SCOPED_TRACE("Triangle plane intersection with v0 being under the plane");
+ MockTriangle target(triangle.v0 - triangle.n * 2, triangle.v1 + triangle.n * 2, triangle.v2 + triangle.n * 2);
+ distance = distanceTrianglePlane(target.v0, target.v1, target.v2, triangle.n, d,
+ &intersectionPoint0, &intersectionPoint1);
+ EXPECT_NEAR(0.0, distance, epsilon);
+ EXPECT_TRUE(intersectionPoint0.isApprox(intersectionPoint1));
+ EXPECT_TRUE(isPointInsideTriangle(intersectionPoint0, target.v0, target.v1, target.v2, target.n));
+ EXPECT_TRUE(isPointInsideTriangle(intersectionPoint0, triangle.v0, triangle.v1, triangle.v2, triangle.n));
+ }
+
+ {
+ SCOPED_TRACE("Triangle plane intersection with v0 and v1 being under the plane");
+ MockTriangle target(triangle.v0 - triangle.n * 2, triangle.v1 - triangle.n * 2, triangle.v2 + triangle.n * 2);
+ distance = distanceTrianglePlane(target.v0, target.v1, target.v2, triangle.n, d,
+ &intersectionPoint0, &intersectionPoint1);
+ EXPECT_NEAR(0.0, distance, epsilon);
+ EXPECT_TRUE(intersectionPoint0.isApprox(intersectionPoint1));
+ EXPECT_TRUE(isPointInsideTriangle(intersectionPoint0, target.v0, target.v1, target.v2, target.n));
+ EXPECT_TRUE(isPointInsideTriangle(intersectionPoint0, triangle.v0, triangle.v1, triangle.v2, triangle.n));
+ }
+
+ {
+ SCOPED_TRACE("Triangle plane intersection with v2 being under the plane");
+ MockTriangle target(triangle.v0 + triangle.n * 2, triangle.v1 + triangle.n * 2, triangle.v2 - triangle.n * 2);
+ distance = distanceTrianglePlane(target.v0, target.v1, target.v2, triangle.n, d,
+ &intersectionPoint0, &intersectionPoint1);
+ EXPECT_NEAR(0.0, distance, epsilon);
+ EXPECT_TRUE(intersectionPoint0.isApprox(intersectionPoint1));
+ EXPECT_TRUE(isPointInsideTriangle(intersectionPoint0, target.v0, target.v1, target.v2, target.n));
+ EXPECT_TRUE(isPointInsideTriangle(intersectionPoint0, triangle.v0, triangle.v1, triangle.v2, triangle.n));
+ }
+}
+
+TEST_F(GeometryTest, PlanePlaneDistance)
+{
+ // Simple test against same
+ double d1 = -tri.n.dot(tri.v0);
+ VectorType point0, point1;
+
+ bool result = doesIntersectPlanePlane(tri.n, d1, tri.n, d1, &point0, &point1);
+ EXPECT_FALSE(result);
+ EXPECT_TRUE(eigenAllNan(point0));
+ EXPECT_TRUE(eigenAllNan(point1));
+ result = doesIntersectPlanePlane(tri.n, -2.0, tri.n, 8.8, &point0, &point1);
+ EXPECT_FALSE(result);
+ EXPECT_TRUE(eigenAllNan(point0));
+ EXPECT_TRUE(eigenAllNan(point1));
+
+ VectorType n2 = VectorType(5, 6, 7);
+ n2.normalize();
+ double d2 = -2;
+ result = doesIntersectPlanePlane(tri.n, d1, n2, d2, &point0, &point1);
+ VectorType output;
+ EXPECT_TRUE(result);
+ EXPECT_FALSE(eigenAllNan(point0));
+ EXPECT_NEAR(0, distancePointPlane(point0, tri.n, d1, &output), epsilon);
+ EXPECT_NEAR(0, distancePointPlane(point1, tri.n, d1, &output), epsilon);
+ EXPECT_FALSE(eigenAllNan(point1));
+ EXPECT_NEAR(0, distancePointPlane(point0, n2, d2, &output), epsilon);
+ EXPECT_NEAR(0, distancePointPlane(point1, n2, d2, &output), epsilon);
+}
+
+typedef std::tuple<Segment, MockTriangle, VectorType, VectorType> SegTriDistanceData;
+void checkSegTriDistance(const SegTriDistanceData& data)
+{
+ std::stringstream errorMessage;
+ Segment segment = std::get<0>(data);
+ MockTriangle tri = std::get<1>(data);
+ VectorType expectedSegmentPoint = std::get<2>(data);
+ VectorType expectedTrianglePoint = std::get<3>(data);
+ double expectedDistance = (expectedSegmentPoint - expectedTrianglePoint).norm();
+ double distance;
+ VectorType segmentPoint, trianglePoint;
+
+ distance = distanceSegmentTriangle(segment.a, segment.b, tri.v0, tri.v1, tri.v2, tri.n,
+ &segmentPoint, &trianglePoint);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(expectedSegmentPoint.isApprox(segmentPoint));
+ EXPECT_TRUE(expectedTrianglePoint.isApprox(trianglePoint));
+
+
+ // Repeat above with segment reversed
+ distance = distanceSegmentTriangle(segment.b, segment.a, tri.v0, tri.v1, tri.v2, tri.n,
+ &segmentPoint, &trianglePoint);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(expectedSegmentPoint.isApprox(segmentPoint));
+ EXPECT_TRUE(expectedTrianglePoint.isApprox(trianglePoint));
+}
+TEST_F(GeometryTest, SegmentTriangleDistance)
+{
+ Segment segment;
+ VectorType intersection;
+ {
+ SCOPED_TRACE("Segment endpoint equivalent to triangle point");
+ segment = Segment(tri.v0, tri.v1 + tri.n * 3);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, segment.a, tri.v0));
+ }
+ {
+ SCOPED_TRACE("Segment endpoint inside triangle on triangle plane");
+ segment = Segment(tri.pointInTriangle(0.5, 0.2), tri.pointInTriangle(2, 2) + tri.n * 4);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, segment.a, segment.a));
+ }
+ {
+ SCOPED_TRACE("Intersection inside triangle");
+ intersection = tri.pointInTriangle(0.5, 0.2);
+ segment = Segment(intersection - tri.n * 4 - tri.v0v1, intersection + tri.n * 4 + tri.v0v1);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, intersection, intersection));
+ }
+ {
+ SCOPED_TRACE("Segment endpoint on triangle edge");
+ segment = Segment(tri.pointInTriangle(0.0, 0.2), tri.pointInTriangle(2, 2) + tri.n * 4);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, segment.a, segment.a));
+ }
+ {
+ SCOPED_TRACE("intersection on triangle edge");
+ intersection = tri.pointInTriangle(0, 0.2);
+ segment = Segment(intersection - tri.n * 3 - tri.v1v2 * .5, intersection + tri.n * 3 + tri.v1v2 * .5);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, intersection, intersection));
+ }
+ {
+ SCOPED_TRACE("segment endpoint is close to point inside of triangle");
+ intersection = tri.pointInTriangle(0.5, 0.2);
+ Segment seg(intersection, intersection + tri.n * 2 + tri.v0v1 * 3);
+ segment = Segment(seg.a + tri.n * 0.1, seg.b + tri.n * 0.1);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, segment.a, intersection));
+ }
+ {
+ SCOPED_TRACE("segment endpoint is close to triangle point v0");
+ Segment seg(tri.v0, tri.v0 - tri.n * 2 - tri.v0v1 * 2);
+ segment = Segment(seg.a - tri.n * 0.1, seg.b - tri.n * 0.1);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, segment.a, tri.v0));
+ }
+ {
+ SCOPED_TRACE("segment endpoint is close to triangle point v1");
+ Segment seg(tri.v1, tri.v1 - tri.n * 2 - tri.v0v1 * 2);
+ segment = Segment(seg.a - tri.n * 0.1, seg.b - tri.n * 0.1);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, segment.a, tri.v1));
+ }
+ {
+ SCOPED_TRACE("segment endpoint is close to triangle point v2");
+ Segment seg(tri.v2, tri.v2 - tri.n * 2 - tri.v1v2 * 2);
+ segment = Segment(seg.a - tri.n * 0.1, seg.b - tri.n * 0.1);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, segment.a, tri.v2));
+ }
+ {
+ SCOPED_TRACE("segment endpoint is close to edge v0v1");
+ intersection = tri.v0 + tri.v0v1 * 0.2;
+ Segment seg(intersection, intersection + tri.n * 2);
+ segment = Segment(seg.a + seg.ab * 0.01, seg.b + seg.ab * 0.01);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, segment.a, intersection));
+ }
+ {
+ SCOPED_TRACE("segment endpoint is close to edge v0v2");
+ intersection = tri.v0 + tri.v0v2 * 0.4;
+ Segment seg(intersection, intersection + tri.n * 2);
+ segment = Segment(seg.a + seg.ab * 0.01, seg.b + seg.ab * 0.01);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, segment.a , intersection));
+ }
+ {
+ SCOPED_TRACE("segment endpoint is close to edge v1v2");
+ intersection = tri.v1 + tri.v1v2 * 0.2;
+ Segment seg(intersection, intersection + tri.n * 2);
+ segment = Segment(seg.a + seg.ab * 0.01, seg.b + seg.ab * 0.01);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, segment.a, intersection));
+ }
+ {
+ SCOPED_TRACE("point on segment is close to triangle vertex v0");
+ segment = Segment(tri.v0 - tri.n * 3, tri.v0 + tri.n * 3);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, tri.v0, tri.v0));
+ }
+ {
+ SCOPED_TRACE("point on segment is close to triangle vertex v1");
+ segment = Segment(tri.v1 - tri.n * 3, tri.v1 + tri.n * 3);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, tri.v1, tri.v1));
+ }
+ {
+ SCOPED_TRACE("point on segment is close to triangle vertex v2");
+ segment = Segment(tri.v2 - tri.n * 3, tri.v2 + tri.n * 3);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, tri.v2, tri.v2));
+ }
+ {
+ SCOPED_TRACE("point on segment is close to edge v0v1");
+ intersection = tri.v0 + tri.v0v1 * 0.2;
+ Segment seg(intersection - tri.n * 3, intersection + tri.n * 2);
+ VectorType cross = tri.n.cross(tri.v0v1);
+ segment = Segment(seg.a - cross * 0.01, seg.b - cross * 0.01);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, intersection - cross * 0.01, intersection));
+ }
+ {
+ SCOPED_TRACE("point on segment is close to edge v0v2");
+ intersection = tri.v0 + tri.v0v2 * 0.2;
+ Segment seg(intersection - tri.n * 3, intersection + tri.n * 2);
+ VectorType cross = tri.n.cross(tri.v0v2);
+ segment = Segment(seg.a + cross * 0.01, seg.b + cross * 0.01);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, intersection + cross * 0.01, intersection));
+ }
+ {
+ SCOPED_TRACE("point on segment is close to edge v1v2");
+ intersection = tri.v1 + tri.v1v2 * 0.2;
+ Segment seg(intersection - tri.n * 3, intersection + tri.n * 2);
+ VectorType cross = tri.n.cross(tri.v1v2);
+ segment = Segment(seg.a - cross * 0.01, seg.b - cross * 0.01);
+ checkSegTriDistance(SegTriDistanceData(segment, tri, intersection - cross * 0.01, intersection));
+ }
+}
+
+typedef std::tuple<MockTriangle, MockTriangle, VectorType, VectorType> TriTriDistanceData;
+void checkTriTriDistance(const TriTriDistanceData& data)
+{
+ MockTriangle t0 = std::get<0>(data);
+ MockTriangle t1 = std::get<1>(data);
+ VectorType expectedT0Point = std::get<2>(data);
+ VectorType expectedT1Point = std::get<3>(data);
+ double expectedDistance = (expectedT1Point - expectedT0Point).norm();
+ double distance;
+ VectorType t0Point, t1Point;
+
+ {
+ SCOPED_TRACE("Normal Test");
+ distance = distanceTriangleTriangle(t0.v0, t0.v1, t0.v2, t1.v0, t1.v1, t1.v2, &t0Point, &t1Point);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(expectedT0Point.isApprox(t0Point));
+ EXPECT_TRUE(expectedT1Point.isApprox(t1Point));
+ }
+
+// {
+// SCOPED_TRACE("Reversed Triangles");
+// distance = TriangleTriangleDistance(t1.v0, t1.v1, t1.v2, t0.v0, t0.v1, t0.v2, &t1Point, &t0Point);
+// EXPECT_NEAR(expectedDistance, distance,epsilon);
+// EXPECT_TRUE(expectedT0Point.isApprox(t0Point));
+// EXPECT_TRUE(expectedT1Point.isApprox(t1Point));
+// }
+
+ {
+ SCOPED_TRACE("Shift t0 edges once");
+ distance = distanceTriangleTriangle(t0.v1, t0.v2, t0.v0, t1.v0, t1.v1, t1.v2, &t0Point, &t1Point);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(expectedT0Point.isApprox(t0Point));
+ EXPECT_TRUE(expectedT1Point.isApprox(t1Point));
+ }
+
+
+ {
+ SCOPED_TRACE("Shift t0 edges twice");
+ distance = distanceTriangleTriangle(t0.v2, t0.v0, t0.v1, t1.v0, t1.v1, t1.v2, &t0Point, &t1Point);
+ EXPECT_NEAR(expectedDistance, distance, epsilon);
+ EXPECT_TRUE(expectedT0Point.isApprox(t0Point));
+ EXPECT_TRUE(expectedT1Point.isApprox(t1Point));
+ }
+}
+
+TEST_F(GeometryTest, distanceTriangleTriangle)
+{
+ MockTriangle t0(VectorType(5, 0, 0), VectorType(0, 2, 2), VectorType(0, -2, -2));
+ MockTriangle t1;
+ {
+ SCOPED_TRACE("vertex t1v0 equal to t0v0");
+ t1 = MockTriangle(t0.v0, t0.v1 + t0.n * 2, t0.v2 + t0.n * 2);
+ checkTriTriDistance(TriTriDistanceData(t1, t0, t0.v0, t0.v0));
+ }
+ {
+ SCOPED_TRACE("vertex t1v0 inside of triangle t0");
+ VectorType intersection = t0.pointInTriangle(0.2, 0.2);
+ t1 = MockTriangle(intersection, t0.v1 + t0.n * 2, t0.v2 + t0.n * 2);
+ checkTriTriDistance(TriTriDistanceData(t1, t0, t1.v0, intersection));
+ }
+ {
+ SCOPED_TRACE("vertex t1v0 close to t0v0");
+ t1 = MockTriangle(t0.v0 + t0.n, t0.v1 + t0.n * 2, t0.v2 + t0.n * 2);
+ checkTriTriDistance(TriTriDistanceData(t1, t0, t1.v0, t0.v0));
+ }
+ {
+ SCOPED_TRACE("vertex t1v0 close to the inside of triangle t0");
+ VectorType intersection = t0.pointInTriangle(0.2, 0.2);
+ t1 = MockTriangle(intersection + t0.n , t0.v1 + t0.n * 2, t0.v2 + t0.n * 2);
+ checkTriTriDistance(TriTriDistanceData(t1, t0, t1.v0, intersection));
+ }
+ {
+ SCOPED_TRACE("edge t1v0v1 through triangle t0");
+ VectorType intersection = t0.pointInTriangle(0.2, 0.2);
+ t1 = MockTriangle(intersection + t0.n * 3, t0.v0 - t0.v0v2 * 4 + t0.n, intersection - t0.n * 4);
+ checkTriTriDistance(TriTriDistanceData(t1, t0, intersection, intersection));
+ }
+ {
+ SCOPED_TRACE("Triangles parallel");
+ t1 = MockTriangle(t0.v0 + tri.n * 3, t0.v1 + tri.n * 3, t0.v2 + tri.n * 3);
+ VectorType closest0, closest1;
+ double distance = distanceTriangleTriangle(t0.v0, t0.v1, t0.v2, t1.v0, t1.v1, t1.v2, &closest0, &closest1);
+ EXPECT_NEAR(3.0, distance, epsilon);
+ }
+ {
+ SCOPED_TRACE("edge t0v0v1 close to t1v0v1");
+ VectorType closest0 = t0.v0 + t0.v0v1 * 0.2;
+ VectorType shift = t0.n.cross(t0.v0v1.normalized());
+ shift.normalize();
+ VectorType closest1 = closest0 - shift * 2;
+ t1 = MockTriangle(closest1 - tri.n * 2, closest1 + tri.n * 2, closest1 + tri.n - shift * 10);
+ checkTriTriDistance(TriTriDistanceData(t1, t0, closest1, closest0));
+ }
+}
+
+TEST_F(GeometryTest, IntersectionsSegmentBox)
+{
+ Eigen::AlignedBox<SizeType, 3> box;
+ {
+ SCOPED_TRACE("No intersection, zero length segment");
+ VectorType point1(0.0, 0.0, 0.0);
+ VectorType point2(0.0, 0.0, 0.0);
+ box.min() = VectorType(1.0 , 1.0, 1.0);
+ box.max() = VectorType(5.0 , 5.0, 5.0);
+ std::vector<VectorType> intersections;
+ intersectionsSegmentBox(point1, point2, box, &intersections);
+ EXPECT_EQ(0, intersections.size());
+ }
+
+ {
+ SCOPED_TRACE("No intersection, zero size box");
+ VectorType point1(0.0, 0.0, 0.0);
+ VectorType point2(0.0, 5.0, 0.0);
+ box.min() = VectorType(1.0 , 1.0, 1.0);
+ box.max() = VectorType(1.0 , 1.0, 1.0);
+ std::vector<VectorType> intersections;
+ intersectionsSegmentBox(point1, point2, box, &intersections);
+ EXPECT_EQ(0, intersections.size());
+ }
+
+ {
+ SCOPED_TRACE("No Intersection, parallel and beyond corners");
+ VectorType point1(-0.0, 0.0, -0.0);
+ VectorType point2(0.0, 5.0, -0.0);
+ box.min() = VectorType(1.0 , 1.0, 1.0);
+ box.max() = VectorType(5.0 , 5.0, 5.0);
+ std::vector<VectorType> intersections;
+ intersectionsSegmentBox(point1, point2, box, &intersections);
+ EXPECT_EQ(0, intersections.size());
+ }
+
+ {
+ SCOPED_TRACE("Entering box, but not leaving");
+ VectorType point1(2.0, 2.0, 0.0);
+ VectorType point2(3.0, 3.0, 2.0);
+ box.min() = VectorType(1.0 , 1.0, 1.0);
+ box.max() = VectorType(5.0 , 5.0, 5.0);
+ std::vector<VectorType> intersections;
+ intersectionsSegmentBox(point1, point2, box, &intersections);
+ EXPECT_EQ(1, intersections.size());
+ EXPECT_TRUE(intersections[0].isApprox(VectorType(2.5, 2.5, 1.0)));
+ }
+
+ {
+ SCOPED_TRACE("Entering and exiting box, through box corners");
+ VectorType point1(0.0, 0.0, 0.0);
+ VectorType point2(6.0, 6.0, 6.0);
+ box.min() = VectorType(1.0 , 1.0, 1.0);
+ box.max() = VectorType(5.0 , 5.0, 5.0);
+ std::vector<VectorType> intersections;
+ intersectionsSegmentBox(point1, point2, box, &intersections);
+ EXPECT_EQ(2, intersections.size());
+ EXPECT_TRUE(intersections[0].isApprox(box.min()) || intersections[0].isApprox(box.max()));
+ EXPECT_TRUE(intersections[1].isApprox(box.min()) || intersections[1].isApprox(box.max()));
+ }
+}
+
+TEST_F(GeometryTest, DoesIntersectBoxCapsule)
+{
+ typedef Eigen::AlignedBox<SizeType, 3> BoxType;
+ {
+ SCOPED_TRACE("No intersection");
+ VectorType bottom(-5.0, 5.0, 0.0);
+ VectorType top(5.0, 5.0, 0.0);
+ double radius = 1.0;
+ BoxType box(VectorType(-1.0 , -1.0, -1.0), VectorType(1.0 , 1.0, 1.0));
+ EXPECT_FALSE(doesIntersectBoxCapsule(bottom, top, radius, box));
+ }
+ {
+ SCOPED_TRACE("Intersection, capsule in middle of box");
+ VectorType bottom(-5.0, -5.0, -5.0);
+ VectorType top(5.0, 5.0, 5.0);
+ double radius = 10.0;
+ BoxType box(VectorType(-1.0 , -1.0, -1.0), VectorType(1.0 , 1.0, 1.0));
+ EXPECT_TRUE(doesIntersectBoxCapsule(bottom, top, radius, box));
+ }
+ {
+ SCOPED_TRACE("No Intersection, box not centered");
+ VectorType bottom(-5.0, -5.0, -5.0);
+ VectorType top(5.0, 5.0, 5.0);
+ double radius = 1.0;
+ BoxType box(VectorType(1.0 , 1.0, -1.0), VectorType(2.0 , 2.0, -2.0));
+ EXPECT_FALSE(doesIntersectBoxCapsule(bottom, top, radius, box));
+ }
+ {
+ SCOPED_TRACE("Intersection, box not centered");
+ VectorType bottom(-5.0, -5.0, -5.0);
+ VectorType top(5.0, 5.0, 5.0);
+ double radius = 1.0;
+ BoxType box(VectorType(0.0 , 0.0, 0.0), VectorType(1.0 , 1.0, 1.0));
+ EXPECT_TRUE(doesIntersectBoxCapsule(bottom, top, radius, box));
+ }
+ {
+ SCOPED_TRACE("No intersection, capsule along edge");
+ VectorType bottom(2.0, -2.0, 2.0);
+ VectorType top(2.0, 2.0, 2.0);
+ double radius = sqrt(2.0) - 1.0;
+ BoxType box(VectorType(-1.0 , -1.0, -1.0), VectorType(1.0 , 1.0, 1.0));
+ EXPECT_FALSE(doesIntersectBoxCapsule(bottom, top, radius, box));
+ }
+ {
+ SCOPED_TRACE("Intersection, capsule along edge");
+ VectorType bottom(2.0, -2.0, 2.0);
+ VectorType top(2.0, 2.0, 2.0);
+ double radius = sqrt(2.0) + 0.1;
+ BoxType box(VectorType(-1.0 , -1.0, -1.0), VectorType(1.0 , 1.0, 1.0));
+ EXPECT_TRUE(doesIntersectBoxCapsule(bottom, top, radius, box));
+ }
+ {
+ SCOPED_TRACE("No Intersection, capsule at corner");
+ VectorType bottom(2.0, 3.0, 1.0);
+ VectorType top(2.0, 1.0, 3.0);
+ double radius = sqrt(3.0) - 0.1;
+ BoxType box(VectorType(-1.0 , -1.0, -1.0), VectorType(1.0 , 1.0, 1.0));
+ EXPECT_FALSE(doesIntersectBoxCapsule(bottom, top, radius, box));
+ }
+ {
+ SCOPED_TRACE("Intersection, capsule at corner");
+ VectorType bottom(2.0, 3.0, 1.0);
+ VectorType top(2.0, 1.0, 3.0);
+ double radius = sqrt(3.0) + 0.1;
+ BoxType box(VectorType(-1.0 , -1.0, -1.0), VectorType(1.0 , 1.0, 1.0));
+ EXPECT_TRUE(doesIntersectBoxCapsule(bottom, top, radius, box));
+ }
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/LinearSolveAndInverseTests.cpp b/SurgSim/Math/UnitTests/LinearSolveAndInverseTests.cpp
new file mode 100644
index 0000000..6ac29e3
--- /dev/null
+++ b/SurgSim/Math/UnitTests/LinearSolveAndInverseTests.cpp
@@ -0,0 +1,279 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 LinearSolveAndInverse.cpp functions.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/LinearSolveAndInverse.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+class LinearSolveAndInverseTests : public ::testing::Test
+{
+private:
+ size_t size;
+
+ void initializeVector(Vector* v)
+ {
+ v->resize(size);
+ for (size_t row = 0; row < size; row++)
+ {
+ (*v)(row) = fmod(-4.1 * row * row + 3.46, 5.0);
+ }
+ }
+
+ void initializeDenseMatrix(Matrix* m)
+ {
+ m->resize(size, size);
+ for (size_t row = 0; row < size; row++)
+ {
+ for (size_t col = 0; col < size; col++)
+ {
+ (*m)(row, col) = fmod((10.3 * cos(static_cast<double>(row * col)) + 3.24), 10.0);
+ }
+ }
+ }
+
+ void initializeDiagonalMatrix(Matrix* m)
+ {
+ m->resize(size, size);
+ m->setZero();
+ for (size_t row = 0; row < size; row++)
+ {
+ (*m)(row, row) = fmod((10.3 * cos(static_cast<double>(row * row)) + 3.24), 10.0);
+ }
+ }
+
+ template <size_t BlockSize>
+ void initializeTriDiagonalBlockMatrix(Matrix* m, bool isSymmetric)
+ {
+ size_t numBlocks = size / BlockSize;
+
+ m->resize(size, size);
+ m->setZero();
+
+ for (size_t rowBlockId = 0; rowBlockId < numBlocks; rowBlockId++)
+ {
+ for (int colBlockId = static_cast<int>(rowBlockId) - 1;
+ colBlockId <= static_cast<int>(rowBlockId) + 1;
+ colBlockId++)
+ {
+ if (colBlockId < 0 || colBlockId >= static_cast<int>(numBlocks))
+ {
+ continue;
+ }
+
+ for (size_t rowInBlockId = 0; rowInBlockId < BlockSize; ++rowInBlockId)
+ {
+ for (size_t colInBlockId = 0; colInBlockId < BlockSize; ++colInBlockId)
+ {
+ size_t row = rowBlockId * BlockSize + rowInBlockId;
+ size_t col = colBlockId * BlockSize + colInBlockId;
+ (*m)(row, col) = fmod((10.3 * cos(static_cast<double>(row * col)) + 3.24), 10.0);
+ }
+ }
+ }
+ }
+
+ if (isSymmetric)
+ {
+ // Force symmetry (lower triangular is copied from the upper triangular)
+ for (size_t row = 0; row < size; ++row)
+ {
+ for (size_t col = row + 1; col < size; ++col)
+ {
+ (*m)(col, row) = (*m)(row, col);
+ }
+ }
+ }
+ }
+
+ void setupTest()
+ {
+ initializeVector(&b);
+ expectedInverse = matrix.inverse();
+ expectedX = expectedInverse * b;
+ }
+
+public:
+
+ void setupDenseMatrixTest()
+ {
+ size = 18;
+ initializeDenseMatrix(&matrix);
+ setupTest();
+ }
+
+ void setupDiagonalMatrixTest()
+ {
+ size = 18;
+ initializeDiagonalMatrix(&matrix);
+ setupTest();
+ }
+
+ template <size_t BlockSize>
+ void setupTriDiagonalBlockMatrixTest(bool isSymmetric = false)
+ {
+ size = BlockSize * 6;
+ initializeTriDiagonalBlockMatrix<BlockSize>(&matrix, isSymmetric);
+ setupTest();
+ }
+
+ Matrix matrix;
+ Matrix inverseMatrix, expectedInverse;
+ Vector b;
+ Vector x, expectedX;
+};
+
+TEST_F(LinearSolveAndInverseTests, DenseMatrixTests)
+{
+ setupDenseMatrixTest();
+
+ LinearSolveAndInverseDenseMatrix solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, DiagonalMatrixTests)
+{
+ setupDiagonalMatrixTest();
+
+ LinearSolveAndInverseDiagonalMatrix solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, TriDiagonalBlockMatrixBlockSize2Tests)
+{
+ setupTriDiagonalBlockMatrixTest<2>();
+
+ LinearSolveAndInverseTriDiagonalBlockMatrix<2> solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, TriDiagonalBlockMatrixBlockSize3Tests)
+{
+ setupTriDiagonalBlockMatrixTest<3>();
+
+ LinearSolveAndInverseTriDiagonalBlockMatrix<3> solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, TriDiagonalBlockMatrixBlockSize4Tests)
+{
+ setupTriDiagonalBlockMatrixTest<4>();
+
+ LinearSolveAndInverseTriDiagonalBlockMatrix<4> solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, TriDiagonalBlockMatrixBlockSize5Tests)
+{
+ setupTriDiagonalBlockMatrixTest<5>();
+
+ LinearSolveAndInverseTriDiagonalBlockMatrix<5> solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, TriDiagonalBlockMatrixBlockSize6Tests)
+{
+ setupTriDiagonalBlockMatrixTest<6>();
+
+ LinearSolveAndInverseTriDiagonalBlockMatrix<6> solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, SymmetricTriDiagonalBlockMatrixBlockSize2Tests)
+{
+ setupTriDiagonalBlockMatrixTest<2>(true);
+
+ LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<2> solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, SymmetricTriDiagonalBlockMatrixBlockSize3Tests)
+{
+ setupTriDiagonalBlockMatrixTest<3>(true);
+
+ LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<3> solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, SymmetricTriDiagonalBlockMatrixBlockSize4Tests)
+{
+ setupTriDiagonalBlockMatrixTest<4>(true);
+
+ LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<4> solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, SymmetricTriDiagonalBlockMatrixBlockSize5Tests)
+{
+ setupTriDiagonalBlockMatrixTest<5>(true);
+
+ LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<5> solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+
+TEST_F(LinearSolveAndInverseTests, SymmetricTriDiagonalBlockMatrixBlockSize6Tests)
+{
+ setupTriDiagonalBlockMatrixTest<6>(true);
+
+ LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<6> solveAndInverse;
+ solveAndInverse(matrix, b, &x, &inverseMatrix);
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+};
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/MakeRigidTransformTests.cpp b/SurgSim/Math/UnitTests/MakeRigidTransformTests.cpp
new file mode 100644
index 0000000..611dd78
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MakeRigidTransformTests.cpp
@@ -0,0 +1,104 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 that exercise the functionality of methods we define related to rigid transforms.
+ */
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "gtest/gtest.h"
+
+
+template <class T>
+class MakeRigidTransform3Tests : public testing::Test
+{
+public:
+ typedef T RigidTransform;
+};
+
+typedef ::testing::Types<SurgSim::Math::RigidTransform3d,
+ SurgSim::Math::RigidTransform3f> RigidTransform3Variants;
+TYPED_TEST_CASE(MakeRigidTransform3Tests, RigidTransform3Variants);
+
+
+template <class T>
+class MakeAllRigidTransformTests : public testing::Test
+{
+public:
+ typedef T RigidTransform;
+};
+
+typedef ::testing::Types<SurgSim::Math::RigidTransform2d,
+ SurgSim::Math::RigidTransform2f,
+ SurgSim::Math::RigidTransform3d,
+ SurgSim::Math::RigidTransform3f> AllRigidTransformVariants;
+TYPED_TEST_CASE(MakeAllRigidTransformTests, AllRigidTransformVariants);
+
+
+/// Test makeRigidTransform using a rotation matrix and a translation
+TYPED_TEST(MakeAllRigidTransformTests, MakeRigidTransformWithMatrix)
+{
+ typedef typename TestFixture::RigidTransform RigidTransform;
+ typedef typename RigidTransform::LinearMatrixType RotationMatrix;
+ typedef typename RigidTransform::TranslationType::VectorType Translation;
+ const int DIM = RigidTransform::Dim;
+
+ RotationMatrix rotationMatrix = RotationMatrix::Identity();
+ rotationMatrix.row(0).swap(rotationMatrix.row(DIM-1));
+ Translation translation(Translation::Random());
+ RigidTransform transform = SurgSim::Math::makeRigidTransform(rotationMatrix, translation);
+
+ typename RigidTransform::MatrixType matrix = transform.matrix();
+ EXPECT_TRUE(rotationMatrix.isApprox(matrix.block(0,0,DIM,DIM), 1e-6f)) <<
+ "Rotation part of transform is not properly set";
+ EXPECT_TRUE(translation.isApprox(matrix.block(0,DIM,DIM,1), 1e-6f)) <<
+ "Translation part of transform is not properly set";
+ EXPECT_NEAR(1.0, matrix(DIM,DIM), 1e-6) <<
+ "Transform matrix is not 1.0 in bottom right corner";
+ EXPECT_TRUE(matrix.block(DIM,0,1,DIM).isApproxToConstant(0.0, 1e-6f)) <<
+ "Bottom row of matrix is not all zeros (except for last column).";
+}
+
+/// Test makeRigidTransform using a quaternion and a translation
+TYPED_TEST(MakeRigidTransform3Tests, MakeRigidTransformWithQuaternion)
+{
+ typedef typename TestFixture::RigidTransform RigidTransform;
+ typedef typename RigidTransform::TranslationType::VectorType Translation;
+ typedef typename RigidTransform::Scalar Scalar;
+ typedef typename Eigen::Quaternion<Scalar> Quaternion;
+ typedef typename RigidTransform::LinearMatrixType RotationMatrix;
+ const int DIM = RigidTransform::Dim;
+
+ Translation translation = Translation::Random();
+ Quaternion quaternion(0.0, 1.0, 0.0, 0.0);
+ RotationMatrix quaternionRotationMatrix;
+ quaternionRotationMatrix << 1.0, 0.0, 0.0,
+ 0.0,-1.0, 0.0,
+ 0.0, 0.0,-1.0;
+ RigidTransform transform = SurgSim::Math::makeRigidTransform(quaternion, translation);
+
+ typename RigidTransform::MatrixType matrix = transform.matrix();
+ EXPECT_TRUE(quaternionRotationMatrix.isApprox(matrix.block(0,0,DIM,DIM), 1e-6f)) <<
+ "Rotation part of transform is not properly set";
+ EXPECT_TRUE(translation.isApprox(matrix.block(0,DIM,DIM,1), 1e-6f)) <<
+ "Translation part of transform is not properly set";
+ EXPECT_NEAR(1.0, matrix(DIM,DIM), 1e-6) <<
+ "Transform matrix is not 1.0 in bottom right corner";
+ EXPECT_TRUE(matrix.block(DIM,0,1,DIM).isApproxToConstant(0.0, 1e-6f)) <<
+ "Bottom row of matrix is not all zeros (except for last column).";
+}
diff --git a/SurgSim/Math/UnitTests/MatrixTests.cpp b/SurgSim/Math/UnitTests/MatrixTests.cpp
new file mode 100644
index 0000000..413006b
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MatrixTests.cpp
@@ -0,0 +1,1368 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 that exercise the functionality of our matrix typedefs, which come
+/// straight from Eigen.
+
+#include <math.h>
+
+#include <Eigen/Geometry> // SurgSim/Math/Matrix.h by itself does not provide cross()
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Matrix.h"
+
+// Define test fixture class templates.
+// We don't really need fixtures as such, but the templatization encodes type.
+
+template <class T>
+class MatrixTestBase : public testing::Test
+{
+public:
+ typedef T Scalar;
+};
+
+
+
+template <class T>
+class Matrix22Tests : public MatrixTestBase<typename T::Scalar>
+{
+public:
+ typedef T Matrix22;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) matrix type aliases, but we got rid of those.
+typedef ::testing::Types<SurgSim::Math::Matrix22d,
+ SurgSim::Math::Matrix22f> Matrix22Variants;
+TYPED_TEST_CASE(Matrix22Tests, Matrix22Variants);
+
+
+template <class T>
+class Matrix33Tests : public MatrixTestBase<typename T::Scalar>
+{
+public:
+ typedef T Matrix33;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) matrix type aliases, but we got rid of those.
+typedef ::testing::Types<SurgSim::Math::Matrix33d,
+ SurgSim::Math::Matrix33f> Matrix33Variants;
+TYPED_TEST_CASE(Matrix33Tests, Matrix33Variants);
+
+
+template <class T>
+class Matrix44Tests : public MatrixTestBase<typename T::Scalar>
+{
+public:
+ typedef T Matrix44;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) matrix type aliases, but we got rid of those.
+typedef ::testing::Types<SurgSim::Math::Matrix44d,
+ SurgSim::Math::Matrix44f> Matrix44Variants;
+TYPED_TEST_CASE(Matrix44Tests, Matrix44Variants);
+
+
+
+template <class T>
+class AllMatrixTests : public MatrixTestBase<typename T::Scalar>
+{
+public:
+ typedef T Matrix;
+};
+
+template <class T>
+class AllDynamicMatrixTests : public MatrixTestBase<typename T::Scalar>
+{
+public:
+ typedef T Matrix;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) matrix type aliases, but we got rid of those.
+typedef ::testing::Types<SurgSim::Math::Matrix22d,
+ SurgSim::Math::Matrix22f,
+ SurgSim::Math::Matrix33d,
+ SurgSim::Math::Matrix33f,
+ SurgSim::Math::Matrix44d,
+ SurgSim::Math::Matrix44f> AllMatrixVariants;
+TYPED_TEST_CASE(AllMatrixTests, AllMatrixVariants);
+
+typedef ::testing::Types<Eigen::MatrixXd,
+ Eigen::MatrixXf,
+ SurgSim::Math::Matrix> AllDynamicMatrixVariants;
+TYPED_TEST_CASE(AllDynamicMatrixTests, AllDynamicMatrixVariants);
+
+template <class T>
+class UnalignedMatrixTests : public MatrixTestBase<typename T::Scalar>
+{
+public:
+ typedef T Matrix;
+};
+
+template <class T>
+class UnalignedDynamicMatrixTests : public MatrixTestBase<typename T::Scalar>
+{
+public:
+ typedef T Matrix;
+};
+
+typedef ::testing::Types<SurgSim::Math::Matrix22d,
+ SurgSim::Math::Matrix22f,
+ SurgSim::Math::Matrix33d,
+ SurgSim::Math::Matrix33f,
+ SurgSim::Math::Matrix44d,
+ SurgSim::Math::Matrix44f> UnalignedMatrixVariants;
+TYPED_TEST_CASE(UnalignedMatrixTests, UnalignedMatrixVariants);
+
+typedef ::testing::Types<Eigen::MatrixXd,
+ Eigen::MatrixXf,
+ SurgSim::Math::Matrix> UnalignedDynamicMatrixVariants;
+TYPED_TEST_CASE(UnalignedDynamicMatrixTests, UnalignedDynamicMatrixVariants);
+
+
+// Now we're ready to start testing...
+
+
+// ==================== CONSTRUCTION & INITIALIZATION ====================
+
+/// Test that matrices can be constructed.
+TYPED_TEST(Matrix22Tests, CanConstruct)
+{
+ typedef typename TestFixture::Matrix22 Matrix22;
+
+ // Warning: Eigen *does not* provide a 1-argument constructor that
+ // initializes all elements to the same value! If you do something like
+ // SurgSim::Math::Matrix22fu oneArg2fu(1.23f);
+ // the argument is converted to an integral type, interpreted as a size,
+ // and promptly ignored because the size is fixed. Oops.
+ // To generate a constant matrix, use Matrix22f::Constant(val).
+
+ Matrix22 default2;
+}
+
+/// Test that matrices can be constructed.
+TYPED_TEST(Matrix33Tests, CanConstruct)
+{
+ typedef typename TestFixture::Matrix33 Matrix33;
+
+ // Warning: Eigen *does not* provide a 1-argument constructor that
+ // initializes all elements to the same value! If you do something like
+ // SurgSim::Math::Matrix22fu oneArg2fu(1.23f);
+ // the argument is converted to an integral type, interpreted as a size,
+ // and promptly ignored because the size is fixed. Oops.
+ // To generate a constant matrix, use Matrix22f::Constant(val).
+
+ Matrix33 default3;
+}
+
+/// Test that matrices can be constructed.
+TYPED_TEST(Matrix44Tests, CanConstruct)
+{
+ typedef typename TestFixture::Matrix44 Matrix44;
+
+ // Warning: Eigen *does not* provide a 1-argument constructor that
+ // initializes all elements to the same value! If you do something like
+ // SurgSim::Math::Matrix22fu oneArg2fu(1.23f);
+ // the argument is converted to an integral type, interpreted as a size,
+ // and promptly ignored because the size is fixed. Oops.
+ // To generate a constant matrix, use Matrix22f::Constant(val).
+
+ Matrix44 default4;
+}
+
+/// Test that the default constructor DOESN'T initialize matrices.
+//
+// Only test the non-vectorized versions. Otherwise, we'd need to
+// allocate memory in a way that guarantees Eigen-compatible alignment.
+//
+// TODO(bert): There is some Eigen flag that causes matrices and matrices to be
+// initialized after all! We should check for that here.
+TYPED_TEST(UnalignedMatrixTests, DefaultConstructorInitialization)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ EXPECT_TRUE(SIZE >= 2 && SIZE <= 4);
+ EXPECT_EQ(SIZE, Matrix::ColsAtCompileTime);
+
+ // Allocate a buffer for the matrix type on stack, based on the size
+ // of the object we're testing. The object will be allocated inside
+ // the buffer using the placement syntax for the new() operator.
+ // Eigen's new operatore will attempt to align returned value on word sized
+ // boundaries, so add 64 bytes to guarantee enough size.
+ unsigned char buffer[sizeof(Matrix) + 64];
+
+ {
+ // Please don't write production (non-test) code that looks like this. =)
+ memset(&buffer, 0xF0, sizeof(buffer));
+ Matrix* matrix = new(&buffer) Matrix;
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ EXPECT_NE(0.0f, (*matrix)(row, col)) << row << "," << col << " was NOT supposed to be zeroed.";
+ }
+ }
+ // Destroying the object is a good idea, even if unnecessary here:
+ matrix->Matrix::~Matrix();
+ }
+}
+
+/// Test setting the matrix using the << syntax.
+TYPED_TEST(Matrix22Tests, ShiftCommaInitialization)
+{
+ typedef typename TestFixture::Matrix22 Matrix22;
+
+ Matrix22 matrix;
+ // Initialize elements in order. Do NOT put parentheses around the list!
+ matrix <<
+ 1.1f, 1.2f,
+ 1.3f, 1.4f;
+ for (int row = 0; row < 2; ++row)
+ {
+ for (int col = 0; col < 2; ++col)
+ {
+ EXPECT_NEAR(1.1 + 0.2*row + 0.1*col, matrix(row, col), 1e-6) <<
+ row << "," << col << " wasn't properly initialized.";
+ }
+ }
+}
+
+/// Test setting the matrix using the << syntax.
+TYPED_TEST(Matrix33Tests, ShiftCommaInitialization)
+{
+ typedef typename TestFixture::Matrix33 Matrix33;
+
+ Matrix33 matrix;
+ // Initialize elements in order. Do NOT put parentheses around the list!
+ matrix <<
+ 1.1f, 1.2f, 1.3f,
+ 1.4f, 1.5f, 1.6f,
+ 1.7f, 1.8f, 1.9f;
+ for (int row = 0; row < 3; ++row)
+ {
+ for (int col = 0; col < 3; ++col)
+ {
+ EXPECT_NEAR(1.1 + 0.3*row + 0.1*col, matrix(row, col), 1e-6) <<
+ row << "," << col << " wasn't properly initialized.";
+ }
+ }
+}
+
+/// Test setting the matrix using the << syntax.
+TYPED_TEST(Matrix44Tests, ShiftCommaInitialization)
+{
+ typedef typename TestFixture::Matrix44 Matrix44;
+
+ Matrix44 matrix;
+ // Initialize elements in order. Do NOT put parentheses around the list!
+ matrix <<
+ 1.1f, 1.2f, 1.3f, 1.4f,
+ 1.5f, 1.6f, 1.7f, 1.8f,
+ 1.9f, 2.0f, 2.1f, 2.2f,
+ 2.3f, 2.4f, 2.5f, 2.6f;
+ for (int row = 0; row < 4; ++row)
+ {
+ for (int col = 0; col < 4; ++col)
+ {
+ EXPECT_NEAR(1.1 + 0.4*row + 0.1*col, matrix(row, col), 1e-6) <<
+ row << "," << col << " wasn't properly initialized.";
+ }
+ }
+}
+
+/// Test getting a zero value usable in expressions.
+TYPED_TEST(AllMatrixTests, ZeroValue)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix matrix = 1000 * Matrix::Zero();
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ EXPECT_NEAR(0.0, matrix(row, col), 1e-20) << row << "," << col << " wasn't properly initialized.";
+ }
+ }
+}
+
+/// Test setting matrices to 0.
+TYPED_TEST(AllMatrixTests, SetToZero)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix matrix;
+ matrix.setZero();
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ EXPECT_NEAR(0.0, matrix(row, col), 1e-20) << row << "," << col << " wasn't properly cleared.";
+ }
+ }
+}
+
+/// Test getting a constant value usable in expressions.
+TYPED_TEST(AllMatrixTests, ConstantValue)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix matrix = 2 * Matrix::Constant(0.5f);
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ EXPECT_NEAR(1.0, matrix(row, col), 1e-6) << row << "," << col << " wasn't properly initialized.";
+ }
+ }
+}
+
+/// Test setting matrices to a constant.
+TYPED_TEST(AllMatrixTests, SetToConstant)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix matrix;
+ matrix.setConstant(7.2f);
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ EXPECT_NEAR(7.2, matrix(row, col), 1e-6) << row << "," << col << " wasn't properly initialized.";
+ }
+ }
+}
+
+/// Test initializing a row-major Eigen matrix from a float array.
+TYPED_TEST(AllMatrixTests, InitializeRowMajorFromArray)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ typedef Eigen::Matrix<T, SIZE, SIZE, Eigen::AutoAlign | Eigen::RowMajor> RMatrix;
+
+ // This array has more elements than we will need.
+ // The element type must match the matrix!
+ const T inputArray[18] =
+ {
+ 0.01f, 1.02f, 2.03f, 3.04f, 4.05f, 5.06f, 6.07f, 7.08f, 8.09f,
+ 9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
+ };
+
+ RMatrix matrix(inputArray);
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ EXPECT_NEAR(0.01 + (row*SIZE + col) * 1.01, matrix(row, col), 1e-6) <<
+ row << "," << col << " wasn't properly initialized.";
+ }
+ }
+}
+
+/// Test initializing a column-major Eigen matrix from a float array.
+TYPED_TEST(AllMatrixTests, InitializeColumnMajorFromArray)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ typedef Eigen::Matrix<T, SIZE, SIZE, Eigen::AutoAlign | Eigen::ColMajor> CMatrix;
+
+ // This array has more elements than we will need.
+ // The element type must match the matrix!
+ const T inputArray[18] =
+ {
+ 0.01f, 1.02f, 2.03f, 3.04f, 4.05f, 5.06f, 6.07f, 7.08f, 8.09f,
+ 9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
+ };
+
+ CMatrix matrix(inputArray);
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ EXPECT_NEAR(0.01 + (col*SIZE + row) * 1.01, matrix(row, col), 1e-6) <<
+ row << "," << col << " wasn't properly initialized.";
+ }
+ }
+}
+
+/// Test initializing from a float array.
+/// Among other things, tests that our matrices are row-major.
+TYPED_TEST(AllMatrixTests, InitializeFromArray)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ // This array has more elements than we will need.
+ // The element type must match the matrix!
+ const T inputArray[18] =
+ {
+ 0.01f, 1.02f, 2.03f, 3.04f, 4.05f, 5.06f, 6.07f, 7.08f, 8.09f,
+ 9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
+ };
+
+ Matrix matrix(inputArray);
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ EXPECT_NEAR(0.01 + (row*SIZE + col) * 1.01, matrix(row, col), 1e-6) <<
+ row << "," << col << " wasn't properly initialized.";
+ }
+ }
+}
+
+// Test conversion to and from yaml node
+TYPED_TEST(AllMatrixTests, YamlConvert)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+
+ // This array has more elements than we will need.
+ // The element type must match the matrix!
+ const T inputArray[18] =
+ {
+ 0.01f, 1.02f, 2.03f, 3.04f, 4.05f, 5.06f, 6.07f, 7.08f, 8.09f,
+ 9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
+ };
+
+ Matrix matrix(inputArray);
+
+ YAML::Node node;
+
+ ASSERT_NO_THROW(node = matrix);
+
+ EXPECT_TRUE(node.IsSequence());
+ EXPECT_EQ(matrix.rows(), node.size());
+
+ ASSERT_NO_THROW({Matrix expected = node.as<Matrix>();});
+ EXPECT_TRUE(matrix.isApprox(node.as<Matrix>()));
+}
+
+/// Test assignment.
+TYPED_TEST(AllMatrixTests, Assign)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix a = Matrix::Constant(6.0f);
+ EXPECT_NEAR(6.0f * SIZE * SIZE, a.sum(), 1e-6);
+ const Matrix b = Matrix::Constant(7.0f);
+ EXPECT_NEAR(7.0f * SIZE * SIZE, b.sum(), 1e-6);
+ a = b;
+ EXPECT_NEAR(7.0f * SIZE * SIZE, a.sum(), 1e-6);
+}
+
+// ==================== ACCESS ====================
+
+/// Access by rows and columns.
+TYPED_TEST(AllMatrixTests, RowsAndColumns)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ typedef Eigen::Matrix<T, SIZE, 1> Vector;
+
+ Matrix a = Matrix::Zero();
+ for (int i = 0; i < SIZE; ++i)
+ {
+ Vector rowVector = Vector::Constant(i + 1.f);
+ a.row(i) = rowVector;
+ }
+
+ for (int i = 0; i < SIZE; ++i)
+ {
+ Vector rowVector = a.row(i);
+ EXPECT_NEAR((i + 1.f) * SIZE, rowVector.sum(), 1e-6);
+ Vector columnVector = a.col(i);
+ EXPECT_NEAR(SIZE * (SIZE+1) / 2., columnVector.sum(), 1e-6);
+ }
+
+ for (int i = 0; i < SIZE; ++i)
+ {
+ Vector columnVector = Vector::Constant(i + 1.f);
+ a.col(i) = columnVector;
+ }
+
+ for (int i = 0; i < SIZE; ++i)
+ {
+ Vector columnVector = a.col(i);
+ EXPECT_NEAR((i + 1.f) * SIZE, columnVector.sum(), 1e-6);
+ Vector rowVector = a.row(i);
+ EXPECT_NEAR(SIZE * (SIZE+1) / 2., rowVector.sum(), 1e-6);
+ }
+}
+
+/// Access to the diagonal.
+TYPED_TEST(AllMatrixTests, Diagonal)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ typedef Eigen::Matrix<T, SIZE, 1> Vector;
+
+ Matrix a = Matrix::Zero();
+ {
+ Vector diagonalVector = Vector::Constant(2.f);
+ a.diagonal() = diagonalVector;
+ }
+ EXPECT_NEAR(2.f * SIZE, a.sum(), 1e-6);
+
+ Matrix b = Matrix::Identity();
+ {
+ EXPECT_NEAR(1.f * SIZE, b.diagonal().sum(), 1e-6);
+ }
+}
+
+// ==================== REPRESENTATION CONVERSIONS ====================
+
+/// Test setting quaternions from an angle/axis rotation.
+TYPED_TEST(Matrix33Tests, FromAngleAxis)
+{
+ typedef typename TestFixture::Matrix33 Matrix33;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ T angle = 0.1f;
+ Vector3 axis = Vector3::UnitZ();
+
+ Matrix33 expectedMatrix;
+ T sinAngle = std::sin(angle);
+ T cosAngle = std::cos(angle);
+ expectedMatrix <<
+ cosAngle, -sinAngle, 0,
+ sinAngle, cosAngle, 0,
+ 0, 0, 1;
+
+ using SurgSim::Math::makeRotationMatrix;
+
+ Matrix33 matrix = makeRotationMatrix(angle, axis);
+ EXPECT_NEAR(0, (matrix - expectedMatrix).norm(), 9e-6) << "The rotation matrix wasn't properly computed.";
+}
+
+
+/// Test extracting an angle/axis rotation from a quaternion.
+TYPED_TEST(Matrix33Tests, ToAngleAxis)
+{
+ typedef typename TestFixture::Matrix33 Matrix33;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ T angle = -0.1f;
+ Vector3 axis = Vector3::UnitZ();
+
+ Matrix33 matrix;
+ T sinAngle = std::sin(angle);
+ T cosAngle = std::cos(angle);
+ matrix <<
+ cosAngle, -sinAngle, 0,
+ sinAngle, cosAngle, 0,
+ 0, 0, 1;
+
+ using SurgSim::Math::computeAngleAndAxis;
+ using SurgSim::Math::computeAngle;
+
+ T angle2;
+ Vector3 axis2;
+ computeAngleAndAxis(matrix, &angle2, &axis2);
+ EXPECT_NEAR(-angle, angle2, 1e-6) << "angle wasn't properly computed.";
+ EXPECT_NEAR(-axis.x(), axis2.x(), 1e-6) << "X wasn't properly computed.";
+ EXPECT_NEAR(-axis.y(), axis2.y(), 1e-6) << "Y wasn't properly computed.";
+ EXPECT_NEAR(-axis.z(), axis2.z(), 1e-6) << "Y wasn't properly computed.";
+
+ EXPECT_NEAR(-angle, computeAngle(matrix), 1e-6) << "angle wasn't properly computed by computeAngle().";
+}
+
+/// Test building a skew symmetric matrix from a vector
+TYPED_TEST(Matrix33Tests, MakeSkewSymmetricMatrixTest)
+{
+ typedef typename TestFixture::Matrix33 Matrix33;
+ typedef typename TestFixture::Scalar T;
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ Vector3 v(static_cast<T>(0.3), static_cast<T>(-1.4), static_cast<T>(8.3));
+ Matrix33 matrix = SurgSim::Math::makeSkewSymmetricMatrix(v);
+ EXPECT_TRUE(matrix.diagonal().isZero());
+ EXPECT_NEAR(static_cast<T>(0.3) , matrix(2, 1), std::numeric_limits<T>::epsilon());
+ EXPECT_NEAR(static_cast<T>(-0.3), matrix(1, 2), std::numeric_limits<T>::epsilon());
+ EXPECT_NEAR(static_cast<T>(-1.4), matrix(0, 2), std::numeric_limits<T>::epsilon());
+ EXPECT_NEAR(static_cast<T>(1.4) , matrix(2, 0), std::numeric_limits<T>::epsilon());
+ EXPECT_NEAR(static_cast<T>(8.3) , matrix(1, 0), std::numeric_limits<T>::epsilon());
+ EXPECT_NEAR(static_cast<T>(-8.3), matrix(0, 1), std::numeric_limits<T>::epsilon());
+}
+
+/// Test extracting a vector from a skew symmetric part of a matrix
+TYPED_TEST(Matrix33Tests, SkewTest)
+{
+ typedef typename TestFixture::Scalar T;
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ Vector3 vExpected(static_cast<T>(0.3), static_cast<T>(-1.4), static_cast<T>(8.3));
+ Vector3 v = SurgSim::Math::skew(SurgSim::Math::makeSkewSymmetricMatrix(vExpected));
+ EXPECT_TRUE(v.isApprox(vExpected));
+}
+
+// ==================== ARITHMETIC ====================
+
+/// Negation (unary minus).
+TYPED_TEST(AllMatrixTests, Negate)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix a = Matrix::Constant(0.1f);
+ EXPECT_NEAR(0.1 * SIZE * SIZE, a.sum(), 1e-6);
+ Matrix b = -a;
+ EXPECT_NEAR(-0.1 * SIZE * SIZE, b.sum(), 1e-6);
+}
+
+/// Addition.
+TYPED_TEST(AllMatrixTests, Add)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix a = Matrix::Constant(0.1f);
+ EXPECT_NEAR(0.1 * SIZE * SIZE, a.sum(), 1e-6);
+ Matrix b = a + Matrix::Ones() + a;
+ EXPECT_NEAR(1.2 * SIZE * SIZE, b.sum(), 1e-6);
+}
+
+/// Subtraction.
+TYPED_TEST(AllMatrixTests, Subtract)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix a = Matrix::Constant(0.1f);
+ EXPECT_NEAR(0.1 * SIZE * SIZE, a.sum(), 1e-6);
+ Matrix b = Matrix::Ones() - a;
+ EXPECT_NEAR(0.9 * SIZE * SIZE, b.sum(), 1e-6);
+}
+
+/// Incrementing by a value.
+TYPED_TEST(AllMatrixTests, AddTo)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix a = Matrix::Constant(0.1f);
+ EXPECT_NEAR(0.1 * SIZE * SIZE, a.sum(), 1e-6);
+ a += Matrix::Ones();
+ EXPECT_NEAR(1.1 * SIZE * SIZE, a.sum(), 1e-6);
+}
+
+/// Decrementing by a value.
+TYPED_TEST(AllMatrixTests, SubtractFrom)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix a = Matrix::Constant(1.1f);
+ EXPECT_NEAR(1.1 * SIZE * SIZE, a.sum(), 1e-6);
+ a -= Matrix::Ones();
+ EXPECT_NEAR(0.1 * SIZE * SIZE, a.sum(), 1e-6);
+}
+
+/// Matrix-scalar multiplication.
+TYPED_TEST(AllMatrixTests, MultiplyMatrixScalar)
+{
+ typedef typename TestFixture::Matrix Matrix;
+
+ Matrix a = Matrix::Random();
+ Matrix b = a * 1.23f;
+ EXPECT_NEAR(1.23 * a.sum(), b.sum(), 1e-6);
+}
+
+/// Scalar-matrix multiplication.
+TYPED_TEST(AllMatrixTests, MultiplyScalarMatrix)
+{
+ typedef typename TestFixture::Matrix Matrix;
+
+ Matrix a = Matrix::Random();
+ Matrix b = 1.23f * a;
+ EXPECT_NEAR(1.23 * a.sum(), b.sum(), 1e-6);
+}
+
+/// Division by scalar.
+TYPED_TEST(AllMatrixTests, DivideScalar)
+{
+ typedef typename TestFixture::Matrix Matrix;
+
+ Matrix a = Matrix::Random();
+ Matrix b = a / 1.23f;
+ EXPECT_NEAR(a.sum() / 1.23, b.sum(), 1e-6);
+}
+
+/// Matrix-vector multiplication.
+TYPED_TEST(AllMatrixTests, MultiplyMatrixVector)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ typedef Eigen::Matrix<T, SIZE, 1> Vector;
+
+ Matrix a = Matrix::Random();
+ Vector v = Vector::Zero();
+ v[0] = 1.f;
+ Vector w = a * v;
+ EXPECT_NEAR(a.col(0).sum(), w.sum(), 1e-6);
+}
+
+/// Matrix-matrix multiplication.
+TYPED_TEST(AllMatrixTests, MultiplyMatrixMatrix)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix a = Matrix::Random();
+ Matrix I = Matrix::Identity();
+ Matrix b = I * a * I;
+ EXPECT_NEAR(0, (b - a).norm(), SIZE * SIZE * 1e-6);
+}
+
+/// Matrix inverse.
+TYPED_TEST(AllMatrixTests, Inverse)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix a = 2.0f * Matrix::Identity() + 0.5f * Matrix::Random(); // try to make an invertible matrix
+ Matrix b = a.inverse();
+ Matrix ab = a * b;
+ EXPECT_NEAR(0, (ab - Matrix::Identity()).norm(), SIZE * SIZE * 1e-6);
+ Matrix ba = b * a;
+ EXPECT_NEAR(0, (ba - Matrix::Identity()).norm(), SIZE * SIZE * 1e-6);
+}
+
+/// Matrix transpose.
+TYPED_TEST(AllMatrixTests, Transpose)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix a;
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ a(row, col) = 2.f * row - 3.f * col;
+ }
+ }
+
+ // Note: DO NOT do things like "a = a.transpose()"; aliasing will result in an error.
+ // You can use transposeInPlace(), or "a = a.transpose().eval()".
+
+ Matrix b = a.transpose();
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ EXPECT_NEAR(2.f * col - 3.f * row, b(row, col), 1e-6);
+ }
+ }
+}
+
+/// Component-wise multiplication.
+TYPED_TEST(AllMatrixTests, ComponentwiseMultiply)
+{
+ typedef typename TestFixture::Matrix Matrix;
+
+ Matrix a = Matrix::Random();
+ Matrix b = Matrix::Identity();
+ Matrix c = a.cwiseProduct(b);
+ EXPECT_NEAR(0, c.sum() - c.diagonal().sum(), 1e-6);
+ EXPECT_NEAR(0, (a - c).diagonal().sum(), 1e-6);
+}
+
+/// Component-wise division.
+TYPED_TEST(AllMatrixTests, ComponentwiseDivide)
+{
+ typedef typename TestFixture::Matrix Matrix;
+
+ Matrix a = Matrix::Random();
+ Matrix b = Matrix::Constant(0.5f);
+ Matrix c = a.cwiseQuotient(b);
+ EXPECT_NEAR(a.sum() * 2, c.sum(), 1e-6);
+}
+
+/// Frobenius norm and its square.
+TYPED_TEST(AllMatrixTests, FrobeniusNorm)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix m;
+ T sumSquares = 0;
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ T value = row + col + 11.0f;
+ m(row, col) = value;
+ sumSquares += value * value;
+ }
+ }
+
+ EXPECT_NEAR(sumSquares, m.squaredNorm(), 1e-4);
+ EXPECT_NEAR(sqrt(sumSquares), m.norm(), 1e-4);
+}
+
+/// L1 (Manhattan) norm and L_Infinity (largest absolute value) norm.
+TYPED_TEST(AllMatrixTests, L1NormAndLInfNorm)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix m;
+ T sumAbsolute = 0;
+ for (int row = 0; row < SIZE; ++row)
+ {
+ for (int col = 0; col < SIZE; ++col)
+ {
+ T value = row + col - (SIZE + 0.6f);
+ m(row, col) = value;
+ // NOTE: DON'T use plain abs(), it may truncate to int!
+ sumAbsolute += std::abs(value);
+ }
+ }
+ // the function is set up so that the (0,0) corner will always have the biggest absolute value
+ T maxAbsolute = (SIZE + 0.6f);
+
+ Matrix n = -m;
+ // Ugh, "template" is required to get this to parse properly. This is
+ // triggered because the test is a part of a template class; you don't
+ // need to do this in a non-template context.
+ EXPECT_NEAR(sumAbsolute, m.template lpNorm<1>(), 1e-4) << "m=" << m;
+ EXPECT_NEAR(sumAbsolute, n.template lpNorm<1>(), 1e-4) << "n=" << n;
+ EXPECT_NEAR(maxAbsolute, m.template lpNorm<Eigen::Infinity>(), 1e-4) << "m=" << m;
+ EXPECT_NEAR(maxAbsolute, n.template lpNorm<Eigen::Infinity>(), 1e-4) << "n=" << m;
+}
+
+/// Minimum and maximum elements.
+TYPED_TEST(AllMatrixTests, MinAndMax)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ const T inputArray[18] =
+ {
+ 0.01f, 1.02f, 2.03f, 3.04f, 4.05f, 5.06f, 6.07f, 7.08f, 8.09f,
+ 9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
+ };
+
+ Matrix m(inputArray);
+ EXPECT_NEAR(inputArray[0], m.minCoeff(), 1e-6);
+ EXPECT_NEAR(inputArray[SIZE*SIZE-1], m.maxCoeff(), 1e-6);
+}
+
+/// Trace.
+TYPED_TEST(AllMatrixTests, Trace)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+
+ Matrix a = Matrix::Random();
+ T expectedTrace = a.diagonal().sum();
+ EXPECT_NEAR(expectedTrace, a.trace(), 1e-6);
+}
+
+/// Determinant.
+TYPED_TEST(AllMatrixTests, Determinant)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ const int SIZE = Matrix::RowsAtCompileTime;
+
+ Matrix m = exp(1.f) * Matrix::Identity();
+ EXPECT_NEAR(exp(1.*SIZE), m.determinant(), 1e-4);
+}
+
+/// Determinant (explicit 2x2 result).
+TYPED_TEST(Matrix22Tests, Determinant22)
+{
+ typedef typename TestFixture::Matrix22 Matrix22;
+ typedef typename TestFixture::Scalar T;
+
+ Matrix22 m = Matrix22::Random();
+ T expectedDeterminant = m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0);
+ EXPECT_NEAR(expectedDeterminant, m.determinant(), 1e-6);
+}
+
+/// Determinant (explicit 3x3 result).
+TYPED_TEST(Matrix33Tests, Determinant33)
+{
+ typedef typename TestFixture::Matrix33 Matrix33;
+ typedef typename TestFixture::Scalar T;
+
+ Matrix33 m = Matrix33::Random();
+ T expectedDeterminant = m.row(0).cross(m.row(1)).dot(m.row(2));
+ EXPECT_NEAR(expectedDeterminant, m.determinant(), 1e-6);
+}
+
+// ==================== SUBMATRICES (EXTENDING/SHRINKING) ====================
+
+/// Extending matrices using the block<r,c>() syntax.
+TYPED_TEST(Matrix22Tests, Extend2to3)
+{
+ typedef typename TestFixture::Matrix22 Matrix22;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 3> Matrix33;
+
+ Matrix22 matrix2;
+ matrix2 <<
+ 1.1f, 1.2f,
+ 1.3f, 1.4f;
+
+ Matrix33 matrix3 = Matrix33::Identity();
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ matrix3.template block<2, 2>(0, 0) = matrix2;
+
+ EXPECT_NEAR(6.0, matrix3.sum(), 1e-6) << "extending was incorrect: " << matrix3;
+}
+
+/// Extending matrices using the block(i,j,r,c) syntax.
+TYPED_TEST(Matrix22Tests, DynamicExtend2to3)
+{
+ typedef typename TestFixture::Matrix22 Matrix22;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 3> Matrix33;
+
+ Matrix22 matrix2;
+ matrix2 <<
+ 1.1f, 1.2f,
+ 1.3f, 1.4f;
+
+ Matrix33 matrix3 = Matrix33::Identity();
+ matrix3.block(0, 0, 2, 2) = matrix2;
+
+ EXPECT_NEAR(6.0, matrix3.sum(), 1e-6) << "extending was incorrect: " << matrix3;
+}
+
+/// Shrinking matrices using the block<r,c>() syntax.
+TYPED_TEST(Matrix22Tests, Shrink3to2)
+{
+ typedef typename TestFixture::Matrix22 Matrix22;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 3> Matrix33;
+
+ Matrix33 matrix3;
+ matrix3 <<
+ 1.1f, 1.2f, 1.3f,
+ 1.4f, 1.5f, 1.6f,
+ 1.7f, 1.8f, 1.9f;
+
+ Matrix22 matrix2;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ matrix2 = matrix3.template block<2, 2>(0, 0);
+
+ EXPECT_NEAR(5.2, matrix2.sum(), 1e-6) << "shrinking was incorrect: " << matrix2;
+}
+
+/// Extending matrices using the block<r,c>() syntax.
+TYPED_TEST(Matrix33Tests, Extend2to3)
+{
+ typedef typename TestFixture::Matrix33 Matrix33;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 2, 2> Matrix22;
+
+ Matrix22 matrix2;
+ matrix2 <<
+ 1.1f, 1.2f,
+ 1.3f, 1.4f;
+
+ Matrix33 matrix3 = Matrix33::Identity();
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ matrix3.template block<2, 2>(0, 0) = matrix2;
+
+ EXPECT_NEAR(6.0, matrix3.sum(), 1e-6) << "extending was incorrect: " << matrix3;
+}
+
+/// Shrinking matrices using the block<r,c>() syntax.
+TYPED_TEST(Matrix33Tests, Shrink3to2)
+{
+ typedef typename TestFixture::Matrix33 Matrix33;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 2, 2> Matrix22;
+
+ Matrix33 matrix3;
+ matrix3 <<
+ 1.1f, 1.2f, 1.3f,
+ 1.4f, 1.5f, 1.6f,
+ 1.7f, 1.8f, 1.9f;
+
+ Matrix22 matrix2;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ matrix2 = matrix3.template block<2, 2>(0, 0);
+
+ EXPECT_NEAR(5.2, matrix2.sum(), 1e-6) << "shrinking was incorrect: " << matrix2;
+}
+
+/// Extending matrices using the block<r,c>() syntax.
+TYPED_TEST(Matrix33Tests, Extend3to4)
+{
+ typedef typename TestFixture::Matrix33 Matrix33;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 4, 4> Matrix44;
+
+ Matrix33 matrix3;
+ matrix3 <<
+ 1.1f, 1.2f, 1.3f,
+ 1.4f, 1.5f, 1.6f,
+ 1.7f, 1.8f, 1.9f;
+
+ Matrix44 matrix4 = Matrix44::Identity();
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ matrix4.template block<3, 3>(0, 0) = matrix3;
+
+ EXPECT_NEAR(14.5, matrix4.sum(), 1e-6) << "extending was incorrect: " << matrix4;
+}
+
+/// Shrinking matrices using the block<r,c>() syntax.
+TYPED_TEST(Matrix33Tests, Shrink4to3)
+{
+ typedef typename TestFixture::Matrix33 Matrix33;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 4, 4> Matrix44;
+
+ Matrix44 matrix4;
+ matrix4 <<
+ 1.1f, 1.2f, 1.3f, 1.4f,
+ 1.5f, 1.6f, 1.7f, 1.8f,
+ 1.9f, 2.0f, 2.1f, 2.2f,
+ 2.3f, 2.4f, 2.5f, 2.6f;
+
+ Matrix33 matrix3;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ matrix3 = matrix4.template block<3, 3>(0, 0);
+
+ EXPECT_NEAR(14.4, matrix3.sum(), 1e-6) << "shrinking was incorrect: " << matrix3;
+}
+
+/// Extending matrices using the block<r,c>() syntax.
+TYPED_TEST(Matrix44Tests, Extend3to4)
+{
+ typedef typename TestFixture::Matrix44 Matrix44;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 3> Matrix33;
+
+ Matrix33 matrix3;
+ matrix3 <<
+ 1.1f, 1.2f, 1.3f,
+ 1.4f, 1.5f, 1.6f,
+ 1.7f, 1.8f, 1.9f;
+
+ Matrix44 matrix4 = Matrix44::Identity();
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ matrix4.template block<3, 3>(0, 0) = matrix3;
+
+ EXPECT_NEAR(14.5, matrix4.sum(), 1e-6) << "extending was incorrect: " << matrix4;
+}
+
+/// Shrinking matrices using the block<r,c>() syntax.
+TYPED_TEST(Matrix44Tests, Shrink4to3)
+{
+ typedef typename TestFixture::Matrix44 Matrix44;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 3> Matrix33;
+
+ Matrix44 matrix4;
+ matrix4 <<
+ 1.1f, 1.2f, 1.3f, 1.4f,
+ 1.5f, 1.6f, 1.7f, 1.8f,
+ 1.9f, 2.0f, 2.1f, 2.2f,
+ 2.3f, 2.4f, 2.5f, 2.6f;
+
+ Matrix33 matrix3;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ matrix3 = matrix4.template block<3, 3>(0, 0);
+
+ EXPECT_NEAR(14.4, matrix3.sum(), 1e-6) << "shrinking was incorrect: " << matrix3;
+}
+
+// ==================== TYPE CONVERSION ====================
+
+/// Typecasting matrices (double <-> float conversions).
+TYPED_TEST(AllMatrixTests, TypeCasting)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Matrix::RowsAtCompileTime;
+ typedef Eigen::Matrix<double, SIZE, SIZE> Matrixd;
+ typedef Eigen::Matrix<float, SIZE, SIZE> Matrixf;
+
+ Matrix a = Matrix::Random();
+ T expectedSum = a.sum();
+
+ // Ugh, "template" is required to get this to parse properly. This is
+ // triggered because the test is a part of a template class; you don't
+ // need to do this in a non-template context.
+ Matrixd d = a.template cast<double>();
+ EXPECT_NEAR(expectedSum, d.sum(), 1e-6);
+ Matrixf f = a.template cast<float>();
+ EXPECT_NEAR(expectedSum, f.sum(), 1e-6);
+}
+
+// ==================== MISCELLANEOUS ====================
+
+/// Reading from and writing to arrays or blocks of double/float in memory.
+TYPED_TEST(AllMatrixTests, ArrayReadWrite)
+{
+ typedef typename TestFixture::Matrix Matrix;
+ typedef typename TestFixture::Scalar T;
+ const int NUM_ELEMENTS = Matrix::SizeAtCompileTime;
+
+
+ const T inputArray[18] =
+ {
+ 0.01f, 1.02f, 2.03f, 3.04f, 4.05f, 5.06f, 6.07f, 7.08f, 8.09f,
+ 9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
+ };
+ T outputArray[18];
+
+ Eigen::Map<const Matrix> inputMatrix(inputArray);
+ Matrix a = inputMatrix;
+
+ Eigen::Map<Matrix> outputMatrix(outputArray);
+ outputMatrix = a;
+
+ for (int i = 0; i < NUM_ELEMENTS; ++i)
+ {
+ EXPECT_NEAR(inputArray[i], outputArray[i], 1e-6);
+ }
+}
+
+// TO DO:
+// non-checked access via coeff()
+// testing numerical validity
+// testing for denormalized numbers
+
+
+namespace
+{
+ template <class T>
+ void testScalar(T valueExpected, T value){}
+
+ template <>
+ void testScalar<double>(double valueExpected, double value)
+ {
+ EXPECT_DOUBLE_EQ(valueExpected, value);
+ }
+
+ template <>
+ void testScalar<float>(float valueExpected, float value)
+ {
+ EXPECT_FLOAT_EQ(valueExpected, value);
+ }
+};
+
+TYPED_TEST(AllDynamicMatrixTests, addSubMatrix)
+{
+ typedef typename TestFixture::Matrix Matrix;
+
+ Matrix m, mInit, m2, m2Init;
+ m.resize(18, 18); m.setRandom(); mInit = m;
+ m2.resize(18, 18); m2.setRandom(); m2Init = m2;
+
+ ASSERT_NO_THROW(SurgSim::Math::addSubMatrix(m2.block(3,3, 3,3), 2,2, 3,3, &m););
+
+ EXPECT_TRUE(m2.isApprox(m2Init));
+ EXPECT_FALSE(m.isApprox(mInit));
+ for (int rowId = 0; rowId < 6; rowId++)
+ {
+ EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
+ EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
+ }
+ for (int rowId = 6; rowId < 9; rowId++)
+ {
+ for (int colId = 6; colId < 9; colId++)
+ {
+ testScalar(mInit(rowId, colId) + m2Init(3 + rowId-6, 3 + colId-6), m(rowId, colId));
+ }
+ }
+ for (int rowId = 9; rowId < 18; rowId++)
+ {
+ EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
+ EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
+ }
+}
+
+TYPED_TEST(AllDynamicMatrixTests, addSubMatrixBlocks)
+{
+ typedef typename TestFixture::Matrix Matrix;
+
+ Matrix m, mInit, m2, m2Init;
+ std::vector<size_t> nodeIds;
+ m.resize(18, 18); m.setRandom(); mInit = m;
+ m2.resize(18, 18); m2.setRandom(); m2Init = m2;
+ nodeIds.push_back(1);
+ nodeIds.push_back(3);
+ nodeIds.push_back(5);
+
+ ASSERT_NO_THROW(SurgSim::Math::addSubMatrix(m2.block(3,3, 9,9), nodeIds, 3, &m););
+ EXPECT_TRUE(m2.isApprox(m2Init));
+ EXPECT_FALSE(m.isApprox(mInit));
+ for (int rowId = 0; rowId < 3; rowId++)
+ {
+ EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
+ EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
+ }
+ for (int rowId = 3; rowId < 6; rowId++)
+ {
+ for (int colId = 3; colId < 6; colId++)
+ {
+ testScalar(mInit(rowId, colId) + m2Init(3 + rowId-3, 3 + colId-3), m(rowId, colId));
+ }
+ for (int colId = 9; colId < 12; colId++)
+ {
+ testScalar(mInit(rowId, colId) + m2Init(3 + rowId-3, 3 + colId-6), m(rowId, colId));
+ }
+ for (int colId = 15; colId < 18; colId++)
+ {
+ testScalar(mInit(rowId, colId) + m2Init(3 + rowId-3, 3 + colId-9), m(rowId, colId));
+ }
+ }
+ for (int rowId = 6; rowId < 9; rowId++)
+ {
+ EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
+ EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
+ }
+ for (int rowId = 9; rowId < 12; rowId++)
+ {
+ for (int colId = 3; colId < 6; colId++)
+ {
+ testScalar(mInit(rowId, colId) + m2Init(3 + rowId-6, 3 + colId-3), m(rowId, colId));
+ }
+ for (int colId = 9; colId < 12; colId++)
+ {
+ testScalar(mInit(rowId, colId) + m2Init(3 + rowId-6, 3 + colId-6), m(rowId, colId));
+ }
+ for (int colId = 15; colId < 18; colId++)
+ {
+ testScalar(mInit(rowId, colId) + m2Init(3 + rowId-6, 3 + colId-9), m(rowId, colId));
+ }
+ }
+ for (int rowId = 12; rowId < 15; rowId++)
+ {
+ EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
+ EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
+ }
+ for (int rowId = 15; rowId < 18; rowId++)
+ {
+ for (int colId = 3; colId < 6; colId++)
+ {
+ testScalar(mInit(rowId, colId) + m2Init(3 + rowId-9, 3 + colId-3), m(rowId, colId));
+ }
+ for (int colId = 9; colId < 12; colId++)
+ {
+ testScalar(mInit(rowId, colId) + m2Init(3 + rowId-9, 3 + colId-6), m(rowId, colId));
+ }
+ for (int colId = 15; colId < 18; colId++)
+ {
+ testScalar(mInit(rowId, colId) + m2Init(3 + rowId-9, 3 + colId-9), m(rowId, colId));
+ }
+ }
+}
+
+TYPED_TEST(AllDynamicMatrixTests, setSubMatrix)
+{
+ typedef typename TestFixture::Matrix Matrix;
+
+ Matrix m, mInit, m2, m2Init;
+ m.resize(18, 18); m.setRandom(); mInit = m;
+ m2.resize(18, 18); m2.setRandom(); m2Init = m2;
+
+ ASSERT_NO_THROW(SurgSim::Math::setSubMatrix(m2.block(3,3, 3,3), 2,2, 3,3, &m););
+ EXPECT_TRUE(m2.isApprox(m2Init));
+ EXPECT_FALSE(m.isApprox(mInit));
+ for (int rowId = 0; rowId < 6; rowId++)
+ {
+ EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
+ EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
+ }
+ for (int rowId = 6; rowId < 6+3; rowId++)
+ {
+ for (int colId = 6; colId < 6+3; colId++)
+ {
+ testScalar(m2Init(3 + rowId-6, 3 + colId-6), m(rowId, colId));
+ }
+ }
+ for (int rowId = 6+3; rowId < 18; rowId++)
+ {
+ EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
+ EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
+ }
+}
+
+TYPED_TEST(AllDynamicMatrixTests, getSubMatrix)
+{
+ typedef typename TestFixture::Matrix Matrix;
+
+ Matrix m, mInit;
+ m.resize(18, 18); m.setRandom(); mInit = m;
+
+ Eigen::Block<Matrix> subMatrix = SurgSim::Math::getSubMatrix(m, 2,2, 3,3);
+ EXPECT_TRUE(m.isApprox(mInit));
+ for (int rowId = 0; rowId < 3; rowId++)
+ {
+ for (int colId = 0; colId < 3; colId++)
+ {
+ testScalar(m(2 * 3 + rowId, 2 * 3 + colId) , subMatrix(rowId, colId));
+ // Also test that the returned value are pointing to the correct data
+ EXPECT_EQ(&subMatrix(rowId, colId), &m(2 * 3 + rowId, 2 * 3 + colId));
+ }
+ }
+}
diff --git a/SurgSim/Math/UnitTests/MeshShapeTests.cpp b/SurgSim/Math/UnitTests/MeshShapeTests.cpp
new file mode 100644
index 0000000..ba3d390
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MeshShapeTests.cpp
@@ -0,0 +1,315 @@
+//// This file is a part of the OpenSurgSim project.
+//// Copyright 2013, SimQuest Solutions Inc.
+////
+//// Licensed under the Apache License, Version 2.0 (the "License");
+//// you may not use this file except in compliance with the License.
+//// You may obtain a copy of the License at
+////
+//// http://www.apache.org/licenses/LICENSE-2.0
+////
+//// Unless required by applicable law or agreed to in writing, software
+//// distributed under the License is distributed on an "AS IS" BASIS,
+//// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//// See the License for the specific language governing permissions and
+//// limitations under the License.
+
+#include <time.h>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/AabbTreeNode.h"
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::DataStructures::TriangleMeshPlain;
+
+// CUBE
+// 3*----------*2
+// / /|
+// 7*----------* |
+// | 6| |
+// | *0 | *1
+// | |/
+// 4*----------*5
+
+static const int cubeNumPoints = 8;
+static const SurgSim::Math::Vector3d cubePoints[8] =
+{
+ SurgSim::Math::Vector3d(-1.0 / 2.0, -1.0 / 2.0, -1.0 / 2.0),
+ SurgSim::Math::Vector3d(1.0 / 2.0, -1.0 / 2.0, -1.0 / 2.0),
+ SurgSim::Math::Vector3d(1.0 / 2.0, 1.0 / 2.0, -1.0 / 2.0),
+ SurgSim::Math::Vector3d(-1.0 / 2.0, 1.0 / 2.0, -1.0 / 2.0),
+
+ SurgSim::Math::Vector3d(-1.0 / 2.0, -1.0 / 2.0, 1.0 / 2.0),
+ SurgSim::Math::Vector3d(1.0 / 2.0, -1.0 / 2.0, 1.0 / 2.0),
+ SurgSim::Math::Vector3d(1.0 / 2.0, 1.0 / 2.0, 1.0 / 2.0),
+ SurgSim::Math::Vector3d(-1.0 / 2.0, 1.0 / 2.0, 1.0 / 2.0)
+};
+
+static const int cubeNumEdges = 12;
+static const int cubeEdges[12][2] =
+{
+ {0, 1}, {3, 2}, {4, 5}, {7, 6}, // +X
+ {0, 3}, {1, 2}, {4, 7}, {5 , 6}, // +Y
+ {0, 4}, {1, 5}, {2, 6}, {3, 7} // +Z
+};
+
+static const int cubeNumTriangles = 12;
+static const int cubeTrianglesCCW[12][3] =
+{
+ {6, 2, 3}, {6, 3, 7}, // Top ( 0 1 0) [6237]
+ {0, 1, 5}, {0, 5, 4}, // Bottom ( 0 -1 0) [0154]
+ {4, 5, 6}, {4, 6, 7}, // Front ( 0 0 1) [4567]
+ {0, 3, 2}, {0, 2, 1}, // Back ( 0 0 -1) [0321]
+ {1, 2, 6}, {1, 6, 5}, // Right ( 1 0 0) [1265]
+ {0, 4, 7}, {0, 7, 3} // Left (-1 0 0) [0473]
+};
+
+class MeshShapeTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ m_numIterations = 100;
+ m_runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ srand((unsigned int)time(nullptr));
+ }
+
+ /// Number of iterations to test
+ int m_numIterations;
+ std::shared_ptr<SurgSim::Framework::Runtime> m_runtime;
+};
+
+TEST_F(MeshShapeTest, InvalidMeshCubeTest)
+{
+ // Cube
+ std::shared_ptr<TriangleMeshPlain> invalidTriMesh = std::make_shared<TriangleMeshPlain>();
+ for (int i = 0; i < cubeNumPoints; i++)
+ {
+ SurgSim::Math::Vector3d p;
+ p[0] = cubePoints[i][0];
+ p[1] = cubePoints[i][1];
+ p[2] = cubePoints[i][2];
+ TriangleMeshPlain::VertexType v(p);
+ invalidTriMesh->addVertex(v);
+ }
+ for (int i = 0; i < cubeNumEdges; i++)
+ {
+ std::array<size_t, 2> edgePoints;
+ for (int j = 0; j < 2; j++)
+ {
+ edgePoints[j] = cubeEdges[i][j];
+ }
+ TriangleMeshPlain::EdgeType e(edgePoints);
+ invalidTriMesh->addEdge(e);
+ }
+ for (int i = 0; i < cubeNumTriangles; i++)
+ {
+ std::array<size_t, 3> trianglePoints;
+ for (int j = 0; j < 3; j++)
+ {
+ // Add an offset of 3 to the indices (=> some of them will be invalid)
+ trianglePoints[j] = cubeTrianglesCCW[i][j] + 3;
+ }
+ TriangleMeshPlain::TriangleType t(trianglePoints);
+ invalidTriMesh->addTriangle(t);
+ }
+
+ EXPECT_THROW(SurgSim::Math::MeshShape invalidMeshShape(*invalidTriMesh), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(MeshShapeTest, EmptyMeshTest)
+{
+ std::shared_ptr<TriangleMeshPlain> emptyMesh = std::make_shared<TriangleMeshPlain>();
+
+ EXPECT_NO_THROW(SurgSim::Math::MeshShape meshShape(*emptyMesh));
+
+ SurgSim::Math::MeshShape meshShape(*emptyMesh);
+ EXPECT_NEAR(0.0, meshShape.getVolume(), 1e-8);
+ EXPECT_TRUE(meshShape.getCenter().isZero());
+ EXPECT_TRUE(meshShape.getSecondMomentOfVolume().isZero());
+ EXPECT_TRUE(meshShape.isValid()); // An empty mesh is regarded as valid.
+
+ SurgSim::Math::MeshShape emptyMeshShape;
+ EXPECT_FALSE(emptyMeshShape.isValid());
+}
+
+TEST_F(MeshShapeTest, ValidMeshTest)
+{
+ {
+ SCOPED_TRACE("Invalid Mesh");
+ auto emptyMesh = std::make_shared<TriangleMeshPlain>();
+ auto meshShape = std::make_shared<SurgSim::Math::MeshShape>(*emptyMesh);
+ auto mesh = meshShape->getMesh();
+
+ for (int i = 0; i < cubeNumEdges; ++i)
+ {
+ std::array<size_t, 2> edgePoints;
+ for (int j = 0; j < 2; j++)
+ {
+ edgePoints[j] = cubeEdges[i][j];
+ }
+ TriangleMeshPlain::EdgeType e(edgePoints);
+ mesh->addEdge(e);
+ }
+
+ EXPECT_FALSE(meshShape->isValid());
+ }
+
+ {
+ SCOPED_TRACE("Valid Mesh");
+ auto emptyMesh = std::make_shared<TriangleMeshPlain>();
+ auto meshShape = std::make_shared<SurgSim::Math::MeshShape>(*emptyMesh);
+ auto mesh = meshShape->getMesh();
+
+ std::shared_ptr<TriangleMeshPlain> invalidTriMesh = std::make_shared<TriangleMeshPlain>();
+ for (int i = 0; i < cubeNumPoints; i++)
+ {
+ SurgSim::Math::Vector3d point(cubePoints[i][0], cubePoints[i][1], cubePoints[i][2]);
+ TriangleMeshPlain::VertexType vertex(point);
+ mesh->addVertex(vertex);
+ }
+
+ EXPECT_TRUE(meshShape->isValid());
+ }
+}
+
+TEST_F(MeshShapeTest, MeshCubeVSBoxTest)
+{
+ for (int iterationID = 0; iterationID < m_numIterations; iterationID++)
+ {
+ double lx = 10.0 * (1.0 / static_cast<double>(iterationID + 1));
+ double ly = 10.0 * (2.0 / static_cast<double>(iterationID + 1));
+ double lz = 10.0 * (static_cast<double>(iterationID + 1) / 3.0);
+
+ // Cube
+ std::shared_ptr<TriangleMeshPlain> mesh = std::make_shared<TriangleMeshPlain>();
+ for (int i = 0; i < cubeNumPoints; i++)
+ {
+ SurgSim::Math::Vector3d p;
+ p[0] = cubePoints[i][0] * lx;
+ p[1] = cubePoints[i][1] * ly;
+ p[2] = cubePoints[i][2] * lz;
+ TriangleMeshPlain::VertexType v(p);
+ mesh->addVertex(v);
+ }
+ for (int i = 0; i < cubeNumEdges; i++)
+ {
+ std::array<size_t, 2> edgePoints;
+ for (int j = 0; j < 2; j++)
+ {
+ edgePoints[j] = cubeEdges[i][j];
+ }
+ TriangleMeshPlain::EdgeType e(edgePoints);
+ mesh->addEdge(e);
+ }
+ for (int i = 0; i < cubeNumTriangles; i++)
+ {
+ std::array<size_t, 3> trianglePoints;
+ for (int j = 0; j < 3; j++)
+ {
+ trianglePoints[j] = cubeTrianglesCCW[i][j];
+ }
+ TriangleMeshPlain::TriangleType t(trianglePoints);
+ mesh->addTriangle(t);
+ }
+
+ SurgSim::Math::MeshShape boxMesh(*mesh);
+
+ SurgSim::Math::BoxShape boxShape(lx, ly, lz);
+
+ EXPECT_NEAR(boxShape.getVolume(), boxMesh.getVolume(), 1e-8);
+ EXPECT_TRUE((boxShape.getCenter() - boxMesh.getCenter()).isZero());
+ EXPECT_TRUE(boxShape.getSecondMomentOfVolume().isApprox(boxMesh.getSecondMomentOfVolume(), 1e-8));
+ }
+}
+
+TEST_F(MeshShapeTest, SerializationTest)
+{
+ const std::string fileName = "MeshShapeData/staple_collision.ply";
+ auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+ EXPECT_NO_THROW(meshShape->load(fileName));
+ EXPECT_TRUE(meshShape->isValid());
+
+ // We chose to let YAML serialization only works with base class pointer.
+ // i.e. We need to serialize 'meshShape' via a SurgSim::Math::Shape pointer.
+ // The usage YAML::Node node = meshShape; will not compile.
+ std::shared_ptr<SurgSim::Math::Shape> shape = meshShape;
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = shape); // YAML::convert<std::shared_ptr<SurgSim::Math::Shape>> will be called.
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ std::shared_ptr<SurgSim::Math::MeshShape> newMeshShape;
+ ASSERT_NO_THROW(newMeshShape = std::dynamic_pointer_cast<SurgSim::Math::MeshShape>(
+ node.as<std::shared_ptr<SurgSim::Math::Shape>>()));
+
+ EXPECT_EQ("SurgSim::Math::MeshShape", newMeshShape->getClassName());
+ EXPECT_EQ(fileName, newMeshShape->getFileName());
+
+ EXPECT_EQ(meshShape->getMesh()->getNumVertices(), newMeshShape->getMesh()->getNumVertices());
+ EXPECT_EQ(meshShape->getMesh()->getNumEdges(), newMeshShape->getMesh()->getNumEdges());
+ EXPECT_EQ(meshShape->getMesh()->getNumTriangles(), newMeshShape->getMesh()->getNumTriangles());
+ EXPECT_TRUE(newMeshShape->isValid());
+}
+
+TEST_F(MeshShapeTest, CreateAabbTreeTest)
+{
+ const std::string fileName = "MeshShapeData/staple_collision.ply";
+ auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+ EXPECT_NO_THROW(meshShape->load(fileName));
+
+ auto tree = meshShape->getAabbTree();
+
+ auto triangles = meshShape->getMesh()->getTriangles();
+ auto vertices = meshShape->getMesh()->getVertices();
+
+ EXPECT_EQ(224u, triangles.size());
+ EXPECT_EQ(504u, vertices.size());
+
+ for (auto it = triangles.begin(); it != triangles.end(); ++it)
+ {
+ auto ids = it->verticesId;
+ EXPECT_TRUE(tree->getAabb().contains(SurgSim::Math::makeAabb(
+ vertices[ids[0]].position, vertices[ids[1]].position, vertices[ids[2]].position)));
+ }
+}
+
+TEST_F(MeshShapeTest, DoLoadTest)
+{
+ SurgSim::Framework::ApplicationData data("config.txt");
+ {
+ auto fileName = std::string("MeshShapeData/staple_collision.ply");
+ auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+
+ EXPECT_NO_THROW(meshShape->load(fileName, data));
+ EXPECT_TRUE(meshShape->isValid());
+ EXPECT_EQ(fileName, meshShape->getFileName());
+ }
+
+ {
+ auto fileName = std::string("MeshShapeData/InvalidMesh.ply");
+ auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+
+ EXPECT_ANY_THROW(meshShape->load(fileName, data));
+ EXPECT_FALSE(meshShape->isValid());
+ EXPECT_EQ(fileName, meshShape->getFileName());
+ }
+
+ {
+ auto fileName = std::string("Nonexistent file");
+ auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+
+ EXPECT_ANY_THROW(meshShape->load(fileName, data));
+ EXPECT_FALSE(meshShape->isValid());
+ EXPECT_EQ(fileName, meshShape->getFileName());
+ }
+}
\ No newline at end of file
diff --git a/SurgSim/Math/UnitTests/MlcpGaussSeidelSolverTests.cpp b/SurgSim/Math/UnitTests/MlcpGaussSeidelSolverTests.cpp
new file mode 100644
index 0000000..21ac352
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpGaussSeidelSolverTests.cpp
@@ -0,0 +1,220 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Gauss-Seidel implementation of the MLCP solver.
+
+#include <math.h>
+#include <memory>
+#include <string>
+
+#include "gtest/gtest.h"
+#include <boost/chrono.hpp>
+
+#include "SurgSim/Math/Valid.h"
+#include "SurgSim/Math/MlcpGaussSeidelSolver.h"
+#include "SurgSim/Math/MlcpSolution.h"
+#include "SurgSim/Testing/MlcpIO/MlcpTestData.h"
+#include "SurgSim/Testing/MlcpIO/ReadText.h"
+
+using SurgSim::Math::isValid;
+using SurgSim::Math::MlcpGaussSeidelSolver;
+
+TEST(MlcpGaussSeidelSolverTests, CanConstruct)
+{
+ //ASSERT_NO_THROW({
+ MlcpGaussSeidelSolver mlcpSolver(1.0, 1.0, 100);
+}
+
+
+static void solveAndCompareResult(const std::string& fileName,
+ double gsSolverPrecision = 1e-9, double gsContactTolerance = 1e-9,
+ int gsMaxIterations = 100)
+{
+ SCOPED_TRACE("while running test " + fileName);
+
+ const std::shared_ptr<MlcpTestData> data = loadTestData(fileName);
+ ASSERT_TRUE(data != nullptr) << "Failed to load " << fileName;
+
+ // NB: need to make the solver calls const-correct.
+ Eigen::MatrixXd A = data->problem.A;
+ Eigen::VectorXd b = data->problem.b;
+ Eigen::VectorXd mu = data->problem.mu;
+ std::vector<SurgSim::Math::MlcpConstraintType> constraintTypes = data->problem.constraintTypes;
+
+ const size_t size = data->getSize();
+ SurgSim::Math::MlcpSolution solution;
+ solution.x.resize(size);
+
+ //################################
+ // Gauss-Seidel solver
+ MlcpGaussSeidelSolver mlcpSolver(gsSolverPrecision, gsContactTolerance,
+ gsMaxIterations);
+
+ solution.x.setZero();
+
+ // XXX set ratio to 1
+ mlcpSolver.solve(data->problem, &solution);
+
+ ASSERT_EQ(size, solution.x.rows());
+ ASSERT_EQ(size, data->expectedLambda.rows());
+ if (size > 0)
+ {
+ ASSERT_TRUE(isValid(solution.x)) << solution.x;
+ }
+
+ // TODO(advornik): Because this is a mixed LCP problem *with friction*, we can't just easily check that
+ // all x are positive (the frictional entries may not be), or that all Ax+b are positive(ditto).
+ // We need to either (a) make the test aware of the meaning of the constraint types, or (b) get rid
+ // of the constraint types and flag the frictional DOFs more directly.
+ //
+ // For now, we check if the MLCP is really a pure LCP (only unilaterals without friction), and we
+ // perform some simple checks that apply to that case and that case only.
+ bool isSimpleLcp = true;
+ for (auto it = constraintTypes.cbegin(); it != constraintTypes.cend(); ++it)
+ {
+ if ((*it) != SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT)
+ {
+ isSimpleLcp = false;
+ }
+ }
+
+ if (isSimpleLcp && (size > 0))
+ {
+ EXPECT_GE(solution.x.minCoeff(), 0.0) << "x contains negative coefficients:" << std::endl << solution.x;
+
+ Eigen::VectorXd c = A * solution.x + b;
+ EXPECT_GE(c.minCoeff(), -gsSolverPrecision) << "Ax+b contains negative coefficients:" << std::endl << c;
+
+ // Orthogonality test should be taking into account the scaling factor
+ double maxAbsForce = abs(solution.x.maxCoeff());
+ if (abs(solution.x.minCoeff()) > maxAbsForce)
+ {
+ maxAbsForce = abs(solution.x.minCoeff());
+ }
+ const double epsilon = 1e-9 * maxAbsForce;
+ EXPECT_NEAR(0.0, c.dot(solution.x), epsilon) << "Ax+b is not orthogonal to x!" << std::endl <<
+ "x:" << std::endl << solution.x << std::endl << "Ax+b:" << std::endl << c;
+ }
+
+ EXPECT_TRUE(solution.x.isApprox(data->expectedLambda)) << "lambda:" << std::endl << solution.x << std::endl <<
+ "expected:" << std::endl << data->expectedLambda;
+
+// double convergenceCriteria=0.0;
+// bool validSignorini=false;
+// int nbContactToSkip=0;
+// int currentAtomicIndex = calculateConvergenceCriteria(lambda, nbContactToSkip, convergenceCriteria,
+// validSignorini, 1.0);
+}
+
+TEST(MlcpGaussSeidelSolverTests, SolveOriginal)
+{
+ const double gsSolverPrecision = 1e-4;
+ const double gsContactTolerance = 2e-4;
+ int gsMaxIterations = 30;
+ solveAndCompareResult("mlcpOriginalTest.txt", gsSolverPrecision, gsContactTolerance, gsMaxIterations);
+}
+
+TEST(MlcpGaussSeidelSolverTests, SolveSequence)
+{
+ for (int i = 0; i <= 9; ++i)
+ {
+ solveAndCompareResult(getTestFileName("mlcpTest", i, ".txt"));
+ }
+}
+
+static void solveRepeatedly(const MlcpTestData& data,
+ /*XXX const */ MlcpGaussSeidelSolver* mlcpSolver,
+ const int repetitions)
+{
+ // NB: need to make the solver calls const-correct.
+ SurgSim::Math::MlcpProblem problem;
+ SurgSim::Math::MlcpSolution solution;
+ std::vector<SurgSim::Math::MlcpConstraintType> constraintTypes;
+
+ const size_t size = data.getSize();
+ solution.x.resize(size);
+
+ for (int i = repetitions; i > 0; --i)
+ {
+ problem = data.problem;
+
+ if (mlcpSolver)
+ {
+ solution.x.setZero();
+ mlcpSolver->solve(problem, &solution);
+ }
+ }
+}
+
+static double measureExecutionTimeUsec(const std::string& fileName,
+ double gsSolverPrecision = 1e-8, double gsContactTolerance = 1e-8,
+ int gsMaxIterations = 20)
+{
+ SCOPED_TRACE("while running test " + fileName);
+
+ const std::shared_ptr<MlcpTestData> data = loadTestData(fileName);
+ EXPECT_TRUE(data != nullptr) << "Failed to load " << fileName;
+
+ MlcpGaussSeidelSolver mlcpSolver(gsSolverPrecision, gsContactTolerance, gsMaxIterations);
+
+ typedef boost::chrono::high_resolution_clock clock;
+ clock::time_point calibrationStart = clock::now();
+
+ const int calibrationRepetitions = 1000;
+ solveRepeatedly(*data, &mlcpSolver, calibrationRepetitions);
+
+ boost::chrono::duration<double> calibrationTime = clock::now() - calibrationStart;
+ double desiredTestTimeSec = 1.0;
+ double desiredRepetitions = desiredTestTimeSec * calibrationRepetitions / calibrationTime.count();
+ const int repetitions = std::max(10, std::min(1000000, static_cast<int>(desiredRepetitions)));
+
+ clock::time_point time0 = clock::now();
+
+ // Do not actually solve the problem; just copy the input data.
+ solveRepeatedly(*data, nullptr, repetitions);
+
+ clock::time_point time1 = clock::now();
+
+ // Actually solve the problem.
+ solveRepeatedly(*data, &mlcpSolver, repetitions);
+
+ clock::time_point time2 = clock::now();
+
+ boost::chrono::duration<double> elapsedSolveTime = time2 - time1;
+ //std::cout << "Elapsed: " << (elapsedSolveTime.count() * 1000.) << " ms" << std::endl;
+ boost::chrono::duration<double> elapsedBaseline = time1 - time0;
+ //std::cout << "Baseline: " << (elapsedBaseline.count() * 1000.) << " ms" << std::endl;
+ double solveTimeUsec = (elapsedSolveTime - elapsedBaseline).count() * 1e6 / repetitions;
+ double copyTimeUsec = elapsedBaseline.count() * 1e6 / repetitions;
+ std::cout << "Average solution time: " << solveTimeUsec << " microseconds (~" << (solveTimeUsec+copyTimeUsec) <<
+ "-" << copyTimeUsec << ") over " << repetitions << " loops" << std::endl;
+ return solveTimeUsec;
+}
+
+TEST(MlcpGaussSeidelSolverTests, MeasureExecutionTimeOriginal)
+{
+ const double gsSolverPrecision = 1e-4;
+ const double gsContactTolerance = 2e-5;
+ int gsMaxIterations = 30;
+ double solveTimeUsec = measureExecutionTimeUsec("mlcpOriginalTest.txt", gsSolverPrecision, gsContactTolerance,
+ gsMaxIterations);
+
+ // When refactoring the MLCP code, it can be very useful to compare the execution time before and after making
+ // changes. But you can't usefully compare execution times on different machines, or with different build
+ // settings, so you'll need to manually uncomment the following line and adjust the time accordingly.
+// EXPECT_LE(solveTimeUsec, 14.0);
+ EXPECT_LE(solveTimeUsec, 1e6);
+}
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpOriginalTest.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpOriginalTest.txt
new file mode 100644
index 0000000..9f83f8d
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpOriginalTest.txt
@@ -0,0 +1,28 @@
+# Original MLCP test - some data from a live simulation
+flags =
+total degrees of freedom = -1
+number of constraints = 6
+number of constraint DOFs ("atomic constraints") = 16
+constraint types = MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_2D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_2D_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT
+E = ( -1.078639261242742e-009 3.8628052700939719e-009 3.4365124678206738e-011 5.2439675317372814e-008 -1.0881681666852216e-010 -5.4598438617436962e-010 1.3895925000950715e-009 -8.0260487492211929e-010 3.182259790575348e-008 1.5418919745158188e-008 -0.0028349485130807293 -0.00024345752383292049 -6.8288003755851215e-005 -0.0024687155316284442 0.00044509381981076065 0.00036473635193496473)
+HCHt = (
+ ( 1.3345657526376539e-008 1.4762129451361012e-012 1.3021510888275589e-009 -3.1071425782284856e-008 -5.6308255099272961e-011 0 0 0 0 0 0 0 0 0 0 0)
+ ( 1.4762129451361044e-012 1.4730637830740187e-008 -1.2917016738606647e-012 -7.1607792168177411e-016 -4.2296549471529904e-012 0 0 0 0 0 0 0 0 0 0 0)
+ ( 1.3021510888275589e-009 -1.2917016738606573e-012 1.350942244549466e-008 -3.3089655917465503e-008 -5.993834037837092e-011 0 0 0 0 0 0 0 0 0 0 0)
+ ( -3.1071425782284856e-008 -7.1607792168177411e-016 -3.3089655917465503e-008 7.9109243566832458e-007 -2.0914474411210304e-011 0 0 0 0 0 0 0 0 0 0 0)
+ ( -5.6308255099272961e-011 -4.2296549471529904e-012 -5.993834037837092e-011 -2.0914474411210346e-011 7.9999738402362291e-007 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 1.5589983172147443e-008 2.4655681638273228e-010 -2.6547210557780089e-010 -4.0242767107961668e-010 2.9172027027174455e-008 -1.4626940755792081e-009 -8.3546545220688397e-009 -6.910879572780377e-012 1.0076117393553975e-009 8.6338319022767194e-009 -1.2342675099707964e-010)
+ ( 0 0 0 0 0 2.4655681638273228e-010 1.2772429953165537e-008 -2.8455178752109829e-010 -7.9973379384833891e-011 5.8411815009243883e-009 -6.474323176106377e-009 -5.7283661469222675e-010 -1.0858340296547962e-009 6.2702667451699401e-009 -1.7749942115993248e-009 -4.3201426346668623e-010)
+ ( 0 0 0 0 0 -2.6547210557780094e-010 -2.8455178752109829e-010 1.5781997723164011e-008 5.6863004741912405e-010 -4.1221084373918575e-008 6.434508335504067e-010 1.5299613175269497e-010 -8.7523797506238338e-009 -1.24204206311367e-009 -6.5906217261540902e-011 8.5224220548311002e-009)
+ ( 0 0 0 0 0 -4.0242767107961668e-010 -7.9973379384833891e-011 5.6863004741912405e-010 7.9763312499397027e-007 1.1507401561300607e-010 3.3984526634410832e-009 8.0413593280745838e-009 -1.1262542833458448e-008 3.4590894897356843e-009 7.1961798751286384e-009 -1.0553214707732758e-008)
+ ( 0 0 0 0 0 2.9172027027174455e-008 5.8411815009243883e-009 -4.1221084373918575e-008 1.1507401561300607e-010 7.8929265028778233e-007 -8.7525370098262112e-009 -2.0706484870371629e-008 2.8946431215678211e-008 9.6712597799773566e-009 2.016714890447026e-008 -2.951199226885018e-008)
+ ( 0 0 0 0 0 -1.4626940755792081e-009 -6.474323176106377e-009 6.434508335504067e-010 3.3984526634410832e-009 -8.7525370098262112e-009 9.3508899127147559e-005 6.1061434208882659e-006 -1.0005996410361315e-005 2.3175132657846745e-005 -5.7725592020145953e-007 -2.6121568520215946e-006)
+ ( 0 0 0 0 0 -8.3546545220688397e-009 -5.7283661469222675e-010 1.5299613175269497e-010 8.0413593280745838e-009 -2.0706484870371629e-008 6.1061434208882693e-006 9.9502091897911858e-005 7.7201461732487004e-006 6.37466867795377e-006 2.3794026441112229e-005 5.9700902633940878e-006)
+ ( 0 0 0 0 0 -6.910879572780377e-012 -1.0858340296547962e-009 -8.7523797506238338e-009 -1.1262542833458448e-008 2.8946431215678211e-008 -1.0005996410361326e-005 7.7201461732486902e-006 0.00010300469354547019 2.0371361118566863e-006 1.2143141546752772e-005 1.9538302894163935e-005)
+ ( 0 0 0 0 0 1.0076117393553975e-009 6.2702667451699401e-009 -1.24204206311367e-009 3.4590894897356843e-009 9.6712597799773566e-009 2.3175132657846745e-005 6.37466867795377e-006 2.0371361118566863e-006 7.024991409575548e-005 4.749834757924254e-006 -3.4246067483125659e-006)
+ ( 0 0 0 0 0 8.6338319022767194e-009 -1.7749942115993248e-009 -6.5906217261540902e-011 7.1961798751286384e-009 2.016714890447026e-008 -5.7725592020145953e-007 2.3794026441112229e-005 1.2143141546752772e-005 4.7498347579242642e-006 6.0599243166315907e-005 7.4677104014104345e-006)
+ ( 0 0 0 0 0 -1.2342675099707964e-010 -4.3201426346668623e-010 8.5224220548311002e-009 -1.0553214707732758e-008 -2.951199226885018e-008 -2.6121568520215946e-006 5.9700902633940878e-006 1.9538302894163935e-005 -3.4246067483125621e-006 7.4677104014104133e-006 5.4786811573213588e-005)
+)
+friction = ( 2.8797662206765437e-216 0 0 0 0.40000000000000002 0.40000000000000002)
+lambda = ( -0.080836084913972622 -0.26223731104011477 -0.18374150677203246 -0.077148138493640786 0.00011316190840491921 6.5804169707683497 -2.1186332046610241 5.7403838888910892 -0.18883274186185561 -0.05530115251517391 24.309512511537893 1.4775577619944267 4.3566278655246844 27.275211987031248 -10.073699584423007 -4.1893349010069185)
+== END ==
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest000.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest000.txt
new file mode 100644
index 0000000..e6bb7cc
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest000.txt
@@ -0,0 +1,12 @@
+# MLCP Test 000 - size 0
+flags =
+total degrees of freedom = 0
+number of constraints = 0
+number of constraint DOFs ("atomic constraints") = 0
+constraint types =
+E = ()
+HCHt = (
+)
+friction = ()
+lambda = ()
+== END ==
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest001.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest001.txt
new file mode 100644
index 0000000..0bbcea7
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest001.txt
@@ -0,0 +1,42 @@
+# MLCP Test 001 - size 3N - All independent (N rigid bodies) bilateral 3D (diagonal block (3) matrix)
+flags = SOLVER_STATUS_METHOD
+total degrees of freedom = 60
+number of constraints = 10
+number of constraint DOFs ("atomic constraints") = 30
+constraint types = MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT
+E = ( -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.00000000 [...]
+HCHt = (
+ ( 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0)
+ ( 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009)
+)
+friction = ( 0 0 0 0 0 0 0 0 0 0)
+lambda = ( 99.999999999999986 199.99999999999997 299.99999999999994 99.999999999999986 199.99999999999997 299.99999999999994 99.999999999999986 199.99999999999997 299.99999999999994 99.999999999999986 199.99999999999997 299.99999999999994 99.999999999999986 199.99999999999997 299.99999999999994 99.999999999999986 199.99999999999997 299.99999999999994 99.999999999999986 199.99999999999997 299.99999999999994 99.999999999999986 199.99999999999997 299.99999999999994 99.999999999999986 199.99 [...]
+== END ==
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest002.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest002.txt
new file mode 100644
index 0000000..f771626
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest002.txt
@@ -0,0 +1,30 @@
+# MLCP Test 002 - size 3N - All dependent (1 rigid body) bilateral 3D (full matrix) - physically possible
+flags = SOLVER_GAUSS_SEIDEL
+total degrees of freedom = 6
+number of constraints = 6
+number of constraint DOFs ("atomic constraints") = 18
+constraint types = MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT
+E = ( -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007)
+HCHt = (
+ ( 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0)
+ ( 0 3.4999999999999999e-009 0 0 -1.5e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009 0 2.5000000000000001e-009 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0)
+ ( 0 0 3.4999999999999999e-009 0 0 -1.5e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 -2.5000000000000001e-009 0 1.0000000000000001e-009 2.5000000000000001e-009 0 1.0000000000000001e-009)
+ ( 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0)
+ ( 0 -1.5e-009 0 0 3.4999999999999999e-009 0 2.5000000000000001e-009 1.0000000000000001e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0)
+ ( 0 0 -1.5e-009 0 0 3.4999999999999999e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 2.5000000000000001e-009 0 1.0000000000000001e-009 -2.5000000000000001e-009 0 1.0000000000000001e-009)
+ ( 1.0000000000000001e-009 -2.5000000000000001e-009 0 1.0000000000000001e-009 2.5000000000000001e-009 0 3.4999999999999999e-009 0 0 -1.5e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0)
+ ( 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0)
+ ( 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 3.4999999999999999e-009 0 0 -1.5e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009 0 2.5000000000000001e-009 1.0000000000000001e-009)
+ ( 1.0000000000000001e-009 2.5000000000000001e-009 0 1.0000000000000001e-009 -2.5000000000000001e-009 0 -1.5e-009 0 0 3.4999999999999999e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0)
+ ( 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0)
+ ( 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 -1.5e-009 0 0 3.4999999999999999e-009 0 2.5000000000000001e-009 1.0000000000000001e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009)
+ ( 1.0000000000000001e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009 0 2.5000000000000001e-009 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 3.4999999999999999e-009 0 0 -1.5e-009 0 0)
+ ( 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 -2.5000000000000001e-009 0 1.0000000000000001e-009 2.5000000000000001e-009 0 3.4999999999999999e-009 0 0 -1.5e-009 0)
+ ( 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009)
+ ( 1.0000000000000001e-009 0 2.5000000000000001e-009 1.0000000000000001e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 -1.5e-009 0 0 3.4999999999999999e-009 0 0)
+ ( 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 2.5000000000000001e-009 0 1.0000000000000001e-009 -2.5000000000000001e-009 0 -1.5e-009 0 0 3.4999999999999999e-009 0)
+ ( 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009)
+)
+friction = ( 0 0 0 0 0 0)
+lambda = ( 113.54027658543633 55.984447685184627 81.845635261303883 -3.3087224502121103e-015 84.261711722572898 124.6596369935406 -18.056081083408476 63.19628379192968 25.729224155848073 10.317760619090549 4.9630836753181653e-015 38.723144908175662 -24.501830651667866 -8.2978167660376485 29.042358681131724 18.422943494849036 4.7416095805929466 3.3087224502121107e-015)
+== END ==
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest003.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest003.txt
new file mode 100644
index 0000000..1573e8c
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest003.txt
@@ -0,0 +1,30 @@
+# MLCP Test 003 - size 3N - All dependent (1 rigid body) bilateral 3D (full matrix) - physically impossible
+flags = SOLVER_GAUSS_SEIDEL
+total degrees of freedom = 6
+number of constraints = 6
+number of constraint DOFs ("atomic constraints") = 18
+constraint types = MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT
+E = ( -0 -0 -0 -1.0000000000000001e-007 -2.0000000000000002e-007 -2.9999999999999999e-007 -2.0000000000000002e-007 -8.0000000000000007e-007 -2.3999999999999999e-006 -3.0000000000000004e-007 -1.8000000000000001e-006 -8.1000000000000004e-006 -4.0000000000000003e-007 -3.2000000000000003e-006 -1.9199999999999999e-005 -4.9999999999999998e-007 -5.0000000000000004e-006 -3.749999999999999e-005)
+HCHt = (
+ ( 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0)
+ ( 0 3.4999999999999999e-009 0 0 -1.5e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009 0 2.5000000000000001e-009 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0)
+ ( 0 0 3.4999999999999999e-009 0 0 -1.5e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 -2.5000000000000001e-009 0 1.0000000000000001e-009 2.5000000000000001e-009 0 1.0000000000000001e-009)
+ ( 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0)
+ ( 0 -1.5e-009 0 0 3.4999999999999999e-009 0 2.5000000000000001e-009 1.0000000000000001e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0)
+ ( 0 0 -1.5e-009 0 0 3.4999999999999999e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 2.5000000000000001e-009 0 1.0000000000000001e-009 -2.5000000000000001e-009 0 1.0000000000000001e-009)
+ ( 1.0000000000000001e-009 -2.5000000000000001e-009 0 1.0000000000000001e-009 2.5000000000000001e-009 0 3.4999999999999999e-009 0 0 -1.5e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0)
+ ( 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0)
+ ( 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 3.4999999999999999e-009 0 0 -1.5e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009 0 2.5000000000000001e-009 1.0000000000000001e-009)
+ ( 1.0000000000000001e-009 2.5000000000000001e-009 0 1.0000000000000001e-009 -2.5000000000000001e-009 0 -1.5e-009 0 0 3.4999999999999999e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0)
+ ( 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0)
+ ( 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 -1.5e-009 0 0 3.4999999999999999e-009 0 2.5000000000000001e-009 1.0000000000000001e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009)
+ ( 1.0000000000000001e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009 0 2.5000000000000001e-009 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 3.4999999999999999e-009 0 0 -1.5e-009 0 0)
+ ( 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 -2.5000000000000001e-009 0 1.0000000000000001e-009 2.5000000000000001e-009 0 3.4999999999999999e-009 0 0 -1.5e-009 0)
+ ( 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009)
+ ( 1.0000000000000001e-009 0 2.5000000000000001e-009 1.0000000000000001e-009 0 -2.5000000000000001e-009 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 -1.5e-009 0 0 3.4999999999999999e-009 0 0)
+ ( 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 2.5000000000000001e-009 0 1.0000000000000001e-009 -2.5000000000000001e-009 0 -1.5e-009 0 0 3.4999999999999999e-009 0)
+ ( 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009 0 0 1.0000000000000001e-009)
+)
+friction = ( 0 0 0 0 0 0)
+lambda = ( -176303.18958583617 -87925.632737205815 -1024484.8029754778 10000.000000000253 -119643.57244199516 -1527437.2554425916 25637.908380427547 -19732.679331495205 -316650.30202035437 -6078.8047888158735 100000.00000000031 -182244.36546376004 326005.38228295406 -2376.207400623372 1258316.7259021834 -177465.42049975123 132786.40422892763 1830000.0000000005)
+== END ==
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest004.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest004.txt
new file mode 100644
index 0000000..65fe771
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest004.txt
@@ -0,0 +1,22 @@
+# MLCP Test 004 - size N - All independent (N rigid bodies) unilateral frictionless (diagonal block (1) matrix = diagonal matrix) - positive violation (nothing to solve)
+flags = SOLVER_STATUS_METHOD
+total degrees of freedom = 60
+number of constraints = 10
+number of constraint DOFs ("atomic constraints") = 10
+constraint types = MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
+E = ( 0.9876159799998131 2.1304951684997055 3.2733743569995983 4.4162535454994911 5.5591327339993839 6.7020119224992767 7.8448911109991686 8.9877702994990614 10.130649487998955 11.273528676498847)
+HCHt = (
+ ( 1.0000000000000003e-009 0 0 0 0 0 0 0 0 0)
+ ( 0 1.0000000000000003e-009 0 0 0 0 0 0 0 0)
+ ( 0 0 1.0000000000000003e-009 0 0 0 0 0 0 0)
+ ( 0 0 0 1.0000000000000003e-009 0 0 0 0 0 0)
+ ( 0 0 0 0 1.0000000000000003e-009 0 0 0 0 0)
+ ( 0 0 0 0 0 1.0000000000000003e-009 0 0 0 0)
+ ( 0 0 0 0 0 0 1.0000000000000003e-009 0 0 0)
+ ( 0 0 0 0 0 0 0 1.0000000000000003e-009 0 0)
+ ( 0 0 0 0 0 0 0 0 1.0000000000000003e-009 0)
+ ( 0 0 0 0 0 0 0 0 0 1.0000000000000003e-009)
+)
+friction = ( 0 0 0 0 0 0 0 0 0 0)
+lambda = ( 0 0 0 0 0 0 0 0 0 0)
+== END ==
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest005.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest005.txt
new file mode 100644
index 0000000..00a0a3b
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest005.txt
@@ -0,0 +1,18 @@
+# MLCP Test 005 - size N - All dependent (1 rigid body) unilateral frictionless (full matrix) - positive violation (nothing to solve). 6 contacts per object, the 6 cardinal points.
+flags = SOLVER_GAUSS_SEIDEL
+total degrees of freedom = 6
+number of constraints = 6
+number of constraint DOFs ("atomic constraints") = 6
+constraint types = MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
+E = ( 1.8882351809998226 2.0869967789998038 0.9938079899999066 2.98142396999972 1.9379255804998179 2.0373063794998085)
+HCHt = (
+ ( 3.47530864197531e-009 -1.4753086419753095e-009 7.530864197530866e-010 1.2469135802469138e-009 9.8765432098765449e-010 1.0123456790123461e-009)
+ ( -1.4753086419753095e-009 3.47530864197531e-009 1.2469135802469138e-009 7.530864197530866e-010 1.0123456790123461e-009 9.8765432098765449e-010)
+ ( 7.530864197530866e-010 1.2469135802469138e-009 1.0308641975308644e-009 9.6913580246913612e-010 8.7654320987654348e-010 1.1234567901234571e-009)
+ ( 1.2469135802469138e-009 7.530864197530866e-010 9.6913580246913612e-010 1.0308641975308644e-009 1.1234567901234571e-009 8.7654320987654348e-010)
+ ( 9.8765432098765449e-010 1.0123456790123461e-009 8.7654320987654348e-010 1.1234567901234571e-009 3.4938271604938288e-009 -1.493827160493828e-009)
+ ( 1.0123456790123461e-009 9.8765432098765449e-010 1.1234567901234571e-009 8.7654320987654348e-010 -1.493827160493828e-009 3.4938271604938288e-009)
+)
+friction = ( 0 0 0 0 0 0)
+lambda = ( 0 0 0 0 0 0)
+== END ==
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest006.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest006.txt
new file mode 100644
index 0000000..9130221
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest006.txt
@@ -0,0 +1,22 @@
+# MLCP Test 006 - size N - All independent (N rigid bodies) unilateral frictionless (diagonal block (1) matrix = diagonal matrix). 1 contact per object, the deepest point penetrated.
+flags = SOLVER_GAUSS_SEIDEL
+total degrees of freedom = 60
+number of constraints = 10
+number of constraint DOFs ("atomic constraints") = 10
+constraint types = MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
+E = ( -0.0020000000000000018 -0.0040000000000000036 -0.0060000000000000053 -0.0080000000000000071 -0.010000000000000009 -0.012000000000000011 -0.014000000000000012 -0.016000000000000014 -0.018000000000000016 -0.020000000000000018)
+HCHt = (
+ ( 1.0000000000000001e-009 0 0 0 0 0 0 0 0 0)
+ ( 0 1.0000000000000001e-009 0 0 0 0 0 0 0 0)
+ ( 0 0 1.0000000000000001e-009 0 0 0 0 0 0 0)
+ ( 0 0 0 1.0000000000000001e-009 0 0 0 0 0 0)
+ ( 0 0 0 0 1.0000000000000001e-009 0 0 0 0 0)
+ ( 0 0 0 0 0 1.0000000000000001e-009 0 0 0 0)
+ ( 0 0 0 0 0 0 1.0000000000000001e-009 0 0 0)
+ ( 0 0 0 0 0 0 0 1.0000000000000001e-009 0 0)
+ ( 0 0 0 0 0 0 0 0 1.0000000000000001e-009 0)
+ ( 0 0 0 0 0 0 0 0 0 1.0000000000000001e-009)
+)
+friction = ( 0 0 0 0 0 0 0 0 0 0)
+lambda = ( 2000000.0000000016 4000000.0000000033 6000000.0000000047 8000000.0000000065 10000000.000000007 12000000.000000009 14000000.000000011 16000000.000000013 18000000.000000015 20000000.000000015)
+== END ==
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest007.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest007.txt
new file mode 100644
index 0000000..f67dceb
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest007.txt
@@ -0,0 +1,18 @@
+# MLCP Test 007 - size N - All dependent (1 rigid body) unilateral frictionless (full matrix). 6 contacts per object, the 6 cardinal points in local frame.
+flags = SOLVER_GAUSS_SEIDEL
+total degrees of freedom = 6
+number of constraints = 6
+number of constraint DOFs ("atomic constraints") = 6
+constraint types = MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
+E = ( 0.89061920100000957 1.0893807989999909 -0.0038079899999064557 1.9838079899999068 0.94030960050000489 1.0396903994999955)
+HCHt = (
+ ( 3.47530864197531e-009 -1.4753086419753095e-009 7.530864197530866e-010 1.2469135802469138e-009 9.8765432098765449e-010 1.0123456790123461e-009)
+ ( -1.4753086419753095e-009 3.47530864197531e-009 1.2469135802469138e-009 7.530864197530866e-010 1.0123456790123461e-009 9.8765432098765449e-010)
+ ( 7.530864197530866e-010 1.2469135802469138e-009 1.0308641975308644e-009 9.6913580246913612e-010 8.7654320987654348e-010 1.1234567901234571e-009)
+ ( 1.2469135802469138e-009 7.530864197530866e-010 9.6913580246913612e-010 1.0308641975308644e-009 1.1234567901234571e-009 8.7654320987654348e-010)
+ ( 9.8765432098765449e-010 1.0123456790123461e-009 8.7654320987654348e-010 1.1234567901234571e-009 3.4938271604938288e-009 -1.4938271604938272e-009)
+ ( 1.0123456790123461e-009 9.8765432098765449e-010 1.1234567901234571e-009 8.7654320987654348e-010 -1.4938271604938272e-009 3.4938271604938267e-009)
+)
+friction = ( 0 0 0 0 0 0)
+lambda = ( 0 0 3693978.323262549 0 0 0)
+== END ==
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest008.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest008.txt
new file mode 100644
index 0000000..5e89abe
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest008.txt
@@ -0,0 +1,24 @@
+# MLCP Test 008 - size 2N - All dependent (1 rigid body) unilateral frictionless (full matrix). 2x6 contacts per object, the 6 cardinal points in local frame against 2 convex planes.
+flags = SOLVER_GAUSS_SEIDEL
+total degrees of freedom = 6
+number of constraints = 12
+number of constraint DOFs ("atomic constraints") = 12
+constraint types = MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT [...]
+E = ( 0.73543550676819003 1.127667777044558 -0.049029033784546046 1.9121323175972942 0.93155164190637407 0.93155164190637407 1.127667777044558 0.73543550676819003 -0.049029033784546046 1.9121323175972942 0.93155164190637407 0.93155164190637407)
+HCHt = (
+ ( 3.4004457081380151e-009 -1.4024437101360178e-009 5.1871205717359543e-010 1.4792899408284021e-009 9.9900099900099875e-010 9.9900099900099875e-010 3.3235994774456308e-009 -1.4792899408284023e-009 1.4024437101360176e-009 4.4186582648121088e-010 9.2215476830861419e-010 9.2215476830861419e-010)
+ ( -1.4024437101360178e-009 3.4004457081380151e-009 1.4792899408284021e-009 5.1871205717359543e-010 9.9900099900099875e-010 9.9900099900099875e-010 -1.4792899408284023e-009 3.3235994774456308e-009 4.4186582648121088e-010 1.4024437101360176e-009 9.2215476830861419e-010 9.2215476830861419e-010)
+ ( 5.1871205717359543e-010 1.4792899408284021e-009 1.0950587873664794e-009 9.0294321063551812e-010 9.9900099900099875e-010 9.9900099900099875e-010 4.4186582648121098e-010 1.4024437101360174e-009 8.2609697994313357e-010 1.0182125566740949e-009 9.2215476830861419e-010 9.2215476830861419e-010)
+ ( 1.4792899408284021e-009 5.1871205717359543e-010 9.0294321063551812e-010 1.0950587873664794e-009 9.9900099900099875e-010 9.9900099900099875e-010 1.4024437101360174e-009 4.4186582648121098e-010 1.0182125566740949e-009 8.2609697994313357e-010 9.2215476830861419e-010 9.2215476830861419e-010)
+ ( 9.9900099900099875e-010 9.9900099900099875e-010 9.9900099900099875e-010 9.9900099900099875e-010 3.4965034965034959e-009 -1.4985014985014984e-009 9.2215476830861419e-010 9.2215476830861419e-010 9.2215476830861419e-010 9.2215476830861419e-010 3.22754168908015e-009 -1.3832321524629216e-009)
+ ( 9.9900099900099875e-010 9.9900099900099875e-010 9.9900099900099875e-010 9.9900099900099875e-010 -1.4985014985014984e-009 3.4965034965034959e-009 9.2215476830861419e-010 9.2215476830861419e-010 9.2215476830861419e-010 9.2215476830861419e-010 -1.3832321524629216e-009 3.22754168908015e-009)
+ ( 3.3235994774456308e-009 -1.4792899408284023e-009 4.4186582648121098e-010 1.4024437101360174e-009 9.2215476830861419e-010 9.2215476830861419e-010 3.4004457081380151e-009 -1.4024437101360178e-009 1.4792899408284021e-009 5.1871205717359543e-010 9.9900099900099875e-010 9.9900099900099875e-010)
+ ( -1.4792899408284023e-009 3.3235994774456308e-009 1.4024437101360174e-009 4.4186582648121098e-010 9.2215476830861419e-010 9.2215476830861419e-010 -1.4024437101360178e-009 3.4004457081380151e-009 5.1871205717359543e-010 1.4792899408284021e-009 9.9900099900099875e-010 9.9900099900099875e-010)
+ ( 1.4024437101360176e-009 4.4186582648121088e-010 8.2609697994313357e-010 1.0182125566740949e-009 9.2215476830861419e-010 9.2215476830861419e-010 1.4792899408284021e-009 5.1871205717359543e-010 1.0950587873664794e-009 9.0294321063551812e-010 9.9900099900099875e-010 9.9900099900099875e-010)
+ ( 4.4186582648121088e-010 1.4024437101360176e-009 1.0182125566740949e-009 8.2609697994313357e-010 9.2215476830861419e-010 9.2215476830861419e-010 5.1871205717359543e-010 1.4792899408284021e-009 9.0294321063551812e-010 1.0950587873664794e-009 9.9900099900099875e-010 9.9900099900099875e-010)
+ ( 9.2215476830861419e-010 9.2215476830861419e-010 9.2215476830861419e-010 9.2215476830861419e-010 3.22754168908015e-009 -1.3832321524629216e-009 9.9900099900099875e-010 9.9900099900099875e-010 9.9900099900099875e-010 9.9900099900099875e-010 3.4965034965034959e-009 -1.4985014985014984e-009)
+ ( 9.2215476830861419e-010 9.2215476830861419e-010 9.2215476830861419e-010 9.2215476830861419e-010 -1.3832321524629216e-009 3.22754168908015e-009 9.9900099900099875e-010 9.9900099900099875e-010 9.9900099900099875e-010 9.9900099900099875e-010 -1.4985014985014984e-009 3.4965034965034959e-009)
+)
+friction = ( 0 0 0 0 0 0 0 0 0 0 0 0)
+lambda = ( 0 0 25520594.196313217 0 0 0 0 0 25520591.510731984 0 0 0)
+== END ==
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest009.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest009.txt
new file mode 100644
index 0000000..96151c0
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest009.txt
@@ -0,0 +1,21 @@
+# MLCP Test 009 - size N - dependent Bilateral3D + unilateral frictionless (full matrix) - 1 rigid body + 1 fem3d linked together, rigid body colliding with plane
+flags = SOLVER_GAUSS_SEIDEL SOLVER_GAUSS_SEIDEL
+total degrees of freedom = 654
+number of constraints = 7
+number of constraint DOFs ("atomic constraints") = 9
+constraint types = MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT MLCP_BILATERAL_3D_CONSTRAINT
+E = ( 0.90998999999999997 0.90998999999999997 -0.090009999999999923 1.9099900000000001 0.90998999999999997 0.90998999999999997 9.9999999999961231e-006 -9.9999999999961231e-006 1.0000000000010001e-005)
+HCHt = (
+ ( 1.0250000000000001e-007 9.7500000000000006e-008 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 0 1.0000000000000001e-007 0)
+ ( 9.7500000000000006e-008 1.0250000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 0 1.0000000000000001e-007 0)
+ ( 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 0 1.0000000000000001e-007 0)
+ ( 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 0 1.0000000000000001e-007 0)
+ ( 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0250000000000001e-007 9.7500000000000006e-008 0 1.0000000000000001e-007 0)
+ ( 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 9.7500000000000006e-008 1.0250000000000001e-007 0 1.0000000000000001e-007 0)
+ ( 0 0 0 0 0 0 3.7980968564082065e-006 3.1102531565232793e-009 -1.0190003838823684e-008)
+ ( 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 1.0000000000000001e-007 3.1102531565232789e-009 3.5138140462957323e-006 4.3038176725570532e-010)
+ ( 0 0 0 0 0 0 -1.0190003838823735e-008 4.3038176725570671e-010 3.6670209077024139e-006)
+)
+friction = ( 0 0 0 0 0 0 0)
+lambda = ( 0 0 926463.49146198574 0 0 0 18.957234335337965 -26363.491940144901 0.41983413283463433)
+== END ==
diff --git a/SurgSim/Math/UnitTests/MockObject.h b/SurgSim/Math/UnitTests/MockObject.h
new file mode 100644
index 0000000..a979b7e
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MockObject.h
@@ -0,0 +1,227 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_UNITTESTS_MOCKOBJECT_H
+#define SURGSIM_MATH_UNITTESTS_MOCKOBJECT_H
+
+#include "SurgSim/Math/OdeEquation.h"
+#include "SurgSim/Math/OdeState.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+class MassPointState : public OdeState
+{
+public:
+ MassPointState() : OdeState()
+ {
+ setNumDof(3, 1);
+ getPositions().setLinSpaced(1.0, 1.3);
+ getVelocities().setLinSpaced(0.4, -0.3);
+ }
+};
+
+class MassPoint : public OdeEquation
+{
+public:
+ /// Constructor
+ /// \param viscosity The mass viscosity
+ explicit MassPoint(double viscosity = 0.0) :
+ m_mass(0.456),
+ m_viscosity(viscosity),
+ m_gravity(0.0, -9.81, 0.0),
+ m_f(3),
+ m_M(3, 3),
+ m_D(3, 3),
+ m_K(3, 3)
+ {
+ this->m_initialState = std::make_shared<MassPointState>();
+ }
+
+ void disableGravity()
+ {
+ m_gravity.setZero();
+ }
+
+ /// Evaluation of the RHS function f(x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the function f(x,v) with
+ /// \return The vector containing f(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeF() or computeFMDK()
+ Vector& computeF(const OdeState& state) override
+ {
+ m_f = m_mass * m_gravity - m_viscosity * state.getVelocities();
+ return m_f;
+ }
+
+ /// Evaluation of the LHS matrix M(x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the matrix M(x,v) with
+ /// \return The matrix M(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeM() or computeFMDK()
+ const Matrix& computeM(const OdeState& state) override
+ {
+ m_M.setIdentity();
+ m_M *= m_mass;
+ return m_M;
+ }
+
+ /// Evaluation of D = -df/dv (x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the Jacobian matrix with
+ /// \return The matrix D = -df/dv(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeD() or computeFMDK()
+ const Matrix& computeD(const OdeState& state) override
+ {
+ m_D.setIdentity();
+ m_D *= m_viscosity;
+ return m_D;
+ }
+
+ /// Evaluation of K = -df/dx (x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the Jacobian matrix with
+ /// \return The matrix K = -df/dx(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeK() or computeFMDK()
+ const Matrix& computeK(const OdeState& state)
+ {
+ m_K.setZero();
+ return m_K;
+ }
+
+ /// Evaluation of f(x,v), M(x,v), D = -df/dv(x,v), K = -df/dx(x,v)
+ /// When all the terms are needed, this method can perform optimization in evaluating everything together
+ /// \param state (x, v) the current position and velocity to evaluate the various terms with
+ /// \param[out] f The RHS f(x,v)
+ /// \param[out] M The matrix M(x,v)
+ /// \param[out] D The matrix D = -df/dv(x,v)
+ /// \param[out] K The matrix K = -df/dx(x,v)
+ /// \note Returns pointers, the internal data will remain unchanged until the next call to computeFMDK() or
+ /// \note computeF(), computeM(), computeD(), computeK()
+ void computeFMDK(const OdeState& state, Vector** f, Matrix** M, Matrix** D, Matrix** K) override
+ {
+ m_M.setIdentity();
+ m_M *= m_mass;
+ m_D.setIdentity();
+ m_D *= m_viscosity;
+ m_K.setZero();
+ m_f = m_mass * m_gravity - m_viscosity * state.getVelocities();
+
+ *f = &m_f;
+ *K = &m_K;
+ *D = &m_D;
+ *M = &m_M;
+ }
+
+ double m_mass, m_viscosity;
+ Vector3d m_gravity;
+ Vector m_f;
+ Matrix m_M, m_D, m_K;
+};
+
+
+
+/// State class for static resolution
+/// It contains 3 nodes with 3 dofs each, with positions (0 0 0) (1 0 0) (2 0 0) and null velocities/accelerations
+class MassPointsStateForStatic : public OdeState
+{
+public:
+ MassPointsStateForStatic() : OdeState()
+ {
+ setNumDof(3, 3);
+ getPositions().segment<3>(3) = Vector3d(1.0, 0.0, 0.0);
+ getPositions().segment<3>(6) = Vector3d(2.0, 0.0, 0.0);
+ }
+};
+
+// Model of 3 nodes connected by springs with the 1st node fixed (no mass, no damping, only deformations)
+class MassPointsForStatic : public SurgSim::Math::OdeEquation
+{
+public:
+ /// Constructor
+ MassPointsForStatic() :
+ m_f(9),
+ m_gravityForce(9),
+ m_K(9, 9)
+ {
+ m_f.setZero();
+ m_K.setZero();
+ m_gravityForce.setZero();
+ m_gravityForce.segment<3>(3) = Vector3d(0.0, 0.01 * -9.81, 0.0);
+ m_gravityForce.segment<3>(6) = Vector3d(0.0, 0.01 * -9.81, 0.0);
+
+ this->m_initialState = std::make_shared<MassPointsStateForStatic>();
+ }
+
+ const Vector& getExternalForces() const
+ {
+ return m_gravityForce;
+ }
+
+ virtual Vector& computeF(const OdeState& state) override
+ {
+ // Internale deformation forces
+ m_f = -computeK(state) * (state.getPositions() - m_initialState->getPositions());
+
+ // Gravity pulling on the free nodes
+ m_f += m_gravityForce;
+
+ return m_f;
+ }
+
+ virtual const Matrix& computeM(const OdeState& state) override
+ {
+ m_M.setZero();
+ return m_M;
+ }
+
+ virtual const Matrix& computeD(const OdeState& state) override
+ {
+ m_D.setZero();
+ return m_D;
+ }
+
+ virtual const Matrix& computeK(const OdeState& state)
+ {
+ // A fake but valid stiffness matrix (node 0 fixed)
+ m_K.setIdentity();
+ m_K.block<6, 6>(3, 3).setConstant(2.0); // This adds coupling between nodes 1 and 2
+ m_K.block<6, 6>(3, 3).diagonal().setConstant(10);
+ return m_K;
+ }
+
+ virtual void computeFMDK(const OdeState& state, Vector** f, Matrix** M, Matrix** D, Matrix** K)
+ override
+ {
+ m_f = computeF(state);
+ m_M = computeM(state);
+ m_D = computeD(state);
+ m_K = computeK(state);
+
+ *f = &m_f;
+ *K = &m_K;
+ *D = &m_D;
+ *M = &m_M;
+ }
+
+private:
+ Vector m_f, m_gravityForce;
+ Matrix m_M, m_D, m_K;
+};
+
+}; // Math
+
+}; // SurgSim
+
+#endif // SURGSIM_MATH_UNITTESTS_MOCKOBJECT_H
diff --git a/SurgSim/Math/UnitTests/MockTriangle.h b/SurgSim/Math/UnitTests/MockTriangle.h
new file mode 100644
index 0000000..b49eaa3
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MockTriangle.h
@@ -0,0 +1,106 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_UNITTESTS_MOCKTRIANGLE_H
+#define SURGSIM_MATH_UNITTESTS_MOCKTRIANGLE_H
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// MockTriangle class used in the unit tests.
+class MockTriangle
+{
+public:
+ // Default constructor.
+ MockTriangle() {}
+
+ // Constructor.
+ MockTriangle(const Vector3d& vertex0, const Vector3d& vertex1, const Vector3d& vertex2) :
+ v0(vertex0), v1(vertex1), v2(vertex2), v0v1(vertex1 - vertex0), v0v2(vertex2 - vertex0),
+ v1v2(vertex2 - vertex1)
+ {
+ n = v0v1.cross(v0v2);
+ n.normalize();
+ }
+
+ // Find a point inside the triangle, given a pair of scaling factor for the edges (v0v1 and v0v2).
+ Vector3d pointInTriangle(double a, double b) const
+ {
+ return v0 + a * v0v1 + b * v0v2;
+ }
+
+ // Move this triangle by the given vector.
+ void translate(const Vector3d& v)
+ {
+ v0 += v;
+ v1 += v;
+ v2 += v;
+ }
+
+ // Rotate this triangle about the x-axis by the given angle.
+ void rotateAboutXBy(double angle)
+ {
+ RigidTransform3d r(Eigen::AngleAxis<double>(angle * (M_PI / 180.0), Vector3d(1, 0, 0)));
+ v0 = r * v0;
+ v1 = r * v1;
+ v2 = r * v2;
+ n = r * n;
+ }
+
+ // Rotate this triangle about the y-axis by the given angle.
+ void rotateAboutYBy(double angle)
+ {
+ SurgSim::Math::RigidTransform3d r(Eigen::AngleAxis<double>(angle * (M_PI / 180.0), Vector3d(0, 1, 0)));
+ v0 = r * v0;
+ v1 = r * v1;
+ v2 = r * v2;
+ n = r * n;
+ }
+
+ // Rotate this triangle about the z-axis by the given angle.
+ void rotateAboutZBy(double angle)
+ {
+ SurgSim::Math::RigidTransform3d r(Eigen::AngleAxis<double>(angle * (M_PI / 180.0), Vector3d(0, 0, 1)));
+ v0 = r * v0;
+ v1 = r * v1;
+ v2 = r * v2;
+ n = r * n;
+ }
+
+ // Vertices of this triangle.
+ Vector3d v0;
+ Vector3d v1;
+ Vector3d v2;
+
+ // Edges of this triangle.
+ Vector3d v0v1;
+ Vector3d v0v2;
+ Vector3d v1v2;
+
+ // Normal of the triangle.
+ Vector3d n;
+};
+
+
+} // namespace Math
+
+} // namespace SurgSim
+
+#endif // SURGSIM_MATH_UNITTESTS_MOCKTRIANGLE_H
\ No newline at end of file
diff --git a/SurgSim/Math/UnitTests/OdeEquationTests.cpp b/SurgSim/Math/UnitTests/OdeEquationTests.cpp
new file mode 100644
index 0000000..40e190e
--- /dev/null
+++ b/SurgSim/Math/UnitTests/OdeEquationTests.cpp
@@ -0,0 +1,117 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 class OdeSolverEulerExplicitModified.
+///
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/OdeEquation.h"
+#include "SurgSim/Math/UnitTests/MockObject.h"
+
+using SurgSim::Math::MassPoint;
+using SurgSim::Math::MassPointState;
+
+TEST(OdeEquationTests, Constructor)
+{
+ ASSERT_NO_THROW({MassPoint m;});
+ ASSERT_NO_THROW({MassPoint* m = new MassPoint; delete m;});
+ ASSERT_NO_THROW({MassPoint* m = new MassPoint[10]; delete [] m;});
+ ASSERT_NO_THROW({std::shared_ptr<MassPoint> m = std::make_shared<MassPoint>(); });
+}
+
+TEST(OdeEquationTests, GetInitialStateTest)
+{
+ MassPoint m;
+ ASSERT_NE(nullptr, m.getInitialState());
+ EXPECT_EQ(3, m.getInitialState()->getPositions().size());
+ EXPECT_EQ(3, m.getInitialState()->getVelocities().size());
+ SurgSim::Math::Vector3d expectedX = SurgSim::Math::Vector3d::LinSpaced(1.0, 1.3);
+ EXPECT_TRUE(m.getInitialState()->getPositions().isApprox(expectedX));
+ SurgSim::Math::Vector3d expectedV = SurgSim::Math::Vector3d::LinSpaced(0.4, -0.3);
+ EXPECT_TRUE(m.getInitialState()->getVelocities().isApprox(expectedV));
+}
+
+TEST(OdeEquationTests, ComputesTest)
+{
+ {
+ SCOPED_TRACE("OdeEquationTests computes tests with 0 viscosity");
+ MassPoint m;
+ MassPointState state;
+ state.getPositions() = SurgSim::Math::Vector3d(1.0, 1.0, 1.0);
+ state.getVelocities() = SurgSim::Math::Vector3d(1.0, 1.0, 1.0);
+
+ SurgSim::Math::Vector3d expectedF = m.m_gravity * m.m_mass;
+ SurgSim::Math::Matrix33d expectedM = m.m_mass * SurgSim::Math::Matrix33d::Identity();
+ SurgSim::Math::Matrix33d expectedD = SurgSim::Math::Matrix33d::Zero();
+ SurgSim::Math::Matrix33d expectedK = SurgSim::Math::Matrix33d::Zero();
+ EXPECT_TRUE(m.computeF(state).isApprox(expectedF));
+ EXPECT_TRUE(m.computeM(state).isApprox(expectedM));
+ EXPECT_TRUE(m.computeD(state).isApprox(expectedD));
+ EXPECT_TRUE(m.computeK(state).isApprox(expectedK));
+ {
+ SurgSim::Math::Vector* F = nullptr;
+ SurgSim::Math::Matrix* M = nullptr;
+ SurgSim::Math::Matrix* D = nullptr;
+ SurgSim::Math::Matrix* K = nullptr;
+
+ m.computeFMDK(state, &F, &M, &D, &K);
+ ASSERT_NE(nullptr, F);
+ EXPECT_TRUE(F->isApprox(expectedF));
+ ASSERT_NE(nullptr, M);
+ EXPECT_TRUE(M->isApprox(expectedM));
+ ASSERT_NE(nullptr, D);
+ EXPECT_TRUE(D->isApprox(expectedD));
+ ASSERT_NE(nullptr, K);
+ EXPECT_TRUE(K->isApprox(expectedK));
+ }
+ }
+
+ {
+ SCOPED_TRACE("OdeEquationTests computes tests with non 0 viscosity");
+ MassPoint m(0.1);
+ MassPointState state;
+ state.getPositions() = SurgSim::Math::Vector3d(1.0, 1.0, 1.0);
+ state.getVelocities() = SurgSim::Math::Vector3d(1.0, 1.0, 1.0);
+
+ SurgSim::Math::Vector3d expectedF = m.m_gravity * m.m_mass - 0.1 * state.getVelocities();
+ SurgSim::Math::Matrix33d expectedM = m.m_mass * SurgSim::Math::Matrix33d::Identity();
+ SurgSim::Math::Matrix33d expectedD = 0.1 * SurgSim::Math::Matrix33d::Identity();
+ SurgSim::Math::Matrix33d expectedK = SurgSim::Math::Matrix33d::Zero();
+ EXPECT_TRUE(m.computeF(state).isApprox(expectedF));
+ EXPECT_TRUE(m.computeM(state).isApprox(expectedM));
+ EXPECT_TRUE(m.computeD(state).isApprox(expectedD));
+ EXPECT_TRUE(m.computeK(state).isApprox(expectedK));
+ {
+ SurgSim::Math::Vector* F = nullptr;
+ SurgSim::Math::Matrix* M = nullptr;
+ SurgSim::Math::Matrix* D = nullptr;
+ SurgSim::Math::Matrix* K = nullptr;
+
+ m.computeFMDK(state, &F, &M, &D, &K);
+ ASSERT_NE(nullptr, F);
+ EXPECT_TRUE(F->isApprox(expectedF));
+ ASSERT_NE(nullptr, M);
+ EXPECT_TRUE(M->isApprox(expectedM));
+ ASSERT_NE(nullptr, D);
+ EXPECT_TRUE(D->isApprox(expectedD));
+ ASSERT_NE(nullptr, K);
+ EXPECT_TRUE(K->isApprox(expectedK));
+ }
+ }
+}
diff --git a/SurgSim/Math/UnitTests/OdeSolverEulerExplicitModifiedTests.cpp b/SurgSim/Math/UnitTests/OdeSolverEulerExplicitModifiedTests.cpp
new file mode 100644
index 0000000..e57ea46
--- /dev/null
+++ b/SurgSim/Math/UnitTests/OdeSolverEulerExplicitModifiedTests.cpp
@@ -0,0 +1,123 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 class OdeSolverEulerExplicitModified.
+///
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/OdeSolverEulerExplicitModified.h"
+#include "SurgSim/Math/OdeSolverLinearEulerExplicitModified.h"
+#include "SurgSim/Math/UnitTests/MockObject.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+template<class T>
+void doConstructorTest()
+{
+ MassPoint m;
+ ASSERT_NO_THROW({T solver(&m);});
+}
+
+TEST(OdeSolverEulerExplicitModified, ConstructorTest)
+{
+ {
+ SCOPED_TRACE("EulerExplicitModified");
+ doConstructorTest<OdeSolverEulerExplicitModified>();
+ }
+ {
+ SCOPED_TRACE("LinearEulerExplicitModified");
+ doConstructorTest<OdeSolverLinearEulerExplicitModified>();
+ }
+}
+
+template<class T>
+void doSolveTest()
+{
+ // Test 2 iterations because Linear solvers have a different algorithm on the 1st pass from the following passes.
+
+ {
+ MassPoint m;
+ MassPointState defaultState, state0, state1, state2;
+ T solver(&m);
+
+ // ma = mg <=> a = g
+ // v(1) = g.dt + v(0)
+ // x(1) = v(1).dt + x(0)
+ ASSERT_NO_THROW({solver.solve(1e-3, state0, &state1);});
+ EXPECT_EQ(defaultState, state0);
+ EXPECT_NE(defaultState, state1);
+ EXPECT_TRUE(state1.getVelocities().isApprox(m.m_gravity * 1e-3 + state0.getVelocities()));
+ EXPECT_TRUE(state1.getPositions().isApprox(state1.getVelocities() * 1e-3 + state0.getPositions()));
+
+ // v(2) = g.dt + v(1)
+ // x(2) = v(2).dt + x(1)
+ ASSERT_NO_THROW({solver.solve(1e-3, state1, &state2);});
+ EXPECT_NE(defaultState, state1);
+ EXPECT_NE(defaultState, state2);
+ EXPECT_NE(state2, state1);
+ EXPECT_TRUE(state2.getVelocities().isApprox(m.m_gravity * 1e-3 + state1.getVelocities()));
+ EXPECT_TRUE(state2.getPositions().isApprox(state2.getVelocities() * 1e-3 + state1.getPositions()));
+ }
+
+ {
+ MassPoint m(0.1);
+ MassPointState defaultState, state0, state1, state2;
+ T solver(&m);
+
+ // ma = mg - c.v <=> a = g - c/m.v
+ // v(1) = (g - c/m.v).dt + v(0)
+ // x(1) = v(1).dt + x(0)
+ ASSERT_NO_THROW({solver.solve(1e-3, state0, &state1);});
+ EXPECT_EQ(defaultState, state0);
+ EXPECT_NE(defaultState, state1);
+ Vector3d acceleration0 = m.m_gravity - 0.1 * state0.getVelocities() / m.m_mass;
+ EXPECT_TRUE(state1.getVelocities().isApprox(acceleration0 * 1e-3 + state0.getVelocities()));
+ EXPECT_TRUE(state1.getPositions().isApprox(state1.getVelocities() * 1e-3 + state0.getPositions()));
+
+ // v(2) = (g - c/m.v).dt + v(1)
+ // x(2) = v(2).dt + x(1)
+ ASSERT_NO_THROW({solver.solve(1e-3, state1, &state2);});
+ EXPECT_NE(defaultState, state1);
+ EXPECT_NE(defaultState, state2);
+ EXPECT_NE(state1, state2);
+ Vector3d acceleration1 = m.m_gravity - 0.1 * state1.getVelocities() / m.m_mass;
+ EXPECT_TRUE(state2.getVelocities().isApprox(acceleration1 * 1e-3 + state1.getVelocities()));
+ EXPECT_TRUE(state2.getPositions().isApprox(state2.getVelocities() * 1e-3 + state1.getPositions()));
+ }
+}
+
+TEST(OdeSolverEulerExplicitModified, SolveTest)
+{
+ {
+ SCOPED_TRACE("EulerExplicitModified");
+ doSolveTest<OdeSolverEulerExplicitModified>();
+ }
+ {
+ SCOPED_TRACE("LinearEulerExplicitModified");
+ doSolveTest<OdeSolverLinearEulerExplicitModified>();
+ }
+}
+
+}; // Math
+
+}; // SurgSim
diff --git a/SurgSim/Math/UnitTests/OdeSolverEulerExplicitTests.cpp b/SurgSim/Math/UnitTests/OdeSolverEulerExplicitTests.cpp
new file mode 100644
index 0000000..3ac254b
--- /dev/null
+++ b/SurgSim/Math/UnitTests/OdeSolverEulerExplicitTests.cpp
@@ -0,0 +1,123 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 class OdeSolverEulerExplicit.
+///
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/OdeSolverEulerExplicit.h"
+#include "SurgSim/Math/OdeSolverLinearEulerExplicit.h"
+#include "SurgSim/Math/UnitTests/MockObject.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+template<class T>
+void doConstructorTest()
+{
+ MassPoint m;
+ ASSERT_NO_THROW({T solver(&m);});
+}
+
+TEST(OdeSolverEulerExplicit, ConstructorTest)
+{
+ {
+ SCOPED_TRACE("EulerExplicit");
+ doConstructorTest<OdeSolverEulerExplicit>();
+ }
+ {
+ SCOPED_TRACE("LinearEulerExplicit");
+ doConstructorTest<OdeSolverLinearEulerExplicit>();
+ }
+}
+
+template<class T>
+void doSolveTest()
+{
+ // Test 2 iterations because Linear solvers have a different algorithm on the 1st pass from the following passes.
+
+ {
+ MassPoint m;
+ MassPointState defaultState, state0, state1, state2;
+ T solver(&m);
+
+ // ma = mg <=> a = g
+ // v(1) = g.dt + v(0)
+ // x(1) = v(0).dt + x(0)
+ ASSERT_NO_THROW({solver.solve(1e-3, state0, &state1);});
+ EXPECT_EQ(defaultState, state0);
+ EXPECT_NE(defaultState, state1);
+ EXPECT_TRUE(state1.getVelocities().isApprox(m.m_gravity * 1e-3 + state0.getVelocities()));
+ EXPECT_TRUE(state1.getPositions().isApprox(state0.getVelocities() * 1e-3 + state0.getPositions()));
+
+ // v(2) = g.dt + v(1)
+ // x(2) = v(1).dt + x(1)
+ ASSERT_NO_THROW({solver.solve(1e-3, state1, &state2);});
+ EXPECT_NE(defaultState, state1);
+ EXPECT_NE(defaultState, state2);
+ EXPECT_NE(state2, state1);
+ EXPECT_TRUE(state2.getVelocities().isApprox(m.m_gravity * 1e-3 + state1.getVelocities()));
+ EXPECT_TRUE(state2.getPositions().isApprox(state1.getVelocities() * 1e-3 + state1.getPositions()));
+ }
+
+ {
+ MassPoint m(0.1);
+ MassPointState defaultState, state0, state1, state2;
+ T solver(&m);
+
+ // ma = mg - c.v <=> a = g - c/m.v
+ // v(1) = (g - c/m.v).dt + v(0)
+ // x(1) = v(0).dt + x(0)
+ ASSERT_NO_THROW({solver.solve(1e-3, state0, &state1);});
+ EXPECT_EQ(defaultState, state0);
+ EXPECT_NE(defaultState, state1);
+ Vector3d acceleration0 = m.m_gravity - 0.1 * state0.getVelocities() / m.m_mass;
+ EXPECT_TRUE(state1.getVelocities().isApprox(acceleration0 * 1e-3 + state0.getVelocities()));
+ EXPECT_TRUE(state1.getPositions().isApprox(state0.getVelocities() * 1e-3 + state0.getPositions()));
+
+ // v(2) = (g - c/m.v).dt + v(1)
+ // x(2) = v(1).dt + x(1)
+ ASSERT_NO_THROW({solver.solve(1e-3, state1, &state2);});
+ EXPECT_NE(defaultState, state1);
+ EXPECT_NE(defaultState, state2);
+ EXPECT_NE(state1, state2);
+ Vector3d acceleration1 = m.m_gravity - 0.1 * state1.getVelocities() / m.m_mass;
+ EXPECT_TRUE(state2.getVelocities().isApprox(acceleration1 * 1e-3 + state1.getVelocities()));
+ EXPECT_TRUE(state2.getPositions().isApprox(state1.getVelocities() * 1e-3 + state1.getPositions()));
+ }
+}
+
+TEST(OdeSolverEulerExplicit, SolveTest)
+{
+ {
+ SCOPED_TRACE("EulerExplicit");
+ doSolveTest<OdeSolverEulerExplicit>();
+ }
+ {
+ SCOPED_TRACE("LinearEulerExplicit");
+ doSolveTest<OdeSolverLinearEulerExplicit>();
+ }
+}
+
+}; // Math
+
+}; // SurgSim
diff --git a/SurgSim/Math/UnitTests/OdeSolverEulerImplicitTests.cpp b/SurgSim/Math/UnitTests/OdeSolverEulerImplicitTests.cpp
new file mode 100644
index 0000000..40cd227
--- /dev/null
+++ b/SurgSim/Math/UnitTests/OdeSolverEulerImplicitTests.cpp
@@ -0,0 +1,122 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 class OdeSolverEulerImplicit.
+///
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/OdeSolverEulerImplicit.h"
+#include "SurgSim/Math/OdeSolverLinearEulerImplicit.h"
+#include "SurgSim/Math/UnitTests/MockObject.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+template<class T>
+void doConstructorTest()
+{
+ MassPoint m;
+ ASSERT_NO_THROW({T solver(&m);});
+}
+
+TEST(OdeSolverEulerImplicit, ConstructorTest)
+{
+ {
+ SCOPED_TRACE("EulerImplicit");
+ doConstructorTest<OdeSolverEulerImplicit>();
+ }
+ {
+ SCOPED_TRACE("LinearEulerImplicit");
+ doConstructorTest<OdeSolverLinearEulerImplicit>();
+ }
+}
+
+template<class T>
+void doSolveTest()
+{
+ // Test 2 iterations because Linear solvers have a different algorithm on the 1st pass from the following passes.
+
+ {
+ MassPoint m;
+ MassPointState defaultState, state0, state1, state2;
+ T solver(&m);
+
+ // ma = mg <=> a = g
+ // v(1) = g.dt + v(0)
+ // x(1) = v(1).dt + x(0)
+ ASSERT_NO_THROW({solver.solve(1e-3, state0, &state1);});
+ EXPECT_EQ(defaultState, state0);
+ EXPECT_NE(defaultState, state1);
+ EXPECT_TRUE(state1.getVelocities().isApprox(m.m_gravity * 1e-3 + state0.getVelocities()));
+ EXPECT_TRUE(state1.getPositions().isApprox(state1.getVelocities() * 1e-3 + state0.getPositions()));
+
+ // v(2) = g.dt + v(1)
+ // x(2) = v(2).dt + x(1)
+ ASSERT_NO_THROW({solver.solve(1e-3, state1, &state2);});
+ EXPECT_NE(defaultState, state1);
+ EXPECT_NE(defaultState, state2);
+ EXPECT_NE(state2, state1);
+ EXPECT_TRUE(state2.getVelocities().isApprox(m.m_gravity * 1e-3 + state1.getVelocities()));
+ EXPECT_TRUE(state2.getPositions().isApprox(state2.getVelocities() * 1e-3 + state1.getPositions()));
+ }
+
+ {
+ MassPoint m(0.1);
+ MassPointState defaultState, state0, state1, state2;
+ T solver(&m);
+
+ // ma = mg - c.v <=> a = g - c/m.v
+ // v(1) = (g - c/m.v(1)).dt + v(0) <=> v(1) = I.(1.0 + dt.c/m)^-1.(g.dt + v(0))
+ // x(1) = v(1).dt + x(0)
+ ASSERT_NO_THROW({solver.solve(1e-3, state0, &state1);});
+ EXPECT_EQ(defaultState, state0);
+ EXPECT_NE(defaultState, state1);
+ Matrix33d systemInverse = Matrix33d::Identity() * 1.0 / (1.0 + 1e-3 * 0.1 / m.m_mass);
+ EXPECT_TRUE(state1.getVelocities().isApprox(systemInverse * (m.m_gravity * 1e-3 + state0.getVelocities())));
+ EXPECT_TRUE(state1.getPositions().isApprox(state1.getVelocities() * 1e-3 + state0.getPositions()));
+
+ // v(2) = (g - c/m.v(2)).dt + v(1) <=> v(2) = I.(1.0 + dt.c/m)^-1.(g.dt + v(1))
+ // x(2) = v(2).dt + x(1)
+ ASSERT_NO_THROW({solver.solve(1e-3, state1, &state2);});
+ EXPECT_NE(defaultState, state1);
+ EXPECT_NE(defaultState, state2);
+ EXPECT_NE(state1, state2);
+ EXPECT_TRUE(state2.getVelocities().isApprox(systemInverse * (m.m_gravity * 1e-3 + state1.getVelocities())));
+ EXPECT_TRUE(state2.getPositions().isApprox(state2.getVelocities() * 1e-3 + state1.getPositions()));
+ }
+}
+
+TEST(OdeSolverEulerImplicit, SolveTest)
+{
+ {
+ SCOPED_TRACE("EulerImplicit");
+ doSolveTest<OdeSolverEulerImplicit>();
+ }
+ {
+ SCOPED_TRACE("LinearEulerImplicit");
+ doSolveTest<OdeSolverLinearEulerImplicit>();
+ }
+}
+
+}; // Math
+
+}; // SurgSim
diff --git a/SurgSim/Math/UnitTests/OdeSolverRungeKutta4Tests.cpp b/SurgSim/Math/UnitTests/OdeSolverRungeKutta4Tests.cpp
new file mode 100644
index 0000000..6ae7165
--- /dev/null
+++ b/SurgSim/Math/UnitTests/OdeSolverRungeKutta4Tests.cpp
@@ -0,0 +1,252 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 OdeSolverRungeKutta4Tests.cpp
+/// Tests for the class OdeSolverRungeKutta4.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/OdeSolverRungeKutta4.h"
+#include "SurgSim/Math/OdeSolverLinearRungeKutta4.h"
+#include "SurgSim/Math/UnitTests/MockObject.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+template<class T>
+void doConstructorTest()
+{
+ MassPoint m;
+ ASSERT_NO_THROW({T solver(&m);});
+}
+
+TEST(OdeSolverRungeKutta4, ConstructorTest)
+{
+ {
+ SCOPED_TRACE("OdeSolverRungeKutta4");
+ doConstructorTest<OdeSolverRungeKutta4>();
+ }
+ {
+ SCOPED_TRACE("OdeSolverLinearRungeKutta4");
+ doConstructorTest<OdeSolverLinearRungeKutta4>();
+ }
+}
+
+namespace
+{
+struct RungeKuttaState
+{
+ RungeKuttaState(){}
+ RungeKuttaState(const Vector& p, const Vector& v) : position(p), velocity(v){}
+ Vector position;
+ Vector velocity;
+};
+
+struct RungeKuttaDerivedState
+{
+ RungeKuttaDerivedState(){}
+ RungeKuttaDerivedState(const Vector& v, const Vector& a) : velocity(v), acceleration(a){}
+ Vector velocity;
+ Vector acceleration;
+};
+
+void integrateRK4(double dt, const MassPoint& m, const RungeKuttaState& yn, RungeKuttaState* yn_plus_1)
+{
+ RungeKuttaDerivedState k1, k2, k3, k4;
+
+ // Problem to solve is
+ // m.a = m.g - c.v which is an ode of order 2 that can be reduced to order 1 as following:
+ // y' = (x)' = ( v ) = f(t, y)
+ // (v) = (m.g/m - c.v/m)
+ // In terms of (x), f(t, (x)) = (v)
+ // (v) (v) (g - c.v/m)
+
+ // Runge Kutta 4 computes y(n+1) = y(n) + 1/6.dt.(k1 + 2 * k2 + 2 * k3 + k4)
+ // with k1 = f(t(n) , y(n) )
+ // with k2 = f(t(n) + dt/2, y(n) + k1 * dt/2)
+ // with k3 = f(t(n) + dt/2, y(n) + k2 * dt/2)
+ // with k4 = f(t(n) + dt , y(n) + k3 * dt )
+
+ // 1st evaluation k1 = f(t(n) , y(n) )
+ k1.velocity = yn.velocity;
+ k1.acceleration = m.m_gravity - m.m_viscosity * yn.velocity / m.m_mass;
+
+ // 2nd evaluation k2 = f(t(n) + dt/2, y(n) + k1 * dt/2)
+ k2.velocity = yn.velocity + k1.acceleration * dt / 2.0;
+ k2.acceleration = m.m_gravity - m.m_viscosity * (yn.velocity + k1.acceleration * dt / 2.0) / m.m_mass;
+
+ // 3rd evaluation k3 = f(t(n) + dt/2, y(n) + k2 * dt/2)
+ k3.velocity = yn.velocity + k2.acceleration * dt / 2.0;
+ k3.acceleration = m.m_gravity - m.m_viscosity * (yn.velocity + k2.acceleration * dt / 2.0) / m.m_mass;
+
+ // 4th evaluation k4 = f(t(n) + dt , y(n) + k3 * dt )
+ k4.velocity = yn.velocity + k3.acceleration * dt;
+ k4.acceleration = m.m_gravity - m.m_viscosity * (yn.velocity + k3.acceleration * dt) / m.m_mass;
+
+ yn_plus_1->position = yn.position + dt / 6.0 *
+ (k1.velocity + k4.velocity + 2.0 * (k2.velocity + k3.velocity));
+ yn_plus_1->velocity = yn.velocity + dt / 6.0 *
+ (k1.acceleration + k4.acceleration + 2.0 * (k2.acceleration + k3.acceleration));
+}
+};
+
+template<class T>
+void doSolveTest()
+{
+ Vector deltaWithoutViscosity;
+ Vector deltaWithViscosity;
+ double dt = 1e-3;
+
+ // Test direction correctness of a moving point under gravity (no viscosity)
+ {
+ MassPoint m;
+ MassPointState defaultState, currentState, newState;
+ defaultState.getVelocities().setZero();
+ defaultState.getPositions().setZero();
+ currentState = defaultState;
+ newState = defaultState;
+
+ T solver(&m);
+ ASSERT_NO_THROW({solver.solve(dt, currentState, &newState);});
+ EXPECT_EQ(defaultState, currentState);
+ EXPECT_NE(defaultState, newState);
+
+ EXPECT_FALSE(newState.getVelocities().isZero());
+ EXPECT_DOUBLE_EQ(0.0, newState.getVelocities().dot(Vector3d::UnitX()));
+ EXPECT_GT(0.0, newState.getVelocities().dot(Vector3d::UnitY()));
+ EXPECT_DOUBLE_EQ(0.0, newState.getVelocities().dot(Vector3d::UnitZ()));
+
+ EXPECT_FALSE(newState.getPositions().isZero());
+ deltaWithoutViscosity = (newState.getPositions() - currentState.getPositions());
+ EXPECT_DOUBLE_EQ(0.0, deltaWithoutViscosity.dot(Vector3d::UnitX()));
+ EXPECT_GT(0.0, deltaWithoutViscosity.dot(Vector3d::UnitY()));
+ EXPECT_DOUBLE_EQ(0.0, deltaWithoutViscosity.dot(Vector3d::UnitZ()));
+ }
+
+ // Test direction correctness of a moving point under gravity (viscosity)
+ {
+ MassPoint m(0.1);
+ MassPointState defaultState, currentState, newState;
+ defaultState.getVelocities().setZero();
+ defaultState.getPositions().setZero();
+ currentState = defaultState;
+ newState = defaultState;
+
+ T solver(&m);
+ ASSERT_NO_THROW({solver.solve(dt, currentState, &newState);});
+ EXPECT_EQ(defaultState, currentState);
+ EXPECT_NE(defaultState, newState);
+
+ EXPECT_FALSE(newState.getVelocities().isZero());
+ EXPECT_FALSE(newState.getVelocities().isApprox(currentState.getVelocities()));
+ EXPECT_DOUBLE_EQ(currentState.getVelocities()[0], newState.getVelocities()[0]);
+ EXPECT_NE(currentState.getVelocities()[1], newState.getVelocities()[1]);
+ EXPECT_LT(newState.getVelocities()[1], currentState.getVelocities()[1]);
+ EXPECT_DOUBLE_EQ(currentState.getVelocities()[2], newState.getVelocities()[2]);
+
+ EXPECT_FALSE(newState.getPositions().isZero());
+ deltaWithViscosity = (newState.getPositions() - currentState.getPositions());
+ EXPECT_DOUBLE_EQ(0.0, deltaWithViscosity.dot(Vector3d::UnitX()));
+ EXPECT_GT(0.0, deltaWithViscosity.dot(Vector3d::UnitY()));
+ EXPECT_DOUBLE_EQ(0.0, deltaWithViscosity.dot(Vector3d::UnitZ()));
+ }
+
+ EXPECT_GT(deltaWithoutViscosity.norm(), deltaWithViscosity.norm());
+
+ // Test Runge Kutta 4 algorithm itself (without viscosity)
+ // Test 2 iterations because Linear solvers have a different algorithm on the 1st pass from the following passes.
+ {
+ SCOPED_TRACE("RK4 without viscosity");
+
+ MassPoint m(0.0);
+ MassPointState currentState, newState, newState2;
+ currentState.getPositions().setLinSpaced(1.0, 3.0);
+ currentState.getVelocities().setConstant(1.0);
+
+ T solver(&m);
+
+ RungeKuttaState yn(currentState.getPositions(), currentState.getVelocities());
+ RungeKuttaState yn_plus_1, yn_plus_2;
+
+ EXPECT_TRUE(currentState.getPositions().isApprox(yn.position));
+ EXPECT_TRUE(currentState.getVelocities().isApprox(yn.velocity));
+
+ // 1st time step
+ integrateRK4(dt, m, yn, &yn_plus_1);
+ ASSERT_NO_THROW({solver.solve(dt, currentState, &newState);});
+
+ EXPECT_TRUE(newState.getPositions().isApprox(yn_plus_1.position));
+ EXPECT_TRUE(newState.getVelocities().isApprox(yn_plus_1.velocity));
+
+ // 2nd time step
+ integrateRK4(dt, m, yn_plus_1, &yn_plus_2);
+ ASSERT_NO_THROW({solver.solve(dt, newState, &newState2);});
+
+ EXPECT_TRUE(newState2.getPositions().isApprox(yn_plus_2.position));
+ EXPECT_TRUE(newState2.getVelocities().isApprox(yn_plus_2.velocity));
+ }
+
+ // Test Runge Kutta 4 algorithm itself (with viscosity)
+ // Test 2 iterations because Linear solvers have a different algorithm on the 1st pass from the following passes.
+ {
+ SCOPED_TRACE("RK4 with viscosityof 0.1");
+
+ MassPoint m(0.1);
+ MassPointState currentState, newState, newState2;
+ currentState.getPositions().setLinSpaced(1.0, 3.0);
+ currentState.getVelocities().setConstant(1.0);
+
+ T solver(&m);
+
+ RungeKuttaState yn(currentState.getPositions(), currentState.getVelocities());
+ RungeKuttaState yn_plus_1, yn_plus_2;
+
+ EXPECT_TRUE(currentState.getPositions().isApprox(yn.position));
+ EXPECT_TRUE(currentState.getVelocities().isApprox(yn.velocity));
+
+ // 1st time step
+ integrateRK4(dt, m, yn, &yn_plus_1);
+ ASSERT_NO_THROW({solver.solve(dt, currentState, &newState);});
+ EXPECT_TRUE(newState.getPositions().isApprox(yn_plus_1.position));
+ EXPECT_TRUE(newState.getVelocities().isApprox(yn_plus_1.velocity));
+
+ // 2nd time step
+ integrateRK4(dt, m, yn_plus_1, &yn_plus_2);
+ ASSERT_NO_THROW({solver.solve(dt, newState, &newState2);});
+ EXPECT_TRUE(newState2.getPositions().isApprox(yn_plus_2.position));
+ EXPECT_TRUE(newState2.getVelocities().isApprox(yn_plus_2.velocity));
+ }
+}
+
+TEST(OdeSolverRungeKutta4, SolveTest)
+{
+ {
+ SCOPED_TRACE("OdeSolverRungeKutta4");
+ doSolveTest<OdeSolverRungeKutta4>();
+ }
+ {
+ SCOPED_TRACE("OdeSolverLinearRungeKutta4");
+ doSolveTest<OdeSolverLinearRungeKutta4>();
+ }
+}
+
+}; // Math
+
+}; // SurgSim
diff --git a/SurgSim/Math/UnitTests/OdeSolverStaticTests.cpp b/SurgSim/Math/UnitTests/OdeSolverStaticTests.cpp
new file mode 100644
index 0000000..9ec7fe6
--- /dev/null
+++ b/SurgSim/Math/UnitTests/OdeSolverStaticTests.cpp
@@ -0,0 +1,85 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 class OdeSolverStatic.
+///
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/OdeSolverStatic.h"
+#include "SurgSim/Math/OdeSolverLinearStatic.h"
+#include "SurgSim/Math/UnitTests/MockObject.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+template<class T>
+void doConstructorTest()
+{
+ MassPointsForStatic m;
+ ASSERT_NO_THROW({T solver(&m);});
+}
+
+TEST(OdeSolverStatic, ConstructorTest)
+{
+ {
+ SCOPED_TRACE("Static");
+ doConstructorTest<OdeSolverStatic>();
+ }
+ {
+ SCOPED_TRACE("LinearStatic");
+ doConstructorTest<OdeSolverLinearStatic>();
+ }
+}
+
+template<class T>
+void doSolveTest()
+{
+ MassPointsForStatic m;
+ MassPointsStateForStatic defaultState, currentState, newState;
+
+ T solver(&m);
+ ASSERT_NO_THROW({solver.solve(1e-3, currentState, &newState);});
+ EXPECT_EQ(defaultState, currentState);
+ EXPECT_NE(defaultState, newState);
+
+ // Solve manually K.(x - x0) = Fext
+ const Vector &Fext = m.getExternalForces();
+ Vector expectedDeltaX = m.computeK(currentState).inverse() * Fext;
+ Vector expectedX = defaultState.getPositions() + expectedDeltaX;
+ EXPECT_TRUE(newState.getPositions().isApprox(expectedX));
+}
+
+TEST(OdeSolverStatic, SolveTest)
+{
+ {
+ SCOPED_TRACE("Static");
+ doSolveTest<OdeSolverStatic>();
+ }
+ {
+ SCOPED_TRACE("LinearStatic");
+ doSolveTest<OdeSolverLinearStatic>();
+ }
+}
+
+}; // Math
+
+}; // SurgSim
diff --git a/SurgSim/Math/UnitTests/OdeSolverTests.cpp b/SurgSim/Math/UnitTests/OdeSolverTests.cpp
new file mode 100644
index 0000000..ea5264f
--- /dev/null
+++ b/SurgSim/Math/UnitTests/OdeSolverTests.cpp
@@ -0,0 +1,112 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 class OdeSolver.
+///
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/OdeSolver.h"
+#include "SurgSim/Math/UnitTests/MockObject.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+namespace
+{
+ const std::string name = "MockOdeSolver";
+};
+
+class MockOdeSolver : public OdeSolver
+{
+public:
+ /// Constructor
+ /// \param equation The ode equation to be solved
+ explicit MockOdeSolver(OdeEquation* equation) : OdeSolver(equation)
+ {
+ this->m_name = name;
+ }
+
+ /// Virtual destructor
+ virtual ~MockOdeSolver()
+ {
+ }
+
+ /// Solves the equation
+ /// \param dt The time step
+ /// \param currentState State at time t
+ /// \param[out] newState State at time t+dt
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState)
+ {
+ this->m_systemMatrix.setIdentity();
+ this->m_compliance.setIdentity();
+ }
+};
+
+TEST(OdeSolver, ConstructorTest)
+{
+ // OdeEquation is tested separately and is considered valid to use here.
+ MassPoint m;
+
+ ASSERT_NO_THROW({MockOdeSolver solver(&m);});
+ {
+ MockOdeSolver solver(&m);
+ EXPECT_EQ(3, solver.getCompliance().rows());
+ EXPECT_EQ(3, solver.getCompliance().cols());
+ EXPECT_EQ(3, solver.getSystemMatrix().rows());
+ EXPECT_EQ(3, solver.getSystemMatrix().cols());
+ }
+
+ ASSERT_NO_THROW({MockOdeSolver* solver = new MockOdeSolver(&m); delete solver;});
+ {
+ MockOdeSolver* solver = new MockOdeSolver(&m);
+ EXPECT_EQ(3, solver->getCompliance().rows());
+ EXPECT_EQ(3, solver->getCompliance().cols());
+ EXPECT_EQ(3, solver->getSystemMatrix().rows());
+ EXPECT_EQ(3, solver->getSystemMatrix().cols());
+ delete solver;
+ }
+
+ ASSERT_NO_THROW({std::shared_ptr<MockOdeSolver> solver = std::make_shared<MockOdeSolver>(&m); });
+ {
+ std::shared_ptr<MockOdeSolver> solver = std::make_shared<MockOdeSolver>(&m);
+ EXPECT_EQ(3, solver->getCompliance().rows());
+ EXPECT_EQ(3, solver->getCompliance().cols());
+ EXPECT_EQ(3, solver->getSystemMatrix().rows());
+ EXPECT_EQ(3, solver->getSystemMatrix().cols());
+ }
+}
+
+TEST(OdeSolver, GetTest)
+{
+ MassPoint m;
+ MassPointState currentState, newState;
+ MockOdeSolver solver(&m);
+
+ solver.solve(1e-3, currentState, &newState);
+ EXPECT_TRUE(solver.getSystemMatrix().isIdentity());
+ EXPECT_TRUE(solver.getCompliance().isIdentity());
+ EXPECT_EQ(name, solver.getName());
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/OdeStateTests.cpp b/SurgSim/Math/UnitTests/OdeStateTests.cpp
new file mode 100644
index 0000000..c4f3e9d
--- /dev/null
+++ b/SurgSim/Math/UnitTests/OdeStateTests.cpp
@@ -0,0 +1,400 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Framework/Assert.h"
+
+using SurgSim::Math::OdeState;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+TEST(OdeStateTest, ConstructorTest)
+{
+ // Test the constructor normally
+ ASSERT_NO_THROW({OdeState state;});
+
+ // Test the object creation through the operator new
+ // Eigen needs special care with fixed-size matrix member variables of a class created dynamically via new.
+ // We are using non fixed-size matrix, so it should be all fine...this is just to make sure.
+ ASSERT_NO_THROW({OdeState* state = new OdeState; delete state; });
+
+ // Test the object creation through the operator new []
+ // Eigen needs special care with fixed-size matrix member variables of a class created dynamically via new [].
+ // We are using non fixed-size matrix, so it should be all fine...this is just to make sure.
+ ASSERT_NO_THROW({OdeState* state = new OdeState[10]; delete [] state; });
+
+ // Test the object creation through std::shared_ptr
+ // Eigen needs special care with fixed-size matrix member variables of a class created dynamically via new.
+ // We are using non fixed-size matrix, so it should be all fine...this is just to make sure.
+ ASSERT_NO_THROW({std::shared_ptr<OdeState> state = std::make_shared<OdeState>();});
+}
+
+TEST(OdeStateTest, AllocateTest)
+{
+ OdeState state;
+ EXPECT_EQ(0u, state.getNumDof());
+ EXPECT_EQ(0u, state.getNumNodes());
+ EXPECT_EQ(0u, state.getNumBoundaryConditions());
+ EXPECT_EQ(0, state.getBoundaryConditions().size());
+ EXPECT_EQ(0, state.getPositions().size());
+ EXPECT_EQ(0, state.getVelocities().size());
+
+ ASSERT_NO_THROW(state.setNumDof(3u, 3u));
+ EXPECT_EQ(9u, state.getNumDof());
+ EXPECT_EQ(3u, state.getNumNodes());
+ EXPECT_EQ(9, state.getPositions().size());
+ EXPECT_EQ(9, state.getVelocities().size());
+ EXPECT_EQ(0u , state.getNumBoundaryConditions());
+ EXPECT_EQ(0 , state.getBoundaryConditions().size());
+}
+
+TEST(OdeStateTest, GetPositionsTest)
+{
+ OdeState state1, state2;
+ state1.setNumDof(3u, 3u);
+ state2.setNumDof(3u, 3u);
+ for(size_t i = 0; i < state1.getNumDof(); i++)
+ {
+ state1.getPositions()[i] = static_cast<double>(i);
+ state2.getPositions()[i] = 0.0;
+ }
+ // state1.m_x contains (0 1 2 3 4 5 6 7 8 9 10) & state2.m_x contains (0 0 0 0 0 0 0 0 0 0 0)
+ EXPECT_NE(state2.getPositions(), state1.getPositions());
+ state2.getPositions() = state1.getPositions();
+ // state1.m_x contains (0 1 2 3 4 5 6 7 8 9 10) & state2.m_x contains (0 1 2 3 4 5 6 7 8 9 10)
+ EXPECT_EQ(state2.getPositions(), state1.getPositions());
+
+ state1.reset();
+ // state1.m_x contains (0 0 0 0 0 0 0 0 0 0 0) & state2.m_x contains (0 1 2 3 4 5 6 7 8 9 10)
+ for(size_t i = 0; i < state1.getNumDof(); i++)
+ {
+ EXPECT_EQ(0.0, state1.getPositions()[i]);
+ EXPECT_EQ(static_cast<double>(i), state2.getPositions()[i]);
+ }
+
+ state2.reset();
+ // state1.m_x contains (0 0 0 0 0 0 0 0 0 0 0) & state2.m_x contains (0 0 0 0 0 0 0 0 0 0 0)
+ EXPECT_EQ(state2.getPositions(), state1.getPositions());
+}
+
+TEST(OdeStateTest, GetVelocitiesTest)
+{
+ OdeState state1, state2;
+ state1.setNumDof(3u, 3u);
+ state2.setNumDof(3u, 3u);
+ for(size_t i = 0; i < state1.getNumDof(); i++)
+ {
+ state1.getVelocities()[i] = static_cast<double>(i);
+ state2.getVelocities()[i] = 0.0;
+ }
+ // state1.m_v contains (0 1 2 3 4 5 6 7 8 9 10) & state2.m_v contains (0 0 0 0 0 0 0 0 0 0 0)
+ EXPECT_NE(state2.getVelocities(), state1.getVelocities());
+ state2.getVelocities() = state1.getVelocities();
+ // state1.m_v contains (0 1 2 3 4 5 6 7 8 9 10) & state2.m_v contains (0 1 2 3 4 5 6 7 8 9 10)
+ EXPECT_EQ(state2.getVelocities(), state1.getVelocities());
+
+ state1.reset();
+ // state1.m_v contains (0 0 0 0 0 0 0 0 0 0 0) & state2.m_v contains (0 1 2 3 4 5 6 7 8 9 10)
+ for(size_t i = 0; i < state1.getNumDof(); i++)
+ {
+ EXPECT_EQ(0.0, state1.getVelocities()[i]);
+ EXPECT_EQ(static_cast<double>(i), state2.getVelocities()[i]);
+ }
+
+ state2.reset();
+ // state1.m_v contains (0 0 0 0 0 0 0 0 0 0 0) & state2.m_v contains (0 0 0 0 0 0 0 0 0 0 0)
+ EXPECT_EQ(state2.getVelocities(), state1.getVelocities());
+}
+
+namespace
+{
+void testBoundaryConditions(const SurgSim::Math::OdeState& state, std::vector<size_t> expectedDofIds)
+{
+ EXPECT_EQ(6u, state.getNumDof());
+ EXPECT_EQ(expectedDofIds.size(), state.getNumBoundaryConditions());
+ ASSERT_EQ(expectedDofIds.size(), state.getBoundaryConditions().size());
+ for (size_t index = 0; index < expectedDofIds.size(); ++index)
+ {
+ EXPECT_EQ(expectedDofIds[index], state.getBoundaryConditions()[index]);
+ }
+ for (size_t dofId = 0; dofId < 6; dofId++)
+ {
+ if (std::find(expectedDofIds.begin(), expectedDofIds.end(), dofId) != expectedDofIds.end())
+ {
+ EXPECT_TRUE(state.isBoundaryCondition(dofId));
+ }
+ else
+ {
+ EXPECT_FALSE(state.isBoundaryCondition(dofId));
+ }
+ }
+ EXPECT_EQ(6, state.getPositions().size());
+ EXPECT_EQ(6, state.getVelocities().size());
+ EXPECT_TRUE(state.getPositions().isZero());
+ EXPECT_TRUE(state.getVelocities().isZero());
+}
+}; // anonymous namespace
+
+TEST(OdeStateTest, AddGetIsBoundaryConditionsTest)
+{
+ {
+ OdeState state;
+ std::vector<size_t> expectedDofIdsBoundaryConditions;
+
+ // Assert trying to add a boundary condition before setting the number of node and dof per node
+ ASSERT_THROW(state.addBoundaryCondition(0u, 0u), SurgSim::Framework::AssertionFailure);
+
+ state.setNumDof(3u, 2u); // Number of dof per node is 3
+
+ SCOPED_TRACE("Testing addBoundaryCondition(size_t nodeId, size_t dofId)");
+
+ state.addBoundaryCondition(0u, 0u);
+ expectedDofIdsBoundaryConditions.push_back((0u * 3u + 0u)); // (node 0, dof 0)
+ testBoundaryConditions(state, expectedDofIdsBoundaryConditions);
+
+ state.addBoundaryCondition(1u, 2u);
+ expectedDofIdsBoundaryConditions.push_back((1u * 3u + 2u)); // (node 1, dof 2)
+ testBoundaryConditions(state, expectedDofIdsBoundaryConditions);
+
+ state.addBoundaryCondition(0u, 2u);
+ expectedDofIdsBoundaryConditions.push_back((0u * 3u + 2u)); // (node 0, dof 2)
+ testBoundaryConditions(state, expectedDofIdsBoundaryConditions);
+
+ // Assert on wrong nodeId
+ ASSERT_THROW(state.addBoundaryCondition(3u, 0u), SurgSim::Framework::AssertionFailure);
+
+ // Assert on wrong dofId
+ ASSERT_THROW(state.addBoundaryCondition(0u, 4u), SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ OdeState state;
+ std::vector<size_t> expectedDofIdsBoundaryConditions;
+
+ // Assert trying to add a boundary condition before setting the number of node and dof per node
+ ASSERT_THROW(state.addBoundaryCondition(0u), SurgSim::Framework::AssertionFailure);
+
+ state.setNumDof(3u, 2u); // Number of dof per node is 3
+
+ SCOPED_TRACE("Testing addBoundaryCondition(size_t nodeId)");
+
+ state.addBoundaryCondition(0u);
+ expectedDofIdsBoundaryConditions.push_back((0u * 3u + 0u)); // (node 0, dof 0)
+ expectedDofIdsBoundaryConditions.push_back((0u * 3u + 1u)); // (node 0, dof 1)
+ expectedDofIdsBoundaryConditions.push_back((0u * 3u + 2u)); // (node 0, dof 2)
+ testBoundaryConditions(state, expectedDofIdsBoundaryConditions);
+
+ // Assert on wrong nodeId
+ ASSERT_THROW(state.addBoundaryCondition(3u), SurgSim::Framework::AssertionFailure);
+
+ state.addBoundaryCondition(1u);
+ expectedDofIdsBoundaryConditions.push_back((1u * 3u + 0u)); // (node 0, dof 0)
+ expectedDofIdsBoundaryConditions.push_back((1u * 3u + 1u)); // (node 0, dof 1)
+ expectedDofIdsBoundaryConditions.push_back((1u * 3u + 2u)); // (node 0, dof 2)
+ testBoundaryConditions(state, expectedDofIdsBoundaryConditions);
+ }
+}
+
+TEST(OdeStateTest, ResetTest)
+{
+ OdeState state1, state2;
+ state1.setNumDof(3u, 3u);
+ state2.setNumDof(3u, 3u);
+ for(size_t i = 0; i < state1.getNumDof(); i++)
+ {
+ state1.getPositions()[i] = static_cast<double>(i);
+ state1.getVelocities()[i] = 2.0*static_cast<double>(i);
+ }
+ state1.addBoundaryCondition(0u, 0u);
+ state1.addBoundaryCondition(state1.getNumNodes() - 1u, 2u);
+ EXPECT_NE(state2, state1);
+
+ state1.reset();
+ EXPECT_EQ(state2, state1);
+ EXPECT_EQ(9u, state1.getNumDof());
+ EXPECT_EQ(3u, state1.getNumNodes());
+ EXPECT_TRUE(state1.getPositions().isZero());
+ EXPECT_TRUE(state1.getVelocities().isZero());
+ EXPECT_EQ(0u, state1.getNumBoundaryConditions());
+ EXPECT_EQ(0, state1.getBoundaryConditions().size());
+}
+
+TEST(OdeStateTest, CopyConstructorAndAssignmentTest)
+{
+ OdeState state, stateAssigned;
+ state.setNumDof(3u, 3u);
+ for(size_t i = 0; i < state.getNumDof(); i++)
+ {
+ state.getPositions()[i] = static_cast<double>(i);
+ state.getVelocities()[i] = 2.0*static_cast<double>(i);
+ }
+ state.addBoundaryCondition(0u, 0u);
+ state.addBoundaryCondition(state.getNumNodes() - 1u, 2u);
+
+ {
+ OdeState stateCopied(state);
+
+ ASSERT_EQ(9u, stateCopied.getNumDof());
+ ASSERT_EQ(state.getNumDof(), stateCopied.getNumDof());
+ ASSERT_EQ(9, stateCopied.getPositions().size());
+ ASSERT_EQ(state.getPositions().size(), stateCopied.getPositions().size());
+ ASSERT_EQ(9, stateCopied.getVelocities().size());
+ ASSERT_EQ(state.getVelocities().size(), stateCopied.getVelocities().size());
+
+ for(size_t i = 0; i < stateCopied.getNumDof(); i++)
+ {
+ EXPECT_NEAR(state.getPositions()[i], stateCopied.getPositions()[i], epsilon);
+ EXPECT_NEAR(static_cast<double>(i), stateCopied.getPositions()[i], epsilon);
+ EXPECT_NEAR(state.getVelocities()[i], stateCopied.getVelocities()[i], epsilon);
+ EXPECT_NEAR(2.0*static_cast<double>(i), stateCopied.getVelocities()[i], epsilon);
+ }
+
+ ASSERT_EQ(2u, stateCopied.getNumBoundaryConditions());
+ ASSERT_EQ(state.getNumBoundaryConditions(), stateCopied.getNumBoundaryConditions());
+ ASSERT_EQ(2, stateCopied.getBoundaryConditions().size());
+ ASSERT_EQ(state.getBoundaryConditions().size(), stateCopied.getBoundaryConditions().size());
+ ASSERT_EQ(0u, stateCopied.getBoundaryConditions()[0]);
+ ASSERT_EQ(state.getBoundaryConditions()[0], stateCopied.getBoundaryConditions()[0]);
+ ASSERT_EQ(state.getNumDof() - 1, stateCopied.getBoundaryConditions()[1]);
+ ASSERT_EQ(state.getBoundaryConditions()[1], stateCopied.getBoundaryConditions()[1]);
+ }
+
+ {
+ stateAssigned = state;
+
+ ASSERT_EQ(9u, stateAssigned.getNumDof());
+ ASSERT_EQ(state.getNumDof(), stateAssigned.getNumDof());
+ ASSERT_EQ(9, stateAssigned.getPositions().size());
+ ASSERT_EQ(state.getPositions().size(), stateAssigned.getPositions().size());
+ ASSERT_EQ(9, stateAssigned.getVelocities().size());
+ ASSERT_EQ(state.getVelocities().size(), stateAssigned.getVelocities().size());
+
+ for(size_t i = 0; i < stateAssigned.getNumDof(); i++)
+ {
+ EXPECT_NEAR(state.getPositions()[i], stateAssigned.getPositions()[i], epsilon);
+ EXPECT_NEAR(static_cast<double>(i), stateAssigned.getPositions()[i], epsilon);
+ EXPECT_NEAR(state.getVelocities()[i], stateAssigned.getVelocities()[i], epsilon);
+ EXPECT_NEAR(2.0*static_cast<double>(i), stateAssigned.getVelocities()[i], epsilon);
+ }
+
+ ASSERT_EQ(2u, stateAssigned.getNumBoundaryConditions());
+ ASSERT_EQ(state.getNumBoundaryConditions(), stateAssigned.getNumBoundaryConditions());
+ ASSERT_EQ(2, stateAssigned.getBoundaryConditions().size());
+ ASSERT_EQ(state.getBoundaryConditions().size(), stateAssigned.getBoundaryConditions().size());
+ ASSERT_EQ(0u, stateAssigned.getBoundaryConditions()[0]);
+ ASSERT_EQ(state.getBoundaryConditions()[0], stateAssigned.getBoundaryConditions()[0]);
+ ASSERT_EQ(state.getNumDof() - 1, stateAssigned.getBoundaryConditions()[1]);
+ ASSERT_EQ(state.getBoundaryConditions()[1], stateAssigned.getBoundaryConditions()[1]);
+ }
+}
+
+TEST(OdeStateTest, ApplyBoundaryConditionsToVectorTest)
+{
+ OdeState state;
+ state.setNumDof(3u, 3u);
+ state.addBoundaryCondition(0u, 1u);
+ state.addBoundaryCondition(state.getNumNodes() - 1u, 2u);
+
+ SurgSim::Math::Vector F(state.getNumDof());
+ F.setLinSpaced(1.0, 2.0);
+ SurgSim::Math::Vector initialF = F;
+
+ state.applyBoundaryConditionsToVector(&F);
+ EXPECT_FALSE(F.isApprox(initialF));
+ for (size_t dofId = 0; dofId < state.getNumDof(); ++dofId)
+ {
+ if (dofId == 1u || dofId == state.getNumDof() - 1u)
+ {
+ EXPECT_NE(initialF[dofId], F[dofId]);
+ EXPECT_EQ(0u, F[dofId]);
+ }
+ else
+ {
+ EXPECT_EQ(initialF[dofId], F[dofId]);
+ }
+ }
+}
+
+TEST(OdeStateTest, ApplyBoundaryConditionsToMatrixTest)
+{
+ OdeState state;
+ state.setNumDof(3u, 3u);
+ state.addBoundaryCondition(0u, 1u);
+ state.addBoundaryCondition(state.getNumNodes() - 1u, 2u);
+
+ size_t numDof = state.getNumDof();
+ SurgSim::Math::Matrix M = 2.0 * SurgSim::Math::Matrix::Ones(numDof, numDof);
+ SurgSim::Math::Matrix initialM = M;
+ SurgSim::Math::Matrix expectedM = M;
+ for (int bcId = 0; bcId < 2; ++bcId)
+ {
+ size_t dofId = state.getBoundaryConditions()[bcId];
+ expectedM.block<1, 9>(dofId, 0).setZero();
+ expectedM.block<9, 1>(0, dofId).setZero();
+ expectedM(dofId, dofId) = 1.0;
+ }
+
+ state.applyBoundaryConditionsToMatrix(&M);
+ EXPECT_FALSE(M.isApprox(initialM));
+ EXPECT_TRUE(M.isApprox(expectedM));
+}
+
+namespace
+{
+void testIsValidWith(double invalidNumber)
+{
+ OdeState invalidStateInfinityOnPosition;
+ invalidStateInfinityOnPosition.setNumDof(3u, 3u);
+ invalidStateInfinityOnPosition.getPositions().setOnes();
+ invalidStateInfinityOnPosition.getPositions()[2] = invalidNumber;
+ EXPECT_FALSE(invalidStateInfinityOnPosition.isValid());
+
+ OdeState invalidStateInfinityOnVelocity;
+ invalidStateInfinityOnVelocity.setNumDof(3u, 3u);
+ invalidStateInfinityOnVelocity.getVelocities().setOnes();
+ invalidStateInfinityOnVelocity.getVelocities()[2] = invalidNumber;
+ EXPECT_FALSE(invalidStateInfinityOnVelocity.isValid());
+}
+}; // anonymous namespace
+
+TEST(OdeStateTest, IsValidTest)
+{
+ OdeState validState;
+ validState.setNumDof(3u, 3u);
+ validState.getPositions().setOnes();
+ EXPECT_TRUE(validState.isValid());
+
+ {
+ SCOPED_TRACE("Test with invalid INF");
+ testIsValidWith(std::numeric_limits<double>::infinity());
+ }
+
+ {
+ SCOPED_TRACE("Test with invalid QuietNaN");
+ testIsValidWith(std::numeric_limits<double>::quiet_NaN());
+ }
+
+ {
+ SCOPED_TRACE("Test with invalid SignalingNaN");
+ testIsValidWith(std::numeric_limits<double>::signaling_NaN());
+ }
+}
diff --git a/SurgSim/Math/UnitTests/QuaternionTests.cpp b/SurgSim/Math/UnitTests/QuaternionTests.cpp
new file mode 100644
index 0000000..4f1d06f
--- /dev/null
+++ b/SurgSim/Math/UnitTests/QuaternionTests.cpp
@@ -0,0 +1,674 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 that exercise the functionality of our quaternion typedefs, which
+/// come straight from Eigen.
+
+#include <math.h>
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "gtest/gtest.h"
+
+// Define test fixture class templates.
+// We don't really need fixtures as such, but the templatization encodes type.
+
+template <class T>
+class QuaternionTests : public testing::Test
+{
+public:
+ typedef T Quaternion;
+ typedef typename T::AngleAxisType AngleAxis;
+ typedef typename T::Scalar Scalar;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) quaternion type aliases, but we got rid of those.
+typedef ::testing::Types<SurgSim::Math::Quaterniond,
+ SurgSim::Math::Quaternionf> QuaternionVariants;
+TYPED_TEST_CASE(QuaternionTests, QuaternionVariants);
+
+
+
+template <class T>
+class UnalignedQuaternionTests : public QuaternionTests<T>
+{
+};
+
+typedef ::testing::Types<SurgSim::Math::Quaterniond,
+ SurgSim::Math::Quaternionf> UnalignedQuaternionVariants;
+TYPED_TEST_CASE(UnalignedQuaternionTests, UnalignedQuaternionVariants);
+
+
+
+// Now we're ready to start testing...
+
+
+// ==================== CONSTRUCTION & INITIALIZATION ====================
+
+/// Test that quaternions can be constructed.
+TYPED_TEST(QuaternionTests, CanConstruct)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ Quaternion defaultConstructed;
+ Quaternion fourArg(static_cast<T>(1.0), static_cast<T>(2.0), static_cast<T>(3.0), static_cast<T>(4.0));
+}
+
+/// Test that the constructor properly initializes quaternions in the WXYZ order.
+TYPED_TEST(QuaternionTests, ConstructorInitialization)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ Quaternion quaternion(static_cast<T>(1.03), static_cast<T>(1.04), static_cast<T>(1.05), static_cast<T>(1.06));
+ EXPECT_NEAR(1.03, quaternion.w(), 1e-6) << "W wasn't properly initialized.";
+ EXPECT_NEAR(1.04, quaternion.x(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(1.05, quaternion.y(), 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(1.06, quaternion.z(), 1e-6) << "Z wasn't properly initialized.";
+}
+
+/// Test setting the quaternion from a 4D vector.
+/// The order of components is XYZW (not WXYZ), which is why doing this may be confusing.
+TYPED_TEST(QuaternionTests, InitializeFromVector4)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ Eigen::Matrix<T, 4, 1> vector4;
+ vector4 << 1.1f, 1.2f, 1.3f, 1.4f;
+
+ // Note: this initializes the quaternion from the vector in the **XYZW** order (not WXYZ!)
+ Quaternion quaternion(vector4);
+ EXPECT_NEAR(1.1, quaternion.x(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(1.2, quaternion.y(), 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(1.3, quaternion.z(), 1e-6) << "Z wasn't properly initialized.";
+ EXPECT_NEAR(1.4, quaternion.w(), 1e-6) << "W wasn't properly initialized.";
+}
+
+/// Test initializing from a float array.
+/// The order of components is XYZW (not WXYZ), which is why doing this may be confusing.
+TYPED_TEST(QuaternionTests, InitializeFromArray)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ Quaternion quaternion(T(3.4), T(0.1), T(1.2), T(2.3));
+
+ EXPECT_NEAR(0.1, quaternion.x(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(1.2, quaternion.y(), 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(2.3, quaternion.z(), 1e-6) << "Z wasn't properly initialized.";
+ EXPECT_NEAR(3.4, quaternion.w(), 1e-6) << "W wasn't properly initialized.";
+}
+
+/// Test getting an identity value usable in expressions.
+TYPED_TEST(QuaternionTests, IdentityValue)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+
+ Quaternion quaternion = Quaternion::Identity();
+ EXPECT_NEAR(1.0, quaternion.w(), 1e-6) << "W wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.x(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.y(), 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.z(), 1e-6) << "Z wasn't properly initialized.";
+}
+
+/// Test setting quaternions to identity.
+TYPED_TEST(QuaternionTests, SetToIdentity)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+
+ Quaternion quaternion;
+ quaternion.setIdentity();
+ EXPECT_NEAR(1.0, quaternion.w(), 1e-6) << "W wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.x(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.y(), 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.z(), 1e-6) << "Z wasn't properly initialized.";
+}
+
+/// Test getting a zero value usable in expressions.
+/// Note: many "4D" operations are not defined on Eigen::Quaternion, but can be performed on quaternion.coeffs().
+TYPED_TEST(QuaternionTests, ZeroValue)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 4, 1> Vector4;
+
+ // There is no Quaternion::Zero(), but you can do this:
+ Quaternion quaternion = Quaternion(Vector4::Zero());
+ EXPECT_NEAR(0.0, quaternion.w(), 1e-6) << "W wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.x(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.y(), 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.z(), 1e-6) << "Z wasn't properly initialized.";
+}
+
+/// Test setting quaternions to zero.
+/// Note: many "4D" operations are not defined on Eigen::Quaternion, but can be performed on quaternion.coeffs().
+TYPED_TEST(QuaternionTests, SetToZero)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+
+ // There is no Quaternion::setZero(), but you can do this:
+ Quaternion quaternion;
+ quaternion.coeffs().setZero();
+ EXPECT_NEAR(0.0, quaternion.w(), 1e-6) << "W wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.x(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.y(), 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(0.0, quaternion.z(), 1e-6) << "Z wasn't properly initialized.";
+}
+
+// Test conversion to and from yaml node
+TYPED_TEST(QuaternionTests, YamlConvert)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ Quaternion quaternion(T(0.1), T(1.2), T(2.3), T(3.4));
+
+ YAML::Node node;
+
+ ASSERT_NO_THROW(node = quaternion);
+
+ EXPECT_TRUE(node.IsSequence());
+ EXPECT_EQ(4u, node.size());
+
+ ASSERT_NO_THROW({Quaternion expected = node.as<Quaternion>();});
+ EXPECT_TRUE(quaternion.isApprox(node.as<Quaternion>()));
+}
+
+
+
+// ==================== REPRESENTATION CONVERSIONS ====================
+
+/// Test setting quaternions from an angle/axis rotation.
+TYPED_TEST(QuaternionTests, FromAngleAxis)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ T angle = 1.6f;
+ Vector3 axis = Vector3(1.f, 2.f, 3.f).normalized();
+
+ using SurgSim::Math::makeRotationQuaternion;
+
+ Quaternion quaternion = makeRotationQuaternion(angle, axis);
+ EXPECT_NEAR(std::cos(angle / 2.0f), quaternion.w(), 1e-6) << "W wasn't properly initialized.";
+ EXPECT_NEAR(axis.x() * std::sin(angle / 2.0f), quaternion.x(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(axis.y() * std::sin(angle / 2.0f), quaternion.y(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(axis.z() * std::sin(angle / 2.0f), quaternion.z(), 1e-6) << "X wasn't properly initialized.";
+}
+
+template<class T>
+void testAngleAxis(const Eigen::Quaternion<T>& q, const Eigen::AngleAxis<T>& expectedAA,
+ bool expectNegatedQuatOppositeAxis = false)
+{
+ using SurgSim::Math::computeAngleAndAxis;
+ using SurgSim::Math::computeAngle;
+
+ Eigen::Matrix<T, 3, 1> axis, axisNeg;
+ T angle, angleNeg;
+
+ computeAngleAndAxis(q, &angle, &axis);
+ EXPECT_NEAR(expectedAA.angle(), angle, 1e-6) << "angle wasn't properly computed.";
+ EXPECT_NEAR(expectedAA.axis()[0], axis.x(), 1e-6) << "X wasn't properly computed.";
+ EXPECT_NEAR(expectedAA.axis()[1], axis.y(), 1e-6) << "Y wasn't properly computed.";
+ EXPECT_NEAR(expectedAA.axis()[2], axis.z(), 1e-6) << "Y wasn't properly computed.";
+ EXPECT_NEAR(expectedAA.angle(), computeAngle(q), 1e-6) << "angle wasn't properly computed by computeAngle().";
+
+ Eigen::Quaternion<T> qNeg = SurgSim::Math::negate(q);
+ computeAngleAndAxis(qNeg, &angleNeg, &axisNeg);
+ EXPECT_NEAR(angle, angleNeg, 1e-6) << "angle wasn't properly computed.";
+ if (expectNegatedQuatOppositeAxis)
+ {
+ EXPECT_NEAR(-axis.x(), axisNeg.x(), 1e-6) << "X wasn't properly computed.";
+ EXPECT_NEAR(-axis.y(), axisNeg.y(), 1e-6) << "Y wasn't properly computed.";
+ EXPECT_NEAR(-axis.z(), axisNeg.z(), 1e-6) << "Y wasn't properly computed.";
+ }
+ else
+ {
+ EXPECT_NEAR(axis.x(), axisNeg.x(), 1e-6) << "X wasn't properly computed.";
+ EXPECT_NEAR(axis.y(), axisNeg.y(), 1e-6) << "Y wasn't properly computed.";
+ EXPECT_NEAR(axis.z(), axisNeg.z(), 1e-6) << "Y wasn't properly computed.";
+ }
+}
+
+/// Test extracting an angle/axis rotation from a quaternion.
+TYPED_TEST(QuaternionTests, ToAngleAxis)
+{
+ using SurgSim::Math::makeRotationQuaternion;
+ using SurgSim::Math::computeAngleAndAxis;
+ using SurgSim::Math::computeAngle;
+
+ typedef typename TestFixture::AngleAxis AngleAxis;
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ Vector3 axis(T(1.0), T(2.0), T(3.0));
+ AngleAxis expectedAA;
+ T angle;
+ axis.normalize();
+
+ // Angle 0 in [0 pi]
+ angle = T(0);
+ // Expected result: 0 (to be in [0 pi]), same axis (same quaternion)
+ {
+ SCOPED_TRACE("Angle = 0");
+ Quaternion quaternion = makeRotationQuaternion(angle, axis); // q=(1 0 0 0)
+ expectedAA.angle() = angle;
+ expectedAA.axis() = Vector3(1, 0, 0);
+
+ testAngleAxis<T>(quaternion, expectedAA);
+ }
+
+ // Angle pi/2 in [0 pi]
+ angle = T(M_PI_2);
+ // Expected result: pi/2 (to be in [0 pi]), same axis (same quaternion)
+ {
+ SCOPED_TRACE("Angle = PI/2");
+ Quaternion quaternion = makeRotationQuaternion(angle, axis);
+ expectedAA.angle() = angle;
+ expectedAA.axis() = axis;
+
+ testAngleAxis<T>(quaternion, expectedAA);
+ }
+
+ // Angle pi-epsilon in [0 pi]
+ // => cos(angle/2) = +epsilon'
+ angle = T(2) * acos(std::numeric_limits<T>::epsilon());
+ // Expected result: pi-epsilon (to be in [0 pi]), same axis (same quaternion)
+ {
+ SCOPED_TRACE("Angle = PI-epsilon");
+ Quaternion quaternion = makeRotationQuaternion(angle, axis);
+ expectedAA.angle() = angle;
+ expectedAA.axis() = axis;
+
+ testAngleAxis<T>(quaternion, expectedAA);
+ }
+
+ // Angle pi
+ angle = T(M_PI);
+ // Expected result: pi (to be in [0 pi]), same axis (same quaternion)
+ // For negated quaternion, we expected angle=pi and opposite axis
+ // Quaternion with w=0 are the only one for which q and -q will give different rotation axis
+ {
+ SCOPED_TRACE("Angle = PI");
+ // Calling makeRotationQuaternion(M_PI, axis) will not set w to 0 (=cos(PI/2)) because of numerical error
+ // So we set it manually to force the test
+ Quaternion quaternion;
+ quaternion.w() = T(0);
+ quaternion.x() = axis[0];
+ quaternion.y() = axis[1];
+ quaternion.z() = axis[2];
+
+ expectedAA.angle() = angle;
+ expectedAA.axis() = axis;
+
+ testAngleAxis<T>(quaternion, expectedAA, true);
+ }
+
+ // Angle pi+epsilon in [pi 2pi]
+ // => cos(angle/2) = -epsilon'
+ angle = T(2) * acos(-std::numeric_limits<T>::epsilon());
+ // Expected result: -(pi+epsilon-2pi) (to be in [0 pi]), opposite axis (modulo 2pi + opposite quaternion)
+ {
+ SCOPED_TRACE("Angle = PI+epsilon");
+ Quaternion quaternion = makeRotationQuaternion(angle, axis);
+ expectedAA.angle() = T(2) * T(M_PI) - angle;
+ expectedAA.axis() = -axis;
+
+ testAngleAxis<T>(quaternion, expectedAA);
+ }
+
+ // Angle in [-2pi -pi]
+ angle = T(-3.56);
+ // Expected result: angle+2PI (to be in [0 pi]), same axis (modulo 2pi + same quaternion)
+ {
+ SCOPED_TRACE("Angle in [-2PI -PI]");
+ Quaternion quaternion = makeRotationQuaternion(angle, axis);
+ expectedAA.angle() = angle + (T)2.0 * T(M_PI);
+ expectedAA.axis() = axis;
+
+ testAngleAxis<T>(quaternion, expectedAA);
+ }
+
+ // Angle in [-pi 0]
+ angle = T(-2.12);
+ // Expected result: Opposite angle (to be in [0 pi]), opposite axis (opposite quaternion)
+ {
+ SCOPED_TRACE("Angle in [-PI 0]");
+ Quaternion quaternion = makeRotationQuaternion(angle, axis);
+ expectedAA.angle() = -angle;
+ expectedAA.axis() = -axis;
+
+ testAngleAxis<T>(quaternion, expectedAA);
+ }
+
+ // Angle in [0 pi]
+ angle = T(0.34);
+ // Expected result: Same angle (already in [0 pi]), same axis (same quaternion)
+ {
+ SCOPED_TRACE("Angle in [0 PI]");
+ Quaternion quaternion = makeRotationQuaternion(angle, axis);
+ expectedAA.angle() = angle;
+ expectedAA.axis() = axis;
+
+ testAngleAxis<T>(quaternion, expectedAA);
+ }
+
+ // Angle in [pi, 2pi]
+ angle = T(4.73);
+ // Expected result: -(angle-2PI) to be in [0, pi], opposite axis (modulo 2pi + opposite quaternion)
+ {
+ SCOPED_TRACE("Angle in [PI 2PI]");
+ Quaternion quaternion = makeRotationQuaternion(angle, axis);
+ expectedAA.angle() = -angle + T(2) * T(M_PI);
+ expectedAA.axis() = -axis;
+
+ testAngleAxis<T>(quaternion, expectedAA);
+ }
+}
+
+/// Test setting a quaternion from a matrix.
+TYPED_TEST(QuaternionTests, FromMatrix)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 3> Matrix33;
+
+ T angle = 0.1f;
+ T sinAngle = std::sin(angle);
+ T cosAngle = std::cos(angle);
+
+ Matrix33 matrix;
+ matrix <<
+ cosAngle, -sinAngle, 0,
+ sinAngle, cosAngle, 0,
+ 0, 0, 1;
+
+ Quaternion quaternion(matrix);
+ EXPECT_NEAR(std::cos(angle / 2), quaternion.w(), 1e-6) << "W wasn't properly computed.";
+ EXPECT_NEAR(0, quaternion.x(), 1e-6) << "X wasn't properly computed.";
+ EXPECT_NEAR(0, quaternion.y(), 1e-6) << "Y wasn't properly computed.";
+ EXPECT_NEAR(std::sin(angle / 2), quaternion.z(), 1e-6) << "Z wasn't properly computed.";
+}
+
+
+/// Test setting a matrix from a quaternion.
+TYPED_TEST(QuaternionTests, ToMatrix)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 3> Matrix33;
+
+ T angle = 0.1f;
+ Quaternion quaternion(std::cos(angle / 2), 0, 0, std::sin(angle / 2));
+
+ Matrix33 expectedMatrix;
+ T sinAngle = std::sin(angle);
+ T cosAngle = std::cos(angle);
+ expectedMatrix <<
+ cosAngle, -sinAngle, 0,
+ sinAngle, cosAngle, 0,
+ 0, 0, 1;
+
+ Matrix33 matrix1 = quaternion.matrix();
+ EXPECT_NEAR(0, (matrix1 - expectedMatrix).norm(), 9e-6) << "The rotation matrix wasn't properly computed" <<
+ " by matrix().";
+ Matrix33 matrix2 = quaternion.toRotationMatrix();
+ EXPECT_NEAR(0, (matrix2 - expectedMatrix).norm(), 9e-6) << "The rotation matrix wasn't properly computed" <<
+ " by toRotationMatrix().";
+}
+
+// ==================== ARITHMETIC ====================
+
+/// Test quaternion conjugate.
+TYPED_TEST(QuaternionTests, Conjugate)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+
+ Quaternion q(2.1f, 2.2f, 2.3f, 2.4f);
+ Quaternion n = q.conjugate();
+ EXPECT_NEAR(2.1, n.w(), 1e-6) << "W wasn't properly initialized.";
+ EXPECT_NEAR(-2.2, n.x(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(-2.3, n.y(), 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(-2.4, n.z(), 1e-6) << "Z wasn't properly initialized.";
+}
+
+/// Test quaternion inverse.
+TYPED_TEST(QuaternionTests, Inverse)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ Quaternion q(2.1f, 2.2f, 2.3f, 2.4f);
+ Quaternion n = q.inverse();
+ T scale = q.squaredNorm();
+
+ EXPECT_NEAR(2.1 / scale, n.w(), 1e-6) << "W wasn't properly initialized.";
+ EXPECT_NEAR(-2.2 / scale, n.x(), 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(-2.3 / scale, n.y(), 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(-2.4 / scale, n.z(), 1e-6) << "Z wasn't properly initialized.";
+
+ Quaternion qn = q * n;
+ EXPECT_NEAR(1.0, qn.w(), 1e-6) << "W of q * q^1 is messed up.";
+ EXPECT_NEAR(0.0, qn.x(), 1e-6) << "X of q * q^1 is messed up.";
+ EXPECT_NEAR(0.0, qn.y(), 1e-6) << "Y of q * q^1 is messed up.";
+ EXPECT_NEAR(0.0, qn.z(), 1e-6) << "Z of q * q^1 is messed up.";
+
+ Quaternion nq = n * q;
+ EXPECT_NEAR(1.0, nq.w(), 1e-6) << "W of q^1 * q is messed up.";
+ EXPECT_NEAR(0.0, nq.x(), 1e-6) << "X of q^1 * q is messed up.";
+ EXPECT_NEAR(0.0, nq.y(), 1e-6) << "Y of q^1 * q is messed up.";
+ EXPECT_NEAR(0.0, nq.z(), 1e-6) << "Z of q^1 * q is messed up.";
+}
+
+/// Test quaternion rotation of vectors.
+TYPED_TEST(QuaternionTests, ApplyToVector)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector;
+
+ using SurgSim::Math::makeRotationQuaternion;
+ Quaternion q = makeRotationQuaternion(static_cast<T>(M_PI_2), Vector(1, 0, 0));
+
+ // You can use this, which is actually more efficient if you can manage to apply the same matrix more than once:
+ Vector qx = q.matrix() * Vector::UnitX();
+ EXPECT_TRUE(qx.isApprox(Vector::UnitX(), 1e-6f)) << qx;
+ // ...or this, which clearly isn't very recommended by the API designers:
+ Vector qx2 = q._transformVector(Vector::UnitX());
+ EXPECT_TRUE(qx2.isApprox(Vector::UnitX(), 1e-6f)) << qx2;
+
+ Vector qy = q.matrix() * Vector::UnitY();
+ EXPECT_TRUE(qy.isApprox(Vector::UnitZ(), 1e-6f)) << qy;
+
+ Vector qz = q.matrix() * Vector::UnitZ();
+ EXPECT_TRUE(qz.isApprox(-Vector::UnitY(), 1e-6f)) << qz;
+}
+
+/// Quaternion norm and its square.
+TYPED_TEST(QuaternionTests, NormAndSquared)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+
+ Quaternion q(3.1f, 3.4f, 3.7f, 4.0f);
+ // sum of the squares of the components
+ double expectedSumSquares = 50.86;
+
+ EXPECT_NEAR(expectedSumSquares, q.squaredNorm(), 1e-6);
+ EXPECT_NEAR(sqrt(expectedSumSquares), q.norm(), 1e-6);
+}
+
+/// Normalization of quaternions.
+TYPED_TEST(QuaternionTests, Normalize)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+
+ Quaternion q(3.1f, 3.4f, 3.7f, 4.0f);
+ // sum of the squares of the components
+ double expectedSumSquares = 50.86;
+
+ EXPECT_NEAR(sqrt(expectedSumSquares), q.norm(), 1e-6);
+
+ // normalized() RETURNS the normalized quaternion, leaving original unchanged.
+ Quaternion qNorm = q.normalized();
+ EXPECT_NEAR(1, qNorm.norm(), 1e-6);
+ EXPECT_NEAR(sqrt(expectedSumSquares), q.norm(), 1e-6);
+
+ // normalize() NORMALIZES the quaternion, modifying it.
+ q.normalize();
+ EXPECT_NEAR(1, q.norm(), 1e-6);
+ EXPECT_NEAR(0, (qNorm.coeffs() - q.coeffs()).norm(), 1e-6);
+}
+
+// ==================== TYPE CONVERSION ====================
+
+/// Typecasting quaternions (double <-> float conversions).
+TYPED_TEST(QuaternionTests, TypeCasting)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef Eigen::Quaternion<double> Quaterniond;
+ typedef Eigen::Quaternion<float> Quaternionf;
+
+ Quaternion q(12.1f, 12.2f, 12.3f, 12.4f);
+ // Ugh, "template" is required to get this to parse properly. This is
+ // triggered because the test is a part of a template class; you don't
+ // need to do this in a non-template context.
+ Quaterniond qd = q.template cast<double>();
+ EXPECT_NEAR(q.norm(), qd.norm(), 1e-6);
+ Quaternionf qf = q.template cast<float>();
+ EXPECT_NEAR(q.norm(), qf.norm(), 1e-4);
+}
+
+// ==================== MISCELLANEOUS ====================
+
+/// Reading from and writing to arrays or blocks of double/float in memory.
+TYPED_TEST(QuaternionTests, ArrayReadWrite)
+{
+ typedef typename TestFixture::Quaternion Quaternion;
+ typedef typename TestFixture::Scalar T;
+
+ const T inputArray[5] = { 12.1f, 12.2f, 12.3f, 12.4f, 12.5f };
+ T outputArray[5];
+
+ // Note that with the current versions of Eigen, you CANNOT say this:
+ // Eigen::Map<SurgSim::Math::Quaternion> q(array);
+ // because you can't map Eigen::Quaternion with non-zero flags!
+ // But mapping a default (AutoAlign) quaternion should be safe even if the buffer is not aligned.
+
+ Eigen::Map<const Eigen::Quaternion<T> > q_in(inputArray);
+ Quaternion q1 = q_in;
+
+ Eigen::Map<Eigen::Quaternion<T> > q_out(outputArray);
+ q_out = q1;
+
+ for (int i = 0; i < 4; ++i)
+ {
+ EXPECT_NEAR(inputArray[i], outputArray[i], 1e-6);
+ }
+}
+
+/// Test quaternion negation.
+TYPED_TEST(QuaternionTests, Negate)
+{
+ using SurgSim::Math::negate;
+
+ typedef typename TestFixture::Scalar T;
+ typedef Eigen::Quaternion<T> Quaternion;
+
+ // Test that 2 quaternions are opposite if they are not equal but give the same rotation
+ for (size_t numLoop = 0; numLoop < 100; numLoop++)
+ {
+ Quaternion q(Eigen::Matrix<T, 4, 1>::Random());
+ q.normalize();
+ Quaternion qNeg = negate(q);
+ EXPECT_FALSE(q.isApprox(qNeg));
+
+ typename Quaternion::Matrix3 m = q.toRotationMatrix();
+ typename Quaternion::Matrix3 mNeg = qNeg.toRotationMatrix();
+ EXPECT_TRUE(m.isApprox(mNeg));
+ }
+}
+
+// ==================== SLERP ====================
+
+/// Test quaternion interpolation.
+TYPED_TEST(QuaternionTests, SlerpInterpolation)
+{
+ using SurgSim::Math::negate;
+
+ typedef typename TestFixture::Scalar T;
+ typedef Eigen::Quaternion<T> Quaternion;
+
+ for (size_t numLoop = 0; numLoop < 100; numLoop++)
+ {
+ Quaternion q;
+ Quaternion q0(Eigen::Matrix<T, 4, 1>::Random());
+ Quaternion q1(Eigen::Matrix<T, 4, 1>::Random());
+ q0.normalize();
+ q1.normalize();
+
+ q = SurgSim::Math::interpolate(q0, q1, static_cast<T>(0.0));
+ EXPECT_TRUE(q.isApprox(q0) || q.isApprox(negate(q0)));
+ q = SurgSim::Math::interpolate(q0, q1, static_cast<T>(1.0));
+ EXPECT_TRUE(q.isApprox(q1) || q.isApprox(negate(q1)));
+
+ q = SurgSim::Math::interpolate(q0, q1, static_cast<T>(0.234));
+ EXPECT_FALSE(q.isApprox(q0) || q.isApprox(negate(q0)));
+ EXPECT_FALSE(q.isApprox(q1) || q.isApprox(negate(q1)));
+
+ q = SurgSim::Math::interpolate(q0, q1, static_cast<T>(0.5));
+ EXPECT_FALSE(q.isApprox(q0) || q.isApprox(negate(q0)));
+ EXPECT_FALSE(q.isApprox(q1) || q.isApprox(negate(q1)));
+ // At t=0.5, the interpolation should return (q0 + q1)/2 normalized
+ // c.f. http://en.wikipedia.org/wiki/Slerp
+ // If the quaternions are over PI angle, the slerp will interpolate between q0 and -q1
+ // in this case, the interpolation is (q0 - q1)/2 normalized
+ // From our specification, both quaternions could be considered negative, so we extend
+ // the tests to these possibilities as well:
+ // (-q0 + q1) / 2 normalized
+ // (-q0 - q1) / 2 normalized
+ Quaternion qHalf0((q0.coeffs() + q1.coeffs()) * 0.5);
+ Quaternion qHalf1((q0.coeffs() - q1.coeffs()) * 0.5);
+ Quaternion qHalf2((-q0.coeffs() + q1.coeffs()) * 0.5);
+ Quaternion qHalf3((-q0.coeffs() - q1.coeffs()) * 0.5);
+ qHalf0.normalize();
+ qHalf1.normalize();
+ qHalf2.normalize();
+ qHalf3.normalize();
+ EXPECT_TRUE(q.isApprox(qHalf0) || q.isApprox(qHalf1) ||
+ q.isApprox(qHalf2) || q.isApprox(qHalf3));
+
+ q = SurgSim::Math::interpolate(q0, q1, static_cast<T>(0.874));
+ EXPECT_FALSE(q.isApprox(q0) || q.isApprox(negate(q0)));
+ EXPECT_FALSE(q.isApprox(q1) || q.isApprox(negate(q1)));
+ }
+}
+
+// TO DO:
+// testing numerical validity
+// testing for denormalized numbers
+// testing degeneracy (norm near 0)
+// compute an orthonormal frame based on a given normal (z-axis)
+// Euler angles (various conventions)
+// power/slerp
diff --git a/SurgSim/Math/UnitTests/RigidTransformTests.cpp b/SurgSim/Math/UnitTests/RigidTransformTests.cpp
new file mode 100644
index 0000000..24727ce
--- /dev/null
+++ b/SurgSim/Math/UnitTests/RigidTransformTests.cpp
@@ -0,0 +1,199 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 that exercise the functionality of our rigid transform typedefs, which
+/// come straight from Eigen.
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "gtest/gtest.h"
+
+template <class T>
+class RigidTransformTestBase : public testing::Test
+{
+public:
+ typedef T RigidTransform;
+ typedef typename T::Scalar Scalar;
+};
+
+
+template <class T>
+class RigidTransform3Tests : public RigidTransformTestBase<T>
+{
+};
+
+typedef ::testing::Types<SurgSim::Math::RigidTransform3d,
+ SurgSim::Math::RigidTransform3f> RigidTransform3Variants;
+TYPED_TEST_CASE(RigidTransform3Tests, RigidTransform3Variants);
+
+
+template <class T>
+class AllRigidTransformTests : public RigidTransformTestBase<T>
+{
+};
+
+typedef ::testing::Types<SurgSim::Math::RigidTransform2d,
+ SurgSim::Math::RigidTransform2f,
+ SurgSim::Math::RigidTransform3d,
+ SurgSim::Math::RigidTransform3f> AllRigidTransformVariants;
+TYPED_TEST_CASE(AllRigidTransformTests, AllRigidTransformVariants);
+
+
+/// Test that rigid transforms can be constructed
+TYPED_TEST(AllRigidTransformTests, CanConstruct)
+{
+ typename TestFixture::RigidTransform transform;
+}
+
+/// Test rigid transforms interpolation
+TYPED_TEST(AllRigidTransformTests, Interpolation)
+{
+ using SurgSim::Math::makeRigidTransform;
+
+ typedef typename TestFixture::Scalar T;
+ typedef Eigen::Quaternion<T> Quaternion;
+ typedef Eigen::Transform<T, 3, Eigen::Isometry> Transform;
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ for (size_t numLoop = 0; numLoop < 100; numLoop++)
+ {
+ Quaternion q0(Eigen::Matrix<T, 4, 1>::Random());
+ Quaternion q1(Eigen::Matrix<T, 4, 1>::Random());
+ q0.normalize();
+ q1.normalize();
+
+ Vector3 t0(Vector3::Random());
+ Vector3 t1(Vector3::Random());
+
+ Transform transform0 = makeRigidTransform(q0, t0);
+ Transform transform1 = makeRigidTransform(q1, t1);
+ {
+ Transform transform = SurgSim::Math::interpolate(transform0, transform1, static_cast<T>(0.0));
+ EXPECT_TRUE(transform.isApprox(transform0));
+ }
+ {
+ Transform transform = SurgSim::Math::interpolate(transform0, transform1, static_cast<T>(1.0));
+ EXPECT_TRUE(transform.isApprox(transform1));
+ }
+
+ {
+ Transform transform = SurgSim::Math::interpolate(transform0, transform1, static_cast<T>(0.234));
+ EXPECT_FALSE(transform.isApprox(transform0));
+ EXPECT_FALSE(transform.isApprox(transform1));
+ }
+
+ {
+ Transform transform = SurgSim::Math::interpolate(transform0, transform1, static_cast<T>(0.5));
+ EXPECT_FALSE(transform.isApprox(transform0));
+ EXPECT_FALSE(transform.isApprox(transform1));
+
+ // At t=0.5, the rotation interpolation should return (q0 + q1)/2 normalized
+ // c.f. http://en.wikipedia.org/wiki/Slerp
+ // If the quaternions are over PI angle, the slerp will interpolate between q0 and -q1
+ // in this case, the interpolation is (q0 - q1)/2 normalized
+ // From our specification, both quaternions could be considered negative, so we extend
+ // the tests to these possibilities as well:
+ // (-q0 + q1) / 2 normalized
+ // (-q0 - q1) / 2 normalized
+ Quaternion qHalf0((q0.coeffs() + q1.coeffs()) * 0.5);
+ Quaternion qHalf1((q0.coeffs() - q1.coeffs()) * 0.5);
+ Quaternion qHalf2((-q0.coeffs() + q1.coeffs()) * 0.5);
+ Quaternion qHalf3((-q0.coeffs() - q1.coeffs()) * 0.5);
+ qHalf0.normalize();
+ qHalf1.normalize();
+ qHalf2.normalize();
+ qHalf3.normalize();
+
+ Vector3 tHalf = (t0 + t1) * 0.5;
+ Transform transformHalf0 = makeRigidTransform(qHalf0, tHalf);
+ Transform transformHalf1 = makeRigidTransform(qHalf1, tHalf);
+ Transform transformHalf2 = makeRigidTransform(qHalf2, tHalf);
+ Transform transformHalf3 = makeRigidTransform(qHalf3, tHalf);
+ EXPECT_TRUE(transform.isApprox(transformHalf0) || transform.isApprox(transformHalf1) ||
+ transform.isApprox(transformHalf2) || transform.isApprox(transformHalf3));
+ }
+
+ {
+ Transform transform = SurgSim::Math::interpolate(transform0, transform1, static_cast<T>(0.839));
+ EXPECT_FALSE(transform.isApprox(transform0));
+ EXPECT_FALSE(transform.isApprox(transform1));
+ }
+ }
+}
+
+TYPED_TEST(AllRigidTransformTests, MakeLookAt)
+{
+ typedef typename TestFixture::Scalar T;
+ typedef Eigen::Transform<T, 3, Eigen::Isometry> Transform;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+ typedef Eigen::Matrix<T, 4, 1> Vector4;
+
+ Vector3 origin(0.0, 0.0, 0.0);
+ Vector3 eye(10.0, 10.0, 10.0);
+ Vector3 up(0.0, 1.0, 0.0);
+
+ Vector4 center4(0.0, 0.0, 0.0, 1.0);
+
+ // This follows the OpenGl convention for the camera view matrix transform, any axis would do see
+ // the documentation for makeRigidTransform and gluLookAt()
+ Vector4 direction4(0.0, 0.0, -1.0, 1.0);
+ Vector4 eye4(10.0, 10.0, 10.0, 1.0);
+
+ Transform transform = SurgSim::Math::makeRigidTransform(eye, origin, up);
+
+ EXPECT_TRUE(eye4.isApprox(transform * center4));
+
+ Vector4 transformed = transform * direction4;
+
+ Vector3 direction3(transformed[0], transformed[1], transformed[2]);
+ EXPECT_TRUE(eye.normalized().isApprox(direction3.normalized()));
+}
+
+// Test conversion to and from yaml node
+TYPED_TEST(AllRigidTransformTests, YamlConvert)
+{
+ using SurgSim::Math::makeRigidTransform;
+
+ typedef typename TestFixture::Scalar T;
+ typedef Eigen::Quaternion<T> Quaternion;
+ typedef Eigen::Transform<T, 3, Eigen::Isometry> Transform;
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ const T inputValues[4] = {1.1f, 2.2f, 3.3f, 4.4f};
+
+ Quaternion quaternion(inputValues);
+ quaternion.normalize();
+
+ Vector3 translation(inputValues);
+
+ Transform transform = makeRigidTransform(quaternion, translation);
+
+ YAML::Node node;
+
+ ASSERT_NO_THROW(node = transform);
+
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(2, node.size());
+
+ Transform expected;
+
+ ASSERT_NO_THROW(expected = node.as<Transform>());
+ EXPECT_TRUE(transform.isApprox(expected));
+}
\ No newline at end of file
diff --git a/SurgSim/Math/UnitTests/ShapeTests.cpp b/SurgSim/Math/UnitTests/ShapeTests.cpp
new file mode 100644
index 0000000..8586127
--- /dev/null
+++ b/SurgSim/Math/UnitTests/ShapeTests.cpp
@@ -0,0 +1,602 @@
+//// This file is a part of the OpenSurgSim project.
+//// Copyright 2013, SimQuest Solutions Inc.
+////
+//// Licensed under the Apache License, Version 2.0 (the "License");
+//// you may not use this file except in compliance with the License.
+//// You may obtain a copy of the License at
+////
+//// http://www.apache.org/licenses/LICENSE-2.0
+////
+//// Unless required by applicable law or agreed to in writing, software
+//// distributed under the License is distributed on an "AS IS" BASIS,
+//// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//// See the License for the specific language governing permissions and
+//// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Math/Shapes.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Matrix33d;
+
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::CapsuleShape;
+using SurgSim::Math::CylinderShape;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::OctreeShape;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::Shape;
+using SurgSim::Math::SphereShape;
+
+namespace {
+ const double epsilon = 1e-10;
+}
+
+class ShapeTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ m_rho = 9000.0;
+ m_radius = 0.01;
+ m_length = 0.1;
+ m_size[0] = 0.1;
+ m_size[1] = 0.2;
+ m_size[2] = 0.3;
+ }
+
+ void TearDown()
+ {
+ }
+
+ // Mass density
+ double m_rho;
+
+ // Radius (sphere/cylinder/capsule)
+ double m_radius;
+
+ // Length (cylinder/capsule)
+ double m_length;
+
+ // Size (box)
+ double m_size[3];
+};
+
+TEST_F(ShapeTest, EncodeEmptyShapeTest)
+{
+ std::shared_ptr<Shape> shape;
+ EXPECT_ANY_THROW(YAML::convert<std::shared_ptr<Shape>>::encode(shape));
+}
+
+TEST_F(ShapeTest, SphereSerializationTest)
+{
+ {
+ YAML::Node node;
+ node["SurgSim::Math::SphereShape"]["Radius"] = m_radius;
+
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = node.as<std::shared_ptr<Shape>>());
+
+ EXPECT_EQ("SurgSim::Math::SphereShape", shape->getClassName());
+ EXPECT_NEAR(m_radius, shape->getValue<double>("Radius"), epsilon);
+ EXPECT_TRUE(shape->isValid());
+ }
+
+ {
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = Shape::getFactory().create("SurgSim::Math::SphereShape"));
+ shape->setValue("Radius", m_radius);
+ EXPECT_TRUE(shape->isValid());
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = shape);
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ ASSERT_TRUE(node["SurgSim::Math::SphereShape"].IsDefined());
+ auto data = node["SurgSim::Math::SphereShape"];
+ EXPECT_EQ(1u, data.size());
+
+ std::shared_ptr<SphereShape> sphereShape;
+ ASSERT_NO_THROW(sphereShape = std::dynamic_pointer_cast<SphereShape>(node.as<std::shared_ptr<Shape>>()));
+ EXPECT_EQ("SurgSim::Math::SphereShape", sphereShape->getClassName());
+ EXPECT_NEAR(m_radius, sphereShape->getValue<double>("Radius"), epsilon);
+ EXPECT_TRUE(sphereShape->isValid());
+ }
+}
+
+TEST_F(ShapeTest, Sphere)
+{
+ ASSERT_NO_THROW(SphereShape sphere(m_radius));
+
+ {
+ SphereShape invalidSphere(-0.1);
+ EXPECT_FALSE(invalidSphere.isValid());
+
+ SphereShape sphere(0.1);
+ EXPECT_TRUE(sphere.isValid());
+ }
+
+ SphereShape sphere(m_radius);
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_SPHERE, sphere.getType());
+ EXPECT_NEAR(m_radius, sphere.getRadius(), epsilon);
+
+ const double& r = m_radius;
+ const double r2 = r * r;
+ double expectedVolume = 4.0 / 3.0 * M_PI * (r2 * r);
+ double expectedMass = m_rho * expectedVolume;
+ double coef = 2.0 / 5.0 * expectedMass * r2;
+ Matrix33d expectedInertia;
+ expectedInertia << coef, 0.0, 0.0,
+ 0.0, coef, 0.0,
+ 0.0, 0.0, coef;
+
+ double volume = sphere.getVolume();
+ Vector3d center = sphere.getCenter();
+ Matrix33d inertia = sphere.getSecondMomentOfVolume() * m_rho;
+
+ EXPECT_NEAR(expectedVolume, volume, epsilon);
+ EXPECT_TRUE(center.isZero());
+ EXPECT_TRUE(expectedInertia.isApprox(inertia));
+ EXPECT_TRUE(sphere.isValid());
+}
+
+TEST_F(ShapeTest, BoxSerializationTest)
+{
+ {
+ YAML::Node node;
+ node["SurgSim::Math::BoxShape"]["SizeX"] = m_size[0];
+ node["SurgSim::Math::BoxShape"]["SizeY"] = m_size[1];
+ node["SurgSim::Math::BoxShape"]["SizeZ"] = m_size[2];
+
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = node.as<std::shared_ptr<Shape>>());
+
+ EXPECT_EQ("SurgSim::Math::BoxShape", shape->getClassName());
+ EXPECT_NEAR(m_size[0], shape->getValue<double>("SizeX"), epsilon);
+ EXPECT_NEAR(m_size[1], shape->getValue<double>("SizeY"), epsilon);
+ EXPECT_NEAR(m_size[2], shape->getValue<double>("SizeZ"), epsilon);
+ EXPECT_TRUE(shape->isValid());
+ }
+
+ {
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = Shape::getFactory().create("SurgSim::Math::BoxShape"));
+ EXPECT_TRUE(shape->isValid()); // BoxShape of size (0, 0, 0) is regarded as 'valid'.
+ shape->setValue("SizeX", m_size[0]);
+ shape->setValue("SizeY", m_size[1]);
+ shape->setValue("SizeZ", m_size[2]);
+ EXPECT_TRUE(shape->isValid());
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = shape);
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ ASSERT_TRUE(node["SurgSim::Math::BoxShape"].IsDefined());
+ auto data = node["SurgSim::Math::BoxShape"];
+ EXPECT_EQ(3u, data.size());
+
+ std::shared_ptr<BoxShape> boxShape;
+ ASSERT_NO_THROW(boxShape = std::dynamic_pointer_cast<BoxShape>(node.as<std::shared_ptr<Shape>>()));
+ EXPECT_EQ("SurgSim::Math::BoxShape", boxShape->getClassName());
+ EXPECT_NEAR(m_size[0], boxShape->getValue<double>("SizeX"), epsilon);
+ EXPECT_NEAR(m_size[1], boxShape->getValue<double>("SizeY"), epsilon);
+ EXPECT_NEAR(m_size[2], boxShape->getValue<double>("SizeZ"), epsilon);
+ EXPECT_TRUE(boxShape->isValid());
+ }
+}
+
+TEST_F(ShapeTest, Box)
+{
+ ASSERT_NO_THROW(BoxShape box(m_size[0], m_size[1], m_size[2]));
+ {
+ BoxShape invalidBox(0.1, -0.1, 0.1);
+ EXPECT_FALSE(invalidBox.isValid());
+
+ BoxShape box(0.1, 0.2, 0.3);
+ EXPECT_TRUE(box.isValid());
+ }
+
+ Vector3d size(m_size[0], m_size[1], m_size[2]);
+ BoxShape box(m_size[0], m_size[1], m_size[2]);
+ EXPECT_NEAR(m_size[0], box.getSizeX(), epsilon);
+ EXPECT_NEAR(m_size[1], box.getSizeY(), epsilon);
+ EXPECT_NEAR(m_size[2], box.getSizeZ(), epsilon);
+ EXPECT_TRUE(box.getSize().isApprox(size));
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_BOX, box.getType());
+ EXPECT_TRUE(box.isValid());
+
+ double expectedVolume = m_size[0] * m_size[1] * m_size[2];
+ double expectedMass = m_rho * expectedVolume;
+ double coef = 1.0 / 12.0 * expectedMass;
+ double x2 = m_size[0] * m_size[0];
+ double y2 = m_size[1] * m_size[1];
+ double z2 = m_size[2] * m_size[2];
+ Matrix33d expectedInertia;
+ expectedInertia << coef * (y2 + z2), 0.0, 0.0,
+ 0.0, coef * (x2 + z2), 0.0,
+ 0.0, 0.0, coef * (x2 + y2);
+
+ double volume = box.getVolume();
+ Vector3d center = box.getCenter();
+ Matrix33d inertia = box.getSecondMomentOfVolume() * m_rho;
+
+ EXPECT_NEAR(expectedVolume, volume, epsilon);
+ EXPECT_TRUE(center.isZero());
+ EXPECT_TRUE(expectedInertia.isApprox(inertia));
+}
+
+TEST_F(ShapeTest, CylinderSerializationTest)
+{
+ {
+ YAML::Node node;
+ node["SurgSim::Math::CylinderShape"]["Length"] = m_length;
+ node["SurgSim::Math::CylinderShape"]["Radius"] = m_length;
+
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = node.as<std::shared_ptr<Shape>>());
+
+ EXPECT_EQ("SurgSim::Math::CylinderShape", shape->getClassName());
+ EXPECT_NEAR(m_length, shape->getValue<double>("Length"), epsilon);
+ EXPECT_NEAR(m_length, shape->getValue<double>("Radius"), epsilon);
+ EXPECT_TRUE(shape->isValid());
+ }
+
+ {
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = Shape::getFactory().create("SurgSim::Math::CylinderShape"));
+ shape->setValue("Length", m_length);
+ shape->setValue("Radius", m_radius);
+ EXPECT_TRUE(shape->isValid());
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = shape);
+
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ ASSERT_TRUE(node["SurgSim::Math::CylinderShape"].IsDefined());
+ auto data = node["SurgSim::Math::CylinderShape"];
+ EXPECT_EQ(2u, data.size());
+
+ std::shared_ptr<CylinderShape> cylinderShape;
+ ASSERT_NO_THROW(cylinderShape = std::dynamic_pointer_cast<CylinderShape>(node.as<std::shared_ptr<Shape>>()));
+ EXPECT_EQ("SurgSim::Math::CylinderShape", cylinderShape->getClassName());
+ EXPECT_NEAR(m_length, cylinderShape->getValue<double>("Length"), epsilon);
+ EXPECT_NEAR(m_radius, cylinderShape->getValue<double>("Radius"), epsilon);
+ EXPECT_TRUE(shape->isValid());
+ }
+}
+
+TEST_F(ShapeTest, Cylinder)
+{
+ ASSERT_NO_THROW(CylinderShape cyliner(m_length, m_radius));
+
+ {
+ CylinderShape invalidCylinder(-0.1, 0.1);
+ EXPECT_FALSE(invalidCylinder.isValid());
+
+ CylinderShape invalidCylinder2(0.1, -0.1);
+ EXPECT_FALSE(invalidCylinder2.isValid());
+
+ CylinderShape validCylinder(0.1, 0.1);
+ EXPECT_TRUE(validCylinder.isValid());
+ }
+
+ CylinderShape cylinder(m_length, m_radius);
+ EXPECT_NEAR(m_length, cylinder.getLength(), epsilon);
+ EXPECT_NEAR(m_radius, cylinder.getRadius(), epsilon);
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_CYLINDER, cylinder.getType());
+
+ double expectedVolume = M_PI * m_radius * m_radius * m_length;
+ double expectedMass = m_rho * expectedVolume;
+
+ double r1sq = m_radius * m_radius;
+ double l2 = m_length * m_length;
+ double coefDir = 1.0 / 2.0 * expectedMass * (r1sq);
+ double coef = 1.0 / 12.0 * expectedMass * (3.0 * (r1sq) + l2);
+ Matrix33d expectedInertia;
+ expectedInertia << coef, 0.0, 0.0,
+ 0.0, coefDir, 0.0,
+ 0.0, 0.0, coef;
+
+ double volume = cylinder.getVolume();
+ Vector3d center = cylinder.getCenter();
+ Matrix33d inertia = cylinder.getSecondMomentOfVolume() * m_rho;
+
+ EXPECT_NEAR(expectedVolume, volume, epsilon);
+ EXPECT_TRUE(center.isZero());
+ EXPECT_TRUE(expectedInertia.isApprox(inertia));
+ EXPECT_TRUE(cylinder.isValid());
+}
+
+TEST_F(ShapeTest, CapsuleSerializationTest)
+{
+ {
+ YAML::Node node;
+ node["SurgSim::Math::CapsuleShape"]["Length"] = m_length;
+ node["SurgSim::Math::CapsuleShape"]["Radius"] = m_length;
+
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = node.as<std::shared_ptr<Shape>>());
+
+ EXPECT_EQ("SurgSim::Math::CapsuleShape", shape->getClassName());
+ EXPECT_NEAR(m_length, shape->getValue<double>("Length"), epsilon);
+ EXPECT_NEAR(m_length, shape->getValue<double>("Radius"), epsilon);
+ EXPECT_TRUE(shape->isValid());
+ }
+
+ {
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = Shape::getFactory().create("SurgSim::Math::CapsuleShape"));
+ shape->setValue("Length", m_length);
+ shape->setValue("Radius", m_radius);
+ EXPECT_TRUE(shape->isValid());
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = shape);
+
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ ASSERT_TRUE(node["SurgSim::Math::CapsuleShape"].IsDefined());
+ auto data = node["SurgSim::Math::CapsuleShape"];
+ EXPECT_EQ(2u, data.size());
+
+ std::shared_ptr<CapsuleShape> capsuleShape;
+ ASSERT_NO_THROW(capsuleShape = std::dynamic_pointer_cast<CapsuleShape>(node.as<std::shared_ptr<Shape>>()));
+ EXPECT_EQ("SurgSim::Math::CapsuleShape", capsuleShape->getClassName());
+ EXPECT_NEAR(m_length, capsuleShape->getValue<double>("Length"), epsilon);
+ EXPECT_NEAR(m_radius, capsuleShape->getValue<double>("Radius"), epsilon);
+ EXPECT_TRUE(capsuleShape->isValid());
+ }
+}
+
+TEST_F(ShapeTest, Capsule)
+{
+ ASSERT_NO_THROW(CapsuleShape capsule(m_length, m_radius));
+
+ {
+ CapsuleShape invalidCapsule(-0.1, 0.1);
+ EXPECT_FALSE(invalidCapsule.isValid());
+
+ CapsuleShape invalidCapsule2(0.1, -0.1);
+ EXPECT_FALSE(invalidCapsule2.isValid());
+
+ CapsuleShape validCapsule(0.1, 0.1);
+ EXPECT_TRUE(validCapsule.isValid());
+ }
+
+ CapsuleShape capsule(m_length, m_radius);
+ EXPECT_NEAR(m_length, capsule.getLength(), epsilon);
+ EXPECT_NEAR(m_radius, capsule.getRadius(), epsilon);
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_CAPSULE, capsule.getType());
+
+ double r2 = m_radius * m_radius;
+ double r3 = r2 * m_radius;
+ double l2 = m_length * m_length;
+
+ double volumeCylinder = M_PI * r2 * m_length;
+ double massCylinder = m_rho * volumeCylinder;
+ double volumeSphere = 4.0 / 3.0 * M_PI * r3;
+ double massSphere = m_rho * volumeSphere;
+ double expectedVolume = volumeCylinder + volumeSphere;
+ double coefDir = 2.0 / 5.0 * massSphere * r2;
+ double coef = coefDir;
+ coefDir += 1.0 / 2.0 * massCylinder * r2;
+ coef += massSphere * (1.0 / 4.0 * l2 + 3.0 / 8.0 * m_radius * m_length);
+ coef += 1.0 / 12.0 * massCylinder * (3 * r2 + l2);
+ Matrix33d expectedInertia;
+ expectedInertia << coef, 0.0, 0.0,
+ 0.0, coefDir, 0.0,
+ 0.0, 0.0, coef;
+
+ double volume = capsule.getVolume();
+ Vector3d center = capsule.getCenter();
+ Matrix33d inertia = capsule.getSecondMomentOfVolume() * m_rho;
+
+ EXPECT_NEAR(expectedVolume, volume, epsilon);
+ EXPECT_TRUE(center.isZero());
+ EXPECT_TRUE(expectedInertia.isApprox(inertia));
+ EXPECT_TRUE(capsule.isValid());
+}
+
+TEST_F(ShapeTest, DoubleSidedPlaneShapeSerializationTest)
+{
+ {
+ YAML::Node node, empty;
+ node["SurgSim::Math::DoubleSidedPlaneShape"] = empty;
+
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = node.as<std::shared_ptr<Shape>>());
+
+ EXPECT_EQ("SurgSim::Math::DoubleSidedPlaneShape", shape->getClassName());
+ EXPECT_TRUE(shape->isValid());
+ }
+
+ {
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = Shape::getFactory().create("SurgSim::Math::DoubleSidedPlaneShape"));
+ EXPECT_TRUE(shape->isValid());
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = shape);
+
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ ASSERT_TRUE(node["SurgSim::Math::DoubleSidedPlaneShape"].IsDefined());
+ auto data = node["SurgSim::Math::DoubleSidedPlaneShape"];
+ EXPECT_EQ(0u, data.size()); // DoubleSidedPlaneShape has no serialized property .
+
+ std::shared_ptr<DoubleSidedPlaneShape> doubleSidedPlaneShape;
+ ASSERT_NO_THROW(doubleSidedPlaneShape =
+ std::dynamic_pointer_cast<DoubleSidedPlaneShape>(node.as<std::shared_ptr<Shape>>()));
+ EXPECT_EQ("SurgSim::Math::DoubleSidedPlaneShape", doubleSidedPlaneShape->getClassName());
+ EXPECT_TRUE(doubleSidedPlaneShape->isValid());
+ }
+}
+
+TEST_F(ShapeTest, DoubleSidedPlaneShape)
+{
+ EXPECT_NO_THROW(DoubleSidedPlaneShape doubleSidedPlaneShape);
+ DoubleSidedPlaneShape doubleSidedPlaneShape;
+
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_DOUBLESIDEDPLANE, doubleSidedPlaneShape.getType());
+ EXPECT_NEAR(0.0, doubleSidedPlaneShape.getVolume(), epsilon);
+ EXPECT_TRUE(doubleSidedPlaneShape.getCenter().isZero());
+ EXPECT_TRUE(doubleSidedPlaneShape.getSecondMomentOfVolume().isApprox(Matrix33d::Zero()));
+ EXPECT_NEAR(0.0, doubleSidedPlaneShape.getD(), epsilon);
+ EXPECT_TRUE(doubleSidedPlaneShape.getNormal().isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE(doubleSidedPlaneShape.isValid());
+}
+
+
+TEST_F(ShapeTest, OctreeSerializationTest)
+{
+ const std::string fileName = "OctreeShapeData/staple.vox";
+ SurgSim::Framework::Runtime runtime("config.txt");
+
+ {
+ YAML::Node node;
+ node["SurgSim::Math::OctreeShape"]["FileName"] = fileName;
+
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = node.as<std::shared_ptr<Shape>>());
+
+ EXPECT_EQ("SurgSim::Math::OctreeShape", shape->getClassName());
+ EXPECT_EQ(fileName, shape->getValue<std::string>("FileName"));
+ EXPECT_TRUE(shape->isValid());
+ }
+
+ {
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = Shape::getFactory().create("SurgSim::Math::OctreeShape"));
+ shape->setValue("FileName", fileName);
+ EXPECT_TRUE(shape->isValid());
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = shape);
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ ASSERT_TRUE(node["SurgSim::Math::OctreeShape"].IsDefined());
+ auto data = node["SurgSim::Math::OctreeShape"];
+ EXPECT_EQ(1u, data.size());
+
+ std::shared_ptr<OctreeShape> newOctreeShape;
+ ASSERT_NO_THROW(newOctreeShape = std::dynamic_pointer_cast<OctreeShape>(node.as<std::shared_ptr<Shape>>()));
+ EXPECT_EQ("SurgSim::Math::OctreeShape", newOctreeShape->getClassName());
+ EXPECT_EQ(fileName, newOctreeShape->getFileName());
+ EXPECT_TRUE(newOctreeShape->isValid());
+ }
+}
+
+TEST_F(ShapeTest, OctreeShape)
+{
+ OctreeShape::NodeType::AxisAlignedBoundingBox boundingBox(Vector3d::Zero(), m_size);
+ std::shared_ptr<OctreeShape::NodeType> node = std::make_shared<OctreeShape::NodeType>(boundingBox);
+
+ {
+ ASSERT_NO_THROW({OctreeShape octree; EXPECT_FALSE(octree.isValid());});
+ ASSERT_NO_THROW({OctreeShape octree(*node); EXPECT_TRUE(octree.isValid());});
+ }
+
+ {
+ OctreeShape octree;
+ EXPECT_EQ(nullptr, octree.getRootNode());
+ octree.setRootNode(node);
+ EXPECT_EQ(node, octree.getRootNode());
+ EXPECT_TRUE(octree.isValid());
+ }
+
+ {
+ SurgSim::Framework::Runtime runtime("config.txt");
+ const std::string fileName = "OctreeShapeData/staple.vox";
+ OctreeShape octree;
+ EXPECT_NO_THROW(octree.setRootNode(node));
+ EXPECT_NO_THROW(octree.load(fileName));
+ EXPECT_EQ(fileName, octree.getFileName());
+
+ EXPECT_EQ(octree.getClassName(), "SurgSim::Math::OctreeShape");
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_OCTREE, octree.getType());
+ EXPECT_THROW(octree.getVolume(), SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(octree.getCenter().isApprox(Vector3d::Zero(), epsilon));
+ EXPECT_THROW(octree.getSecondMomentOfVolume(), SurgSim::Framework::AssertionFailure);
+ EXPECT_EQ(fileName, octree.getFileName());
+ EXPECT_TRUE(octree.isValid());
+ }
+
+ {
+ SCOPED_TRACE("Load nonexistent file will throw");
+ SurgSim::Framework::ApplicationData appData("config.txt");
+ const std::string fileName = "Nonexistent file";
+ OctreeShape octree;
+ EXPECT_ANY_THROW(octree.load(fileName, appData));
+ }
+
+ {
+ SCOPED_TRACE("Load existent file containing invalid Octree will throw");
+ SurgSim::Framework::ApplicationData appData("config.txt");
+ const std::string fileName = "OctreeShapeData/invalid-staple.vox";
+ OctreeShape octree;
+ EXPECT_ANY_THROW(octree.load(fileName, appData));
+ }
+}
+
+
+TEST_F(ShapeTest, PlaneShapeSerializationTest)
+{
+ {
+ YAML::Node node, empty;
+ node["SurgSim::Math::PlaneShape"] = empty;
+
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = node.as<std::shared_ptr<Shape>>());
+
+ EXPECT_EQ("SurgSim::Math::PlaneShape", shape->getClassName());
+ EXPECT_TRUE(shape->isValid());
+ }
+
+ {
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = Shape::getFactory().create("SurgSim::Math::PlaneShape"));
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = shape);
+
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ ASSERT_TRUE(node["SurgSim::Math::PlaneShape"].IsDefined());
+ auto data = node["SurgSim::Math::PlaneShape"];
+ EXPECT_EQ(0u, data.size()); //PlaneShape has no serialized property.
+
+ std::shared_ptr<PlaneShape> planeShape;
+ ASSERT_NO_THROW(planeShape = std::dynamic_pointer_cast<PlaneShape>(node.as<std::shared_ptr<Shape>>()));
+ EXPECT_EQ("SurgSim::Math::PlaneShape", planeShape->getClassName());
+ EXPECT_TRUE(planeShape->isValid());
+ }
+}
+
+TEST_F(ShapeTest, PlaneShape)
+{
+ EXPECT_NO_THROW(PlaneShape planeShape);
+ PlaneShape planeShape;
+
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_PLANE, planeShape.getType());
+ EXPECT_NEAR(0.0, planeShape.getVolume(), epsilon);
+ EXPECT_TRUE(planeShape.getCenter().isZero());
+ EXPECT_TRUE(planeShape.getSecondMomentOfVolume().isApprox(Matrix33d::Zero()));
+ EXPECT_NEAR(0.0, planeShape.getD(), epsilon);
+ EXPECT_TRUE(planeShape.getNormal().isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE(planeShape.isValid());
+}
\ No newline at end of file
diff --git a/SurgSim/Math/UnitTests/SurfaceMeshShapeTests.cpp b/SurgSim/Math/UnitTests/SurfaceMeshShapeTests.cpp
new file mode 100644
index 0000000..d8c1024
--- /dev/null
+++ b/SurgSim/Math/UnitTests/SurfaceMeshShapeTests.cpp
@@ -0,0 +1,144 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/SurfaceMeshShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::DataStructures::TriangleMeshPlain;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::SurfaceMeshShape;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector3d;
+
+class SurfaceMeshShapeTest : public ::testing::Test
+{
+public:
+
+ void SetUp()
+ {
+ m_radius = 2.34;
+ m_thickness = 1e-2;
+ m_center = Vector3d(1.3, 3.4, 5.6);
+ m_expectedVolume = M_PI * m_radius * m_radius * m_thickness;
+ m_expectedMatrix.setZero();
+ m_expectedMatrix(0, 0) = m_expectedVolume * m_radius * m_radius / 4.0;
+ m_expectedMatrix(1, 1) = m_expectedVolume * m_radius * m_radius / 4.0;
+ m_expectedMatrix(2, 2) = m_expectedVolume * m_radius * m_radius / 2.0;
+ }
+
+ void TearDown()
+ {
+ }
+
+ double m_radius;
+ double m_thickness;
+ Vector3d m_center;
+
+ double m_expectedVolume;
+ Matrix33d m_expectedMatrix;
+
+ TriangleMeshPlain buildDiskZ(const Quaterniond& q, const Vector3d& center, double radius) const
+ {
+ size_t totalNumNodes = 501; // 1 center + lots on perimeter
+ double deltaAngle = 2.0 * M_PI / static_cast<double>(totalNumNodes - 1);
+ TriangleMeshPlain disk;
+
+ // Add the center point
+ disk.addVertex(TriangleMeshPlain::VertexType(center));
+ // Add the peripheral points
+ for (size_t nodeId = 0; nodeId < totalNumNodes - 1; ++nodeId)
+ {
+ double angle = deltaAngle * nodeId;
+ Vector3d p(m_radius * cos(angle), m_radius * sin(angle), 0.0);
+ disk.addVertex(TriangleMeshPlain::VertexType(q._transformVector(p) + center));
+ }
+
+ // Define the triangles
+ for (size_t triId = 1; triId < totalNumNodes - 1; ++triId)
+ {
+ std::array<size_t, 3> indices = {{ 0, triId, triId + 1}};
+ disk.addTriangle(TriangleMeshPlain::TriangleType(indices));
+ }
+ return disk;
+ }
+};
+
+TEST_F(SurfaceMeshShapeTest, EmptyMeshTest)
+{
+ TriangleMeshPlain emptyMesh;
+ std::shared_ptr<SurfaceMeshShape> diskShape = std::make_shared<SurfaceMeshShape>(emptyMesh, m_thickness);
+
+ EXPECT_NEAR(0.0, diskShape->getVolume(), 1e-9);
+ EXPECT_TRUE(diskShape->getCenter().isZero());
+ EXPECT_TRUE(diskShape->getSecondMomentOfVolume().isZero());
+}
+
+TEST_F(SurfaceMeshShapeTest, DiskShapeTest)
+{
+ TriangleMeshPlain diskMesh = buildDiskZ(Quaterniond::Identity(), m_center, m_radius);
+ std::shared_ptr<SurfaceMeshShape> diskShape = std::make_shared<SurfaceMeshShape>(diskMesh, m_thickness);
+
+ EXPECT_NEAR(m_expectedVolume, diskShape->getVolume(), 1e-2);
+ EXPECT_TRUE(diskShape->getCenter().isApprox(m_center, 1e-2));
+ EXPECT_TRUE(diskShape->getSecondMomentOfVolume().isApprox(m_expectedMatrix, 1e-2));
+}
+
+TEST_F(SurfaceMeshShapeTest, NonAlignedDiskShapeTest)
+{
+ Quaterniond q(1.3, 5.3, -8.2, 2.4);
+ q.normalize();
+ TriangleMeshPlain diskMesh = buildDiskZ(q, m_center, m_radius);
+ std::shared_ptr<SurfaceMeshShape> diskShape = std::make_shared<SurfaceMeshShape>(diskMesh, m_thickness);
+
+ Matrix33d rotatedExpectedMatrix = q.toRotationMatrix() * m_expectedMatrix * q.toRotationMatrix().transpose();
+
+ EXPECT_NEAR(m_expectedVolume, diskShape->getVolume(), 1e-2);
+ EXPECT_TRUE(diskShape->getCenter().isApprox(m_center, 1e-2));
+ EXPECT_TRUE(diskShape->getSecondMomentOfVolume().isApprox(rotatedExpectedMatrix, 1e-2));
+}
+
+
+TEST_F(SurfaceMeshShapeTest, SerializationTest)
+{
+ const std::string fileName = "MeshShapeData/staple_collision.ply";
+ auto surfaceMeshShape = std::make_shared<SurgSim::Math::SurfaceMeshShape>();
+ surfaceMeshShape->setFileName(fileName);
+
+ // We chose to let YAML serialization only works with base class pointer.
+ // i.e. We need to serialize 'surfaceMeshShape' via a SurgSim::Math::Shape pointer.
+ // The usage YAML::Node node = surfaceMeshShape; will not compile.
+ std::shared_ptr<SurgSim::Math::Shape> shape = surfaceMeshShape;
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = shape); // YAML::convert<std::shared_ptr<SurgSim::Math::Shape>> will be called.
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ std::shared_ptr<SurgSim::Math::SurfaceMeshShape> newSurfaceMesh;
+ ASSERT_NO_THROW(newSurfaceMesh = std::dynamic_pointer_cast<SurgSim::Math::SurfaceMeshShape>(
+ node.as<std::shared_ptr<SurgSim::Math::Shape>>()));
+
+ EXPECT_EQ("SurgSim::Math::SurfaceMeshShape", newSurfaceMesh->getClassName());
+ EXPECT_EQ(fileName, newSurfaceMesh->getFileName());
+ EXPECT_EQ(surfaceMeshShape->getMesh()->getNumVertices(), newSurfaceMesh->getMesh()->getNumVertices());
+ EXPECT_EQ(surfaceMeshShape->getMesh()->getNumEdges(), newSurfaceMesh->getMesh()->getNumEdges());
+ EXPECT_EQ(surfaceMeshShape->getMesh()->getNumTriangles(), newSurfaceMesh->getMesh()->getNumTriangles());
+}
diff --git a/SurgSim/Math/UnitTests/TriangleTriangleContactCalculationTests.cpp b/SurgSim/Math/UnitTests/TriangleTriangleContactCalculationTests.cpp
new file mode 100644
index 0000000..a391511
--- /dev/null
+++ b/SurgSim/Math/UnitTests/TriangleTriangleContactCalculationTests.cpp
@@ -0,0 +1,184 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/UnitTests/TriangleTriangleTestParameters.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+class TriangleTriangleContactCalculationTest : public ::testing::Test, public TriangleTriangleTestParameters
+{
+protected:
+ void checkEqual(const Vector3d& v1, const Vector3d& v2)
+ {
+ if (v1.isZero(Geometry::DistanceEpsilon))
+ {
+ EXPECT_TRUE(v2.isZero(Geometry::DistanceEpsilon));
+ }
+ else
+ {
+ EXPECT_TRUE(v1.isApprox(v2, Geometry::DistanceEpsilon));
+ }
+ }
+
+ void testTriangleTriangleContactCalculation(const TriangleTriangleTestCase& data)
+ {
+ SCOPED_TRACE(std::get<0>(data));
+
+ MockTriangle t0 = std::get<1>(data);
+ MockTriangle t1 = std::get<2>(data);
+ bool contactExpected = std::get<3>(data);
+ bool checkForPenetrationPoints = std::get<4>(data);
+ Vector3d expectedT0Point = std::get<5>(data);
+ Vector3d expectedT1Point = std::get<6>(data);
+
+ double expectedPenetrationDepth = (expectedT1Point - expectedT0Point).norm();
+ double penetrationDepth;
+ Vector3d t0Point, t1Point, normal;
+ bool contactFound = false;
+ std::string traceMessage[6] = {"Normal Test",
+ "Shift t0 edges once",
+ "Shift t0 edges twice",
+ "Switched triangles: Normal Test",
+ "Switched triangles: Shift t1 edges once",
+ "Switched triangles: Shift t1 edges twise"
+ };
+ for (int count = 0; count < 6; ++count)
+ {
+ SCOPED_TRACE(traceMessage[count]);
+
+ switch (count)
+ {
+ case 0:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangle(t0.v0, t0.v1, t0.v2, t1.v0, t1.v1, t1.v2,
+ &penetrationDepth, &t0Point, &t1Point, &normal););
+ break;
+ case 1:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangle(t0.v1, t0.v2, t0.v0, t1.v0, t1.v1, t1.v2,
+ &penetrationDepth, &t0Point, &t1Point, &normal););
+ break;
+ case 2:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangle(t0.v2, t0.v0, t0.v1, t1.v0, t1.v1, t1.v2,
+ &penetrationDepth, &t0Point, &t1Point, &normal););
+ break;
+ case 3:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangle(t1.v0, t1.v1, t1.v2, t0.v0, t0.v1, t0.v2,
+ &penetrationDepth, &t1Point, &t0Point, &normal););
+ break;
+ case 4:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangle(t1.v1, t1.v2, t1.v0, t0.v0, t0.v1, t0.v2,
+ &penetrationDepth, &t1Point, &t0Point, &normal););
+ break;
+ case 5:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangle(t1.v2, t1.v0, t1.v1, t0.v0, t0.v1, t0.v2,
+ &penetrationDepth, &t1Point, &t0Point, &normal););
+ break;
+ }
+
+ EXPECT_EQ(contactExpected, contactFound);
+ if (contactFound)
+ {
+ if (checkForPenetrationPoints && count < 3)
+ {
+ EXPECT_NEAR(expectedPenetrationDepth, penetrationDepth, Geometry::DistanceEpsilon);
+ checkEqual(expectedT0Point, t0Point);
+ checkEqual(expectedT1Point, t1Point);
+ }
+
+ // Check that t0Point is on the plane of t0.
+ double t0SignedDistance = std::abs(t0Point.dot(t0.n) - t0.v0.dot(t0.n));
+ EXPECT_NEAR(t0SignedDistance, 0.0, Geometry::DistanceEpsilon);
+
+ // Check that t1Point is on the plane of t1.
+ double t1SignedDistance = std::abs(t1Point.dot(t1.n) - t1.v0.dot(t1.n));
+ EXPECT_NEAR(t1SignedDistance, 0.0, Geometry::DistanceEpsilon);
+
+ // Check that t0Point is inside t0.
+ Vector3d bary0;
+ barycentricCoordinates(t0Point, t0.v0, t0.v1, t0.v2, &bary0);
+ bool isBary0WithinTriangle =
+ bary0[0] >= -Geometry::DistanceEpsilon && bary0[0] <= (1.0 + Geometry::DistanceEpsilon) &&
+ bary0[1] >= -Geometry::DistanceEpsilon && bary0[1] <= (1.0 + Geometry::DistanceEpsilon) &&
+ bary0[2] >= -Geometry::DistanceEpsilon && bary0[2] <= (1.0 + Geometry::DistanceEpsilon);
+ EXPECT_TRUE(isBary0WithinTriangle);
+
+ // Check that t1Point is inside t1.
+ Vector3d bary1;
+ barycentricCoordinates(t1Point, t1.v0, t1.v1, t1.v2, &bary1);
+ bool isBary1WithinTriangle =
+ bary1[0] >= -Geometry::DistanceEpsilon && bary1[0] <= (1.0 + Geometry::DistanceEpsilon) &&
+ bary1[1] >= -Geometry::DistanceEpsilon && bary1[1] <= (1.0 + Geometry::DistanceEpsilon) &&
+ bary1[2] >= -Geometry::DistanceEpsilon && bary1[2] <= (1.0 + Geometry::DistanceEpsilon);
+ EXPECT_TRUE(isBary1WithinTriangle);
+
+ // Check if the penetration depth when applied as correction, separates the triangles.
+ // First move the triangles apart by just short of the penetration depth, to make sure
+ // the triangles are still colliding.
+ {
+ Vector3d correction = normal * (0.5 * penetrationDepth - Geometry::DistanceEpsilon);
+ if (count > 2)
+ {
+ // Switched triangles.
+ correction = -correction;
+ }
+ MockTriangle correctedT0(t0);
+ correctedT0.translate(correction);
+ MockTriangle correctedT1(t1);
+ correctedT1.translate(-correction);
+ EXPECT_TRUE(doesIntersectTriangleTriangle(correctedT0.v0, correctedT0.v1, correctedT0.v2,
+ correctedT1.v0, correctedT1.v1, correctedT1.v2));
+ }
+ // Now move the triangles apart by just a little farther than the penetration depth, to establish
+ // that the triangles are not colliding.
+ {
+ Vector3d correction = normal * (0.5 * penetrationDepth + Geometry::DistanceEpsilon);
+ if (count > 2)
+ {
+ // Switched triangles.
+ correction = -correction;
+ }
+ MockTriangle correctedT0(t0);
+ correctedT0.translate(correction);
+ MockTriangle correctedT1(t1);
+ correctedT1.translate(-correction);
+ EXPECT_FALSE(doesIntersectTriangleTriangle(correctedT0.v0, correctedT0.v1, correctedT0.v2,
+ correctedT1.v0, correctedT1.v1, correctedT1.v2));
+ }
+ }
+ }
+ }
+};
+
+TEST_F(TriangleTriangleContactCalculationTest, TestCases)
+{
+ for (auto it = m_testCases.begin(); it != m_testCases.end(); ++it)
+ {
+ testTriangleTriangleContactCalculation(*it);
+ }
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Math/UnitTests/TriangleTriangleIntersectionTests.cpp b/SurgSim/Math/UnitTests/TriangleTriangleIntersectionTests.cpp
new file mode 100644
index 0000000..9b59fd3
--- /dev/null
+++ b/SurgSim/Math/UnitTests/TriangleTriangleIntersectionTests.cpp
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/UnitTests/TriangleTriangleTestParameters.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+class TriangleTriangleIntersectionTest : public ::testing::Test, public TriangleTriangleTestParameters
+{
+};
+
+TEST_F(TriangleTriangleIntersectionTest, TestCases)
+{
+ for (auto it = m_testCases.begin(); it != m_testCases.end(); ++it)
+ {
+ SCOPED_TRACE(std::get<0>(*it));
+
+ MockTriangle t0 = std::get<1>(*it);
+ MockTriangle t1 = std::get<2>(*it);
+ bool intersectionExpected = std::get<3>(*it);
+
+ EXPECT_EQ(intersectionExpected, doesIntersectTriangleTriangle(t0.v0, t0.v1, t0.v2, t1.v0, t1.v1, t1.v2));
+ EXPECT_EQ(intersectionExpected, doesIntersectTriangleTriangle(t0.v1, t0.v2, t0.v0, t1.v0, t1.v1, t1.v2));
+ EXPECT_EQ(intersectionExpected, doesIntersectTriangleTriangle(t0.v2, t0.v0, t0.v1, t1.v0, t1.v1, t1.v2));
+ EXPECT_EQ(intersectionExpected, doesIntersectTriangleTriangle(t1.v0, t1.v1, t1.v2, t0.v0, t0.v1, t0.v2));
+ EXPECT_EQ(intersectionExpected, doesIntersectTriangleTriangle(t1.v1, t1.v2, t1.v0, t0.v0, t0.v1, t0.v2));
+ EXPECT_EQ(intersectionExpected, doesIntersectTriangleTriangle(t1.v2, t1.v0, t1.v1, t0.v0, t0.v1, t0.v2));
+ }
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Math/UnitTests/TriangleTriangleTestParameters.h b/SurgSim/Math/UnitTests/TriangleTriangleTestParameters.h
new file mode 100644
index 0000000..3ddb889
--- /dev/null
+++ b/SurgSim/Math/UnitTests/TriangleTriangleTestParameters.h
@@ -0,0 +1,244 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License";
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_MATH_UNITTESTS_TRIANGLETRIANGLETESTPARAMETERS_H
+#define SURGSIM_MATH_UNITTESTS_TRIANGLETRIANGLETESTPARAMETERS_H
+
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/UnitTests/MockTriangle.h"
+#include "SurgSim/Math/Geometry.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// A base class for triangle-triangle tests, which has a preset list of test cases.
+class TriangleTriangleTestParameters
+{
+protected:
+ typedef std::tuple<std::string, // String to describe the scenario.
+ MockTriangle, // The first triangle.
+ MockTriangle, // The second triangle.
+ bool, // Flag to indicate if the two triangles are expected to be found intersecting.
+ bool, // Flag to indicate if expected contact info is available to check against.
+ Vector3d, // Expected penetration point in the first triangle.
+ Vector3d> // Expected penetration point in the second triangle.
+ TriangleTriangleTestCase;
+
+ /// A list of common test cases.
+ std::vector<TriangleTriangleTestCase> m_testCases;
+
+ /// Default constructor.
+ TriangleTriangleTestParameters()
+ {
+ double d = 5.0 * Geometry::DistanceEpsilon;
+ MockTriangle t0(Vector3d(-5, 0, 0), Vector3d(0, 10, 0), Vector3d(5, 0, 0));
+ MockTriangle t1;
+ {
+ std::string scenario = "vertex t1v0 inside t0v0";
+ t1 = MockTriangle(t0.v0 + Vector3d(0, 0, d), t0.v1 + t0.n * 2, t0.v2 + t0.n * 2);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t1, t0, true, true, t1.v0, t0.v0));
+ }
+ {
+ std::string scenario = "vertex t1v0 inside of triangle t0";
+ Vector3d t1v0 = t0.pointInTriangle(0.2, 0.2);
+ t1 = MockTriangle(t1v0 + Vector3d(0, 0, d), t0.v1 + t0.n * 2, t0.v2 + t0.n * 2);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t1, t0, true, true, t1.v0, t1v0));
+ }
+ {
+ std::string scenario = "vertex t1v0, t1v1 inside of triangle t0, at same depth";
+ Vector3d t1v0 = t0.pointInTriangle(0.2, 0.2);
+ Vector3d t1v1 = t0.pointInTriangle(0.4, 0.4);
+ t1 = MockTriangle(t1v0 + Vector3d(0, 0, d), t1v1 + Vector3d(0, 0, d), t0.v2 + t0.n * 2);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t1, t0, true, false, t1.v1, t1v1));
+ }
+ {
+ std::string scenario = "vertex t1v0, t1v1 inside of triangle t0, depth of t1v0 < t1v1";
+ Vector3d t1v0 = t0.pointInTriangle(0.2, 0.2);
+ Vector3d t1v1 = t0.pointInTriangle(0.4, 0.4);
+ t1 = MockTriangle(t1v0 + Vector3d(0, 0, d), t1v1 + Vector3d(0, 0, d * 2.0), t0.v2 + t0.n * 2);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t1, t0, true, true, t1.v1, t1v1));
+ }
+ {
+ std::string scenario = "vertex t1v0, t1v1 inside of triangle t0, depth of t1v0 > t1v1";
+ Vector3d t1v0 = t0.pointInTriangle(0.2, 0.2);
+ Vector3d t1v1 = t0.pointInTriangle(0.4, 0.4);
+ t1 = MockTriangle(t1v0 + Vector3d(0, 0, d * 2.0), t1v1 + Vector3d(0, 0, d), t0.v2 + t0.n * 2);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t1, t0, true, true, t1.v0, t1v0));
+ }
+ {
+ std::string scenario = "vertex t1v0 close to t0v0";
+ t1 = MockTriangle(t0.v0 + t0.n, t0.v1 + t0.n * 2, t0.v2 + t0.n * 2);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t1, t0, false, false, t1.v0, t0.v0));
+ }
+ {
+ std::string scenario = "vertex t1v0 close to the inside of triangle t0";
+ Vector3d intersection = t0.pointInTriangle(0.2, 0.2);
+ t1 = MockTriangle(intersection + t0.n , t0.v1 + t0.n * 2, t0.v2 + t0.n * 2);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t1, t0, false, false, t1.v0, intersection));
+ }
+ {
+ std::string scenario = "edge t1v0v1 through triangle t0";
+ Vector3d t1v0 = t0.pointInTriangle(0.2, 0.2);
+ t1 = MockTriangle(t1v0 + t0.n * 3, t0.v0 - t0.v0v2 * 4 + t0.n, t1v0 - t0.n * 4);
+ Vector3d t0p;
+ Vector3d t1p;
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, false, t0p, t1p));
+ }
+ {
+ std::string scenario = "Triangles parallel";
+ t1 = MockTriangle(t0.v0 + t0.n * 3, t0.v1 + t0.n * 3, t0.v2 + t0.n * 3);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t1, t0, false, false, t1.v0, t1.v0));
+ }
+
+ MockTriangle T0(Vector3d(-5.0, 0, 0), Vector3d(5, 0, 0), Vector3d(0, 10, 0));
+ T0.translate(Vector3d(0, -3.333333333, 0));
+ MockTriangle T1(Vector3d(-5.0, 0, 0), Vector3d(5, 0, 0), Vector3d(0, 10, 0));
+ T1.translate(Vector3d(0, -10, 0));
+ T1.rotateAboutXBy(-90.0);
+
+ {
+ std::string scenario = "vertex t1v0 inside t0 - 1";
+ MockTriangle t0(T0);
+ Vector3d t0p(0, 0, 0);
+ MockTriangle t1(T1);
+ t1.translate(Vector3d(0.0, 0.0, -0.1));
+ Vector3d t1p(0, 0, -0.1);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, true, t0p, t1p));
+ }
+ {
+ std::string scenario = "vertex t1v0 inside t0 - 2";
+ MockTriangle t0(T0);
+ Vector3d t0p;
+ MockTriangle t1(T1);
+ t1.translate(Vector3d(0.0, -3.3, -0.1));
+ Vector3d t1p;
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, false, t0p, t1p));
+ }
+ {
+ std::string scenario = "vertex t1v0 inside t0 - 3";
+ MockTriangle t0(T0);
+ Vector3d t0p(0, -3.22222222, 0);
+ MockTriangle t1(T1);
+ t1.translate(Vector3d(0.0, -3.22222222, -0.1));
+ Vector3d t1p(0, -3.22222222, -0.1);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, true, t0p, t1p));
+ }
+ {
+ std::string scenario = "vertex t1v0 inside t0 - 4";
+ MockTriangle t0(T0);
+ Vector3d t0p(0, 0, 0);
+ MockTriangle t1(T1);
+ t1.translate(Vector3d(0.0, 0.0, -0.6));
+ Vector3d t1p(0, 0, -0.6);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, true, t0p, t1p));
+ }
+ {
+ std::string scenario = "vertex t1v0 inside t0 - 5";
+ MockTriangle t0(T0);
+ Vector3d t0p(0, 0, 0);
+ MockTriangle t1(T1);
+ t1.rotateAboutZBy(180.0);
+ t1.translate(Vector3d(0.0, 0.0, -6.6));
+ Vector3d t1p(0, 0, -6.6);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, true, t0p, t1p));
+ }
+ {
+ std::string scenario = "vertex t1v0 inside t0 - 6";
+ MockTriangle t0(T0);
+ Vector3d t0p(t0.v2);
+ MockTriangle t1(T1);
+ t1.rotateAboutZBy(180.0);
+ t1.translate(Vector3d(0.0, 0.0, -6.7));
+ Vector3d t1p(0, 0, 0);
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, true, t0p, t1p));
+ }
+ {
+ std::string scenario = "edge (t1v0,t1v1) inside t0 - 1";
+ MockTriangle t0(T0);
+ Vector3d t0p;
+ MockTriangle t1(T1);
+ t1.translate(Vector3d(-3.0, 0.0, -6.0));
+ Vector3d t1p;
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, false, t0p, t1p));
+ }
+ {
+ std::string scenario = "edge (t1v0,t1v1) inside t0 - 2";
+ MockTriangle t0(T0);
+ Vector3d t0p;
+ MockTriangle t1(T1);
+ t1.rotateAboutZBy(90.0);
+ t1.translate(Vector3d(0.0, -3.5, -9.0));
+ Vector3d t1p;
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, false, t0p, t1p));
+ }
+ {
+ std::string scenario = "edge (t1v0,t1v1) inside t0 - 3";
+ MockTriangle t0(T0);
+ Vector3d t0p;
+ MockTriangle t1(T1);
+ t1.rotateAboutYBy(180.0);
+ t1.rotateAboutZBy(90.0);
+ t1.translate(Vector3d(0.0, -4.0, 6.0));
+ Vector3d t1p;
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, false, t0p, t1p));
+ }
+ {
+ std::string scenario = "edge (t1v0,t1v1) inside t0 - 4";
+ MockTriangle t0(T0);
+ Vector3d t0p;
+ MockTriangle t1(T1);
+ t1.rotateAboutYBy(180.0);
+ t1.rotateAboutZBy(90.0);
+ t1.translate(Vector3d(4.0, -4.0, 6.0));
+ Vector3d t1p;
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, false, t0p, t1p));
+ }
+ {
+ std::string scenario = "Failed case in Stapler demo - 1";
+ MockTriangle t0(Vector3d(-0.0063320380397585046, 0.0028276973112210521, 0.014661107730129588),
+ Vector3d(-0.012108385700376603, 0.0012180224983028599, 0.011926511653863735),
+ Vector3d(-0.016994324947197881, -0.011073183474260971, 0.022191024814086323));
+ Vector3d t0p;
+ MockTriangle t1(Vector3d(-0.031071999999999999, 0.0028570000000000002, 0.012547000000000001),
+ Vector3d(-0.0016770000000000001, -0.0010070000000000001, 0.0048640000000000003),
+ Vector3d(-0.0013829999999999999, -0.0010030000000000000, 0.012973000000000000));
+ Vector3d t1p;
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, false, t0p, t1p));
+ }
+ {
+ std::string scenario = "Failed case in Stapler demo - 2";
+ MockTriangle t0(Vector3d(3.4602053093157404, -1.1441614263267368, 37.870346680755349),
+ Vector3d(3.3720821269094003, -0.20927613787449697, 118.95490947665477),
+ Vector3d(3.2033802246727975, -36.106495941162471, 119.36861234419522));
+ Vector3d t0p;
+ MockTriangle t1(Vector3d(0.50000000000000002, 3.8999999999999999, 53.099999999999996),
+ Vector3d(0.50000000000000002, -3.8999999999999999, 53.099999999999996),
+ Vector3d(21.299999999999999, 0.00000000000000000, 53.299999999999997));
+ Vector3d t1p;
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, false, t0p, t1p));
+ }
+ }
+};
+
+
+} // namespace Math
+
+} // namespace SurgSim
+
+#endif // SURGSIM_MATH_UNITTESTS_TRIANGLETRIANGLETESTPARAMETERS_H
\ No newline at end of file
diff --git a/SurgSim/Math/UnitTests/ValidTests.cpp b/SurgSim/Math/UnitTests/ValidTests.cpp
new file mode 100644
index 0000000..5e8520b
--- /dev/null
+++ b/SurgSim/Math/UnitTests/ValidTests.cpp
@@ -0,0 +1,779 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 that exercise the isValid() functions.
+
+#include <limits>
+#include <iostream>
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Valid.h"
+#include <Eigen/Core>
+#include "gtest/gtest.h"
+
+// Define test fixture class templates.
+// We don't really need fixtures as such, but the templatization encodes type.
+
+template <class T>
+class ValidTests : public testing::Test
+{
+public:
+ typedef T Scalar;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) matrix type aliases, but we got rid of those.
+typedef ::testing::Types<double, float> FloatingPointVariants;
+TYPED_TEST_CASE(ValidTests, FloatingPointVariants);
+
+// Now we're ready to start testing...
+
+
+TYPED_TEST(ValidTests, ValidScalars)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::isValid;
+
+ EXPECT_TRUE(isValid(static_cast<Scalar>(0)));
+ EXPECT_TRUE(isValid(static_cast<Scalar>(1)));
+ EXPECT_TRUE(isValid(std::numeric_limits<Scalar>::denorm_min()));
+ EXPECT_FALSE(isValid(std::numeric_limits<Scalar>::quiet_NaN()));
+ EXPECT_FALSE(isValid(std::numeric_limits<Scalar>::signaling_NaN()));
+ EXPECT_FALSE(isValid(std::numeric_limits<Scalar>::infinity()));
+}
+
+TYPED_TEST(ValidTests, SubnormalScalars)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::isSubnormal;
+
+ EXPECT_FALSE(isSubnormal(static_cast<Scalar>(0)));
+ EXPECT_FALSE(isSubnormal(static_cast<Scalar>(1)));
+ EXPECT_TRUE(isSubnormal(std::numeric_limits<Scalar>::denorm_min()));
+ EXPECT_FALSE(isSubnormal(std::numeric_limits<Scalar>::quiet_NaN()));
+ EXPECT_FALSE(isSubnormal(std::numeric_limits<Scalar>::signaling_NaN()));
+ EXPECT_FALSE(isSubnormal(std::numeric_limits<Scalar>::infinity()));
+}
+
+TYPED_TEST(ValidTests, SubnormalArithmetic)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::isSubnormal;
+
+ Scalar x = 1;
+ EXPECT_FALSE(isSubnormal(x));
+
+ int normalSteps;
+ for (normalSteps = 0; normalSteps < 1000000; ++normalSteps)
+ {
+ if (isSubnormal(x) || (x == 0))
+ {
+ break;
+ }
+ x /= 2;
+ }
+ EXPECT_GT(normalSteps, 0);
+
+ int subnormalSteps;
+ for (subnormalSteps = 0; subnormalSteps < 1000000; ++subnormalSteps)
+ {
+ if (!isSubnormal(x) || (x == 0))
+ {
+ break;
+ }
+ x /= 2;
+ }
+ EXPECT_GT(subnormalSteps, 0);
+}
+
+template <typename T>
+static void matrixCheckHelper(const T& validMatrix)
+{
+ // Assumes T is a matrix, 2x2 or larger
+
+ typedef T Matrix;
+ typedef typename Matrix::Scalar Scalar;
+
+ using SurgSim::Math::isValid;
+ using SurgSim::Math::isSubnormal;
+
+ {
+ Matrix matrix = validMatrix;
+ EXPECT_TRUE(isValid(matrix));
+ EXPECT_FALSE(isSubnormal(matrix));
+ matrix(0, 1) = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(isValid(matrix));
+ EXPECT_TRUE(isSubnormal(matrix));
+ }
+ {
+ Matrix matrix = validMatrix;
+ EXPECT_TRUE(isValid(matrix));
+ EXPECT_FALSE(isSubnormal(matrix));
+ matrix(0, 0) = std::numeric_limits<Scalar>::infinity();
+ EXPECT_FALSE(isValid(matrix));
+ EXPECT_FALSE(isSubnormal(matrix));
+ matrix(1, 1) = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_FALSE(isValid(matrix));
+ EXPECT_TRUE(isSubnormal(matrix));
+ }
+ {
+ Matrix matrix = validMatrix;
+ EXPECT_TRUE(isValid(matrix));
+ EXPECT_FALSE(isSubnormal(matrix));
+ matrix(1, 0) = std::numeric_limits<Scalar>::quiet_NaN();
+ EXPECT_FALSE(isValid(matrix));
+ EXPECT_FALSE(isSubnormal(matrix));
+ }
+}
+
+TYPED_TEST(ValidTests, MatrixChecks)
+{
+ typedef typename TestFixture::Scalar Scalar;
+
+ {
+ Eigen::Matrix<Scalar, 2, 2, Eigen::RowMajor> matrix;
+ matrix.setIdentity();
+ matrixCheckHelper(matrix);
+ matrix.setZero();
+ matrixCheckHelper(matrix);
+ }
+ {
+ Eigen::Matrix<Scalar, 3, 3, Eigen::ColMajor> matrix;
+ matrix.setIdentity();
+ matrixCheckHelper(matrix);
+ }
+ {
+ Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> matrix;
+ matrix.setIdentity();
+ matrixCheckHelper(matrix);
+ }
+ {
+ Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor> matrix;
+ matrix.setIdentity();
+ matrixCheckHelper(matrix);
+ }
+
+ {
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> matrix;
+ matrix.setIdentity(11, 11);
+ matrixCheckHelper(matrix);
+ }
+}
+
+template <typename T>
+static void vectorCheckHelper(const T& validVector)
+{
+ // Assumes T is a vector, size 2 or larger
+
+ typedef T Vector;
+ typedef typename Vector::Scalar Scalar;
+
+ using SurgSim::Math::isValid;
+ using SurgSim::Math::isSubnormal;
+
+ {
+ Vector vector = validVector;
+ EXPECT_TRUE(isValid(vector));
+ EXPECT_FALSE(isSubnormal(vector));
+ vector[0] = static_cast<Scalar>(1);
+ EXPECT_TRUE(isValid(vector));
+ EXPECT_FALSE(isSubnormal(vector));
+ }
+ {
+ Vector vector = validVector;
+ EXPECT_TRUE(isValid(vector));
+ EXPECT_FALSE(isSubnormal(vector));
+ vector[1] = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(isValid(vector));
+ EXPECT_TRUE(isSubnormal(vector));
+ }
+ {
+ Vector vector = validVector;
+ EXPECT_TRUE(isValid(vector));
+ EXPECT_FALSE(isSubnormal(vector));
+ vector[0] = std::numeric_limits<Scalar>::infinity();
+ EXPECT_FALSE(isValid(vector));
+ EXPECT_FALSE(isSubnormal(vector));
+ vector[1] = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_FALSE(isValid(vector));
+ EXPECT_TRUE(isSubnormal(vector));
+ }
+ {
+ Vector vector = validVector;
+ EXPECT_TRUE(isValid(vector));
+ EXPECT_FALSE(isSubnormal(vector));
+ vector[1] = std::numeric_limits<Scalar>::quiet_NaN();
+ EXPECT_FALSE(isValid(vector));
+ EXPECT_FALSE(isSubnormal(vector));
+ }
+}
+
+TYPED_TEST(ValidTests, VectorChecks)
+{
+ typedef typename TestFixture::Scalar Scalar;
+
+ {
+ Eigen::Matrix<Scalar, 2, 1> vector;
+ vector.setZero();
+ vectorCheckHelper(vector);
+ vector.setZero();
+ vectorCheckHelper(vector);
+ }
+ {
+ Eigen::Matrix<Scalar, 3, 1> vector;
+ vector.setZero();
+ vectorCheckHelper(vector);
+ }
+ {
+ Eigen::Matrix<Scalar, 4, 1> vector;
+ vector.setZero();
+ vectorCheckHelper(vector);
+ }
+ {
+ Eigen::Matrix<Scalar, 4, 1, Eigen::AutoAlign> vector;
+ vector.setZero();
+ vectorCheckHelper(vector);
+ }
+
+ {
+ Eigen::Matrix<Scalar, Eigen::Dynamic, 1> vector;
+ vector.setZero(11);
+ vectorCheckHelper(vector);
+ }
+}
+
+TYPED_TEST(ValidTests, QuaternionChecks)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::isValid;
+ using SurgSim::Math::isSubnormal;
+
+ Eigen::Quaternion<Scalar> quaternion(1, 0, 0, 0);
+ EXPECT_TRUE(isValid(quaternion));
+ EXPECT_FALSE(isSubnormal(quaternion));
+
+ quaternion.x() = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(isValid(quaternion));
+ EXPECT_TRUE(isSubnormal(quaternion));
+
+ quaternion = Eigen::Quaternion<Scalar>(std::numeric_limits<Scalar>::infinity(), 0, 0, 0);
+ EXPECT_FALSE(isValid(quaternion));
+ EXPECT_FALSE(isSubnormal(quaternion));
+
+ quaternion.z() = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_FALSE(isValid(quaternion));
+ EXPECT_TRUE(isSubnormal(quaternion));
+}
+
+TYPED_TEST(ValidTests, AngleAxisChecks)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::isValid;
+ using SurgSim::Math::isSubnormal;
+
+ Eigen::AngleAxis<Scalar> rotation = Eigen::AngleAxis<Scalar>::Identity();
+ EXPECT_TRUE(isValid(rotation));
+ EXPECT_FALSE(isSubnormal(rotation));
+
+ rotation.angle() = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(isValid(rotation));
+ EXPECT_TRUE(isSubnormal(rotation));
+
+ rotation = Eigen::AngleAxis<Scalar>::Identity();
+ rotation.axis()[2] = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(isValid(rotation));
+ EXPECT_TRUE(isSubnormal(rotation));
+
+ rotation = Eigen::AngleAxis<Scalar>::Identity();
+ rotation.angle() = std::numeric_limits<Scalar>::infinity();
+ EXPECT_FALSE(isValid(rotation));
+ EXPECT_FALSE(isSubnormal(rotation));
+ rotation.axis()[1] = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_FALSE(isValid(rotation));
+ EXPECT_TRUE(isSubnormal(rotation));
+
+ rotation = Eigen::AngleAxis<Scalar>::Identity();
+ rotation.axis()[0] = std::numeric_limits<Scalar>::quiet_NaN();
+ EXPECT_FALSE(isValid(rotation));
+ EXPECT_FALSE(isSubnormal(rotation));
+ rotation.angle() = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_FALSE(isValid(rotation));
+ EXPECT_TRUE(isSubnormal(rotation));
+}
+
+TYPED_TEST(ValidTests, Rotation2DChecks)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::isValid;
+ using SurgSim::Math::isSubnormal;
+
+ Eigen::Rotation2D<Scalar> rotation(0);
+ EXPECT_TRUE(isValid(rotation));
+ EXPECT_FALSE(isSubnormal(rotation));
+
+ rotation.angle() = static_cast<Scalar>(1);
+ EXPECT_TRUE(isValid(rotation));
+ EXPECT_FALSE(isSubnormal(rotation));
+
+ rotation.angle() = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(isValid(rotation));
+ EXPECT_TRUE(isSubnormal(rotation));
+
+ rotation = Eigen::Rotation2D<Scalar>(std::numeric_limits<Scalar>::infinity());
+ EXPECT_FALSE(isValid(rotation));
+ EXPECT_FALSE(isSubnormal(rotation));
+}
+
+template <typename T>
+static void transformCheckHelper()
+{
+ // Assumes T is an Eigen::Transform type of some sort
+
+ typedef T Transform;
+ typedef typename Transform::Scalar Scalar;
+
+ using SurgSim::Math::isValid;
+ using SurgSim::Math::isSubnormal;
+
+ Transform transform = Transform::Identity();
+ EXPECT_TRUE(isValid(transform));
+ EXPECT_FALSE(isSubnormal(transform));
+
+ transform(1, 1) = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(isValid(transform));
+ EXPECT_TRUE(isSubnormal(transform));
+
+ transform = Transform::Identity();
+ transform(0, 0) = std::numeric_limits<Scalar>::quiet_NaN();
+ EXPECT_FALSE(isValid(transform));
+ EXPECT_FALSE(isSubnormal(transform));
+
+ transform(0, 1) = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_FALSE(isValid(transform));
+ EXPECT_TRUE(isSubnormal(transform));
+}
+
+TYPED_TEST(ValidTests, TransformChecks)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::isValid;
+ using SurgSim::Math::isSubnormal;
+
+ transformCheckHelper<Eigen::Transform<Scalar, 2, Eigen::Isometry>>();
+ transformCheckHelper<Eigen::Transform<Scalar, 3, Eigen::Isometry>>();
+ transformCheckHelper<Eigen::Transform<Scalar, 4, Eigen::Isometry>>();
+ transformCheckHelper<Eigen::Transform<Scalar, 4, Eigen::Isometry>>();
+ transformCheckHelper<Eigen::Transform<Scalar, 4, Eigen::Affine>>();
+ transformCheckHelper<Eigen::Transform<Scalar, 4, Eigen::AffineCompact>>();
+}
+
+TYPED_TEST(ValidTests, ClearSubnormalScalars)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::setSubnormalToZero;
+ using SurgSim::Math::isValid;
+ Scalar x;
+
+ x = 0;
+ EXPECT_FALSE(setSubnormalToZero(&x));
+ EXPECT_EQ(0, x);
+
+ x = -1;
+ EXPECT_FALSE(setSubnormalToZero(&x));
+ EXPECT_EQ(-1, x);
+
+ x = std::numeric_limits<Scalar>::infinity();
+ EXPECT_FALSE(setSubnormalToZero(&x));
+ EXPECT_FALSE(isValid(x));
+
+ x = std::numeric_limits<Scalar>::quiet_NaN();
+ EXPECT_FALSE(setSubnormalToZero(&x));
+ EXPECT_FALSE(isValid(x));
+
+ x = std::numeric_limits<Scalar>::signaling_NaN();
+ EXPECT_FALSE(setSubnormalToZero(&x));
+ EXPECT_FALSE(isValid(x));
+
+ x = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(setSubnormalToZero(&x));
+ EXPECT_EQ(0, x);
+}
+
+template <typename T>
+static void compareMatrices(const T& a, const T& b)
+{
+ typedef T Matrix;
+ typedef typename Matrix::Index Index;
+
+ EXPECT_EQ(a.rows(), b.rows());
+ EXPECT_EQ(a.cols(), b.cols());
+
+ const Index numColumns = std::min(a.cols(), b.cols());
+ const Index numRows = std::min(a.rows(), b.rows());
+
+ for (Index j = 0; j < numColumns; ++j)
+ {
+ for (Index i = 0; i < numRows; ++i)
+ {
+ bool isValidAij = SurgSim::Math::isValid(a.coeff(i, j));
+ bool isValidBij = SurgSim::Math::isValid(b.coeff(i, j));
+ EXPECT_EQ(isValidAij, isValidBij) << "i = " << i << ", j = " << j <<
+ ", Aij = " << a.coeff(i, j) << ", Bij = " << b.coeff(i, j);
+ if (isValidAij && isValidBij)
+ {
+ // In general, floating point equality checks are bad, but here they are needed.
+ EXPECT_EQ(a.coeff(i, j), b.coeff(i, j)) << "i = " << i << ", j = " << j;
+ }
+ }
+ }
+}
+
+template <typename T>
+static void matrixSetSubnormalHelper(const T& validMatrix)
+{
+ // Assumes T is a matrix, 2x2 or larger
+
+ typedef T Matrix;
+ typedef typename Matrix::Scalar Scalar;
+
+ EXPECT_TRUE(SurgSim::Math::isValid(validMatrix));
+ EXPECT_FALSE(SurgSim::Math::isSubnormal(validMatrix));
+
+ using SurgSim::Math::setSubnormalToZero;
+
+ {
+ Matrix a = validMatrix;
+ Matrix b = a;
+ EXPECT_FALSE(setSubnormalToZero(&a));
+ EXPECT_FALSE(setSubnormalToZero(&b));
+ compareMatrices(a, b);
+ }
+ {
+ Matrix a = validMatrix;
+ a(0, 1) = 0;
+ Matrix b = a;
+ b(0, 1) = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_FALSE(setSubnormalToZero(&a));
+ EXPECT_TRUE(setSubnormalToZero(&b));
+ compareMatrices(a, b);
+ }
+ {
+ Matrix a = validMatrix;
+ a(0, 0) = std::numeric_limits<Scalar>::infinity();
+ Matrix b = a;
+ EXPECT_FALSE(setSubnormalToZero(&a));
+ EXPECT_FALSE(setSubnormalToZero(&b));
+ compareMatrices(a, b);
+ }
+ {
+ Matrix a = validMatrix;
+ a(1, 0) = std::numeric_limits<Scalar>::quiet_NaN();
+ a(1, 1) = 0;
+ Matrix b = a;
+ b(1, 1) = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_FALSE(setSubnormalToZero(&a));
+ EXPECT_TRUE(setSubnormalToZero(&b));
+ compareMatrices(a, b);
+ }
+}
+
+TYPED_TEST(ValidTests, ClearSubnormalMatrix)
+{
+ typedef typename TestFixture::Scalar Scalar;
+
+ {
+ Eigen::Matrix<Scalar, 2, 2, Eigen::RowMajor> matrix;
+ matrix.setConstant(123);
+ matrixSetSubnormalHelper(matrix);
+ matrix.setZero();
+ matrixSetSubnormalHelper(matrix);
+ }
+ {
+ Eigen::Matrix<Scalar, 3, 3, Eigen::ColMajor> matrix;
+ matrix.setConstant(123);
+ matrixSetSubnormalHelper(matrix);
+ }
+ {
+ Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> matrix;
+ matrix.setConstant(123);
+ matrixSetSubnormalHelper(matrix);
+ }
+ {
+ Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor> matrix;
+ matrix.setConstant(123);
+ matrixSetSubnormalHelper(matrix);
+ }
+
+ {
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> matrix;
+ matrix.setConstant(11, 13, static_cast<Scalar>(123));
+ matrixSetSubnormalHelper(matrix);
+ }
+}
+
+template <typename T>
+static void vectorSetSubnormalHelper(const T& validVector)
+{
+ // Assumes T is a vector, size 2 or larger
+
+ typedef T Vector;
+ typedef typename Vector::Scalar Scalar;
+
+ using SurgSim::Math::setSubnormalToZero;
+
+ {
+ Vector a = validVector;
+ Vector b = a;
+ EXPECT_FALSE(setSubnormalToZero(&a));
+ EXPECT_FALSE(setSubnormalToZero(&b));
+ compareMatrices(a, b);
+ }
+ {
+ Vector a = validVector;
+ a[0] = 0;
+ Vector b = a;
+ b[0] = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_FALSE(setSubnormalToZero(&a));
+ EXPECT_TRUE(setSubnormalToZero(&b));
+ compareMatrices(a, b);
+ }
+ {
+ Vector a = validVector;
+ a[1] = std::numeric_limits<Scalar>::infinity();
+ Vector b = a;
+ EXPECT_FALSE(setSubnormalToZero(&a));
+ EXPECT_FALSE(setSubnormalToZero(&b));
+ compareMatrices(a, b);
+ }
+ {
+ Vector a = validVector;
+ a[0] = std::numeric_limits<Scalar>::quiet_NaN();
+ a[1] = 0;
+ Vector b = a;
+ b[1] = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_FALSE(setSubnormalToZero(&a));
+ EXPECT_TRUE(setSubnormalToZero(&b));
+ compareMatrices(a, b);
+ }
+}
+
+TYPED_TEST(ValidTests, ClearSubnormalVector)
+{
+ typedef typename TestFixture::Scalar Scalar;
+
+ {
+ Eigen::Matrix<Scalar, 2, 1> vector;
+ vector.setConstant(543);
+ vectorSetSubnormalHelper(vector);
+ vector.setZero();
+ vectorSetSubnormalHelper(vector);
+ }
+ {
+ Eigen::Matrix<Scalar, 3, 1> vector;
+ vector.setConstant(543);
+ vectorSetSubnormalHelper(vector);
+ }
+ {
+ Eigen::Matrix<Scalar, 4, 1> vector;
+ vector.setConstant(543);
+ vectorSetSubnormalHelper(vector);
+ }
+ {
+ Eigen::Matrix<Scalar, 4, 1, Eigen::AutoAlign> vector;
+ vector.setConstant(543);
+ vectorSetSubnormalHelper(vector);
+ }
+
+ {
+ Eigen::Matrix<Scalar, Eigen::Dynamic, 1> vector;
+ vector.setConstant(21, static_cast<Scalar>(543));
+ vectorSetSubnormalHelper(vector);
+ }
+}
+
+TYPED_TEST(ValidTests, ClearSubnormalQuaternion)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::setSubnormalToZero;
+
+ Eigen::Quaternion<Scalar> quaternion(1, 0, 0, 0);
+ EXPECT_FALSE(setSubnormalToZero(&quaternion));
+
+ {
+ Eigen::Quaternion<Scalar> q2 = quaternion;
+ q2.x() = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(setSubnormalToZero(&q2));
+ compareMatrices(quaternion.coeffs(), q2.coeffs());
+ }
+ {
+ Eigen::Quaternion<Scalar> q3 = quaternion;
+ q3.y() = std::numeric_limits<Scalar>::infinity();
+ EXPECT_FALSE(setSubnormalToZero(&q3));
+ EXPECT_FALSE(SurgSim::Math::isValid(q3));
+
+ Eigen::Quaternion<Scalar> q4 = q3;
+ q4.z() = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(setSubnormalToZero(&q4));
+ compareMatrices(q3.coeffs(), q4.coeffs());
+ }
+}
+
+template <typename T>
+static void compareAngleAxis(const T& a, const T& b)
+{
+ bool isValidAngleA = SurgSim::Math::isValid(a.angle());
+ bool isValidAngleB = SurgSim::Math::isValid(b.angle());
+
+ EXPECT_EQ(isValidAngleA, isValidAngleB) << " angle A = " << a.angle() << ", B = " << b.angle();
+ if (isValidAngleA && isValidAngleB)
+ {
+ // In general, floating point equality checks are bad, but here they are needed.
+ EXPECT_EQ(a.angle(), b.angle());
+ }
+
+ compareMatrices(a.axis(), b.axis());
+}
+
+TYPED_TEST(ValidTests, ClearSubnormalAngleAxis)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::setSubnormalToZero;
+
+ typedef Eigen::AngleAxis<Scalar> AngleAxis;
+ typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+
+ AngleAxis rotation;
+
+ rotation = AngleAxis(-1, Vector3(1, 2, 3));
+ EXPECT_FALSE(setSubnormalToZero(&rotation));
+ compareAngleAxis(AngleAxis(-1, Vector3(1, 2, 3)), rotation);
+
+ rotation = AngleAxis(std::numeric_limits<Scalar>::denorm_min(), Vector3(1, 2, 3));
+ EXPECT_TRUE(setSubnormalToZero(&rotation));
+ compareAngleAxis(AngleAxis(0, Vector3(1, 2, 3)), rotation);
+
+ rotation = AngleAxis(-1, Vector3(std::numeric_limits<Scalar>::denorm_min(), 2, 3));
+ EXPECT_TRUE(setSubnormalToZero(&rotation));
+ compareAngleAxis(AngleAxis(-1, Vector3(0, 2, 3)), rotation);
+
+ rotation = AngleAxis(-1, Vector3(1, std::numeric_limits<Scalar>::infinity(), 3));
+ EXPECT_FALSE(setSubnormalToZero(&rotation));
+ compareAngleAxis(AngleAxis(-1, Vector3(1, std::numeric_limits<Scalar>::infinity(), 3)), rotation);
+
+ rotation = AngleAxis(std::numeric_limits<Scalar>::denorm_min(),
+ Vector3(1, 2, std::numeric_limits<Scalar>::infinity()));
+ EXPECT_TRUE(setSubnormalToZero(&rotation));
+ compareAngleAxis(AngleAxis(0, Vector3(1, 2, std::numeric_limits<Scalar>::infinity())), rotation);
+}
+
+TYPED_TEST(ValidTests, ClearSubnormalRotation2D)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::setSubnormalToZero;
+
+ Eigen::Rotation2D<Scalar> rotation(0);
+ EXPECT_FALSE(setSubnormalToZero(&rotation));
+ EXPECT_EQ(0, rotation.angle());
+
+ rotation.angle() = static_cast<Scalar>(1);
+ EXPECT_FALSE(setSubnormalToZero(&rotation));
+ EXPECT_EQ(1, rotation.angle());
+
+ rotation.angle() = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(setSubnormalToZero(&rotation));
+ EXPECT_EQ(0, rotation.angle());
+
+ rotation = Eigen::Rotation2D<Scalar>(std::numeric_limits<Scalar>::infinity());
+ EXPECT_FALSE(setSubnormalToZero(&rotation));
+ EXPECT_FALSE(SurgSim::Math::isValid(rotation.angle()));
+}
+
+template <typename T>
+static void transformSetSubnormalHelper()
+{
+ // Assumes T is an Eigen::Transform type of some sort
+
+ typedef T Transform;
+ typedef typename Transform::Scalar Scalar;
+
+ using SurgSim::Math::setSubnormalToZero;
+
+ Transform transform = Transform::Identity();
+ EXPECT_FALSE(setSubnormalToZero(&transform));
+ compareMatrices(Transform::Identity().matrix(), transform.matrix());
+
+ {
+ Transform t2 = transform;
+ t2(0, 1) = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(setSubnormalToZero(&t2));
+ compareMatrices(Transform::Identity().matrix(), transform.matrix());
+ }
+ {
+ Transform t3 = transform;
+ t3(0, 1) = std::numeric_limits<Scalar>::quiet_NaN();
+ Transform t4 = t3;
+ EXPECT_FALSE(setSubnormalToZero(&t4));
+ compareMatrices(t3.matrix(), t4.matrix());
+ }
+ {
+ Transform t5 = transform;
+ t5(0, 1) = std::numeric_limits<Scalar>::quiet_NaN();
+ t5(1, 0) = 0;
+ Transform t6 = t5;
+ t6(1, 0) = std::numeric_limits<Scalar>::denorm_min();
+ EXPECT_TRUE(setSubnormalToZero(&t6));
+ compareMatrices(t5.matrix(), t6.matrix());
+ }
+}
+
+TYPED_TEST(ValidTests, ClearSubnormalTransform)
+{
+ typedef typename TestFixture::Scalar Scalar;
+
+ transformSetSubnormalHelper<Eigen::Transform<Scalar, 2, Eigen::Isometry>>();
+ transformSetSubnormalHelper<Eigen::Transform<Scalar, 3, Eigen::Isometry>>();
+ transformSetSubnormalHelper<Eigen::Transform<Scalar, 4, Eigen::Isometry>>();
+ transformSetSubnormalHelper<Eigen::Transform<Scalar, 4, Eigen::Isometry>>();
+ transformSetSubnormalHelper<Eigen::Transform<Scalar, 4, Eigen::Affine>>();
+ transformSetSubnormalHelper<Eigen::Transform<Scalar, 4, Eigen::AffineCompact>>();
+}
+
+TYPED_TEST(ValidTests, Blocks)
+{
+ typedef typename TestFixture::Scalar Scalar;
+ using SurgSim::Math::isValid;
+ using SurgSim::Math::isSubnormal;
+ using SurgSim::Math::setSubnormalToZero;
+
+ {
+ Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> matrix;
+ matrix.setConstant(123);
+ EXPECT_TRUE(isValid(matrix.template block<2, 2>(1, 1)));
+ EXPECT_FALSE(isSubnormal(matrix.template block<2, 2>(1, 1)));
+ {
+ auto submatrix = matrix.template block<2, 2>(1, 1);
+ EXPECT_FALSE(setSubnormalToZero(&submatrix));
+ }
+ }
+ {
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> matrix;
+ matrix.setConstant(11, 13, static_cast<Scalar>(123));
+ EXPECT_TRUE(isValid(matrix.template block<6, 6>(3, 5)));
+ EXPECT_FALSE(isSubnormal(matrix.template block<6, 6>(3, 5)));
+ {
+ auto submatrix = matrix.template block<6, 6>(3, 5);
+ EXPECT_FALSE(setSubnormalToZero(&submatrix));
+ }
+ }
+}
diff --git a/SurgSim/Math/UnitTests/VectorTests.cpp b/SurgSim/Math/UnitTests/VectorTests.cpp
new file mode 100644
index 0000000..17cb45d
--- /dev/null
+++ b/SurgSim/Math/UnitTests/VectorTests.cpp
@@ -0,0 +1,1379 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 that exercise the functionality of our vector typedefs, which come
+/// straight from Eigen.
+
+#include <vector>
+
+#include <math.h>
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "gtest/gtest.h"
+
+// Define test fixture class templates.
+// We don't really need fixtures as such, but the templatization encodes type.
+
+template <class T>
+class VectorTestBase : public testing::Test
+{
+public:
+ typedef T Scalar;
+};
+
+
+
+template <class T>
+class Vector2Tests : public VectorTestBase<typename T::Scalar>
+{
+public:
+ typedef T Vector2;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) vector type aliases, but we got rid of those.
+typedef ::testing::Types<SurgSim::Math::Vector2d,
+ SurgSim::Math::Vector2f> Vector2Variants;
+TYPED_TEST_CASE(Vector2Tests, Vector2Variants);
+
+
+template <class T>
+class Vector3Tests : public VectorTestBase<typename T::Scalar>
+{
+public:
+ typedef T Vector3;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) vector type aliases, but we got rid of those.
+typedef ::testing::Types<SurgSim::Math::Vector3d,
+ SurgSim::Math::Vector3f> Vector3Variants;
+TYPED_TEST_CASE(Vector3Tests, Vector3Variants);
+
+
+template <class T>
+class Vector4Tests : public VectorTestBase<typename T::Scalar>
+{
+public:
+ typedef T Vector4;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) vector type aliases, but we got rid of those.
+typedef ::testing::Types<SurgSim::Math::Vector4d,
+ SurgSim::Math::Vector4f> Vector4Variants;
+TYPED_TEST_CASE(Vector4Tests, Vector4Variants);
+
+
+template <class T>
+class Vector6Tests : public VectorTestBase<typename T::Scalar>
+{
+public:
+ typedef T Vector6;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) vector type aliases, but we got rid of those.
+typedef ::testing::Types<SurgSim::Math::Vector6d,
+ SurgSim::Math::Vector6f> Vector6Variants;
+TYPED_TEST_CASE(Vector6Tests, Vector6Variants);
+
+
+template <class T>
+class AllVectorTests : public VectorTestBase<typename T::Scalar>
+{
+public:
+ typedef T Vector;
+};
+
+template <class T>
+class AllDynamicVectorTests : public VectorTestBase<typename T::Scalar>
+{
+public:
+ typedef T Vector;
+};
+
+// This used to contain aligned (via Eigen::AutoAlign) vector type aliases, but we got rid of those.
+typedef ::testing::Types<SurgSim::Math::Vector2d,
+ SurgSim::Math::Vector2f,
+ SurgSim::Math::Vector3d,
+ SurgSim::Math::Vector3f,
+ SurgSim::Math::Vector4d,
+ SurgSim::Math::Vector4f,
+ SurgSim::Math::Vector6d,
+ SurgSim::Math::Vector6f> AllVectorVariants;
+TYPED_TEST_CASE(AllVectorTests, AllVectorVariants);
+
+typedef ::testing::Types<Eigen::VectorXd,
+ Eigen::VectorXf,
+ SurgSim::Math::Vector> AllDynamicVectorVariants;
+TYPED_TEST_CASE(AllDynamicVectorTests, AllDynamicVectorVariants);
+
+template <class T>
+class UnalignedVectorTests : public VectorTestBase<typename T::Scalar>
+{
+public:
+ typedef T Vector;
+};
+
+template <class T>
+class UnalignedDynamicVectorTests : public VectorTestBase<typename T::Scalar>
+{
+public:
+ typedef T Vector;
+};
+
+typedef ::testing::Types<SurgSim::Math::Vector2d,
+ SurgSim::Math::Vector2f,
+ SurgSim::Math::Vector3d,
+ SurgSim::Math::Vector3f,
+ SurgSim::Math::Vector4d,
+ SurgSim::Math::Vector4f,
+ SurgSim::Math::Vector6d,
+ SurgSim::Math::Vector6f> UnalignedVectorVariants;
+TYPED_TEST_CASE(UnalignedVectorTests, UnalignedVectorVariants);
+
+typedef ::testing::Types<Eigen::VectorXd,
+ Eigen::VectorXf,
+ SurgSim::Math::Vector> UnalignedDynamicVectorVariants;
+TYPED_TEST_CASE(UnalignedDynamicVectorTests, UnalignedDynamicVectorVariants);
+
+
+// Now we're ready to start testing...
+
+
+// ==================== CONSTRUCTION & INITIALIZATION ====================
+
+/// Test that vectors can be constructed.
+TYPED_TEST(Vector2Tests, CanConstruct)
+{
+ typedef typename TestFixture::Vector2 Vector2;
+ typedef typename TestFixture::Scalar T;
+
+ // Warning: Eigen *does not* provide a 1-argument constructor that
+ // initializes all elements to the same value! If you do something like
+ // SurgSim::Math::Vector2f oneArg2f(1.23f);
+ // the argument is converted to an integral type, interpreted as a size,
+ // and promptly ignored because the size is fixed. Oops.
+ // To generate a constant vector, use Vector2f::Constant(val).
+
+ Vector2 default2;
+ Vector2 twoArg2(static_cast<T>(1.0), static_cast<T>(2.0));
+}
+
+/// Test that vectors can be constructed.
+TYPED_TEST(Vector3Tests, CanConstruct)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename TestFixture::Scalar T;
+
+ // Warning: Eigen *does not* provide a 1-argument constructor that
+ // initializes all elements to the same value! If you do something like
+ // SurgSim::Math::Vector2fu oneArg2fu(1.23f);
+ // the argument is converted to an integral type, interpreted as a size,
+ // and promptly ignored because the size is fixed. Oops.
+ // To generate a constant vector, use Vector2f::Constant(val).
+
+ Vector3 default3;
+ Vector3 threeArg3(static_cast<T>(1.0), static_cast<T>(2.0), static_cast<T>(3.0));
+}
+
+/// Test that vectors can be constructed.
+TYPED_TEST(Vector4Tests, CanConstruct)
+{
+ typedef typename TestFixture::Vector4 Vector4;
+ typedef typename TestFixture::Scalar T;
+
+ // Warning: Eigen *does not* provide a 1-argument constructor that
+ // initializes all elements to the same value! If you do something like
+ // SurgSim::Math::Vector2fu oneArg2fu(1.23f);
+ // the argument is converted to an integral type, interpreted as a size,
+ // and promptly ignored because the size is fixed. Oops.
+ // To generate a constant vector, use Vector2f::Constant(val).
+
+ Vector4 default4;
+ Vector4 fourArg4(static_cast<T>(1.0), static_cast<T>(2.0), static_cast<T>(3.0), static_cast<T>(4.0));
+}
+
+/// Test that the N-argument constructor properly initializes vectors.
+TYPED_TEST(Vector2Tests, NArgumentConstructorInitialization)
+{
+ typedef typename TestFixture::Vector2 Vector2;
+ typedef typename TestFixture::Scalar T;
+
+ Vector2 vector(static_cast<T>(1.01), static_cast<T>(1.02));
+ EXPECT_NEAR(1.01, vector[0], 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(1.02, vector[1], 1e-6) << "Y wasn't properly initialized.";
+}
+
+/// Test that the N-argument constructor properly initializes vectors.
+TYPED_TEST(Vector3Tests, NArgumentConstructorInitialization)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename TestFixture::Scalar T;
+
+ Vector3 vector(static_cast<T>(1.03), static_cast<T>(1.04), static_cast<T>(1.05));
+ EXPECT_NEAR(1.03, vector[0], 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(1.04, vector[1], 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(1.05, vector[2], 1e-6) << "Z wasn't properly initialized.";
+}
+
+/// Test that the N-argument constructor properly initializes vectors.
+TYPED_TEST(Vector4Tests, NArgumentConstructorInitialization)
+{
+ typedef typename TestFixture::Vector4 Vector4;
+ typedef typename TestFixture::Scalar T;
+
+ Vector4 vector(static_cast<T>(1.06), static_cast<T>(1.07), static_cast<T>(1.08), static_cast<T>(1.09));
+ EXPECT_NEAR(1.06, vector[0], 1e-6) << "X wasn't properly initialized.";
+ EXPECT_NEAR(1.07, vector[1], 1e-6) << "Y wasn't properly initialized.";
+ EXPECT_NEAR(1.08, vector[2], 1e-6) << "Z wasn't properly initialized.";
+ EXPECT_NEAR(1.09, vector[3], 1e-6) << "W wasn't properly initialized.";
+}
+
+/// Test that the default constructor DOESN'T initialize vectors.
+//
+// Only test the non-vectorized versions. Otherwise, we'd need to
+// allocate memory in a way that guarantees Eigen-compatible alignment.
+//
+// TODO(bert): There is some Eigen flag that causes matrices and vectors to be
+// initialized after all! We should check for that here.
+TYPED_TEST(UnalignedVectorTests, DefaultConstructorInitialization)
+{
+ typedef typename TestFixture::Vector Vector;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ EXPECT_TRUE(SIZE >= 2 && SIZE <= 6);
+ EXPECT_EQ(1, Vector::ColsAtCompileTime);
+
+ // Allocate a buffer for various vector types on stack, based on the size
+ // of the largest object we're testing. Objects will be allocated inside
+ // the buffer using the placement syntax for the new() operator.
+ // Eigen's new operatore will attempt to align returned value on word sized
+ // boundaries, so add 64 bytes to guarantee enough size.
+ unsigned char buffer[sizeof(Vector) + 64];
+
+ {
+ // Please don't write production (non-test) code that looks like this. =)
+ memset(&buffer, 0xF0, sizeof(buffer));
+ Vector* vector = new(&buffer) Vector;
+ for (int i = 0; i < SIZE; ++i)
+ {
+ EXPECT_NE(0.0f, (*vector)[i]) << i << " was NOT supposed to be zeroed.";
+ }
+ // Destroying the object is a good idea, even if unnecessary here:
+ vector->Vector::~Vector();
+ }
+}
+
+/// Test setting the vector using the << syntax.
+TYPED_TEST(Vector2Tests, ShiftCommaInitialization)
+{
+ typedef typename TestFixture::Vector2 Vector2;
+ typedef typename Vector2::Scalar T;
+
+ Vector2 vector;
+ // Initialize elements in order. Do NOT put parentheses around the list!
+ vector << static_cast<T>(1.1), static_cast<T>(1.2);
+ EXPECT_NEAR(2.3, vector.sum(), 1e-6) << "initialization was incorrect: " << vector;
+}
+
+/// Test setting the vector using the << syntax.
+TYPED_TEST(Vector3Tests, ShiftCommaInitialization)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename Vector3::Scalar T;
+
+ Vector3 vector;
+ // Initialize elements in order. Do NOT put parentheses around the list!
+ vector << static_cast<T>(1.1), static_cast<T>(1.2), static_cast<T>(1.3);
+ EXPECT_NEAR(3.6, vector.sum(), 1e-6) << "initialization was incorrect: " << vector;
+}
+
+/// Test setting the vector using the << syntax.
+TYPED_TEST(Vector4Tests, ShiftCommaInitialization)
+{
+ typedef typename TestFixture::Vector4 Vector4;
+ typedef typename Vector4::Scalar T;
+
+ Vector4 vector;
+ // Initialize elements in order. Do NOT put parentheses around the list!
+ vector << static_cast<T>(1.1), static_cast<T>(1.2), static_cast<T>(1.3), static_cast<T>(1.4);
+ EXPECT_NEAR(5.0, vector.sum(), 1e-6) << "initialization was incorrect: " << vector;
+}
+
+/// Test setting the vector using the << syntax.
+TYPED_TEST(Vector6Tests, ShiftCommaInitialization)
+{
+ typedef typename TestFixture::Vector6 Vector6;
+ typedef typename Vector6::Scalar T;
+
+ Vector6 vector;
+ // Initialize elements in order. Do NOT put parentheses around the list!
+ vector << static_cast<T>(1.1), static_cast<T>(1.2), static_cast<T>(1.3), static_cast<T>(1.4),
+ static_cast<T>(1.5), static_cast<T>(1.6);
+ EXPECT_NEAR(8.1, vector.sum(), 1e-6) << "initialization was incorrect: " << vector;
+}
+
+/// Test getting a zero value usable in expressions.
+TYPED_TEST(AllVectorTests, ZeroValue)
+{
+ typedef typename TestFixture::Vector Vector;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ Vector vector = 1000 * Vector::Zero();
+ for (int i = 0; i < SIZE; ++i)
+ {
+ EXPECT_NEAR(0.0, vector[i], 1e-20) << i << " wasn't properly initialized.";
+ }
+}
+
+/// Test setting vectors to 0.
+TYPED_TEST(AllVectorTests, SetToZero)
+{
+ typedef typename TestFixture::Vector Vector;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ Vector vector;
+ vector.setZero();
+ for (int i = 0; i < SIZE; ++i)
+ {
+ EXPECT_NEAR(0.0, vector[i], 1e-20) << i << " wasn't properly cleared.";
+ }
+}
+
+/// Test getting a constant value usable in expressions.
+TYPED_TEST(AllVectorTests, ConstantValue)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename Vector::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ Vector vector = static_cast<T>(2) * Vector::Constant(static_cast<T>(0.5));
+ for (int i = 0; i < SIZE; ++i)
+ {
+ EXPECT_NEAR(static_cast<T>(1.0), vector[i], 1e-6) << i << " wasn't properly initialized.";
+ }
+}
+
+/// Test setting vectors to a constant.
+TYPED_TEST(AllVectorTests, SetToConstant)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename Vector::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ Vector vector;
+ vector.setConstant(static_cast<T>(7.2));
+ for (int i = 0; i < SIZE; ++i)
+ {
+ EXPECT_NEAR(static_cast<T>(7.2), vector[i], 1e-6) << i << " wasn't properly initialized.";
+ }
+}
+
+/// Test initializing from a float array.
+TYPED_TEST(AllVectorTests, SetFromArray)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ // This array has more elements than we will need.
+ // The element type must match the vector!
+ const T inputArray[6] = { static_cast<T>(0.1), static_cast<T>(1.2), static_cast<T>(2.3),
+ static_cast<T>(3.4), static_cast<T>(4.5), static_cast<T>(5.6) };
+
+ Vector vector(inputArray);
+ for (int i = 0; i < SIZE; ++i)
+ {
+ EXPECT_NEAR(static_cast<T>(0.1 + i*1.1), vector[i], 1e-6) << i << " wasn't properly initialized.";
+ }
+}
+
+// Test conversion to and from yaml node
+TYPED_TEST(AllVectorTests, YamlConvert)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+
+ T testData[6] = { static_cast<T>(3.1), static_cast<T>(3.4), static_cast<T>(3.7),
+ static_cast<T>(4.0), static_cast<T>(4.3), static_cast<T>(4.6) };
+ Vector original(testData);
+
+ YAML::Node node;
+
+ ASSERT_NO_THROW(node = original);
+
+ EXPECT_TRUE(node.IsSequence());
+ EXPECT_EQ(original.size(), node.size());
+
+ ASSERT_NO_THROW({Vector expected = node.as<Vector>();});
+ EXPECT_TRUE(original.isApprox(node.as<Vector>()));
+}
+
+
+/// Test assignment.
+TYPED_TEST(AllVectorTests, Assign)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArrayA[6] = { static_cast<T>(6.1), static_cast<T>(6.2), static_cast<T>(6.3),
+ static_cast<T>(6.4), static_cast<T>(6.5), static_cast<T>(6.6) };
+ const T inputArrayB[6] = { static_cast<T>(7.1), static_cast<T>(7.2), static_cast<T>(7.3),
+ static_cast<T>(7.4), static_cast<T>(7.5), static_cast<T>(7.6) };
+
+ Vector a(inputArrayA);
+ // sum of the first SIZE elements of inputArrayA
+ double expectedSumA = SIZE * (SIZE*0.05 + 6.05);
+ EXPECT_NEAR(static_cast<T>(expectedSumA), a.sum(), 5e-6);
+ const Vector b(inputArrayB);
+ // sum of the first SIZE elements of inputArrayB
+ double expectedSumB = SIZE * (SIZE*0.05 + 7.05);
+ EXPECT_NEAR(static_cast<T>(expectedSumB), b.sum(), 5e-6);
+ a = b;
+ EXPECT_NEAR(static_cast<T>(expectedSumB), a.sum(), 5e-6);
+}
+
+// ==================== ARITHMETIC ====================
+
+/// Negation (unary minus).
+TYPED_TEST(AllVectorTests, Negate)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(2.1), static_cast<T>(2.2), static_cast<T>(2.3),
+ static_cast<T>(2.4), static_cast<T>(2.5), static_cast<T>(2.6) };
+ // sum of the first SIZE elements of inputArray
+ double expectedSum = SIZE * (SIZE*0.05 + 2.05);
+
+ Vector v(inputArray);
+ Vector n = -v;
+ EXPECT_NEAR(static_cast<T>(-expectedSum), n.sum(), 1e-6);
+}
+
+/// Addition.
+TYPED_TEST(AllVectorTests, Add)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(2.1), static_cast<T>(2.2), static_cast<T>(2.3),
+ static_cast<T>(2.4), static_cast<T>(2.5), static_cast<T>(2.6) };
+ // sum of the first SIZE elements of inputArray
+ double expectedSum = SIZE * (SIZE * 0.05 + 2.05);
+
+ Vector v(inputArray);
+ Vector w = v + Vector::Ones() + v;
+
+ EXPECT_NEAR(static_cast<T>(2 * expectedSum) + SIZE, w.sum(), 1e-6);
+}
+
+/// Subtraction.
+TYPED_TEST(AllVectorTests, Subtract)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(2.1), static_cast<T>(2.2), static_cast<T>(2.3),
+ static_cast<T>(2.4), static_cast<T>(2.5), static_cast<T>(2.6) };
+ // sum of the first SIZE elements of inputArray
+ double expectedSum = SIZE * (SIZE*0.05 + 2.05);
+
+ Vector v(inputArray);
+ Vector w = v - Vector::Ones();
+
+ EXPECT_NEAR(static_cast<T>(expectedSum) - SIZE, w.sum(), 1e-6);
+}
+
+/// Incrementing by a value.
+TYPED_TEST(AllVectorTests, AddTo)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(2.1), static_cast<T>(2.2), static_cast<T>(2.3),
+ static_cast<T>(2.4), static_cast<T>(2.5), static_cast<T>(2.6) };
+ // sum of the first SIZE elements of inputArray
+ double expectedSum = SIZE * (SIZE*0.05 + 2.05);
+
+ Vector v(inputArray);
+ v += Vector::Ones();
+ EXPECT_NEAR(static_cast<T>(expectedSum) + SIZE, v.sum(), 1e-6);
+}
+
+/// Decrementing by a value.
+TYPED_TEST(AllVectorTests, SubtractFrom)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(2.1), static_cast<T>(2.2), static_cast<T>(2.3),
+ static_cast<T>(2.4), static_cast<T>(2.5), static_cast<T>(2.6) };
+ // sum of the first SIZE elements of inputArray
+ double expectedSum = SIZE * (SIZE*0.05 + 2.05);
+
+ Vector v(inputArray);
+ v -= Vector::Ones();
+ EXPECT_NEAR(static_cast<T>(expectedSum) - SIZE, v.sum(), 1e-6);
+}
+
+/// Vector-scalar multiplication.
+TYPED_TEST(AllVectorTests, MultiplyVectorScalar)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(2.1), static_cast<T>(2.2), static_cast<T>(2.3),
+ static_cast<T>(2.4), static_cast<T>(2.5), static_cast<T>(2.6) };
+ // sum of the first SIZE elements of inputArray
+ double expectedSum = SIZE * (SIZE*0.05 + 2.05);
+
+ Vector v(inputArray);
+ Vector w = v * static_cast<T>(1.23);
+
+ EXPECT_NEAR(static_cast<T>(1.23 * expectedSum), w.sum(), 1e-6);
+}
+
+/// Scalar-vector multiplication.
+TYPED_TEST(AllVectorTests, MultiplyScalarVector)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(2.1), static_cast<T>(2.2), static_cast<T>(2.3),
+ static_cast<T>(2.4), static_cast<T>(2.5), static_cast<T>(2.6) };
+ // sum of the first SIZE elements of inputArray
+ double expectedSum = SIZE * (SIZE*0.05 + 2.05);
+
+ Vector v(inputArray);
+ Vector w = static_cast<T>(1.23) * v;
+
+ EXPECT_NEAR(static_cast<T>(1.23 * expectedSum), w.sum(), 1e-6);
+}
+
+/// Division by scalar.
+TYPED_TEST(AllVectorTests, DivideScalar)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(2.1), static_cast<T>(2.2), static_cast<T>(2.3),
+ static_cast<T>(2.4), static_cast<T>(2.5), static_cast<T>(2.6) };
+ // sum of the first SIZE elements of inputArray
+ double expectedSum = SIZE * (SIZE*0.05 + 2.05);
+
+ Vector v(inputArray);
+ Vector w = v / static_cast<T>(1.23);
+
+ EXPECT_NEAR(static_cast<T>(expectedSum / 1.23), w.sum(), 1e-6);
+}
+
+/// Component-wise multiplication.
+TYPED_TEST(AllVectorTests, ComponentwiseMultiply)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(3.1), static_cast<T>(3.4), static_cast<T>(3.7),
+ static_cast<T>(4.0), static_cast<T>(4.3), static_cast<T>(4.6) };
+ // sum of the squares of the first SIZE elements of inputArray
+ double expectedSumSquares = SIZE * (SIZE * (SIZE*0.03 + 0.885) + 8.695);
+
+ Vector v(inputArray);
+ // use the component-wise Eigen matrix operation:
+ Vector w = v.cwiseProduct(v);
+ EXPECT_NEAR(static_cast<T>(expectedSumSquares), w.sum(), 1e-6);
+ // OR, the same thing done via conversion to arrays:
+ w = v.array() * v.array();
+ EXPECT_NEAR(static_cast<T>(expectedSumSquares), w.sum(), 1e-6);
+}
+
+/// Component-wise division.
+TYPED_TEST(AllVectorTests, ComponentwiseDivide)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(3.1), static_cast<T>(3.4), static_cast<T>(3.7),
+ static_cast<T>(4.0), static_cast<T>(4.3), static_cast<T>(4.6) };
+
+ Vector v(inputArray);
+ Vector u = static_cast<T>(2)*v;
+ // use the component-wise Eigen matrix operation:
+ Vector w = u.cwiseQuotient(v);
+ EXPECT_NEAR(static_cast<T>(2*SIZE), w.sum(), 1e-6);
+ // OR, the same thing done via conversion to arrays:
+ w = u.array() / v.array();
+ EXPECT_NEAR(static_cast<T>(2*SIZE), w.sum(), 1e-6);
+}
+
+/// Dot product.
+TYPED_TEST(AllVectorTests, DotProduct)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(3.1), static_cast<T>(3.4), static_cast<T>(3.7),
+ static_cast<T>(4.0), static_cast<T>(4.3), static_cast<T>(4.6) };
+ // sum of the squares of the first SIZE elements of inputArray
+ double expectedSumSquares = SIZE * (SIZE * (SIZE*0.03 + 0.885) + 8.695);
+
+ Vector v(inputArray);
+ EXPECT_NEAR(static_cast<T>(expectedSumSquares), v.dot(v), 1e-6);
+}
+
+/// Cross product.
+TYPED_TEST(Vector3Tests, CrossProduct)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename Vector3::Scalar T;
+
+ Vector3 v;
+ v << static_cast<T>(3.4), static_cast<T>(5.6), static_cast<T>(7.8);
+ Vector3 u = -v;
+ u[0] = v[0];
+ Vector3 w = v.cross(u);
+ std::cout << w << std::endl;
+ EXPECT_NEAR(0, w[0], 1e-6);
+ EXPECT_GT(abs(w[1]), 1); // nonzero
+ EXPECT_GT(abs(w[2]), 1); // nonzero
+}
+
+/// Outer product.
+TYPED_TEST(AllVectorTests, OuterProduct)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+ typedef Eigen::Matrix<T, SIZE, SIZE> Matrix;
+
+ const T inputArray[6] = { static_cast<T>(3.1), static_cast<T>(3.4), static_cast<T>(3.7),
+ static_cast<T>(4.0), static_cast<T>(4.3), static_cast<T>(4.6) };
+ // sum of the squares of the first SIZE elements of inputArray
+ double expectedSumSquares = SIZE * (SIZE * (SIZE*0.03 + 0.885) + 8.695);
+
+ Vector v(inputArray);
+ // You have to write out the outer product, like this:
+ Matrix m = v * v.transpose();
+
+ // TODO(bert): maybe needs better testing here?
+ Vector u = v / v.squaredNorm();
+ EXPECT_NEAR(static_cast<T>(expectedSumSquares), (m*u).squaredNorm(), 1e-3);
+}
+
+/// Euclidean norm and its square.
+TYPED_TEST(AllVectorTests, NormAndSquared)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(3.1), static_cast<T>(3.4), static_cast<T>(3.7),
+ static_cast<T>(4.0), static_cast<T>(4.3), static_cast<T>(4.6) };
+ // sum of the squares of the first SIZE elements of inputArray
+ double expectedSumSquares = SIZE * (SIZE * (SIZE*0.03 + 0.885) + 8.695);
+
+ Vector v(inputArray);
+ EXPECT_NEAR(static_cast<T>(expectedSumSquares), v.squaredNorm(), 1e-6);
+ EXPECT_NEAR(sqrt(static_cast<T>(expectedSumSquares)), v.norm(), 1e-6);
+}
+
+/// L1 (Manhattan) norm and L_Infinity (largest absolute value) norm.
+TYPED_TEST(AllVectorTests, L1NormAndLInfNorm)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(2.1), static_cast<T>(2.2), static_cast<T>(2.3),
+ static_cast<T>(2.4), static_cast<T>(2.5), static_cast<T>(2.6) };
+ // sum of the first SIZE elements of inputArray
+ double expectedSum = SIZE * (SIZE*0.05 + 2.05);
+
+ Vector v(inputArray);
+ Vector w = -v;
+ // Ugh, "template" is required to get this to parse properly. This is
+ // triggered because the test is a part of a template class; you don't
+ // need to do this in a non-template context.
+ EXPECT_NEAR(static_cast<T>(expectedSum), v.template lpNorm<1>(), 1e-6);
+ EXPECT_NEAR(static_cast<T>(expectedSum), w.template lpNorm<1>(), 1e-6);
+ EXPECT_NEAR(inputArray[SIZE-1], v.template lpNorm<Eigen::Infinity>(), 1e-6);
+ EXPECT_NEAR(inputArray[SIZE-1], w.template lpNorm<Eigen::Infinity>(), 1e-6);
+}
+
+/// Normalization of vectors.
+TYPED_TEST(AllVectorTests, Normalize)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(3.1), static_cast<T>(3.4), static_cast<T>(3.7),
+ static_cast<T>(4.0), static_cast<T>(4.3), static_cast<T>(4.6) };
+ // sum of the squares of the first SIZE elements of inputArray
+ double expectedSumSquares = SIZE * (SIZE * (SIZE*0.03 + 0.885) + 8.695);
+
+ Vector v(inputArray);
+ EXPECT_NEAR(sqrt(expectedSumSquares), v.norm(), 1e-6);
+
+ // normalized() RETURNS the normalized vector, leaving original unchanged.
+ Vector u = v.normalized();
+ EXPECT_NEAR(static_cast<T>(1), u.norm(), 1e-6);
+ EXPECT_NEAR(sqrt(static_cast<T>(expectedSumSquares)), v.norm(), 1e-6);
+ // normalize() NORMALIZES the vector, modifying it.
+ v.normalize();
+ EXPECT_NEAR(static_cast<T>(1), v.norm(), 1e-6);
+ EXPECT_NEAR(static_cast<T>(0), (u - v).norm(), 1e-6);
+}
+
+/// Minimum and maximum elements.
+TYPED_TEST(AllVectorTests, MinAndMax)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(3.1), static_cast<T>(3.4), static_cast<T>(3.7),
+ static_cast<T>(4.0), static_cast<T>(4.3), static_cast<T>(4.6) };
+
+ Vector v(inputArray);
+ EXPECT_NEAR(inputArray[0], v.minCoeff(), 1e-6);
+ EXPECT_NEAR(inputArray[SIZE-1], v.maxCoeff(), 1e-6);
+}
+
+// ==================== SUBVECTORS (EXTENDING/SHRINKING) ====================
+
+/// Extending vectors using the head<r>() syntax.
+TYPED_TEST(Vector2Tests, Extend2to3)
+{
+ typedef typename TestFixture::Vector2 Vector2;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ Vector2 vector2;
+ vector2 << static_cast<T>(1.1), static_cast<T>(1.2);
+ Vector3 vector3;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ vector3.template head<2>() = vector2;
+ vector3[2] = static_cast<T>(0);
+ EXPECT_NEAR(2.3, vector3.sum(), 1e-6) << "extending was incorrect: " << vector3;
+}
+
+/// Extending vectors using the head(r) syntax.
+TYPED_TEST(Vector2Tests, DynamicExtend2to3)
+{
+ typedef typename TestFixture::Vector2 Vector2;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ Vector2 vector2;
+ vector2 << static_cast<T>(1.1), static_cast<T>(1.2);
+ Vector3 vector3;
+ vector3.head(2) = vector2;
+ vector3[2] = static_cast<T>(0);
+ EXPECT_NEAR(2.3, vector3.sum(), 1e-6) << "extending was incorrect: " << vector3;
+}
+
+/// Extending vectors using the block<r,c>() syntax.
+TYPED_TEST(Vector2Tests, BlockExtend2to3)
+{
+ typedef typename TestFixture::Vector2 Vector2;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ Vector2 vector2;
+ vector2 << static_cast<T>(1.1), static_cast<T>(1.2);
+ Vector3 vector3;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ vector3.template block<2, 1>(0, 0) = vector2;
+ vector3(2, 0) = static_cast<T>(0);
+ EXPECT_NEAR(2.3, vector3.sum(), 1e-6) << "extending was incorrect: " << vector3;
+}
+
+/// Extending vectors using the block(i,j,r,c) syntax.
+TYPED_TEST(Vector2Tests, DynamicBlockExtend2to3)
+{
+ typedef typename TestFixture::Vector2 Vector2;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ Vector2 vector2;
+ vector2 << static_cast<T>(1.1), static_cast<T>(1.2);
+ Vector3 vector3;
+ vector3.block(0, 0, 2, 1) = vector2;
+ vector3(2, 0) = static_cast<T>(0);
+ EXPECT_NEAR(2.3, vector3.sum(), 1e-6) << "extending was incorrect: " << vector3;
+}
+
+/// Shrinking vectors using the head<r>() syntax.
+TYPED_TEST(Vector2Tests, Shrink3to2)
+{
+ typedef typename TestFixture::Vector2 Vector2;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ Vector3 vector3;
+ vector3 << static_cast<T>(1.1), static_cast<T>(1.2), static_cast<T>(1.3);
+ Vector2 vector2;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ vector2 = vector3.template head<2>();
+ EXPECT_NEAR(2.3, vector2.sum(), 1e-6) << "shrinking was incorrect: " << vector2;
+}
+
+/// Extending vectors using the head<r>() syntax.
+TYPED_TEST(Vector3Tests, Extend2to3)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 2, 1> Vector2;
+
+ Vector2 vector2;
+ vector2 << static_cast<T>(1.1), static_cast<T>(1.2);
+ Vector3 vector3;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ vector3.template head<2>() = vector2;
+ vector3[2] = static_cast<T>(0);
+ EXPECT_NEAR(2.3, vector3.sum(), 1e-6) << "extending was incorrect: " << vector3;
+}
+
+/// Shrinking vectors using the head<r>() syntax.
+TYPED_TEST(Vector3Tests, Shrink3to2)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 2, 1> Vector2;
+
+ Vector3 vector3;
+ vector3 << static_cast<T>(1.1), static_cast<T>(1.2), static_cast<T>(1.3);
+ Vector2 vector2;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ vector2 = vector3.template head<2>();
+ EXPECT_NEAR(2.3, vector2.sum(), 1e-6) << "shrinking was incorrect" << vector2;
+}
+
+/// Extending vectors using the head<r>() syntax.
+TYPED_TEST(Vector3Tests, Extend3to4)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 4, 1> Vector4;
+
+ Vector3 vector3;
+ vector3 << static_cast<T>(1.1), static_cast<T>(1.2), static_cast<T>(1.3);
+ Vector4 vector4;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ vector4.template head<3>() = vector3;
+ vector4[3] = static_cast<T>(0);
+ EXPECT_NEAR(3.6, vector4.sum(), 1e-6) << "extending was incorrect" << vector4;
+}
+
+/// Shrinking vectors using the head<r>() syntax.
+TYPED_TEST(Vector3Tests, Shrink4to3)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 4, 1> Vector4;
+
+ Vector4 vector4;
+ vector4 << static_cast<T>(1.1), static_cast<T>(1.2), static_cast<T>(1.3), static_cast<T>(1.4);
+ Vector3 vector3;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ vector3 = vector4.template head<3>();
+ EXPECT_NEAR(3.6, vector3.sum(), 1e-6) << "shrinking was incorrect" << vector3;
+}
+
+/// Extending vectors using the head<r>() syntax.
+TYPED_TEST(Vector4Tests, Extend3to4)
+{
+ typedef typename TestFixture::Vector4 Vector4;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ Vector3 vector3;
+ vector3 << static_cast<T>(1.1), static_cast<T>(1.2), static_cast<T>(1.3);
+ Vector4 vector4;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ vector4.template head<3>() = vector3;
+ vector4[3] = static_cast<T>(0);
+ EXPECT_NEAR(3.6, vector4.sum(), 1e-6) << "extending was incorrect" << vector4;
+}
+
+/// Shrinking vectors using the head<r>() syntax.
+TYPED_TEST(Vector4Tests, Shrink4to3)
+{
+ typedef typename TestFixture::Vector4 Vector4;
+ typedef typename TestFixture::Scalar T;
+
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ Vector4 vector4;
+ vector4 << static_cast<T>(1.1), static_cast<T>(1.2), static_cast<T>(1.3), static_cast<T>(1.4);
+ Vector3 vector3;
+ // Ugh, this is efficient but "template" is required to get it to parse
+ // properly. This is triggered because the test is a part of a template
+ // class; you don't need to do this in a non-template context.
+ vector3 = vector4.template head<3>();
+ EXPECT_NEAR(3.6, vector3.sum(), 1e-6) << "shrinking was incorrect" << vector3;
+}
+
+/// Extend Euclidean N-vector [a_i] to homogeneous (N+1)-vector [a_i 1].
+TYPED_TEST(AllVectorTests, HomogeneousExtend)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+ typedef Eigen::Matrix<T, SIZE+1, 1> HVector;
+
+ const T inputArray[6] = { static_cast<T>(10.1), static_cast<T>(10.2), static_cast<T>(10.3),
+ static_cast<T>(10.4), static_cast<T>(10.5), static_cast<T>(10.6) };
+
+ Vector v(inputArray);
+ HVector h = v.homogeneous();
+ for (int i = 0; i < SIZE; ++i)
+ {
+ EXPECT_NEAR(inputArray[i], h[i], 1e-6) << "homogeneous form garbled.";
+ }
+ EXPECT_NEAR(1.0, h[SIZE], 1e-6) << "homogeneous form garbled.";
+}
+
+/// Shrink a homogeneous (N+1)-vector [a_i 1] to Euclidean N-vector [a_i].
+///
+/// Note that if the last element is not 1, the result will be divided by it,
+/// which is helpful in projective geometry but may not be what you wanted!
+TYPED_TEST(AllVectorTests, HomogeneousShrink)
+{
+ typedef typename TestFixture::Vector HVector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = HVector::RowsAtCompileTime - 1;
+ typedef Eigen::Matrix<T, SIZE, 1> Vector;
+
+ const T inputArray[6] = { static_cast<T>(10.1), static_cast<T>(10.2), static_cast<T>(10.3),
+ static_cast<T>(10.4), static_cast<T>(10.5), static_cast<T>(10.6) };
+
+ HVector h(inputArray);
+ h[SIZE] = 2; // makes calculating expected values simpler =)
+ Vector v = h.hnormalized();
+ for (int i = 0; i < SIZE; ++i)
+ {
+ EXPECT_NEAR(inputArray[i]/2, v[i], 1e-6) << "Euclidean form from homogeneous garbled.";
+ }
+}
+
+// ==================== TYPE CONVERSION ====================
+
+/// Typecasting vectors (double <-> float conversions).
+TYPED_TEST(AllVectorTests, TypeCasting)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+ typedef Eigen::Matrix<double, SIZE, 1> Vectord;
+ typedef Eigen::Matrix<float, SIZE, 1> Vectorf;
+
+ const T inputArray[6] = { static_cast<T>(12.1), static_cast<T>(12.2), static_cast<T>(12.3),
+ static_cast<T>(12.4), static_cast<T>(12.5), static_cast<T>(12.6) };
+ // sum of the first SIZE elements of inputArray
+ T expectedSum = SIZE * (SIZE*static_cast<T>(0.05) + static_cast<T>(12.05));
+
+ Vector v(inputArray);
+ // Ugh, "template" is required to get this to parse properly. This is
+ // triggered because the test is a part of a template class; you don't
+ // need to do this in a non-template context.
+ Vectord vd = v.template cast<double>();
+ EXPECT_NEAR(static_cast<double>(expectedSum), vd.sum(), 1e-5);
+ Vectorf vf = v.template cast<float>();
+ EXPECT_NEAR(static_cast<float>(expectedSum), vf.sum(), 1e-5);
+}
+
+// ==================== MISCELLANEOUS ====================
+
+/// Reading from and writing to arrays or blocks of double/float in memory.
+TYPED_TEST(AllVectorTests, ArrayReadWrite)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+ const int SIZE = Vector::RowsAtCompileTime;
+
+ const T inputArray[6] = { static_cast<T>(12.1), static_cast<T>(12.2), static_cast<T>(12.3),
+ static_cast<T>(12.4), static_cast<T>(12.5), static_cast<T>(12.6) };
+ T outputArray[6];
+
+ Eigen::Map<const Vector> v_in(inputArray);
+ Vector v1 = v_in;
+
+ Eigen::Map<Vector> v_out(outputArray);
+ v_out = v1;
+
+ for (int i = 0; i < SIZE; ++i)
+ {
+ EXPECT_NEAR(inputArray[i], outputArray[i], 1e-6);
+ }
+}
+
+TYPED_TEST(AllVectorTests, Interpolate)
+{
+ typedef typename TestFixture::Vector Vector;
+ typedef typename TestFixture::Scalar T;
+
+ T epsilon = static_cast<T>(1e-6);
+
+ T prevArray[6] = { static_cast<T>(3.1), static_cast<T>(3.4), static_cast<T>(3.7),
+ static_cast<T>(4.0), static_cast<T>(4.3), static_cast<T>(4.6) };
+ T nextArray[6] = { static_cast<T>(7.2), static_cast<T>(0.6), static_cast<T>(4.8),
+ static_cast<T>(5.1), static_cast<T>(8.9), static_cast<T>(1.5) };
+ T interpArray[6];
+
+ Vector prev(prevArray);
+ Vector next(nextArray);
+ Vector interp;
+
+ // 0.0
+ interpArray[0] = static_cast<T>(3.1) * static_cast<T>(1.0) + static_cast<T>(7.2) * static_cast<T>(0.0);
+ interpArray[1] = static_cast<T>(3.4) * static_cast<T>(1.0) + static_cast<T>(0.6) * static_cast<T>(0.0);
+ interpArray[2] = static_cast<T>(3.7) * static_cast<T>(1.0) + static_cast<T>(4.8) * static_cast<T>(0.0);
+ interpArray[3] = static_cast<T>(4.0) * static_cast<T>(1.0) + static_cast<T>(5.1) * static_cast<T>(0.0);
+ interpArray[4] = static_cast<T>(4.3) * static_cast<T>(1.0) + static_cast<T>(8.9) * static_cast<T>(0.0);
+ interpArray[5] = static_cast<T>(4.6) * static_cast<T>(1.0) + static_cast<T>(1.5) * static_cast<T>(0.0);
+ interp = Vector(interpArray);
+ EXPECT_TRUE(interp.isApprox(prev));
+ EXPECT_TRUE(interp.isApprox(SurgSim::Math::interpolate(prev, next, static_cast<T>(0.0)), epsilon));
+
+ // 1.0
+ interpArray[0] = static_cast<T>(3.1) * static_cast<T>(0.0) + static_cast<T>(7.2) * static_cast<T>(1.0);
+ interpArray[1] = static_cast<T>(3.4) * static_cast<T>(0.0) + static_cast<T>(0.6) * static_cast<T>(1.0);
+ interpArray[2] = static_cast<T>(3.7) * static_cast<T>(0.0) + static_cast<T>(4.8) * static_cast<T>(1.0);
+ interpArray[3] = static_cast<T>(4.0) * static_cast<T>(0.0) + static_cast<T>(5.1) * static_cast<T>(1.0);
+ interpArray[4] = static_cast<T>(4.3) * static_cast<T>(0.0) + static_cast<T>(8.9) * static_cast<T>(1.0);
+ interpArray[5] = static_cast<T>(4.6) * static_cast<T>(0.0) + static_cast<T>(1.5) * static_cast<T>(1.0);
+ interp = Vector(interpArray);
+ EXPECT_TRUE(interp.isApprox(next));
+ EXPECT_TRUE(interp.isApprox(SurgSim::Math::interpolate(prev, next, static_cast<T>(1.0)), epsilon));
+
+ // 0.5
+ interpArray[0] = static_cast<T>(3.1) * static_cast<T>(0.5) + static_cast<T>(7.2) * static_cast<T>(0.5);
+ interpArray[1] = static_cast<T>(3.4) * static_cast<T>(0.5) + static_cast<T>(0.6) * static_cast<T>(0.5);
+ interpArray[2] = static_cast<T>(3.7) * static_cast<T>(0.5) + static_cast<T>(4.8) * static_cast<T>(0.5);
+ interpArray[3] = static_cast<T>(4.0) * static_cast<T>(0.5) + static_cast<T>(5.1) * static_cast<T>(0.5);
+ interpArray[4] = static_cast<T>(4.3) * static_cast<T>(0.5) + static_cast<T>(8.9) * static_cast<T>(0.5);
+ interpArray[5] = static_cast<T>(4.6) * static_cast<T>(0.5) + static_cast<T>(1.5) * static_cast<T>(0.5);
+ interp = Vector(interpArray);
+ EXPECT_TRUE(interp.isApprox(SurgSim::Math::interpolate(prev, next, static_cast<T>(0.5)), epsilon));
+
+ // 0.886
+ interpArray[0] = static_cast<T>(3.1) * static_cast<T>(0.114) + static_cast<T>(7.2) * static_cast<T>(0.886);
+ interpArray[1] = static_cast<T>(3.4) * static_cast<T>(0.114) + static_cast<T>(0.6) * static_cast<T>(0.886);
+ interpArray[2] = static_cast<T>(3.7) * static_cast<T>(0.114) + static_cast<T>(4.8) * static_cast<T>(0.886);
+ interpArray[3] = static_cast<T>(4.0) * static_cast<T>(0.114) + static_cast<T>(5.1) * static_cast<T>(0.886);
+ interpArray[4] = static_cast<T>(4.3) * static_cast<T>(0.114) + static_cast<T>(8.9) * static_cast<T>(0.886);
+ interpArray[5] = static_cast<T>(4.6) * static_cast<T>(0.114) + static_cast<T>(1.5) * static_cast<T>(0.886);
+ interp = Vector(interpArray);
+ EXPECT_TRUE(interp.isApprox(SurgSim::Math::interpolate(prev, next, static_cast<T>(0.886)), epsilon));
+
+ // 0.623
+ interpArray[0] = static_cast<T>(3.1) * static_cast<T>(0.377) + static_cast<T>(7.2) * static_cast<T>(0.623);
+ interpArray[1] = static_cast<T>(3.4) * static_cast<T>(0.377) + static_cast<T>(0.6) * static_cast<T>(0.623);
+ interpArray[2] = static_cast<T>(3.7) * static_cast<T>(0.377) + static_cast<T>(4.8) * static_cast<T>(0.623);
+ interpArray[3] = static_cast<T>(4.0) * static_cast<T>(0.377) + static_cast<T>(5.1) * static_cast<T>(0.623);
+ interpArray[4] = static_cast<T>(4.3) * static_cast<T>(0.377) + static_cast<T>(8.9) * static_cast<T>(0.623);
+ interpArray[5] = static_cast<T>(4.6) * static_cast<T>(0.377) + static_cast<T>(1.5) * static_cast<T>(0.623);
+ interp = Vector(interpArray);
+ EXPECT_TRUE(interp.isApprox(SurgSim::Math::interpolate(prev, next, static_cast<T>(0.623)), epsilon));
+}
+
+
+// TO DO:
+// testing numerical validity
+// testing for denormalized numbers
+// testing degeneracy (norm near 0)
+// compute an orthonormal frame based on a given normal (z-axis)
+
+TYPED_TEST(AllDynamicVectorTests, CanResize)
+{
+ typedef typename TestFixture::Vector Vector;
+
+ ASSERT_NO_THROW({Vector a; a.resize(10);});
+}
+
+namespace
+{
+ template <class T>
+ void testScalar(T valueExpected, T value){}
+
+ template <>
+ void testScalar<double>(double valueExpected, double value)
+ {
+ EXPECT_DOUBLE_EQ(valueExpected, value);
+ }
+
+ template <>
+ void testScalar<float>(float valueExpected, float value)
+ {
+ EXPECT_FLOAT_EQ(valueExpected, value);
+ }
+};
+
+TYPED_TEST(AllDynamicVectorTests, addSubVector)
+{
+ typedef typename TestFixture::Vector Vector;
+
+ Vector v, vInit, v2, v2Init;
+ v.resize(18); v.setRandom(); vInit = v;
+ v2.resize(18); v2.setRandom(); v2Init = v2;
+
+ ASSERT_NO_THROW(SurgSim::Math::addSubVector(v2.segment(3,3), 2, 3, &v););
+ EXPECT_TRUE(v2.isApprox(v2Init));
+ EXPECT_FALSE(v.isApprox(vInit));
+ for (int dofId = 0; dofId < 6; dofId++)
+ {
+ testScalar(vInit[dofId], v[dofId]);
+ }
+ for (int dofId = 6; dofId < 9; dofId++)
+ {
+ testScalar(vInit[dofId] + v2Init[3 + dofId-6], v[dofId]);
+ }
+ for (int dofId = 9; dofId < 18; dofId++)
+ {
+ testScalar(vInit[dofId], v[dofId]);
+ }
+}
+
+TYPED_TEST(AllDynamicVectorTests, addSubVectorBlocks)
+{
+ typedef typename TestFixture::Vector Vector;
+
+ Vector v, vInit, v2, v2Init;
+ std::vector<size_t> nodeIds;
+ v.resize(18); v.setRandom(); vInit = v;
+ v2.resize(18); v2.setRandom(); v2Init = v2;
+ nodeIds.push_back(1);
+ nodeIds.push_back(3);
+ nodeIds.push_back(5);
+
+ ASSERT_NO_THROW(SurgSim::Math::addSubVector(v2.segment(3,15), nodeIds, 3, &v););
+ EXPECT_TRUE(v2.isApprox(v2Init));
+ EXPECT_FALSE(v.isApprox(vInit));
+ for (int dofId = 0; dofId < 3; dofId++)
+ {
+ testScalar(vInit[dofId], v[dofId]);
+ }
+ for (int dofId = 3; dofId < 6; dofId++)
+ {
+ testScalar(vInit[dofId] + v2Init[3 + (dofId - 3)], v[dofId]);
+ }
+ for (int dofId = 6; dofId < 9; dofId++)
+ {
+ testScalar(vInit[dofId], v[dofId]);
+ }
+ for (int dofId = 9; dofId < 12; dofId++)
+ {
+ testScalar(vInit[dofId] + v2Init[3 + (dofId - 6)], v[dofId]);
+ }
+ for (int dofId = 12; dofId < 15; dofId++)
+ {
+ testScalar(vInit[dofId], v[dofId]);
+ }
+ for (int dofId = 15; dofId < 18; dofId++)
+ {
+ testScalar(vInit[dofId] + v2Init[3 + (dofId - 9)], v[dofId]);
+ }
+}
+
+TYPED_TEST(AllDynamicVectorTests, setSubVector)
+{
+ typedef typename TestFixture::Vector Vector;
+
+ Vector v, vInit, v2, v2Init;
+ v.resize(18); v.setRandom(); vInit = v;
+ v2.resize(18); v2.setRandom(); v2Init = v2;
+
+ ASSERT_NO_THROW(SurgSim::Math::setSubVector(v2.segment(3,3), 2, 3, &v););
+ EXPECT_TRUE(v2.isApprox(v2Init));
+ EXPECT_FALSE(v.isApprox(vInit));
+ for (int dofId = 0; dofId < 6; dofId++)
+ {
+ testScalar(vInit[dofId], v[dofId]);
+ }
+ for (int dofId = 6; dofId < 9; dofId++)
+ {
+ testScalar(v2Init[3 + dofId-6], v[dofId]);
+ }
+ for (int dofId = 9; dofId < 18; dofId++)
+ {
+ testScalar(vInit[dofId], v[dofId]);
+ }
+}
+
+TYPED_TEST(AllDynamicVectorTests, getSubVector)
+{
+ typedef typename TestFixture::Vector Vector;
+
+ Vector v, vInit;
+ v.resize(18); v.setRandom(); vInit = v;
+
+ Eigen::VectorBlock<Vector> subVector = SurgSim::Math::getSubVector(v, 2, 3);
+ EXPECT_TRUE(v.isApprox(vInit));
+ for (int dofId = 0; dofId < 3; dofId++)
+ {
+ testScalar(v[2 * 3 + dofId], subVector[dofId]);
+ // Also test that the returned value are pointing to the correct data
+ EXPECT_EQ(&subVector[dofId], &v[2 * 3 + dofId]);
+ }
+}
+
+TYPED_TEST(AllDynamicVectorTests, getSubVectorBlocks)
+{
+ typedef typename TestFixture::Vector Vector;
+
+ Vector v, vInit, v2;
+ std::vector<size_t> nodeIds;
+ v.resize(18); v.setRandom(); vInit = v;
+ v2.resize(9); v2.setZero();
+ nodeIds.push_back(1);
+ nodeIds.push_back(3);
+ nodeIds.push_back(5);
+
+ EXPECT_EQ(18, v.size());
+ EXPECT_TRUE(v.isApprox(vInit));
+ EXPECT_EQ(9, v2.size());
+ EXPECT_TRUE(v2.isZero());
+ ASSERT_NO_THROW(SurgSim::Math::getSubVector(v, nodeIds, 3, &v2););
+ EXPECT_EQ(18, v.size());
+ EXPECT_TRUE(v.isApprox(vInit));
+ EXPECT_EQ(9, v2.size());
+ EXPECT_FALSE(v2.isZero());
+ for (int dofId = 0; dofId < 3; dofId++)
+ {
+ testScalar(v[3 + dofId], v2[dofId]);
+ }
+ for (int dofId = 3; dofId < 6; dofId++)
+ {
+ testScalar(v[9 + (dofId - 3)], v2[dofId]);
+ }
+ for (int dofId = 6; dofId < 9; dofId++)
+ {
+ testScalar(v[15 + (dofId - 6)], v2[dofId]);
+ }
+}
+
+template <class Vector>
+void testOrthonormalBasis(const Vector& i, const Vector& j, const Vector& k)
+{
+ typedef typename Vector::Scalar T;
+
+ T precision = Eigen::NumTraits<T>::dummy_precision();
+
+ EXPECT_NEAR(i.dot(j), 0.0, precision);
+ EXPECT_NEAR(i.dot(k), 0.0, precision);
+ EXPECT_NEAR(j.dot(i), 0.0, precision);
+ EXPECT_NEAR(j.dot(k), 0.0, precision);
+ EXPECT_NEAR(k.dot(i), 0.0, precision);
+ EXPECT_NEAR(k.dot(j), 0.0, precision);
+
+ EXPECT_TRUE(i.cross(j).isApprox(k));
+ EXPECT_TRUE(j.cross(k).isApprox(i));
+ EXPECT_TRUE(k.cross(i).isApprox(j));
+
+ EXPECT_NEAR(i.norm(), 1.0, precision);
+ EXPECT_NEAR(j.norm(), 1.0, precision);
+ EXPECT_NEAR(k.norm(), 1.0, precision);
+}
+
+TYPED_TEST(Vector3Tests, buildOrthonormalBasis)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename Vector3::Scalar T;
+ const int VOpt = Vector3::Options;
+
+ Vector3 i(static_cast<T>(1.54), static_cast<T>(-4.25), static_cast<T>(0.983));
+ Vector3 j, k;
+ T precision = Eigen::NumTraits<T>::dummy_precision();
+
+ // Assert if 1 parameter is nullptr
+ ASSERT_ANY_THROW((SurgSim::Math::buildOrthonormalBasis<T, VOpt>(nullptr, &j, &k)));
+ ASSERT_ANY_THROW((SurgSim::Math::buildOrthonormalBasis<T, VOpt>(&i, nullptr, &k)));
+ ASSERT_ANY_THROW((SurgSim::Math::buildOrthonormalBasis<T, VOpt>(&i, &j, nullptr)));
+
+ // Assert if 2 parameters are nullptr
+ ASSERT_ANY_THROW((SurgSim::Math::buildOrthonormalBasis<T, VOpt>(nullptr, nullptr, &k)));
+ ASSERT_ANY_THROW((SurgSim::Math::buildOrthonormalBasis<T, VOpt>(nullptr, &j, nullptr)));
+ ASSERT_ANY_THROW((SurgSim::Math::buildOrthonormalBasis<T, VOpt>(&i, nullptr, nullptr)));
+
+ // Assert if 3 parameters are nullptr
+ ASSERT_ANY_THROW((SurgSim::Math::buildOrthonormalBasis<T, VOpt>(nullptr, nullptr, nullptr)));
+
+ // Input parameter 'i' = (0, 0, 0)
+ Vector3 zero = Vector3::Zero();
+ ASSERT_NO_THROW(EXPECT_EQ(false, SurgSim::Math::buildOrthonormalBasis(&zero, &j, &k)));
+
+ // Input parameter 'i' = (0, 0, 0) + (epsilon, epsilon, epsilon)
+ Vector3 closeToZero = Vector3::Constant(precision);
+ ASSERT_NO_THROW(EXPECT_EQ(false, SurgSim::Math::buildOrthonormalBasis(&closeToZero, &j, &k)));
+
+ // Input parameter 'i' is already normalized
+ {
+ Vector3 i(static_cast<T>(1.54), static_cast<T>(-4.25), static_cast<T>(0.983));
+ Vector3 j, k;
+
+ i.normalize();
+ ASSERT_NO_THROW(EXPECT_EQ(true, SurgSim::Math::buildOrthonormalBasis(&i, &j, &k)));
+ testOrthonormalBasis(i, j, k);
+ }
+
+ // Input parameter 'i' is not already normalized
+ {
+ Vector3 i(static_cast<T>(1.54), static_cast<T>(-4.25), static_cast<T>(0.983));
+ Vector3 j, k;
+
+ ASSERT_NO_THROW(EXPECT_EQ(true, SurgSim::Math::buildOrthonormalBasis(&i, &j, &k)));
+ testOrthonormalBasis(i, j, k);
+ }
+}
diff --git a/SurgSim/Math/UnitTests/config.txt.in b/SurgSim/Math/UnitTests/config.txt.in
new file mode 100644
index 0000000..9c1cf05
--- /dev/null
+++ b/SurgSim/Math/UnitTests/config.txt.in
@@ -0,0 +1 @@
+${CMAKE_CURRENT_BINARY_DIR}/Data/
\ No newline at end of file
diff --git a/SurgSim/Math/Valid-inl.h b/SurgSim/Math/Valid-inl.h
new file mode 100644
index 0000000..2ef3708
--- /dev/null
+++ b/SurgSim/Math/Valid-inl.h
@@ -0,0 +1,275 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 isValid(), isSubnormal() and setSubnormalToZero().
+
+#ifndef SURGSIM_MATH_VALID_INL_H
+#define SURGSIM_MATH_VALID_INL_H
+
+#include <boost/math/special_functions/fpclassify.hpp>
+
+
+namespace SurgSim
+{
+namespace Math
+{
+
+namespace internal
+{
+
+template <typename T, class V>
+class PredicateAlwaysTrueVisitor
+{
+public:
+ typedef typename T::Index Index;
+ typedef typename T::Scalar Scalar;
+
+ PredicateAlwaysTrueVisitor() : m_result(true)
+ {
+ }
+
+ inline bool getResult() const
+ {
+ return m_result;
+ }
+
+ inline void init(const Scalar& value, Index i, Index j)
+ {
+ if (! V::evaluate(value))
+ {
+ m_result = false;
+ }
+ }
+
+ inline void operator()(const Scalar& value, Index i, Index j)
+ {
+ if (! V::evaluate(value))
+ {
+ m_result = false;
+ }
+ }
+
+private:
+ bool m_result;
+};
+
+template <typename T>
+class ValidVisitor : public PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>
+{
+public:
+ typedef typename PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>::Scalar Scalar;
+
+ static bool evaluate(const Scalar& value)
+ {
+ // The extra parentheses protect from pain if isfinite() is also defined as a macro.
+ return (boost::math::isfinite)(value);
+ }
+};
+
+template <typename T>
+class NonSubnormalVisitor : public PredicateAlwaysTrueVisitor<T, NonSubnormalVisitor<T>>
+{
+public:
+ typedef typename PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>::Scalar Scalar;
+
+ static bool evaluate(const Scalar& value)
+ {
+ // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
+ return ((boost::math::fpclassify)(value) != FP_SUBNORMAL);
+ }
+};
+
+}; // namespace internal
+
+
+inline bool isValid(float value)
+{
+ // The extra parentheses protect from pain if isfinite() is also defined as a macro.
+ return (boost::math::isfinite)(value);
+}
+
+inline bool isValid(double value)
+{
+ // The extra parentheses protect from pain if isfinite() is also defined as a macro.
+ return (boost::math::isfinite)(value);
+}
+
+template <typename T>
+inline bool isValid(const Eigen::DenseBase<T>& value)
+{
+ internal::ValidVisitor<T> visitor;
+ value.visit(visitor);
+ return visitor.getResult();
+}
+
+template <typename T>
+inline bool isValid(const Eigen::QuaternionBase<T>& value)
+{
+ return isValid(value.coeffs());
+}
+
+template <typename T>
+inline bool isValid(const Eigen::AngleAxis<T>& value)
+{
+ return isValid(value.angle()) && isValid(value.axis());
+}
+
+template <typename T>
+inline bool isValid(const Eigen::Rotation2D<T>& value)
+{
+ return isValid(value.angle());
+}
+
+template <typename T, int D, int M, int O>
+inline bool isValid(const Eigen::Transform<T, D, M, O>& value)
+{
+ return isValid(value.matrix());
+}
+
+inline bool isSubnormal(float value)
+{
+ // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
+ return ((boost::math::fpclassify)(value) == FP_SUBNORMAL);
+}
+
+inline bool isSubnormal(double value)
+{
+ // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
+ return ((boost::math::fpclassify)(value) == FP_SUBNORMAL);
+}
+
+template <typename T>
+inline bool isSubnormal(const Eigen::DenseBase<T>& value)
+{
+ internal::NonSubnormalVisitor<T> visitor;
+ value.visit(visitor);
+ // The visitor checks whether *all* entries are "non-subnormal", i.e. false means some are subnormal.
+ // This is a consequence of deriving from PredicateAlwaysTrueVisitor.
+ return ! visitor.getResult();
+}
+
+template <typename T>
+inline bool isSubnormal(const Eigen::QuaternionBase<T>& value)
+{
+ return isSubnormal(value.coeffs());
+}
+
+template <typename T>
+inline bool isSubnormal(const Eigen::AngleAxis<T>& value)
+{
+ return isSubnormal(value.angle()) || isSubnormal(value.axis());
+}
+
+template <typename T>
+inline bool isSubnormal(const Eigen::Rotation2D<T>& value)
+{
+ return isSubnormal(value.angle());
+}
+
+template <typename T, int D, int M, int O>
+inline bool isSubnormal(const Eigen::Transform<T, D, M, O>& value)
+{
+ return isSubnormal(value.matrix());
+}
+
+inline bool setSubnormalToZero(float* value)
+{
+ // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
+ if ((boost::math::fpclassify)(*value) != FP_SUBNORMAL)
+ {
+ return false;
+ }
+ else
+ {
+ *value = 0.0f;
+ return true;
+ }
+}
+
+inline bool setSubnormalToZero(double* value)
+{
+ // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
+ if ((boost::math::fpclassify)(*value) != FP_SUBNORMAL)
+ {
+ return false;
+ }
+ else
+ {
+ *value = 0.0;
+ return true;
+ }
+}
+
+template <typename T>
+inline bool setSubnormalToZero(Eigen::DenseBase<T>* value)
+{
+ if (! isSubnormal(*value)) // optimize for the common case where nothing is subnormal
+ {
+ return false;
+ }
+
+ // TODO(bert): This is a simple implementation; it could be much more optimized by e.g. unrolling loops along
+ // the lines of the implementation of Eigen::DenseBase<T>::visit(). Unfortunately, we can't just *use* Eigen's
+ // visitors here, because the visitor API doesn't allow modifying the values.
+
+ typedef typename Eigen::DenseBase<T>::Index Index;
+ const Index numColumns = value->cols();
+ const Index numRows = value->rows();
+
+ for (Index j = 0; j < numColumns; ++j)
+ {
+ for (Index i = 0; i < numRows; ++i)
+ {
+ // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
+ if ((boost::math::fpclassify)(value->coeffRef(i, j)) == FP_SUBNORMAL)
+ {
+ value->coeffRef(i, j) = 0;
+ }
+ }
+ }
+ return true;
+}
+
+template <typename T>
+inline bool setSubnormalToZero(Eigen::QuaternionBase<T>* value)
+{
+ return setSubnormalToZero(&(value->coeffs()));
+}
+
+template <typename T>
+inline bool setSubnormalToZero(Eigen::AngleAxis<T>* value)
+{
+ bool angleChanged = setSubnormalToZero(&(value->angle()));
+ bool axisChanged = setSubnormalToZero(&(value->axis()));
+ return angleChanged || axisChanged; // careful about short-circuiting!
+}
+
+template <typename T>
+inline bool setSubnormalToZero(Eigen::Rotation2D<T>* value)
+{
+ return setSubnormalToZero(&(value->angle()));
+}
+
+template <typename T, int D, int M, int O>
+inline bool setSubnormalToZero(Eigen::Transform<T, D, M, O>* value)
+{
+ return setSubnormalToZero(&(value->matrix()));
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_VALID_INL_H
diff --git a/SurgSim/Math/Valid.h b/SurgSim/Math/Valid.h
new file mode 100644
index 0000000..4954638
--- /dev/null
+++ b/SurgSim/Math/Valid.h
@@ -0,0 +1,237 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 isValid(), isSubnormal() and setSubnormalToZero().
+
+#ifndef SURGSIM_MATH_VALID_H
+#define SURGSIM_MATH_VALID_H
+
+#include <math.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// Check if a <code>float</code> value is valid.
+/// Zero, subnormal and normal numbers are valid; infinities and NaNs are not.
+/// \param value the value to check.
+/// \returns true if valid, false if not.
+inline bool isValid(float value);
+
+/// Check if a <code>double</code> value is valid.
+/// Zero, subnormal and normal numbers are valid; infinities and NaNs are not.
+/// \param value the value to check.
+/// \returns true if valid, false if not.
+inline bool isValid(double value);
+
+/// Check if a matrix or a vector is valid.
+/// These quantities are valid if all of their elements are valid.
+/// Zero, subnormal and normal numbers are valid; infinities and NaNs are not.
+/// \tparam T the base type used to describe the matrix or vector. Can usually be deduced.
+/// \param value the matrix or vector value to check.
+/// \returns true if valid, false if not.
+template <typename T>
+inline bool isValid(const Eigen::DenseBase<T>& value);
+
+/// Check if a quaternion is valid.
+/// Quaternions are valid if all of their components are valid.
+/// Zero, subnormal and normal numbers are valid; infinities and NaNs are not.
+/// \tparam T the base type used to describe the quaternion. Can usually be deduced.
+/// \param value the quaternion value to check.
+/// \returns true if valid, false if not.
+template <typename T>
+inline bool isValid(const Eigen::QuaternionBase<T>& value);
+
+/// Check if an angle/axis 3D rotation is valid.
+/// Angle/axis rotations are valid if the angle and the axis components are valid.
+/// Zero, subnormal and normal numbers are valid; infinities and NaNs are not.
+/// \tparam T the scalar type used to describe the rotation. Can usually be deduced.
+/// \param value the rotation value to check.
+/// \returns true if valid, false if not.
+template <typename T>
+inline bool isValid(const Eigen::AngleAxis<T>& value);
+
+/// Check if a 2D rotation is valid.
+/// 2D rotations are valid if the rotation angle is valid.
+/// Zero, subnormal and normal numbers are valid; infinities and NaNs are not.
+/// \tparam T the scalar type used to describe the rotation. Can usually be deduced.
+/// \param value the rotation value to check.
+/// \returns true if valid, false if not.
+template <typename T>
+inline bool isValid(const Eigen::Rotation2D<T>& value);
+
+/// Check if a transform is valid.
+/// Transforms are valid if all of their components are valid.
+/// Zero, subnormal and normal numbers are valid; infinities and NaNs are not.
+/// \tparam T the scalar type used to describe the transform. Can usually be deduced.
+/// \tparam D the dimension used to describe the transform. Can usually be deduced.
+/// \tparam M the mode value used to describe the transform. Can usually be deduced.
+/// \tparam O the options value used to describe the transform. Can usually be deduced.
+/// \param value the transform value to check.
+/// \returns true if valid, false if not.
+template <typename T, int D, int M, int O>
+inline bool isValid(const Eigen::Transform<T, D, M, O>& value);
+
+
+
+/// Check if a <code>float</code> value is subnormal.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<float>::%denorm_min() <=
+/// x < std::numeric_limits<float>::%min()</code>, and can result in very slow floating point calculations
+/// under some conditions.
+/// \param value the value to check.
+/// \returns true if subnormal; false if not (normal, zero, infinite or NaN).
+inline bool isSubnormal(float value);
+
+/// Check if a <code>double</code> value is subnormal.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<double>::%denorm_min() <=
+/// x < std::numeric_limits<double>::%min()</code>, and can result in very slow floating point
+/// calculations under some conditions.
+/// \param value the value to check.
+/// \returns true if subnormal; false if not (normal, zero, infinite or NaN).
+inline bool isSubnormal(double value);
+
+/// Check if a matrix or a vector contains any subnormal floating-point values.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<T>::%denorm_min() <= x
+/// < std::numeric_limits<T>::%min()</code>, and can result in very slow floating point calculations under
+/// some conditions.
+/// \tparam T the base type used to describe the matrix or vector. Can usually be deduced.
+/// \param value the matrix or vector value to check.
+/// \returns true if any value is subnormal; false if none are (i.e. each is normal, zero, infinite or NaN).
+template <typename T>
+inline bool isSubnormal(const Eigen::DenseBase<T>& value);
+
+/// Check if a quaternion contains any subnormal floating-point values.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<T>::%denorm_min() <= x
+/// < std::numeric_limits<T>::%min()</code>, and can result in very slow floating point calculations under
+/// some conditions.
+/// \tparam T the base type used to describe the quaternion. Can usually be deduced.
+/// \param value the quaternion value to check.
+/// \returns true if any value is subnormal; false if none are (i.e. each is normal, zero, infinite or NaN).
+template <typename T>
+inline bool isSubnormal(const Eigen::QuaternionBase<T>& value);
+
+/// Check if an angle/axis 3D rotation contains any subnormal floating-point values.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<T>::%denorm_min() <= x
+/// < std::numeric_limits<T>::%min()</code>, and can result in very slow floating point calculations under
+/// some conditions.
+/// \tparam T the scalar type used to describe the rotation. Can usually be deduced.
+/// \param value the rotation value to check.
+/// \returns true if any value is subnormal; false if none are (i.e. each is normal, zero, infinite or NaN).
+template <typename T>
+inline bool isSubnormal(const Eigen::AngleAxis<T>& value);
+
+/// Check if a 2D rotation is described by an angle that is subnormal.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<T>::%denorm_min() <= x
+/// < std::numeric_limits<T>::%min()</code>, and can result in very slow floating point calculations under
+/// some conditions.
+/// \tparam T the scalar type used to describe the rotation. Can usually be deduced.
+/// \param value the 2D rotation value to check.
+/// \returns true if the angle is subnormal; false if not (normal, zero, infinite or NaN).
+template <typename T>
+inline bool isSubnormal(const Eigen::Rotation2D<T>& value);
+
+/// Check if a transform contains any subnormal floating-point values.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<T>::%denorm_min() <= x
+/// < std::numeric_limits<T>::%min()</code>, and can result in very slow floating point calculations under
+/// some conditions.
+/// \tparam T the scalar type used to describe the transform. Can usually be deduced.
+/// \tparam D the dimension used to describe the transform. Can usually be deduced.
+/// \tparam M the mode value used to describe the transform. Can usually be deduced.
+/// \tparam O the options value used to describe the transform. Can usually be deduced.
+/// \param value the transform value to check.
+/// \returns true if any value is subnormal; false if none are (i.e. each is normal, zero, infinite or NaN).
+template <typename T, int D, int M, int O>
+inline bool isSubnormal(const Eigen::Transform<T, D, M, O>& value);
+
+
+
+/// If the <code>float</code> value is subnormal, set it to zero.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<float>::%denorm_min() <=
+/// x < std::numeric_limits<float>::%min()</code>, and can result in very slow floating point calculations
+/// under some conditions.
+/// \param [in,out] value the value to check and possibly modify.
+/// \returns true if the value was modified.
+inline bool setSubnormalToZero(float* value);
+
+/// If the <code>double</code> value is subnormal, set it to zero.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<double>::%denorm_min() <=
+/// x < std::numeric_limits<double>::%min()</code>, and can result in very slow floating point calculations
+/// under some conditions.
+/// \param [in,out] value the value to check and possibly modify.
+/// \returns true if the value was modified.
+inline bool setSubnormalToZero(double* value);
+
+/// Set all subnormal values in a matrix or a vector to zero.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<T>::%denorm_min() <= x
+/// < std::numeric_limits<T>::%min()</code>, and can result in very slow floating point calculations under
+/// some conditions.
+/// \tparam T the base type used to describe the matrix or vector. Can usually be deduced.
+/// \param [in,out] value the matrix or vector value to check and possibly modify.
+/// \returns true if any value was modified.
+template <typename T>
+inline bool setSubnormalToZero(Eigen::DenseBase<T>* value);
+
+/// Set all subnormal values in a quaternion to zero.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<T>::%denorm_min() <= x
+/// < std::numeric_limits<T>::%min()</code>, and can result in very slow floating point calculations under
+/// some conditions.
+/// \tparam T the base type used to describe the quaternion. Can usually be deduced.
+/// \param [in,out] value the quaternion value to check and possibly modify.
+/// \returns true if any value was modified.
+template <typename T>
+inline bool setSubnormalToZero(Eigen::QuaternionBase<T>* value);
+
+/// Set all subnormal values in an angle/axis 3D rotation to zero.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<T>::%denorm_min() <= x
+/// < std::numeric_limits<T>::%min()</code>, and can result in very slow floating point calculations under
+/// some conditions.
+/// \tparam T the scalar type used to describe the rotation. Can usually be deduced.
+/// \param [in,out] value the rotation value to check and possibly modify.
+/// \returns true if any value was modified.
+template <typename T>
+inline bool setSubnormalToZero(Eigen::AngleAxis<T>* value);
+
+/// If the angle of a 2D rotation is subnormal, set it to zero.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<T>::%denorm_min() <= x
+/// < std::numeric_limits<T>::%min()</code>, and can result in very slow floating point calculations under
+/// some conditions.
+/// \tparam T the scalar type used to describe the rotation. Can usually be deduced.
+/// \param [in,out] value the rotation value to check and possibly modify.
+/// \returns true if the value was modified.
+template <typename T>
+inline bool setSubnormalToZero(Eigen::Rotation2D<T>* value);
+
+/// Set all subnormal values in a transform to zero.
+/// Subnormal values have absolute values in the range <code>std::numeric_limits<T>::%denorm_min() <= x
+/// < std::numeric_limits<T>::%min()</code>, and can result in very slow floating point calculations under
+/// some conditions.
+/// \tparam T the base type used to describe the transform. Can usually be deduced.
+/// \param [in,out] value the transform value to check and possibly modify.
+/// \returns true if any value was modified.
+template <typename T, int D, int M, int O>
+inline bool setSubnormalToZero(Eigen::Transform<T, D, M, O>* value);
+
+}; // namespace Math
+}; // namespace SurgSim
+
+
+#include "SurgSim/Math/Valid-inl.h"
+
+
+#endif // SURGSIM_MATH_VALID_H
diff --git a/SurgSim/Math/Vector.h b/SurgSim/Math/Vector.h
new file mode 100644
index 0000000..68a919e
--- /dev/null
+++ b/SurgSim/Math/Vector.h
@@ -0,0 +1,199 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 small fixed-size vector types.
+
+#ifndef SURGSIM_MATH_VECTOR_H
+#define SURGSIM_MATH_VECTOR_H
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// A 2D vector of floats.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<float, 2, 1> Vector2f;
+
+/// A 3D vector of floats.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<float, 3, 1> Vector3f;
+
+/// A 4D vector of floats.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<float, 4, 1> Vector4f;
+
+/// A 6D vector of floats.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<float, 6, 1> Vector6f;
+
+/// A 2D vector of doubles.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<double, 2, 1> Vector2d;
+
+/// A 3D vector of doubles.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<double, 3, 1> Vector3d;
+
+/// A 4D vector of doubles.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<double, 4, 1> Vector4d;
+
+/// A 6D matrix of doubles.
+/// This type (and any structs that contain it) can be safely allocated via new.
+typedef Eigen::Matrix<double, 6, 1> Vector6d;
+
+/// A dynamic size column vector
+typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
+
+/// Helper method to add a sub-vector into a vector, for the sake of clarity
+/// \tparam Vector The vector type
+/// \tparam SubVector The sub-vector type
+/// \param subVector The sub-vector
+/// \param blockId The block index in vector
+/// \param blockSize The block size
+/// \param[out] vector The vector to add the sub-vector into
+template <class Vector, class SubVector>
+void addSubVector(const SubVector& subVector, size_t blockId, size_t blockSize, Vector* vector)
+{
+ vector->segment(blockSize * blockId, blockSize) += subVector;
+}
+
+/// Helper method to add a sub-vector per block into a vector, for the sake of clarity
+/// \tparam Vector The vector type
+/// \tparam SubVector The sub-vector type
+/// \param subVector The sub-vector (containing all the blocks)
+/// \param blockIds Vector of block indices (for accessing vector) corresponding to the blocks in sub-vector
+/// \param blockSize The block size
+/// \param[out] vector The vector to add the sub-vector blocks into
+template <class Vector, class SubVector>
+void addSubVector(const SubVector& subVector, const std::vector<size_t> blockIds, size_t blockSize, Vector* vector)
+{
+ const size_t numBlocks = blockIds.size();
+
+ for (size_t block = 0; block < numBlocks; block++)
+ {
+ size_t blockId = blockIds[block];
+
+ vector->segment(blockSize * blockId, blockSize) += subVector.segment(blockSize * block, blockSize);
+ }
+}
+
+/// Helper method to set a sub-vector into a vector, for the sake of clarity
+/// \tparam Vector The vector type
+/// \tparam SubVector The sub-vector type
+/// \param subVector The sub-vector
+/// \param blockId The block index in vector
+/// \param blockSize The size of the sub-vector
+/// \param[out] vector The vector to set the sub-vector into
+template <class Vector, class SubVector>
+void setSubVector(const SubVector& subVector, size_t blockId, size_t blockSize, Vector* vector)
+{
+ vector->segment(blockSize * blockId, blockSize) = subVector;
+}
+
+/// Helper method to access a sub-vector from a vector, for the sake of clarity
+/// \tparam Vector The vector type to get the sub-vector from
+/// \param vector The vector to get the sub-vector from
+/// \param blockId The block index
+/// \param blockSize The block size
+/// \return The requested sub-vector
+/// \note Disable cpplint warnings for use of non-const reference
+/// \note Eigen has a specific type for VectorBlock that we want to return with read/write access
+/// \note therefore the Vector from which the VectorBlock is built from must not be const
+template <class Vector>
+Eigen::VectorBlock<Vector> getSubVector(Vector& vector, size_t blockId, size_t blockSize) // NOLINT
+{
+ return vector.segment(blockSize * blockId, blockSize);
+}
+
+/// Helper method to get a sub-vector per block from a vector, for the sake of clarity
+/// \tparam Vector The vector type
+/// \tparam SubVector The sub-vector type
+/// \param vector The vector (containing the blocks in a sparse manner)
+/// \param blockIds Vector of block indices (for accessing vector) corresponding to the blocks in vector
+/// \param blockSize The block size
+/// \param[out] subVector The sub-vector to store the requested blocks (blockIds) from vector into
+template <class Vector, class SubVector>
+void getSubVector(const Vector& vector, const std::vector<size_t> blockIds, size_t blockSize, SubVector* subVector)
+{
+ const size_t numBlocks = blockIds.size();
+
+ for (size_t block = 0; block < numBlocks; block++)
+ {
+ size_t blockId = blockIds[block];
+
+ subVector->segment(blockSize * block, blockSize) = vector.segment(blockSize * blockId, blockSize);
+ }
+}
+
+/// Interpolate (slerp) between 2 vectors
+/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
+/// \tparam size the size of the vectors. Can be deduced.
+/// \tparam TOpt the option flags (alignment etc.) used for the Vector arguments. Can be deduced.
+/// \param previous The starting vector (at time 0.0).
+/// \param next The ending vector (at time 1.0).
+/// \param t The interpolation time requested. Within [0..1], although note bounds are not checked.
+/// \returns the transform resulting in the slerp interpolation at time t.
+/// \note t=0 => returns vector 'previous'
+/// \note t=1 => returns vector 'next'
+template <typename T, int size, int TOpt>
+Eigen::Matrix<T, size, 1, TOpt> interpolate(
+ const Eigen::Matrix<T, size, 1, TOpt> &previous,
+ const Eigen::Matrix<T, size, 1, TOpt> &next,
+ T t)
+{
+ return previous + t * (next - previous);
+}
+
+/// Helper method to construct an orthonormal basis (i, j, k) given the 1st vector direction
+/// \tparam T the numeric data type used for the vector argument. Can usually be deduced.
+/// \tparam VOpt the option flags (alignment etc.) used for the vector argument. Can be deduced.
+/// \param[in, out] i Should provide the 1st direction on input. The 1st vector of the basis (i, j, k) on output.
+/// \param[out] j, k The 2nd and 3rd orthonormal vectors of the basis (i, j, k)
+/// \return True if (i, j, k) has been built successfully, False if 'i' is a (or close to a) null vector
+/// \note If any of the parameter is a nullptr, an exception will be raised
+template <class T, int VOpt>
+bool buildOrthonormalBasis(Eigen::Matrix<T, 3, 1, VOpt>* i,
+ Eigen::Matrix<T, 3, 1, VOpt>* j,
+ Eigen::Matrix<T, 3, 1, VOpt>* k)
+{
+ SURGSIM_ASSERT(i != nullptr) << "Parameter [in, out] 'i' is a nullptr";
+ SURGSIM_ASSERT(j != nullptr) << "Parameter [out] 'j' is a nullptr";
+ SURGSIM_ASSERT(k != nullptr) << "Parameter [out] 'k' is a nullptr";
+
+ if (i->isZero())
+ {
+ return false;
+ }
+
+ i->normalize();
+ *j = i->unitOrthogonal();
+ *k = i->cross(*j);
+
+ return true;
+}
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_VECTOR_H
diff --git a/SurgSim/Physics/BuildMlcp.cpp b/SurgSim/Physics/BuildMlcp.cpp
new file mode 100644
index 0000000..a524f4b
--- /dev/null
+++ b/SurgSim/Physics/BuildMlcp.cpp
@@ -0,0 +1,119 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <Eigen/Core>
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+
+#include "SurgSim/Physics/BuildMlcp.h"
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+BuildMlcp::BuildMlcp(bool doCopyState) : Computation(doCopyState)
+{}
+
+BuildMlcp::~BuildMlcp()
+{}
+
+std::shared_ptr<PhysicsManagerState>
+ BuildMlcp::doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+{
+ // Copy state to new state
+ std::shared_ptr<PhysicsManagerState> result = state;
+ MlcpMapping<Representation> representationsMapping;
+ MlcpMapping<Constraint> constraintsMapping;
+
+ size_t numAtomicConstraint = 0;
+ size_t numConstraint = 0;
+ size_t numDof = 0;
+
+ // Calculate numAtomicConstraint and numConstraint
+ auto const activeConstraints = result->getActiveConstraints();
+ numConstraint = activeConstraints.size();
+ for (auto it = activeConstraints.cbegin(); it != activeConstraints.cend(); it++)
+ {
+ constraintsMapping.setValue((*it).get(), static_cast<ptrdiff_t>(numAtomicConstraint));
+ numAtomicConstraint += (*it)->getNumDof();
+ }
+ result->setConstraintsMapping(constraintsMapping);
+
+ // Calculate numDof size
+ auto const activeRepresentations = result->getActiveRepresentations();
+ for (auto it = activeRepresentations.cbegin(); it != activeRepresentations.cend(); it++)
+ {
+ representationsMapping.setValue((*it).get(), numDof);
+ numDof += (*it)->getNumDof();
+ }
+ result->setRepresentationsMapping(representationsMapping);
+
+ // Resize the Mlcp problem
+ result->getMlcpProblem().A.resize(numAtomicConstraint, numAtomicConstraint);
+ result->getMlcpProblem().A.setZero();
+ result->getMlcpProblem().b.resize(numAtomicConstraint);
+ result->getMlcpProblem().b.setZero();
+ result->getMlcpProblem().H.resize(numAtomicConstraint, numDof);
+ result->getMlcpProblem().H.setZero();
+ result->getMlcpProblem().CHt.resize(numDof, numAtomicConstraint);
+ result->getMlcpProblem().CHt.setZero();
+ result->getMlcpProblem().mu.resize(numConstraint);
+ result->getMlcpProblem().mu.setZero();
+ result->getMlcpProblem().constraintTypes.clear();
+
+ // Resize the Mlcp solution
+ result->getMlcpSolution().dofCorrection.resize(numDof);
+ result->getMlcpSolution().dofCorrection.setZero();
+ result->getMlcpSolution().x.resize(numAtomicConstraint);
+ result->getMlcpSolution().x.setZero();
+
+ // Fill up the Mlcp problem
+ for (auto it = activeConstraints.begin(); it != activeConstraints.end(); it++)
+ {
+ ptrdiff_t indexConstraint = result->getConstraintsMapping().getValue((*it).get());
+ SURGSIM_ASSERT(indexConstraint >= 0) << "Index for constraint is invalid: " << indexConstraint << std::endl;
+
+ std::shared_ptr<ConstraintImplementation> side0 = (*it)->getImplementations().first;
+ std::shared_ptr<ConstraintImplementation> side1 = (*it)->getImplementations().second;
+ SURGSIM_ASSERT(side0) << "Constraint does not have a side0" << std::endl;
+ SURGSIM_ASSERT(side1) << "Constraint does not have a side1" << std::endl;
+ std::shared_ptr<Localization> localization0 = (*it)->getLocalizations().first;
+ std::shared_ptr<Localization> localization1 = (*it)->getLocalizations().second;
+ SURGSIM_ASSERT(localization0) << "ConstraintImplementation does not have a localization on side0";
+ SURGSIM_ASSERT(localization1) << "ConstraintImplementation does not have a localization on side1";
+ const MlcpMapping<Representation>& mapping = result->getRepresentationsMapping();
+ ptrdiff_t indexRepresentation0 = mapping.getValue(localization0->getRepresentation().get());
+ ptrdiff_t indexRepresentation1 = mapping.getValue(localization1->getRepresentation().get());
+ SURGSIM_ASSERT(indexRepresentation0 >= 0) << "Index for representation 0 is invalid: " <<
+ indexRepresentation0;
+ SURGSIM_ASSERT(indexRepresentation1 >= 0) << "Index for representation 1 is invalid: " <<
+ indexRepresentation1;
+
+ (*it)->build(dt, &result->getMlcpProblem(), indexRepresentation0, indexRepresentation1, indexConstraint);
+ }
+
+ return result;
+}
+
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/BuildMlcp.h b/SurgSim/Physics/BuildMlcp.h
new file mode 100644
index 0000000..698c8a6
--- /dev/null
+++ b/SurgSim/Physics/BuildMlcp.h
@@ -0,0 +1,46 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_BUILDMLCP_H
+#define SURGSIM_PHYSICS_BUILDMLCP_H
+
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+/// Build an mlcp from a list of constraints stored in a PhysicsManagerState
+class BuildMlcp : public Computation
+{
+public:
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit BuildMlcp(bool doCopyState = false);
+
+ /// Destructor
+ virtual ~BuildMlcp();
+
+protected:
+ /// Override doUpdate from superclass
+ virtual std::shared_ptr<PhysicsManagerState>
+ doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+};
+
+}; // namespace Physics
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_BUILDMLCP_H
diff --git a/SurgSim/Physics/CMakeLists.txt b/SurgSim/Physics/CMakeLists.txt
new file mode 100644
index 0000000..751cf0c
--- /dev/null
+++ b/SurgSim/Physics/CMakeLists.txt
@@ -0,0 +1,172 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+set(SURGSIM_PHYSICS_SOURCES
+ BuildMlcp.cpp
+ Computation.cpp
+ Constraint.cpp
+ ConstraintComponent.cpp
+ ConstraintData.cpp
+ ConstraintImplementation.cpp
+ ConstraintImplementationFactory.cpp
+ ContactConstraintGeneration.cpp
+ DcdCollision.cpp
+ DeformableCollisionRepresentation.cpp
+ DeformableRepresentation.cpp
+ Fem1DElementBeam.cpp
+ Fem1DPlyReaderDelegate.cpp
+ Fem1DRepresentation.cpp
+ Fem1DRepresentationLocalization.cpp
+ Fem2DElementTriangle.cpp
+ Fem2DPlyReaderDelegate.cpp
+ Fem2DRepresentation.cpp
+ Fem2DRepresentationLocalization.cpp
+ Fem3DElementCorotationalTetrahedron.cpp
+ Fem3DElementCube.cpp
+ Fem3DElementTetrahedron.cpp
+ Fem3DPlyReaderDelegate.cpp
+ Fem3DRepresentation.cpp
+ Fem3DRepresentationBilateral3D.cpp
+ Fem3DRepresentationContact.cpp
+ Fem3DRepresentationLocalization.cpp
+ FemElement.cpp
+ FemPlyReaderDelegate.cpp
+ FemRepresentation.cpp
+ FemRepresentationParameters.cpp
+ FixedRepresentation.cpp
+ FixedRepresentationBilateral3D.cpp
+ FixedRepresentationContact.cpp
+ FreeMotion.cpp
+ LinearSpring.cpp
+ Localization.cpp
+ MassSpringRepresentation.cpp
+ MassSpringRepresentationContact.cpp
+ MassSpringRepresentationLocalization.cpp
+ MlcpPhysicsProblem.cpp
+ PhysicsConvert.cpp
+ PhysicsManager.cpp
+ PhysicsManagerState.cpp
+ PostUpdate.cpp
+ PreUpdate.cpp
+ PushResults.cpp
+ Representation.cpp
+ RigidCollisionRepresentation.cpp
+ RigidRepresentation.cpp
+ RigidRepresentationBase.cpp
+ RigidRepresentationBilateral3D.cpp
+ RigidRepresentationContact.cpp
+ RigidRepresentationLocalization.cpp
+ RigidRepresentationState.cpp
+ SolveMlcp.cpp
+ UpdateCollisionRepresentations.cpp
+ VirtualToolCoupler.cpp
+)
+
+set(SURGSIM_PHYSICS_HEADERS
+ BuildMlcp.h
+ Computation.h
+ Constraint.h
+ ConstraintComponent.h
+ ConstraintData.h
+ ConstraintImplementation.h
+ ConstraintImplementationFactory.h
+ ContactConstraintData.h
+ ContactConstraintGeneration.h
+ DcdCollision.h
+ DeformableCollisionRepresentation.h
+ DeformableRepresentation.h
+ Fem1DElementBeam.h
+ Fem1DPlyReaderDelegate.h
+ Fem1DRepresentation.h
+ Fem1DRepresentationLocalization.h
+ Fem2DElementTriangle.h
+ Fem2DPlyReaderDelegate.h
+ Fem2DRepresentation.h
+ Fem2DRepresentationLocalization.h
+ Fem3DElementCorotationalTetrahedron.h
+ Fem3DElementCube.h
+ Fem3DElementTetrahedron.h
+ Fem3DPlyReaderDelegate.h
+ Fem3DRepresentation.h
+ Fem3DRepresentationBilateral3D.h
+ Fem3DRepresentationContact.h
+ Fem3DRepresentationLocalization.h
+ FemElement.h
+ FemPlyReaderDelegate.h
+ FemRepresentation.h
+ FemRepresentationParameters.h
+ FixedRepresentation.h
+ FixedRepresentationBilateral3D.h
+ FixedRepresentationContact.h
+ FixedRepresentationLocalization.h
+ FreeMotion.h
+ LinearSpring.h
+ Localization.h
+ Mass.h
+ MassSpringRepresentation.h
+ MassSpringRepresentationContact.h
+ MassSpringRepresentationLocalization.h
+ MlcpMapping.h
+ MlcpPhysicsProblem.h
+ MlcpPhysicsProblem-inl.h
+ MlcpPhysicsSolution.h
+ PhysicsConvert.h
+ PhysicsManager.h
+ PhysicsManagerState.h
+ PostUpdate.h
+ PreUpdate.h
+ PushResults.h
+ Representation.h
+ RigidCollisionRepresentation.h
+ RigidRepresentation.h
+ RigidRepresentationBase.h
+ RigidRepresentationBase-inl.h
+ RigidRepresentationBilateral3D.h
+ RigidRepresentationContact.h
+ RigidRepresentationLocalization.h
+ RigidRepresentationState.h
+ SolveMlcp.h
+ Spring.h
+ UpdateCollisionRepresentations.h
+ VirtualToolCoupler.h
+)
+
+surgsim_add_library(
+ SurgSimPhysics
+ "${SURGSIM_PHYSICS_SOURCES}"
+ "${SURGSIM_PHYSICS_HEADERS}"
+ "SurgSim/Physics"
+)
+
+SET(LIBS
+ SurgSimCollision
+)
+
+target_link_libraries(SurgSimPhysics ${LIBS})
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+
+ if(BUILD_RENDER_TESTING)
+ add_subdirectory(RenderTests)
+ endif()
+
+ if(BUILD_PERFORMANCE_TESTING)
+ add_subdirectory(PerformanceTests)
+ endif()
+endif()
+
+set_target_properties(SurgSimPhysics PROPERTIES FOLDER "Physics")
diff --git a/SurgSim/Physics/Computation.cpp b/SurgSim/Physics/Computation.cpp
new file mode 100644
index 0000000..caac79f
--- /dev/null
+++ b/SurgSim/Physics/Computation.cpp
@@ -0,0 +1,99 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/Computation.h"
+
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+Computation::Computation(bool doCopyState) : m_copyState(doCopyState)
+{
+
+}
+
+Computation::~Computation()
+{
+
+}
+
+std::shared_ptr<PhysicsManagerState> Computation::update(double dt, const std::shared_ptr<PhysicsManagerState>& state)
+{
+ return std::move(doUpdate(dt,std::move(preparePhysicsState(state))));
+}
+
+void Computation::setDoCopyState(bool val)
+{
+ m_copyState = val;
+}
+
+bool Computation::isCopyingState()
+{
+ return m_copyState;
+}
+
+std::shared_ptr<PhysicsManagerState> Computation::preparePhysicsState(const std::shared_ptr<PhysicsManagerState>& state)
+{
+ // Compile the list of active representations and set it on the state.
+ std::vector<std::shared_ptr<Representation>> activeRepresentations;
+ auto representations = state->getRepresentations();
+ activeRepresentations.reserve(representations.size());
+ for (auto it = representations.begin(); it != representations.end(); ++it)
+ {
+ if ((*it)->isActive())
+ {
+ activeRepresentations.push_back(*it);
+ }
+ }
+ state->setActiveRepresentations(activeRepresentations);
+
+ // Compile the list of active constraints and set it on the state.
+ std::vector<std::shared_ptr<Constraint>> activeConstraints;
+ size_t size = 0;
+ int constraintTypeEnd = static_cast<int>(CONSTRAINT_GROUP_TYPE_COUNT);
+ for (int constraintType = 0 ; constraintType < constraintTypeEnd ; constraintType++)
+ {
+ size += state->getConstraintGroup(constraintType).size();
+ }
+ activeConstraints.reserve(size);
+
+ for (int constraintType = 0 ; constraintType < constraintTypeEnd ; constraintType++)
+ {
+ auto constraints = state->getConstraintGroup(constraintType);
+ for (auto it = constraints.begin(); it != constraints.end(); it++)
+ {
+ if ((*it)->isActive())
+ {
+ activeConstraints.push_back(*it);
+ }
+ }
+ }
+ state->setActiveConstraints(activeConstraints);
+
+ if (m_copyState)
+ {
+ return std::move(std::make_shared<PhysicsManagerState>(*state));
+ }
+ else
+ {
+ return std::move(state);
+ }
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/Computation.h b/SurgSim/Physics/Computation.h
new file mode 100644
index 0000000..bd9cbf9
--- /dev/null
+++ b/SurgSim/Physics/Computation.h
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_COMPUTATION_H
+#define SURGSIM_PHYSICS_COMPUTATION_H
+
+#include <vector>
+#include <memory>
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class PhysicsManagerState;
+
+/// Encapsulates a calculation over a selection of objects, needs to be subclassed to be used
+class Computation
+{
+public:
+
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit Computation(bool doCopyState);
+
+ /// Destructor
+ virtual ~Computation();
+
+ /// Public Interface execute this objects computations, dt is the time from the last update
+ /// call in seconds.
+ /// \param dt The time passed from the last update in seconds.
+ /// \param state The physics state.
+ /// \return The changed state of the, depending on the setting of doCopyState this is either the same instance
+ /// or a copied instance of the physics state.
+ std::shared_ptr<PhysicsManagerState> update(double dt, const std::shared_ptr<PhysicsManagerState>& state);
+
+ /// Sets up whether the computation will copy the state of PhysicsManagerState before executing.
+ /// \param val Whether to create a copy of the PhysicsState before running the update fuction.
+ void setDoCopyState(bool val);
+
+ /// Query if this object is copying the PhysicsManagerState.
+ /// \return true if copying the state, false if not.
+ bool isCopyingState();
+
+protected:
+
+ /// Override this function to implement the computations specific behavior
+ virtual std::shared_ptr<PhysicsManagerState> doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state) = 0;
+
+private:
+ bool m_copyState;
+
+ /// Copy the PhysicsManagerState object when isCopyingState() is true
+ std::shared_ptr<PhysicsManagerState> preparePhysicsState(const std::shared_ptr<PhysicsManagerState>& state);
+};
+
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_COMPUTATION_H
diff --git a/SurgSim/Physics/Constraint.cpp b/SurgSim/Physics/Constraint.cpp
new file mode 100644
index 0000000..a789fad
--- /dev/null
+++ b/SurgSim/Physics/Constraint.cpp
@@ -0,0 +1,160 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/Localization.h"
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Constraint::Constraint(
+ std::shared_ptr<ConstraintData> data,
+ std::shared_ptr<ConstraintImplementation> implementation0,
+ std::shared_ptr<Localization> localization0,
+ std::shared_ptr<ConstraintImplementation> implementation1,
+ std::shared_ptr<Localization> localization1)
+{
+ setInformation(data, implementation0, localization0, implementation1, localization1);
+}
+
+Constraint::~Constraint()
+{
+}
+
+
+const std::pair<std::shared_ptr<ConstraintImplementation>, std::shared_ptr<ConstraintImplementation>>&
+ Constraint::getImplementations() const
+{
+ return m_implementations;
+}
+
+const std::pair<std::shared_ptr<Localization>, std::shared_ptr<Localization>>& Constraint::getLocalizations() const
+{
+ return m_localizations;
+}
+
+
+std::shared_ptr<ConstraintData> Constraint::getData() const
+{
+ return m_data;
+}
+
+size_t Constraint::getNumDof() const
+{
+ return m_numDof;
+}
+
+SurgSim::Math::MlcpConstraintType Constraint::getType()
+{
+ return m_constraintType;
+}
+
+void Constraint::build(double dt,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation0,
+ size_t indexOfRepresentation1,
+ size_t indexOfConstraint)
+{
+ doBuild(dt, *m_data.get(), mlcp, indexOfRepresentation0, indexOfRepresentation1, indexOfConstraint);
+
+ m_implementations.first->build(
+ dt,
+ *m_data.get(),
+ m_localizations.first,
+ mlcp,
+ indexOfRepresentation0,
+ indexOfConstraint,
+ CONSTRAINT_POSITIVE_SIDE);
+
+ m_implementations.second->build(
+ dt,
+ *m_data.get(),
+ m_localizations.second,
+ mlcp,
+ indexOfRepresentation1,
+ indexOfConstraint,
+ CONSTRAINT_NEGATIVE_SIDE);
+
+ mlcp->constraintTypes.push_back(m_constraintType);
+}
+
+bool Constraint::isActive()
+{
+ return m_localizations.first->getRepresentation()->isActive() &&
+ m_localizations.second->getRepresentation()->isActive();
+}
+
+void Constraint::doBuild(double dt,
+ const ConstraintData& data,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation0,
+ size_t indexOfRepresentation1,
+ size_t indexOfConstraint)
+{
+}
+
+void Constraint::setInformation(
+ std::shared_ptr<ConstraintData> data,
+ std::shared_ptr<ConstraintImplementation> implementation0,
+ std::shared_ptr<Localization> localization0,
+ std::shared_ptr<ConstraintImplementation> implementation1,
+ std::shared_ptr<Localization> localization1)
+{
+ SURGSIM_ASSERT(data != nullptr) << "ConstraintData can't be nullptr";
+ SURGSIM_ASSERT(implementation0 != nullptr) << "First implementation can't be nullptr";
+ SURGSIM_ASSERT(localization0 != nullptr) << "First localization can't be nullptr";
+ SURGSIM_ASSERT(implementation1 != nullptr) << "Second implementation can't be nullptr";
+ SURGSIM_ASSERT(localization1 != nullptr) << "Second localization can't be nullptr";
+
+ SURGSIM_ASSERT(localization0->getRepresentation() != nullptr)
+ << "First localization must have a Representation";
+ SURGSIM_ASSERT(localization1->getRepresentation() != nullptr)
+ << "Second localization must have a Representation";
+
+ SURGSIM_ASSERT(implementation0->getRepresentationType() == localization0->getRepresentation()->getType())
+ << "The representation associated with the first localization must be compatible with the first "
+ "implementation.";
+ SURGSIM_ASSERT(implementation1->getRepresentationType() == localization1->getRepresentation()->getType())
+ << "The representation associated with the second localization must be compatible with the second "
+ "implementation.";
+
+ SURGSIM_ASSERT(implementation0->getMlcpConstraintType() != SurgSim::Math::MLCP_INVALID_CONSTRAINT) <<
+ "First implementation has an invalid constraint type";
+ SURGSIM_ASSERT(implementation1->getMlcpConstraintType() != SurgSim::Math::MLCP_INVALID_CONSTRAINT) <<
+ "Second implementation has an invalid constraint type";
+
+ SURGSIM_ASSERT(implementation0->getMlcpConstraintType() == implementation1->getMlcpConstraintType()) <<
+ "Implementations have incompatible implementations first( " << implementation0->getMlcpConstraintType() <<
+ " != " << implementation1->getMlcpConstraintType() << " )";
+ SURGSIM_ASSERT(implementation0->getNumDof() == implementation1->getNumDof()) <<
+ "Both sides of the constraint should have the same number of Dof ("<< implementation0->getNumDof() <<
+ " != " << implementation1->getNumDof() <<")";
+
+ m_data = data;
+ m_implementations = std::make_pair(implementation0, implementation1);
+ m_localizations = std::make_pair(localization0, localization1);
+ m_numDof = implementation0->getNumDof();
+ m_constraintType = implementation0->getMlcpConstraintType();
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/Constraint.h b/SurgSim/Physics/Constraint.h
new file mode 100644
index 0000000..f2d71bc
--- /dev/null
+++ b/SurgSim/Physics/Constraint.h
@@ -0,0 +1,132 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_CONSTRAINT_H
+#define SURGSIM_PHYSICS_CONSTRAINT_H
+
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+
+#include <memory>
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Base class for all physics constraints. Contains data specific to the constraint and a pair of implementations.
+class Constraint
+{
+public:
+ /// Sets all the values for this constraints, does validation on the parameters and will throw it something
+ /// is wrong with the constraint.
+ /// \param data The data for this constraint.
+ /// \param implementation0, implementation1 Both sides implementation of the constraint.
+ /// \param localization0, localization1 Both localizations of the representations involved in this constraint.
+ Constraint(
+ std::shared_ptr<ConstraintData> data,
+ std::shared_ptr<ConstraintImplementation> implementation0,
+ std::shared_ptr<Localization> localization0,
+ std::shared_ptr<ConstraintImplementation> implementation1,
+ std::shared_ptr<Localization> localization1);
+
+ /// Destructor
+ virtual ~Constraint();
+
+ /// Sets all the values for this constraints, does validation on the parameters and will throw it something
+ /// is wrong with the constraint.
+ /// \param data The data for this constraint.
+ /// \param implementation0, implementation1 Both sides implementation of the constraint.
+ /// \param localization0, localization1 Both localizations of the representations involved in this constraint.
+ void setInformation(
+ std::shared_ptr<ConstraintData> data,
+ std::shared_ptr<ConstraintImplementation> implementation0,
+ std::shared_ptr<Localization> localization0,
+ std::shared_ptr<ConstraintImplementation> implementation1,
+ std::shared_ptr<Localization> localization1);
+
+ /// Gets both sides implementation as a pair.
+ /// \return the pair of implementations forming this constraint.
+ const std::pair<std::shared_ptr<ConstraintImplementation>, std::shared_ptr<ConstraintImplementation>>&
+ getImplementations() const;
+
+ /// Gets both sides Localization as a pair.
+ /// \return the pair of localizations forming this constraint.
+ const std::pair<std::shared_ptr<Localization>, std::shared_ptr<Localization>>&
+ getLocalizations() const;
+
+ /// Gets the data associated to this constraint.
+ /// \return The data associated to this constraint.
+ std::shared_ptr<ConstraintData> getData() const;
+
+ /// Gets the number of degree of freedom for this constraint.
+ /// \return The number of degree of freedom for this constraint.
+ size_t getNumDof() const;
+
+ /// Gets the ConstraintType for this constraint.
+ /// \return The type.
+ SurgSim::Math::MlcpConstraintType getType();
+
+ /// Builds subset of an Mlcp physics problem associated to this constraint.
+ /// \param dt The time step.
+ /// \param [in,out] mlcpPhysicsProblem The Mlcp physics problem to be filled up.
+ /// \param indexOfRepresentation0 The index of the 1st representation in the Mlcp.
+ /// \param indexOfRepresentation1 The index of the 2nd representation in the Mlcp.
+ /// \param indexOfConstraint The index of this constraint in the Mlcp.
+ void build(double dt,
+ MlcpPhysicsProblem* mlcpPhysicsProblem,
+ size_t indexOfRepresentation0,
+ size_t indexOfRepresentation1,
+ size_t indexOfConstraint);
+
+ /// \return Whether this constraint is active.
+ bool isActive();
+
+private:
+ /// Specific data associated to this constraint
+ std::shared_ptr<ConstraintData> m_data;
+
+ /// Pair of implementations defining the 2 sides of the constraint
+ std::pair<std::shared_ptr<ConstraintImplementation>, std::shared_ptr<ConstraintImplementation>> m_implementations;
+ std::pair<std::shared_ptr<Localization>, std::shared_ptr<Localization>> m_localizations;
+
+ /// The degrees of freedom that this constraint has
+ size_t m_numDof;
+
+ /// The type of this constraint
+ SurgSim::Math::MlcpConstraintType m_constraintType;
+
+ /// Builds subset of an Mlcp physics problem associated to this constraint user-defined call for extra treatment
+ /// \param dt The time step
+ /// \param data The data specific to this constraint
+ /// \param [in,out] mlcpPhysicsProblem The Mlcp physics problem to be filled up
+ /// \param indexOfRepresentation0 The index of the 1st representation in the Mlcp
+ /// \param indexOfRepresentation1 The index of the 2nd representation in the Mlcp
+ /// \param indexOfConstraint The index of this constraint in the Mlcp
+ virtual void doBuild(double dt,
+ const ConstraintData& data,
+ MlcpPhysicsProblem* mlcpPhysicsProblem,
+ size_t indexOfRepresentation0,
+ size_t indexOfRepresentation1,
+ size_t indexOfConstraint);
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_CONSTRAINT_H
diff --git a/SurgSim/Physics/ConstraintComponent.cpp b/SurgSim/Physics/ConstraintComponent.cpp
new file mode 100644
index 0000000..a5e99de
--- /dev/null
+++ b/SurgSim/Physics/ConstraintComponent.cpp
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/ConstraintComponent.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+ConstraintComponent::ConstraintComponent(const std::string& name) : Component(name)
+{
+}
+
+ConstraintComponent::~ConstraintComponent()
+{
+}
+
+void ConstraintComponent::setConstraint(std::shared_ptr<Constraint> constraint)
+{
+ m_constraint = constraint;
+}
+
+std::shared_ptr<Constraint> ConstraintComponent::getConstraint() const
+{
+ return m_constraint;
+}
+
+bool ConstraintComponent::doInitialize()
+{
+ return m_constraint != nullptr;
+};
+
+bool ConstraintComponent::doWakeUp()
+{
+ return true;
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/ConstraintComponent.h b/SurgSim/Physics/ConstraintComponent.h
new file mode 100644
index 0000000..8435631
--- /dev/null
+++ b/SurgSim/Physics/ConstraintComponent.h
@@ -0,0 +1,66 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_CONSTRAINTCOMPONENT_H
+#define SURGSIM_PHYSICS_CONSTRAINTCOMPONENT_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Framework/Component.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class Constraint;
+
+/// Component class for inserting constraints between physics representations into the scene.
+class ConstraintComponent : public SurgSim::Framework::Component
+{
+public:
+ /// Constructor
+ /// \param name Name of the component
+ explicit ConstraintComponent(const std::string& name);
+
+ /// Destructor
+ virtual ~ConstraintComponent();
+
+ /// Set the constraint associated with this component
+ /// \param constraint The constraint to be set
+ void setConstraint(std::shared_ptr<Constraint> constraint);
+
+ /// Get the constraint associated with this component
+ /// \return The associated constraint
+ std::shared_ptr<Constraint> getConstraint() const;
+
+protected:
+ /// The stored constraint
+ std::shared_ptr<Constraint> m_constraint;
+
+ /// Initialize the component
+ /// \return true if successful
+ virtual bool doInitialize() override;
+
+ /// Wakeup the component
+ /// \return true if successful
+ virtual bool doWakeUp() override;
+};
+
+}; // namespace SurgSim
+}; // namespace Physics
+
+#endif // SURGSIM_PHYSICS_CONSTRAINTCOMPONENT_H
diff --git a/SurgSim/Physics/ConstraintData.cpp b/SurgSim/Physics/ConstraintData.cpp
new file mode 100644
index 0000000..f8d6113
--- /dev/null
+++ b/SurgSim/Physics/ConstraintData.cpp
@@ -0,0 +1,33 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/ConstraintData.h"
+
+namespace SurgSim{
+
+namespace Physics{
+
+ConstraintData::ConstraintData()
+{
+
+}
+ConstraintData::~ConstraintData()
+{
+
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/ConstraintData.h b/SurgSim/Physics/ConstraintData.h
new file mode 100644
index 0000000..b19f0fd
--- /dev/null
+++ b/SurgSim/Physics/ConstraintData.h
@@ -0,0 +1,40 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_CONSTRAINTDATA_H
+#define SURGSIM_PHYSICS_CONSTRAINTDATA_H
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Base class for all CosntraintData
+/// Derived classes should be specific to a given constraint
+class ConstraintData
+{
+public:
+ /// Default Constructor
+ ConstraintData();
+
+ /// Destructor
+ virtual ~ConstraintData();
+};
+
+}; // namespace Physics
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_CONSTRAINTDATA_H
diff --git a/SurgSim/Physics/ConstraintImplementation.cpp b/SurgSim/Physics/ConstraintImplementation.cpp
new file mode 100644
index 0000000..548b275
--- /dev/null
+++ b/SurgSim/Physics/ConstraintImplementation.cpp
@@ -0,0 +1,36 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+ConstraintImplementation::ConstraintImplementation()
+{
+}
+
+ConstraintImplementation::~ConstraintImplementation()
+{
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/ConstraintImplementation.h b/SurgSim/Physics/ConstraintImplementation.h
new file mode 100644
index 0000000..51607ea
--- /dev/null
+++ b/SurgSim/Physics/ConstraintImplementation.h
@@ -0,0 +1,116 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_CONSTRAINTIMPLEMENTATION_H
+#define SURGSIM_PHYSICS_CONSTRAINTIMPLEMENTATION_H
+
+#include <memory>
+
+#include <Eigen/Core>
+
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Enum defining on which side of the constraint an implementation is (positive or negative side)
+/// \note A constraint can be seen as C: side0 = side1 <=> C: side0 - side1 = 0
+/// \note Therefore 1 side will have a positive sign (+1), and 1 side a negative sign (-1)
+enum ConstraintSideSign {CONSTRAINT_POSITIVE_SIDE, CONSTRAINT_NEGATIVE_SIDE};
+
+/// Base class for all constraint implementations. A ConstraintImplementation defines 1 side of a constraint.
+class ConstraintImplementation
+{
+public:
+ /// Constructor
+ /// \note Localization embbed the representation, so it is fully defined
+ ConstraintImplementation();
+
+ /// Destructor
+ virtual ~ConstraintImplementation();
+
+ /// Gets the number of degree of freedom for this implementation
+ /// \return The number of degree of freedom for this implementation
+ size_t getNumDof() const
+ {
+ return doGetNumDof();
+ }
+
+ /// Gets the Mixed Linear Complementarity Problem constraint type for this ConstraintImplementation
+ /// \return The MLCP constraint type corresponding to this constraint implementation
+ virtual SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const = 0;
+
+ /// Gets the Type of representation that this implementation is concerned with
+ /// \return RepresentationType for this implementation
+ virtual RepresentationType getRepresentationType() const = 0;
+
+ /// Builds the subset of an Mlcp physics problem associated to this implementation
+ /// \param dt The time step
+ /// \param data The data associated to the constraint
+ /// \param localization The localization for this implementation
+ /// \param [in, out] mlcp The Mixed LCP physics problem to fill up
+ /// \param indexOfRepresentation The index of the representation (associated to this implementation) in the mlcp
+ /// \param indexOfConstraint The index of the constraint in the mlcp
+ /// \param sign The sign of this implementation in the constraint (positive or negative side)
+ void build(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+ {
+ doBuild(dt, data, localization, mlcp, indexOfRepresentation, indexOfConstraint, sign);
+ }
+
+protected:
+ /// Preallocated variable for derived implementations of doBuild.
+ Eigen::SparseVector<double, 0, ptrdiff_t> m_newH;
+
+private:
+
+ /// Does get number of degree of freedom
+ /// \return The number of degree of freedom associated to this implementation
+ virtual size_t doGetNumDof() const = 0;
+
+ /// Builds the subset of an Mlcp physics problem associated to this implementation
+ /// \param dt The time step
+ /// \param data The data associated to the constraint
+ /// \param localization The localization for the constraint
+ /// \param [in, out] mlcp The Mixed LCP physics problem to fill up
+ /// \param indexOfRepresentation The index of the representation (associated to this implementation) in the mlcp
+ /// \param indexOfConstraint The index of the constraint in the mlcp
+ /// \param sign The sign of this implementation in the constraint (positive or negative side)
+ virtual void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) = 0;
+
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_CONSTRAINTIMPLEMENTATION_H
diff --git a/SurgSim/Physics/ConstraintImplementationFactory.cpp b/SurgSim/Physics/ConstraintImplementationFactory.cpp
new file mode 100644
index 0000000..2e05ca1
--- /dev/null
+++ b/SurgSim/Physics/ConstraintImplementationFactory.cpp
@@ -0,0 +1,62 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Physics/ConstraintImplementationFactory.h"
+#include "SurgSim/Physics/FixedRepresentationContact.h"
+#include "SurgSim/Physics/RigidRepresentationContact.h"
+#include "SurgSim/Physics/Fem3DRepresentationContact.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+ConstraintImplementationFactory::ConstraintImplementationFactory()
+{
+ addImplementation(std::make_shared<FixedRepresentationContact>());
+ addImplementation(std::make_shared<RigidRepresentationContact>());
+ addImplementation(std::make_shared<Fem3DRepresentationContact>());
+}
+
+ConstraintImplementationFactory::~ConstraintImplementationFactory()
+{
+}
+
+std::shared_ptr<ConstraintImplementation> ConstraintImplementationFactory::getImplementation(
+ RepresentationType representationType,
+ SurgSim::Math::MlcpConstraintType constraintType) const
+{
+ SURGSIM_ASSERT(representationType >= 0 && representationType < REPRESENTATION_TYPE_COUNT) <<
+ "Invalid representation type " << representationType;
+ SURGSIM_ASSERT(constraintType >= 0 && constraintType < SurgSim::Math::MLCP_NUM_CONSTRAINT_TYPES) <<
+ "Invalid constraint type " << constraintType;
+
+ auto implementation = m_implementations[representationType][constraintType];
+ SURGSIM_LOG_IF(implementation == nullptr, SurgSim::Framework::Logger::getDefaultLogger(), WARNING) <<
+ "No constraint implementation for representation type (" << representationType <<
+ ") and constraint type (" << constraintType << ")";
+
+ return implementation;
+}
+
+void ConstraintImplementationFactory::addImplementation(std::shared_ptr<ConstraintImplementation> implementation)
+{
+ m_implementations[implementation->getRepresentationType()][implementation->getMlcpConstraintType()] =
+ implementation;
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/ConstraintImplementationFactory.h b/SurgSim/Physics/ConstraintImplementationFactory.h
new file mode 100644
index 0000000..58f60cc
--- /dev/null
+++ b/SurgSim/Physics/ConstraintImplementationFactory.h
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_CONSTRAINTIMPLEMENTATIONFACTORY_H
+#define SURGSIM_PHYSICS_CONSTRAINTIMPLEMENTATIONFACTORY_H
+
+#include <memory>
+
+#include "SurgSim/Math/MlcpConstraintType.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class ConstraintImplementation;
+
+/// This class manages ConstraintImplementations, and can be used to look up the correct implementation
+/// by representation and constraint type.
+/// The only maintenance that needs to be done right now when a new
+/// ConstraintImplementation is added is to add a call into the constructor.
+class ConstraintImplementationFactory
+{
+public:
+
+ /// Constructor
+ ConstraintImplementationFactory();
+
+ /// Destructor
+ ~ConstraintImplementationFactory();
+
+ /// Get the instance of a ConstraintImplementation for a specific representation and
+ /// constraint type.
+ /// \param representationType Type of the representation.
+ /// \param constraintType Type of the constraint.
+ /// \return a pointer to an implementation if the implementation can be found, nullptr otherwise.
+ std::shared_ptr<ConstraintImplementation> getImplementation(
+ RepresentationType representationType,
+ SurgSim::Math::MlcpConstraintType constraintType) const;
+
+private:
+
+ /// Add an implementation to the internal directory.
+ /// \param implementation The ConstraintImplementation to add.
+ void addImplementation(std::shared_ptr<ConstraintImplementation> implementation);
+
+ /// Lookup table for constrain implementations
+ std::shared_ptr<ConstraintImplementation>
+ m_implementations[REPRESENTATION_TYPE_COUNT][SurgSim::Math::MLCP_NUM_CONSTRAINT_TYPES];
+
+
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Physics/ContactConstraintData.h b/SurgSim/Physics/ContactConstraintData.h
new file mode 100644
index 0000000..77f03fb
--- /dev/null
+++ b/SurgSim/Physics/ContactConstraintData.h
@@ -0,0 +1,82 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_CONTACTCONSTRAINTDATA_H
+#define SURGSIM_PHYSICS_CONTACTCONSTRAINTDATA_H
+
+#include "SurgSim/Physics/ConstraintData.h"
+
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Class for Frictionless contact (only needs a plane equation)
+class ContactConstraintData : public ConstraintData
+{
+public:
+ /// Default constructor
+ ContactConstraintData() :
+ ConstraintData(),
+ m_distance(0.0)
+ {
+ m_normal.setZero();
+ }
+
+ /// Destructor
+ virtual ~ContactConstraintData()
+ {
+ }
+
+ /// Sets the plane equation of the frictionless contact
+ /// \param n The plane normal (normalized vector)
+ /// \param d The plane distance to the origin
+ /// \note The plane is defined by { P | n.P + d = 0 }
+ void setPlaneEquation(const SurgSim::Math::Vector3d& n, double d)
+ {
+ m_normal = n;
+ m_distance = d;
+ }
+
+ /// Gets the plane normal vector
+ /// \return The plane equation's normal vector (normalized vector)
+ const SurgSim::Math::Vector3d& getNormal() const
+ {
+ return m_normal;
+ }
+
+ /// Gets the plane distance to the origin
+ /// \return The plane equation's distance to the origin
+ double getDistance() const
+ {
+ return m_distance;
+ }
+
+private:
+ /// Plane equation normal vector (normalized vector)
+ SurgSim::Math::Vector3d m_normal;
+
+ /// Plane equation distance to origin
+ double m_distance;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_CONTACTCONSTRAINTDATA_H
diff --git a/SurgSim/Physics/ContactConstraintGeneration.cpp b/SurgSim/Physics/ContactConstraintGeneration.cpp
new file mode 100644
index 0000000..ce9afe6
--- /dev/null
+++ b/SurgSim/Physics/ContactConstraintGeneration.cpp
@@ -0,0 +1,187 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/ContactConstraintGeneration.h"
+
+#include <utility>
+#include <vector>
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+ContactConstraintGeneration::ContactConstraintGeneration(bool doCopyState) : Computation(doCopyState)
+{
+ m_logger = SurgSim::Framework::Logger::getLogger("ContactConstraintGeneration");
+}
+
+ContactConstraintGeneration::~ContactConstraintGeneration()
+{
+
+}
+
+std::shared_ptr<PhysicsManagerState> ContactConstraintGeneration::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ auto result = state;
+ auto pairs = result->getCollisionPairs();
+
+ auto pairsIt = std::begin(pairs);
+ auto pairsEnd = std::end(pairs);
+
+ std::vector<std::shared_ptr<Constraint>> constraints;
+
+ // This will check all collision pairs for contacts, then iterate over all
+ // the contacts and for each contact will create a constraint between the
+ // sides of the collisionpair and the localizations created from the contact
+ // point. The list of all the constraints will be added back into the
+ // Physics state as a result of this computation
+ for (; pairsIt != pairsEnd; ++pairsIt)
+ {
+ if ((*pairsIt)->hasContacts())
+ {
+ auto contactsIt = std::begin((*pairsIt)->getContacts());
+ auto contactsEnd = std::end((*pairsIt)->getContacts());
+ for (; contactsIt != contactsEnd; ++contactsIt)
+ {
+ std::pair<std::shared_ptr<Localization>, std::shared_ptr<Localization>> localizations;
+
+ auto collisionRepresentations = (*pairsIt)->getRepresentations();
+
+ auto collisionToPhysicsMap = state->getCollisionToPhysicsMap();
+ auto foundFirst = collisionToPhysicsMap.find(collisionRepresentations.first);
+ auto foundSecond = collisionToPhysicsMap.find(collisionRepresentations.second);
+ if (foundFirst == collisionToPhysicsMap.end() || foundSecond == collisionToPhysicsMap.end())
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << __FUNCTION__ << " Not creating a constraint. " <<
+ collisionRepresentations.first->getName() << " and/or " <<
+ collisionRepresentations.second->getName() << " does not have a physics representation";
+ continue;
+ }
+ std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>> physicsRepresentations;
+ physicsRepresentations.first = foundFirst->second;
+ physicsRepresentations.second = foundSecond->second;
+
+ if (!(physicsRepresentations.first->isActive() && physicsRepresentations.second->isActive()))
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << __FUNCTION__ << " Not creating a constraint. " <<
+ physicsRepresentations.first->getName() << " and/or " <<
+ physicsRepresentations.second->getName() << " is not an active physics representation";
+ continue;
+ }
+
+ localizations.first = makeLocalization(physicsRepresentations.first, collisionRepresentations.first,
+ (*contactsIt)->penetrationPoints.first);
+ localizations.second = makeLocalization(physicsRepresentations.second, collisionRepresentations.second,
+ (*contactsIt)->penetrationPoints.second);
+ if (localizations.first != nullptr && localizations.second != nullptr)
+ {
+ std::pair< std::shared_ptr<ConstraintImplementation>, std::shared_ptr<ConstraintImplementation>>
+ implementations;
+
+ // HS-2013-jul-12 The type of constraint is fixed here right now, to get to a constraint
+ // that we can change we probably will need to predefine collision pairs and their appropriate
+ // contact constraints so we can look up which constraint to use here
+
+ implementations.first = m_factory.getImplementation(
+ localizations.first->getRepresentation()->getType(),
+ SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT);
+
+ implementations.second = m_factory.getImplementation(
+ localizations.second->getRepresentation()->getType(),
+ SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT);
+
+ if (implementations.first != nullptr && implementations.second != nullptr)
+ {
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation((*contactsIt)->normal, (*contactsIt)->depth);
+
+ constraints.push_back(std::make_shared<Constraint>(
+ data,
+ implementations.first,
+ localizations.first,
+ implementations.second,
+ localizations.second));
+ }
+ }
+ }
+ }
+ }
+
+ result->setConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT, constraints);
+
+ return std::move(result);
+}
+
+std::shared_ptr<Localization> ContactConstraintGeneration::makeLocalization(
+ std::shared_ptr<SurgSim::Physics::Representation> physicsRepresentation,
+ std::shared_ptr<SurgSim::Collision::Representation> collisionRepresentation,
+ const SurgSim::DataStructures::Location& location)
+{
+ std::shared_ptr<Localization> result;
+ if (physicsRepresentation != nullptr)
+ {
+ if (location.rigidLocalPosition.hasValue())
+ {
+ // Move the local position from the collision representation that created the location
+ // to local coordinates of the physics representation that is creating a localization
+ SurgSim::DataStructures::Location physicsLocation = location;
+ physicsLocation.rigidLocalPosition.setValue(
+ physicsRepresentation->getLocalPose().inverse() *
+ collisionRepresentation->getLocalPose() *
+ location.rigidLocalPosition.getValue());
+ result = physicsRepresentation->createLocalization(physicsLocation);
+ }
+ else
+ {
+ result = physicsRepresentation->createLocalization(location);
+ }
+
+ if (result != nullptr)
+ {
+ // HS 2013-jun-20 this is not quite right, we are passing in the pointer to the representation
+ // that we just asked about, but I don't know if we can use shared_from_this from the inside and
+ // still get the same reference count as with this shared_ptr
+ result->setRepresentation(physicsRepresentation);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << __FUNCTION__ <<
+ " Cannot create Localization from " << physicsRepresentation->getName();
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << __FUNCTION__ <<
+ " Cannot create Localization from nullptr.";
+ }
+ return result;
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/ContactConstraintGeneration.h b/SurgSim/Physics/ContactConstraintGeneration.h
new file mode 100644
index 0000000..c6b155a
--- /dev/null
+++ b/SurgSim/Physics/ContactConstraintGeneration.h
@@ -0,0 +1,99 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_CONTACTCONSTRAINTGENERATION_H
+#define SURGSIM_PHYSICS_CONTACTCONSTRAINTGENERATION_H
+
+#include <memory>
+
+#include "SurgSim/Physics/Computation.h"
+#include "SurgSim/Physics/ConstraintImplementationFactory.h"
+
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Logger;
+}
+
+namespace DataStructures
+{
+struct Location;
+}
+namespace Collision
+{
+class Representation;
+}
+
+namespace Physics
+{
+
+class PhysicsManagerState;
+class Localization;
+
+/// Generate a constraint for every contact that was calculated.
+/// The general algorithm is such, for each pair of Collision Representations that has Contacts
+/// For each contact:
+/// - Generate a localization for each side of the pair
+/// - Find the correct CollisionImplementation for the Representation and type of constraint, note that currently
+/// we use only one type of constraint for the collision
+/// - Create the ConstraintData for the contact with
+/// - Create a constraint from, the ContactImplmentation, Localization and ConstraintData
+/// - Add it to the list of Constraints
+/// At the end those constraints are added as contact constraints to the physics state
+class ContactConstraintGeneration : public Computation
+{
+public:
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit ContactConstraintGeneration(bool doCopyState = false);
+
+ /// Destructor
+ ~ContactConstraintGeneration();
+
+private:
+
+ /// For looking up instances of constrain implementations
+ ConstraintImplementationFactory m_factory;
+
+ /// The logger for this class
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+
+ /// Overridden function from Computation, the actual work is done here
+ /// \param dt The time passed from the last update in seconds.
+ /// \param state The physics state.
+ /// \return The changed state of the, depending on the setting of doCopyState this is either the same instance
+ /// or a copied instance of the physics state.
+ virtual std::shared_ptr<PhysicsManagerState> doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state) override;
+
+ /// Generate a localization from a Collision Representation.
+ /// \param physicsRepresentation The physics representation.
+ /// \param collisionRepresentation The collision representation.
+ /// \param location The location generated by the contact calculation.
+ /// \return The localization for the collision representations physics representation.
+ std::shared_ptr<Localization> makeLocalization(
+ std::shared_ptr<SurgSim::Physics::Representation> physicsRepresentation,
+ std::shared_ptr<SurgSim::Collision::Representation> collisionRepresentation,
+ const SurgSim::DataStructures::Location& location);
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif
diff --git a/SurgSim/Physics/DcdCollision.cpp b/SurgSim/Physics/DcdCollision.cpp
new file mode 100644
index 0000000..255d7db
--- /dev/null
+++ b/SurgSim/Physics/DcdCollision.cpp
@@ -0,0 +1,160 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <vector>
+
+#include "SurgSim/Physics/DcdCollision.h"
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Collision/DcdCollision.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+using SurgSim::Collision::CollisionPair;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+DcdCollision::DcdCollision(bool doCopyState) : Computation(doCopyState)
+{
+ populateCalculationTable();
+}
+
+DcdCollision::~DcdCollision()
+{
+}
+
+std::shared_ptr<PhysicsManagerState> DcdCollision::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+ updatePairs(result);
+
+ std::vector<std::shared_ptr<CollisionPair>> pairs = result->getCollisionPairs();
+
+ auto it = pairs.cbegin();
+ auto itEnd = pairs.cend();
+ while (it != itEnd)
+ {
+ m_contactCalculations[(*it)->getFirst()->getShapeType()][(*it)->getSecond()->getShapeType()]->
+ calculateContact(*it);
+ ++it;
+ }
+
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> representations =
+ state->getCollisionRepresentations();
+ for (auto representation = std::begin(representations); representation != std::end(representations);
+ ++representation)
+ {
+ (*representation)->getCollisions().publish();
+ }
+
+ return result;
+}
+
+void DcdCollision::populateCalculationTable()
+{
+ for (int i = 0; i < SurgSim::Math::SHAPE_TYPE_COUNT; ++i)
+ {
+ for (int j = 0; j < SurgSim::Math::SHAPE_TYPE_COUNT; ++j)
+ {
+ m_contactCalculations[i][j].reset(new SurgSim::Collision::DefaultContactCalculation(false));
+ }
+ }
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::SphereSphereDcdContact>());
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::SphereDoubleSidedPlaneDcdContact>());
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::SpherePlaneDcdContact>());
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::BoxCapsuleDcdContact>());
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::BoxDoubleSidedPlaneDcdContact>());
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::BoxPlaneDcdContact>());
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::BoxSphereDcdContact>());
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::CapsuleSphereDcdContact>());
+
+ // Add the Octree contact calculations using the box contact calculations
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::OctreeDcdContact>(
+ std::make_shared<SurgSim::Collision::BoxCapsuleDcdContact>()));
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::OctreeDcdContact>(
+ std::make_shared<SurgSim::Collision::BoxDoubleSidedPlaneDcdContact>()));
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::OctreeDcdContact>(
+ std::make_shared<SurgSim::Collision::BoxPlaneDcdContact>()));
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::OctreeDcdContact>(
+ std::make_shared<SurgSim::Collision::BoxSphereDcdContact>()));
+
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::TriangleMeshPlaneDcdContact>());
+ setDcdContactInTable(std::make_shared<SurgSim::Collision::TriangleMeshTriangleMeshDcdContact>());
+}
+
+void DcdCollision::updatePairs(std::shared_ptr<PhysicsManagerState> state)
+{
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> representations =
+ state->getCollisionRepresentations();
+
+ if (representations.size() > 1)
+ {
+ for (auto it = std::begin(representations); it != std::end(representations); ++it)
+ {
+ (*it)->getCollisions().unsafeGet().clear();
+ }
+
+ std::vector<std::shared_ptr<CollisionPair>> pairs;
+ auto firstEnd = std::end(representations);
+ --firstEnd;
+ for (auto first = std::begin(representations); first != firstEnd; ++first)
+ {
+ auto second = first;
+ ++second;
+ for (; second != std::end(representations); ++second)
+ {
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>();
+ pair->setRepresentations(*first,*second);
+ pairs.push_back(pair);
+ }
+ }
+
+ std::vector<std::shared_ptr<CollisionPair>> excludedPairs = state->getExcludedCollisionPairs();
+ for (auto it = excludedPairs.cbegin(); it != excludedPairs.cend(); ++it)
+ {
+ auto candidate = std::find_if(pairs.begin(), pairs.end(), [&it](const std::shared_ptr<CollisionPair> &pair)
+ {
+ return (pair->getFirst() == (*it)->getFirst() && pair->getSecond() == (*it)->getSecond())
+ || (pair->getFirst() == (*it)->getSecond() && pair->getSecond() == (*it)->getFirst());
+ });
+
+ if (candidate != pairs.end())
+ {
+ pairs.erase(candidate);
+ }
+ }
+
+ state->setCollisionPairs(pairs);
+ }
+}
+
+void DcdCollision::setDcdContactInTable(std::shared_ptr<SurgSim::Collision::ContactCalculation> dcdContact)
+{
+ std::pair<int,int> shapeTypes = dcdContact->getShapeTypes();
+ m_contactCalculations[shapeTypes.first][shapeTypes.second] = dcdContact;
+ if (shapeTypes.first != shapeTypes.second)
+ {
+ m_contactCalculations[shapeTypes.second][shapeTypes.first] = dcdContact;
+ }
+}
+
+}; // Physics
+}; // SurgSim
+
diff --git a/SurgSim/Physics/DcdCollision.h b/SurgSim/Physics/DcdCollision.h
new file mode 100644
index 0000000..fc547ea
--- /dev/null
+++ b/SurgSim/Physics/DcdCollision.h
@@ -0,0 +1,85 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_DCDCOLLISION_H
+#define SURGSIM_PHYSICS_DCDCOLLISION_H
+
+#include <memory>
+
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+class ContactCalculation;
+}
+
+namespace Physics
+{
+class PhysicsManagerState;
+
+/// Computation to determine the contacts between a list of CollisionPairs.
+/// This Computation class takes a list of representations, it will generate a list of collision pairs
+/// from this list on every frame, for each CollisionPair, it uses a two dimensional table of
+/// function objects (ContactCalculation) to determine how to calculate a contact between the two
+/// members of each pair, if no specific function exists a default function will be used.
+/// will update the collision pairs accordingly.
+/// \note HS-2013-may-24 Currently handles only RigidRepresentation, all others will be ignored
+
+class DcdCollision : public Computation
+{
+public:
+
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit DcdCollision(bool doCopyState = false);
+
+ /// Destructor
+ virtual ~DcdCollision();
+
+protected:
+
+ /// Executes the update operation, overridden from Computation.
+ /// \param dt The time passed.
+ /// \param state The PhysicsManagerState from previous computation.
+ virtual std::shared_ptr<PhysicsManagerState> doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state) override;
+
+private:
+
+ /// Initializes the table of ContactCalculation objects
+ void populateCalculationTable();
+
+ /// Updates the collision pairs
+ void updatePairs(std::shared_ptr<PhysicsManagerState> state);
+
+ /// Function to populate the m_contactCalculations table for each DcdContact class.
+ void setDcdContactInTable(std::shared_ptr<SurgSim::Collision::ContactCalculation> dcdContact);
+
+ /// Table containing contact calculation, the indices indicate the type of
+ /// the first pair object and the second pair object in order
+ std::shared_ptr<SurgSim::Collision::ContactCalculation> m_contactCalculations[SurgSim::Math::SHAPE_TYPE_COUNT]
+ [SurgSim::Math::SHAPE_TYPE_COUNT];
+
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif
diff --git a/SurgSim/Physics/DeformableCollisionRepresentation.cpp b/SurgSim/Physics/DeformableCollisionRepresentation.cpp
new file mode 100644
index 0000000..e724e21
--- /dev/null
+++ b/SurgSim/Physics/DeformableCollisionRepresentation.cpp
@@ -0,0 +1,147 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+#include "SurgSim/Physics/DeformableRepresentation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::DeformableCollisionRepresentation,
+ DeformableCollisionRepresentation);
+
+DeformableCollisionRepresentation::DeformableCollisionRepresentation(const std::string& name) :
+ SurgSim::Collision::Representation(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(DeformableCollisionRepresentation, std::shared_ptr<SurgSim::Math::Shape>,
+ Shape, getShape, setShape);
+}
+
+DeformableCollisionRepresentation::~DeformableCollisionRepresentation()
+{
+}
+
+void DeformableCollisionRepresentation::setMesh(std::shared_ptr<SurgSim::DataStructures::TriangleMesh> mesh)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Can't set mesh after initialization.";
+ SURGSIM_ASSERT(mesh != nullptr) << "Can't use nullptr mesh.";
+
+ m_shape = std::make_shared<SurgSim::Math::MeshShape>(*mesh);
+ m_mesh = m_shape->getMesh();
+}
+
+std::shared_ptr<SurgSim::DataStructures::TriangleMesh> DeformableCollisionRepresentation::getMesh() const
+{
+ return m_mesh;
+}
+
+void DeformableCollisionRepresentation::update(const double& dt)
+{
+ auto physicsRepresentation = m_deformable.lock();
+ SURGSIM_ASSERT(nullptr != physicsRepresentation) <<
+ "Failed to update. The DeformableCollisionRepresentation either was not attached to a "
+ "Physics::Representation or the Physics::Representation has expired.";
+
+ auto odeState = physicsRepresentation->getCurrentState();
+ const size_t numNodes = odeState->getNumNodes();
+
+ SURGSIM_ASSERT(m_mesh->getNumVertices() == numNodes) <<
+ "The number of nodes in the deformable does not match the number of vertices in the mesh.";
+
+ for (size_t nodeId = 0; nodeId < numNodes; ++nodeId)
+ {
+ m_mesh->setVertexPosition(nodeId, odeState->getPosition(nodeId));
+ }
+ m_mesh->update();
+ m_shape->updateAabbTree();
+}
+
+bool DeformableCollisionRepresentation::doInitialize()
+{
+ bool result = false;
+ if (nullptr != m_shape && m_shape->isValid())
+ {
+ m_mesh = m_shape->getMesh();
+ result = true;
+ }
+
+ return result;
+}
+
+bool DeformableCollisionRepresentation::doWakeUp()
+{
+ auto physicsRepresentation = m_deformable.lock();
+ SURGSIM_ASSERT(nullptr != physicsRepresentation) <<
+ "The Physics::Representation referred by this DeformableCollisionRepresentation has expired.";
+
+ auto state = physicsRepresentation->getCurrentState();
+ SURGSIM_ASSERT(nullptr != state) <<
+ "DeformableRepresentation " << physicsRepresentation->getName() << " holds an empty OdeState.";
+ SURGSIM_ASSERT(nullptr != m_mesh) << "m_mesh is empty.";
+ SURGSIM_ASSERT(m_mesh->getNumVertices() == state->getNumNodes()) <<
+ "The number of nodes in the deformable does not match the number of vertices in the mesh.";
+
+ update(0.0);
+ return true;
+}
+
+int DeformableCollisionRepresentation::getShapeType() const
+{
+ SURGSIM_ASSERT(nullptr != m_shape) << "No mesh/shape assigned to DeformableCollisionRepresentation " << getName();
+ return m_shape->getType();
+}
+
+void DeformableCollisionRepresentation::setShape(std::shared_ptr<SurgSim::Math::Shape> shape)
+{
+ SURGSIM_ASSERT(shape->getType() == SurgSim::Math::SHAPE_TYPE_MESH)
+ << "Deformable collision shape has to be a mesh. But what passed in is " << shape->getType();
+
+ auto meshShape = std::dynamic_pointer_cast<SurgSim::Math::MeshShape>(shape);
+ m_shape = meshShape;
+ m_mesh = meshShape->getMesh();
+}
+
+const std::shared_ptr<SurgSim::Math::Shape> DeformableCollisionRepresentation::getShape() const
+{
+ return m_shape;
+}
+
+void DeformableCollisionRepresentation::setDeformableRepresentation(
+ std::shared_ptr<SurgSim::Physics::DeformableRepresentation>representation)
+{
+ m_deformable = representation;
+}
+
+const std::shared_ptr<SurgSim::Physics::DeformableRepresentation>
+ DeformableCollisionRepresentation::getDeformableRepresentation() const
+{
+ auto physicsRepresentation = m_deformable.lock();
+ SURGSIM_ASSERT(physicsRepresentation != nullptr) <<
+ "Failed to get the deformable representation. The DeformableCollisionRepresentation either was not "
+ "attached to a Physics::Representation or the Physics::Representation has expired.";
+
+ return physicsRepresentation;
+}
+
+} // namespace Physics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Physics/DeformableCollisionRepresentation.h b/SurgSim/Physics/DeformableCollisionRepresentation.h
new file mode 100644
index 0000000..f7543ed
--- /dev/null
+++ b/SurgSim/Physics/DeformableCollisionRepresentation.h
@@ -0,0 +1,104 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_DEFORMABLECOLLISIONREPRESENTATION_H
+#define SURGSIM_PHYSICS_DEFORMABLECOLLISIONREPRESENTATION_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class TriangleMesh;
+}
+
+namespace Math
+{
+class Shape;
+class MeshShape;
+}
+
+namespace Physics
+{
+class DeformableRepresentation;
+
+SURGSIM_STATIC_REGISTRATION(DeformableCollisionRepresentation);
+
+/// A collision representation that can be attached to a deformable, when this contains a mesh with the same number
+/// of vertices as the deformable has nodes, the mesh vertices will move to match the positions of the nodes in
+/// the deformable.
+class DeformableCollisionRepresentation : public SurgSim::Collision::Representation
+{
+public:
+
+ /// Constructor
+ /// \param name Name of the Representation
+ explicit DeformableCollisionRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~DeformableCollisionRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::DeformableCollisionRepresentation);
+
+ /// Set the mesh to be used in this collision representation
+ /// the vertices in the mesh need to be the same number as the vertices in the deformable representation.
+ /// \param mesh The mesh to be used for the collision calculation and updates
+ /// \note The shape held by this deformable collision representation will be updated as well.
+ void setMesh(std::shared_ptr<SurgSim::DataStructures::TriangleMesh> mesh);
+
+ /// \return The mesh that is part of this representation
+ std::shared_ptr<SurgSim::DataStructures::TriangleMesh> getMesh() const;
+
+ /// Set the shape for this collision representation, has to be a SurgSim::Math::MeshShape.
+ /// \param shape The shape to be used.
+ /// \note The mesh held by this deformable collision representation will be updated as well.
+ void setShape(std::shared_ptr<SurgSim::Math::Shape> shape);
+
+ virtual const std::shared_ptr<SurgSim::Math::Shape> getShape() const override;
+
+ /// Sets the deformable to which this collision representation is connected
+ /// \param representation The deformable that will be used to update the contained mesh
+ void setDeformableRepresentation(std::shared_ptr<SurgSim::Physics::DeformableRepresentation> representation);
+
+ /// \return The deformable that is used to update the contained mesh
+ const std::shared_ptr<SurgSim::Physics::DeformableRepresentation> getDeformableRepresentation() const;
+
+ virtual int getShapeType() const override;
+
+ virtual void update(const double& dt) override;
+
+private:
+ virtual bool doInitialize() override;
+ virtual bool doWakeUp() override;
+
+ /// Shape used for collision detection
+ std::shared_ptr<SurgSim::Math::MeshShape> m_shape;
+
+ /// Mesh used for collision detection
+ std::shared_ptr<SurgSim::DataStructures::TriangleMesh> m_mesh;
+
+ /// Reference to the deformable driving changes to this mesh
+ std::weak_ptr<SurgSim::Physics::DeformableRepresentation> m_deformable;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Physics/DeformableRepresentation.cpp b/SurgSim/Physics/DeformableRepresentation.cpp
new file mode 100644
index 0000000..e99f8e2
--- /dev/null
+++ b/SurgSim/Physics/DeformableRepresentation.cpp
@@ -0,0 +1,327 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/OdeSolverEulerExplicit.h"
+#include "SurgSim/Math/OdeSolverEulerExplicitModified.h"
+#include "SurgSim/Math/OdeSolverEulerImplicit.h"
+#include "SurgSim/Math/OdeSolverRungeKutta4.h"
+#include "SurgSim/Math/OdeSolverLinearEulerExplicit.h"
+#include "SurgSim/Math/OdeSolverLinearEulerExplicitModified.h"
+#include "SurgSim/Math/OdeSolverLinearEulerImplicit.h"
+#include "SurgSim/Math/OdeSolverLinearRungeKutta4.h"
+#include "SurgSim/Math/OdeSolverLinearStatic.h"
+#include "SurgSim/Math/OdeSolverStatic.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/DeformableRepresentation.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+DeformableRepresentation::DeformableRepresentation(const std::string& name) :
+ Representation(name),
+ SurgSim::Math::OdeEquation(),
+ m_numDofPerNode(0),
+ m_integrationScheme(SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(DeformableRepresentation, SurgSim::Math::IntegrationScheme, IntegrationScheme,
+ getIntegrationScheme, setIntegrationScheme);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(DeformableRepresentation, std::shared_ptr<SurgSim::Collision::Representation>,
+ CollisionRepresentation, getCollisionRepresentation, setCollisionRepresentation);
+}
+
+DeformableRepresentation::~DeformableRepresentation()
+{
+}
+void DeformableRepresentation::resetState()
+{
+ Representation::resetState();
+
+ // Reminder: m_initialState is being held in OdeEquation
+ *m_currentState = *m_initialState;
+ *m_previousState = *m_initialState;
+ // m_newState does not need to be reset, it is a temporary variable
+ *m_finalState = *m_initialState;
+}
+
+void DeformableRepresentation::setInitialState(
+ std::shared_ptr<SurgSim::Math::OdeState> initialState)
+{
+ // This initializes and allocates the m_initialState data member
+ m_initialState = initialState;
+
+ m_previousState = std::make_shared<SurgSim::Math::OdeState>(*m_initialState);
+ m_currentState = std::make_shared<SurgSim::Math::OdeState>(*m_initialState);
+ m_newState = std::make_shared<SurgSim::Math::OdeState>(*m_initialState);
+ m_finalState = std::make_shared<SurgSim::Math::OdeState>(*m_initialState);
+
+ // Set the representation number of degree of freedom
+ setNumDof(m_initialState->getNumDof());
+
+ m_externalGeneralizedForce.resize(getNumDof());
+ m_externalGeneralizedStiffness.resize(getNumDof(), getNumDof());
+ m_externalGeneralizedDamping.resize(getNumDof(), getNumDof());
+ m_externalGeneralizedForce.setZero();
+ m_externalGeneralizedStiffness.setZero();
+ m_externalGeneralizedDamping.setZero();
+}
+
+const std::shared_ptr<SurgSim::Math::OdeState> DeformableRepresentation::getCurrentState() const
+{
+ return m_currentState;
+}
+
+const std::shared_ptr<SurgSim::Math::OdeState> DeformableRepresentation::getPreviousState() const
+{
+ return m_previousState;
+}
+
+const std::shared_ptr<SurgSim::Math::OdeState> DeformableRepresentation::getFinalState() const
+{
+ return m_finalState;
+}
+
+size_t DeformableRepresentation::getNumDofPerNode() const
+{
+ return m_numDofPerNode;
+}
+
+void DeformableRepresentation::setIntegrationScheme(SurgSim::Math::IntegrationScheme integrationScheme)
+{
+ SURGSIM_ASSERT(!isAwake()) << "You cannot set the integration scheme after the component has been awoken";
+ m_integrationScheme = integrationScheme;
+}
+
+SurgSim::Math::IntegrationScheme DeformableRepresentation::getIntegrationScheme() const
+{
+ return m_integrationScheme;
+}
+
+const SurgSim::Math::Vector& DeformableRepresentation::getExternalGeneralizedForce() const
+{
+ return m_externalGeneralizedForce;
+}
+
+const SurgSim::Math::Matrix& DeformableRepresentation::getExternalGeneralizedStiffness() const
+{
+ return m_externalGeneralizedStiffness;
+}
+
+const SurgSim::Math::Matrix& DeformableRepresentation::getExternalGeneralizedDamping() const
+{
+ return m_externalGeneralizedDamping;
+}
+
+const SurgSim::Math::Matrix& DeformableRepresentation::getComplianceMatrix() const
+{
+ SURGSIM_ASSERT(m_odeSolver) << "Ode solver not initialized, it should have been initialized on wake-up";
+
+ return m_odeSolver->getCompliance();
+}
+
+void DeformableRepresentation::update(double dt)
+{
+ if (! isActive())
+ {
+ return;
+ }
+
+ SURGSIM_ASSERT(m_odeSolver != nullptr) <<
+ "Ode solver has not been set yet. Did you call beforeUpdate() ?";
+ SURGSIM_ASSERT(m_initialState != nullptr) <<
+ "Initial state has not been set yet. Did you call setInitialState() ?";
+
+ // Solve the ode
+ m_odeSolver->solve(dt, *m_currentState, m_newState.get());
+
+ // Back up the current state into the previous state (by swapping)
+ m_currentState.swap(m_previousState);
+ // Make the new state, the current state (by swapping)
+ m_currentState.swap(m_newState);
+
+ if (!m_currentState->isValid())
+ {
+ SURGSIM_LOG(SurgSim::Framework::Logger::getDefaultLogger(), DEBUG)
+ << getName() << " deactivated :" << std::endl
+ << "position=(" << m_currentState->getPositions().transpose() << ")" << std::endl
+ << "velocity=(" << m_currentState->getVelocities().transpose() << ")" << std::endl;
+
+ setLocalActive(false);
+ }
+}
+
+void DeformableRepresentation::afterUpdate(double dt)
+{
+ if (! isActive())
+ {
+ return;
+ }
+
+ driveSceneElementPose(SurgSim::Math::RigidTransform3d::Identity());
+
+ // Back up the current state into the final state
+ *m_finalState = *m_currentState;
+
+ // Reset the external generalized force, stiffness and damping
+ m_externalGeneralizedForce.setZero();
+ m_externalGeneralizedStiffness.setZero();
+ m_externalGeneralizedDamping.setZero();
+}
+
+void DeformableRepresentation::applyCorrection(double dt,
+ const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity)
+{
+ if (!isActive())
+ {
+ return;
+ }
+
+ m_currentState->getPositions() += deltaVelocity * dt;
+ m_currentState->getVelocities() += deltaVelocity;
+
+ if (!m_currentState->isValid())
+ {
+ SURGSIM_LOG(SurgSim::Framework::Logger::getDefaultLogger(), DEBUG)
+ << getName() << " deactivated :" << std::endl
+ << "position=(" << m_currentState->getPositions() << ")" << std::endl
+ << "velocity=(" << m_currentState->getVelocities() << ")" << std::endl;
+
+ setLocalActive(false);
+ }
+}
+
+void DeformableRepresentation::deactivateAndReset(void)
+{
+ SURGSIM_LOG(SurgSim::Framework::Logger::getDefaultLogger(), DEBUG)
+ << getName() << " deactivated and reset:" << std::endl
+ << "position=(" << m_currentState->getPositions() << ")" << std::endl
+ << "velocity=(" << m_currentState->getVelocities() << ")" << std::endl;
+
+ resetState();
+ setLocalActive(false);
+}
+
+void DeformableRepresentation::setCollisionRepresentation(
+ std::shared_ptr<SurgSim::Collision::Representation> representation)
+{
+ if (m_collisionRepresentation != representation)
+ {
+ // If we have an old collision representation clear the dependency if it was a deformable collision
+ // representation
+ auto oldCollisionRep =
+ std::dynamic_pointer_cast<DeformableCollisionRepresentation>(m_collisionRepresentation);
+ if (oldCollisionRep != nullptr)
+ {
+ oldCollisionRep->setDeformableRepresentation(nullptr);
+ }
+
+ Representation::setCollisionRepresentation(representation);
+
+ // If its a RigidCollisionRepresentation connect with this representation
+ auto newCollisionRep = std::dynamic_pointer_cast<DeformableCollisionRepresentation>(representation);
+ if (newCollisionRep != nullptr)
+ {
+ newCollisionRep->setDeformableRepresentation(
+ std::static_pointer_cast<DeformableRepresentation>(getSharedPtr()));
+ }
+ }
+}
+
+bool DeformableRepresentation::doWakeUp()
+{
+ using SurgSim::Math::OdeSolverEulerExplicit;
+ using SurgSim::Math::OdeSolverEulerExplicitModified;
+ using SurgSim::Math::OdeSolverEulerImplicit;
+ using SurgSim::Math::OdeSolverRungeKutta4;
+ using SurgSim::Math::OdeSolverStatic;
+ using SurgSim::Math::OdeSolverLinearEulerExplicit;
+ using SurgSim::Math::OdeSolverLinearEulerExplicitModified;
+ using SurgSim::Math::OdeSolverLinearEulerImplicit;
+ using SurgSim::Math::OdeSolverLinearRungeKutta4;
+ using SurgSim::Math::OdeSolverLinearStatic;
+
+ using SurgSim::Math::LinearSolveAndInverseDenseMatrix;
+
+ // Transform the state with the initial pose
+ transformState(m_initialState, getPose());
+ *m_previousState = *m_initialState;
+ *m_currentState = *m_initialState;
+ *m_newState = *m_initialState;
+ *m_finalState = *m_initialState;
+
+ // Since the pose is now embedded in the state, reset element and local pose to identity.
+ setLocalPose(SurgSim::Math::RigidTransform3d::Identity());
+ std::shared_ptr<SurgSim::Framework::SceneElement> sceneElement = getSceneElement();
+ if (sceneElement != nullptr)
+ {
+ sceneElement->setPose(SurgSim::Math::RigidTransform3d::Identity());
+ }
+
+ // Set the ode solver using the chosen integration scheme
+ switch (m_integrationScheme)
+ {
+ case SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER:
+ m_odeSolver = std::make_shared<OdeSolverEulerExplicit>(this);
+ break;
+ case SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER:
+ m_odeSolver = std::make_shared<OdeSolverEulerExplicitModified>(this);
+ break;
+ case SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER:
+ m_odeSolver = std::make_shared<OdeSolverEulerImplicit>(this);
+ break;
+ case SurgSim::Math::INTEGRATIONSCHEME_STATIC:
+ m_odeSolver = std::make_shared<OdeSolverStatic>(this);
+ break;
+ case SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4:
+ m_odeSolver = std::make_shared<OdeSolverRungeKutta4>(this);
+ break;
+ case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER:
+ m_odeSolver = std::make_shared<OdeSolverLinearEulerExplicit>(this);
+ break;
+ case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER:
+ m_odeSolver = std::make_shared<OdeSolverLinearEulerExplicitModified>(this);
+ break;
+ case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER:
+ m_odeSolver = std::make_shared<OdeSolverLinearEulerImplicit>(this);
+ break;
+ case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC:
+ m_odeSolver = std::make_shared<OdeSolverLinearStatic>(this);
+ break;
+ case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4:
+ m_odeSolver = std::make_shared<OdeSolverLinearRungeKutta4>(this);
+ break;
+ default:
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Ode solver (integration scheme) not initialized, the integration scheme is invalid";
+ return false;
+ }
+
+ // No assumption is made on the linear solver, we instantiate a general dense matrix solver
+ m_odeSolver->setLinearSolver(std::make_shared<LinearSolveAndInverseDenseMatrix>());
+
+ return true;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/DeformableRepresentation.h b/SurgSim/Physics/DeformableRepresentation.h
new file mode 100644
index 0000000..e486680
--- /dev/null
+++ b/SurgSim/Physics/DeformableRepresentation.h
@@ -0,0 +1,192 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 DeformableRepresentation.h
+/// Base class for all deformable representations (abstract class)
+
+#ifndef SURGSIM_PHYSICS_DEFORMABLEREPRESENTATION_H
+#define SURGSIM_PHYSICS_DEFORMABLEREPRESENTATION_H
+
+#include <memory>
+
+#include "SurgSim/Math/OdeEquation.h"
+#include "SurgSim/Math/OdeSolver.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+class Localization;
+
+/// Base class for all deformable representations MassSprings, Finite Element Models,...
+/// \note It is both a Physics::Representation and a Math::OdeEquation
+/// \note It holds the representation states (common to all deformable) except the initial state,
+/// \note which is being held by the OdeEquation (initial condition of the ode problem).
+/// \note It holds the initial pose, which should be set before setting the initial state so the states
+/// \note can be properly transformed.
+/// \note The current pose is always identity and therefore cannot be set. Calling setPose will raise an exception.
+/// \note It holds the force vector; the mass, damping and stiffness matrices
+/// \note Derived classes must implement the Representation API and the OdeEquation API, also set
+/// \note m_numDofPerNode and call Representation::setNumDof()
+class DeformableRepresentation :
+ public Representation,
+ public SurgSim::Math::OdeEquation
+{
+public:
+ /// Constructor
+ /// \param name The deformable representation's name
+ explicit DeformableRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~DeformableRepresentation();
+
+ virtual void resetState() override;
+
+ virtual void setInitialState(std::shared_ptr<SurgSim::Math::OdeState> initialState);
+
+ virtual const std::shared_ptr<SurgSim::Math::OdeState> getCurrentState() const;
+
+ virtual const std::shared_ptr<SurgSim::Math::OdeState> getPreviousState() const;
+
+ virtual const std::shared_ptr<SurgSim::Math::OdeState> getFinalState() const;
+
+ /// Gets the number of degrees of freedom per node
+ /// \return The number of degrees of freedom per node for this Deformable Representation
+ size_t getNumDofPerNode() const;
+
+ /// Sets the numerical integration scheme
+ /// \param integrationScheme The integration scheme to use
+ /// \note Calling setIntegrationScheme after the component has been awoken will raise an assert
+ void setIntegrationScheme(SurgSim::Math::IntegrationScheme integrationScheme);
+
+ /// Gets the numerical integration scheme
+ /// \return The integration scheme currently in use
+ SurgSim::Math::IntegrationScheme getIntegrationScheme() const;
+
+ /// Add an external generalized force applied on a specific localization
+ /// \param localization where the generalized force is applied
+ /// \param generalizedForce The force to apply (of dimension getNumDofPerNode())
+ /// \param K The stiffness matrix associated with the generalized force (Jacobian of the force w.r.t dof's position)
+ /// \param D The damping matrix associated with the generalized force (Jacobian of the force w.r.t dof's velocity)
+ virtual void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K = SurgSim::Math::Matrix(),
+ const SurgSim::Math::Matrix& D = SurgSim::Math::Matrix()) = 0;
+
+ /// \return the external generalized force vector
+ const SurgSim::Math::Vector& getExternalGeneralizedForce() const;
+
+ /// \return the external generalized stiffness matrix
+ const SurgSim::Math::Matrix& getExternalGeneralizedStiffness() const;
+
+ /// \return the external generalized damping matrix
+ const SurgSim::Math::Matrix& getExternalGeneralizedDamping() const;
+
+ /// Gets the compliance matrix associated with motion
+ /// \return The compliance matrix
+ /// \note The compliance matrix is computed automatically by the ode solver in the method 'update'
+ /// \note So one iteration needs to happen before retrieving a compliance matrix
+ const SurgSim::Math::Matrix& getComplianceMatrix() const;
+
+ virtual void update(double dt) override;
+
+ virtual void afterUpdate(double dt) override;
+
+ virtual void applyCorrection(double dt, const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity) override;
+
+ /// Deactivate and call resetState
+ void deactivateAndReset(void);
+
+ /// Set the collision representation for this physics representation, when the collision object
+ /// is involved in a collision, the collision should be resolved inside the dynamics calculation.
+ /// Specializes for discarding anything besides a rigid collision representation.
+ /// \param representation The collision representation to be used.
+ virtual void setCollisionRepresentation(
+ std::shared_ptr<SurgSim::Collision::Representation> representation) override;
+
+protected:
+ virtual bool doWakeUp() override;
+
+ /// Transform a state using a given transformation
+ /// \param[in,out] state The state to be transformed
+ /// \param transform The transformation to apply
+ virtual void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform) = 0;
+
+ /// The previous state inside the calculation loop, this has no meaning outside of the loop
+ std::shared_ptr<SurgSim::Math::OdeState> m_previousState;
+
+ /// The currently calculated state inside the physics loop, after the whole calculation is done this will
+ /// become m_finalState
+ std::shared_ptr<SurgSim::Math::OdeState> m_currentState;
+
+ /// New state is a temporary variable to store the newly computed state
+ std::shared_ptr<SurgSim::Math::OdeState> m_newState;
+
+ /// Last valid state (a.k.a final state)
+ /// \note Backup of the current state for thread-safety access while the current state is being recomputed.
+ std::shared_ptr<SurgSim::Math::OdeState> m_finalState;
+
+ /// External generalized force, stiffness and damping applied on the deformable representation
+ /// @{
+ SurgSim::Math::Vector m_externalGeneralizedForce;
+ SurgSim::Math::Matrix m_externalGeneralizedStiffness;
+ SurgSim::Math::Matrix m_externalGeneralizedDamping;
+ /// @}
+
+ /// Force applied on the deformable representation
+ SurgSim::Math::Vector m_f;
+
+ /// Mass matrix
+ SurgSim::Math::Matrix m_M;
+
+ /// Damping matrix
+ SurgSim::Math::Matrix m_D;
+
+ /// Stiffness matrix
+ SurgSim::Math::Matrix m_K;
+
+ /// Number of degrees of freedom per node (varies per deformable model)
+ /// \note MUST be set by the derived classes
+ size_t m_numDofPerNode;
+
+ /// Numerical Integration scheme (dynamic explicit/implicit solver)
+ SurgSim::Math::IntegrationScheme m_integrationScheme;
+
+ /// Specify if the Ode Solver needs to be (re)loaded (do not exist yet, or integration scheme has changed)
+ bool m_needToReloadOdeSolver;
+
+ /// Ode solver (its type depends on the numerical integration scheme)
+ std::shared_ptr<SurgSim::Math::OdeSolver> m_odeSolver;
+
+private:
+ /// NO copy constructor
+ DeformableRepresentation(const DeformableRepresentation&);
+
+ /// NO assignment operator
+ DeformableRepresentation& operator =(const DeformableRepresentation&);
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_DEFORMABLEREPRESENTATION_H
+
+
+
diff --git a/SurgSim/Physics/Fem1DElementBeam.cpp b/SurgSim/Physics/Fem1DElementBeam.cpp
new file mode 100644
index 0000000..32470cf
--- /dev/null
+++ b/SurgSim/Physics/Fem1DElementBeam.cpp
@@ -0,0 +1,390 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+
+using SurgSim::Math::addSubMatrix;
+using SurgSim::Math::addSubVector;
+using SurgSim::Math::getSubMatrix;
+using SurgSim::Math::getSubVector;
+using SurgSim::Math::setSubMatrix;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem1DElementBeam::Fem1DElementBeam(std::array<size_t, 2> nodeIds)
+ : m_G(0.0),
+ m_restLength(0.0),
+ m_radius(0.0),
+ m_A(0.0),
+ m_haveShear(true),
+ m_shearFactor(5.0 / 8.0),
+ m_Asy(0.0),
+ m_Asz(0.0),
+ m_Phi_y(0.0),
+ m_Phi_z(0.0),
+ m_Iy(0.0),
+ m_Iz(0.0),
+ m_J(0.0)
+{
+ // 6 dof per node (x, y, z, thetaX, thetaY, thetaZ)
+ setNumDofPerNode(6);
+
+ m_nodeIds.assign(nodeIds.cbegin(), nodeIds.cend());
+}
+
+void Fem1DElementBeam::setRadius(double radius)
+{
+ SURGSIM_ASSERT(radius != 0.0) << "The beam radius cannot be set to 0";
+ SURGSIM_ASSERT(radius > 0.0) << "The beam radius cannot be negative (trying to set it to " << radius << ")";
+
+ m_radius = radius;
+}
+
+double Fem1DElementBeam::getRadius() const
+{
+ return m_radius;
+}
+
+bool Fem1DElementBeam::getShearingEnabled() const
+{
+ return m_haveShear;
+}
+
+void Fem1DElementBeam::setShearingEnabled(bool enabled)
+{
+ m_haveShear = enabled;
+}
+
+double Fem1DElementBeam::getVolume(const SurgSim::Math::OdeState& state) const
+{
+ const Vector3d A = state.getPosition(m_nodeIds[0]);
+ const Vector3d B = state.getPosition(m_nodeIds[1]);
+
+ return m_A * (B - A).norm();
+}
+
+void Fem1DElementBeam::initialize(const SurgSim::Math::OdeState& state)
+{
+ // Test the validity of the physical parameters
+ FemElement::initialize(state);
+
+ SURGSIM_ASSERT(m_radius > 0) << "Fem1DElementBeam radius should be positive and non-zero. Did you call "
+ "setCrossSectionCircular(radius) ?";
+
+ m_A = M_PI * (m_radius * m_radius);
+ m_Iz = M_PI * (m_radius * m_radius * m_radius * m_radius) / 4.0;
+ m_Iy = m_Iz;
+ m_J = m_Iz + m_Iy;
+
+ // Store the rest state for this beam in m_x0
+ getSubVector(state.getPositions(), m_nodeIds, 6, &m_x0);
+
+ m_restLength = (m_x0.segment<3>(6) - m_x0.segment<3>(0)).norm();
+ SURGSIM_ASSERT(m_restLength > 0) << "Fem1DElementBeam rest length is zero (degenerate beam)";
+
+ computeInitialRotation(state);
+
+ // Pre-compute the mass and stiffness matrix
+ computeMass(state, &m_M);
+ computeStiffness(state, &m_K);
+}
+
+void Fem1DElementBeam::addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F, double scale)
+{
+ Eigen::Matrix<double, 12, 1> x, f;
+
+ // K.U = F_ext
+ // K.(x - x0) = F_ext
+ // 0 = F_ext + F_int, with F_int = -K.(x - x0)
+ getSubVector(state.getPositions(), m_nodeIds, 6, &x);
+ f = (-scale) * m_K * (x - m_x0);
+ addSubVector(f, m_nodeIds, 6, F);
+}
+
+void Fem1DElementBeam::addMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* M, double scale)
+{
+ addSubMatrix(m_M * scale, m_nodeIds, 6, M);
+}
+
+void Fem1DElementBeam::addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D, double scale)
+{
+}
+
+void Fem1DElementBeam::addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K, double scale)
+{
+ addSubMatrix(m_K * scale, getNodeIds(), 6, K);
+}
+
+void Fem1DElementBeam::addFMDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ SurgSim::Math::Matrix* M, SurgSim::Math::Matrix* D, SurgSim::Math::Matrix* K)
+{
+ // Assemble the mass matrix
+ addMass(state, M);
+
+ // No damping matrix as we are using linear elasticity (not visco-elasticity)
+
+ // Assemble the stiffness matrix
+ addStiffness(state, K);
+
+ // Assemble the force vector
+ addForce(state, F);
+}
+
+void Fem1DElementBeam::addMatVec(const SurgSim::Math::OdeState& state, double alphaM, double alphaD,
+ double alphaK, const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F)
+{
+ using SurgSim::Math::addSubVector;
+ using SurgSim::Math::getSubVector;
+
+ if (alphaM == 0.0 && alphaK == 0.0)
+ {
+ return;
+ }
+
+ Eigen::Matrix<double, 12, 1> extractedX, extractedResult;
+ getSubVector(x, m_nodeIds, 6, &extractedX);
+
+ // Adds the mass contribution
+ if (alphaM != 0.0)
+ {
+ extractedResult = alphaM * (m_M * extractedX);
+ addSubVector(extractedResult, m_nodeIds, 6, F);
+ }
+
+ // Adds the damping contribution (No damping)
+
+ // Adds the stiffness contribution
+ if (alphaK != 0.0)
+ {
+ extractedResult = alphaK * (m_K * extractedX);
+ addSubVector(extractedResult, m_nodeIds, 6, F);
+ }
+}
+
+void Fem1DElementBeam::computeMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 12, 12>* M)
+{
+ double& L = m_restLength;
+ double L2 = L * L;
+ double AL = m_A * L;
+ double AL2 = AL * L;
+ double m = AL * m_rho;
+
+ m_MLocal.setZero();
+
+ // From pg 294 of "Theory of Matrix Structural Analysis" from J.S. Przemieniecki:
+
+ // Mass matrix for node0 / node0
+ m_MLocal(0, 0) = 1.0 / 3.0;
+ m_MLocal(1, 1) = 13.0 / 35.0 + 6.0 * m_Iz / (5.0 * AL2);
+ m_MLocal(1, 5) = 11.0 * L / 210.0 + m_Iz / (10.0 * AL);
+ m_MLocal(2, 2) = 13.0 / 35.0 + 6.0 * m_Iy / (5.0 * AL2);
+ m_MLocal(2, 4) = -11.0 * L / 210.0 - m_Iy / (10.0 * AL);
+ m_MLocal(3, 3) = m_J / (3.0 * m_A);
+ m_MLocal(4, 2) = -11.0 * L / 210.0 - m_Iy / (10.0 * AL);
+ m_MLocal(4, 4) = L2 / 105.0 + 2.0 * m_Iy / (15.0 * m_A);
+ m_MLocal(5, 1) = 11.0 * L / 210.0 + m_Iz / (10.0 * AL);
+ m_MLocal(5, 5) = L2 / 105.0 + 2.0 * m_Iz / (15.0 * m_A);
+
+ // Mass matrix for node1 / node1
+ m_MLocal(6, 6) = 1.0 / 3.0;
+ m_MLocal(7, 7) = 13.0 / 35.0 + 6.0 * m_Iz / (5.0 * AL2);
+ m_MLocal(7, 11) = -11.0 * L / 210.0 - m_Iz / (10.0 * AL);
+ m_MLocal(8, 8) = 13.0 / 35.0 + 6.0 * m_Iy / (5.0 * AL2);
+ m_MLocal(8, 10) = 11.0 * L / 210.0 + m_Iy / (10.0 * AL);
+ m_MLocal(9, 9) = m_J / (3.0 * m_A);
+ m_MLocal(10, 8) = 11.0 * L / 210.0 + m_Iy / (10.0 * AL);
+ m_MLocal(10, 10) = L2 / 105.0 + 2.0 * m_Iy / (15.0 * m_A);
+ m_MLocal(11, 7) = -11.0 * L / 210.0 - m_Iz / (10.0 * AL);
+ m_MLocal(11, 11) = L2 / 105.0 + 2.0 * m_Iz / (15.0 * m_A);
+
+ // Mass matrix for node1 / node0
+ m_MLocal(6, 0) = 1.0 / 6.0;
+ m_MLocal(7, 1) = 9.0 / 70.0 - 6.0 * m_Iz / (5.0 * AL2);
+ m_MLocal(7, 5) = 13.0 * L / 420.0 - m_Iz / (10.0 * AL);
+ m_MLocal(8, 2) = 9.0 / 70.0 - 6.0 * m_Iy / (5.0 * AL2);
+ m_MLocal(8, 4) = -13.0 * L / 420.0 + m_Iy / (10.0 * AL);
+ m_MLocal(9, 3) = m_J / (6.0 * m_A);
+ m_MLocal(10, 2) = 13.0 * L / 420.0 - m_Iy / (10.0 * AL);
+ m_MLocal(10, 4) = -L2 / 140.0 - m_Iy / (30.0 * m_A);
+ m_MLocal(11, 1) = -13.0 * L / 420.0 + m_Iz / (10.0 * AL);
+ m_MLocal(11, 5) = -L2 / 140.0 - m_Iz / (30.0 * m_A);
+
+ // Mass matrix for node0 / node1
+ m_MLocal(0, 6) = 1.0 / 6.0;
+ m_MLocal(1, 7) = 9.0 / 70.0 - 6.0 * m_Iz / (5.0 * AL2);
+ m_MLocal(1, 11) = -13.0 * L / 420.0 + m_Iz / (10.0 * AL);
+ m_MLocal(2, 8) = 9.0 / 70.0 - 6.0 * m_Iy / (5.0 * AL2);
+ m_MLocal(2, 10) = 13.0 * L / 420.0 - m_Iy / (10.0 * AL);
+ m_MLocal(3, 9) = m_J / (6.0 * m_A);
+ m_MLocal(4, 8) = -13.0 * L / 420.0 + m_Iy / (10.0 * AL);
+ m_MLocal(4, 10) = -L2 / 140.0 - m_Iy / (30.0 * m_A);
+ m_MLocal(5, 7) = 13.0 * L / 420.0 - m_Iz / (10.0 * AL);
+ m_MLocal(5, 11) = -L2 / 140.0 - m_Iz / (30.0 * m_A);
+
+ //m_MLocal.setIdentity();
+
+ m_MLocal *= m;
+
+ // Transformation Local -> Global
+ m_M = m_R0 * m_MLocal * m_R0.transpose();
+}
+
+void Fem1DElementBeam::computeStiffness(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 12, 12>* k)
+{
+ double& L = m_restLength;
+ double L2 = L * L;
+ double L3 = L2 * L;
+
+ // General expresson for shear modulus in terms of Young's modulus and Poisson's ratio
+ m_G = m_E / (2.0 * (1.0 + m_nu));
+
+ if (m_haveShear)
+ {
+ // Special values for solid cross sections.
+ m_Asy = m_A * 5.0 / 6.0;
+ m_Asz = m_A * 5.0 / 6.0;
+ // From pg 80 of "Theory of Matrix Structural Analysis" from J.S. Przemieniecki:
+ m_Phi_y = 12.0 * m_E * m_Iz / (m_G * m_Asy * L2);
+ m_Phi_z = 12.0 * m_E * m_Iy / (m_G * m_Asz * L2);
+ }
+ else
+ {
+ m_Asy = 0.0;
+ m_Asz = 0.0;
+ m_Phi_y = 0.0;
+ m_Phi_z = 0.0;
+ }
+
+ m_KLocal.setZero();
+
+ // From pg 79 of "Theory of Matrix Structural Analysis" from J.S. Przemieniecki:
+
+ // Stiffness matrix node 1 / node 1
+ m_KLocal(0, 0) = m_E * m_A / L;
+ m_KLocal(1, 1) = 12 * m_E * m_Iz / (L3 * (1 + m_Phi_y));
+ m_KLocal(2, 2) = 12 * m_E * m_Iy / (L3 * (1 + m_Phi_z));
+ m_KLocal(3, 3) = m_G * m_J / L;
+ m_KLocal(4, 4) = (4 + m_Phi_z) * m_E * m_Iy / (L * (1 + m_Phi_z));
+ m_KLocal(4, 2) = -6 * m_E * m_Iy / (L2 * (1 + m_Phi_z)); // Symmetric
+ m_KLocal(2, 4) = -6 * m_E * m_Iy / (L2 * (1 + m_Phi_z)); // Symmetric
+ m_KLocal(5, 5) = (4 + m_Phi_y) * m_E * m_Iz / (L * (1 + m_Phi_y));
+ m_KLocal(5, 1) = 6 * m_E * m_Iz / (L2 * (1 + m_Phi_y)); // Symmetric
+ m_KLocal(1, 5) = 6 * m_E * m_Iz / (L2 * (1 + m_Phi_y)); // Symmetric
+
+ // Stiffness matrix node 2 / node 2
+ m_KLocal(6, 6) = m_E * m_A / L;
+ m_KLocal(7, 7) = 12 * m_E * m_Iz / (L3 * (1 + m_Phi_y));
+ m_KLocal(8, 8) = 12 * m_E * m_Iy / (L3 * (1 + m_Phi_z));
+ m_KLocal(9, 9) = m_G * m_J / L;
+ m_KLocal(10, 10) = (4 + m_Phi_z) * m_E * m_Iy / (L * (1 + m_Phi_z));
+ m_KLocal(10, 8) = 6 * m_E * m_Iy / (L2 * (1 + m_Phi_z)); // Symmetric
+ m_KLocal(8, 10) = 6 * m_E * m_Iy / (L2 * (1 + m_Phi_z)); // Symmetric
+ m_KLocal(11, 11) = (4 + m_Phi_y) * m_E * m_Iz / (L * (1 + m_Phi_y));
+ m_KLocal(11, 7) = -6 * m_E * m_Iz / (L2 * (1 + m_Phi_y)); // Symmetric
+ m_KLocal(7, 11) = -6 * m_E * m_Iz / (L2 * (1 + m_Phi_y)); // Symmetric
+
+ // Stiffness matrix node 2 / node 1
+ m_KLocal(6, 0) = -m_E * m_A / L;
+ m_KLocal(7, 1) = -12 * m_E * m_Iz / (L3 * (1 + m_Phi_y));
+ m_KLocal(8, 2) = -12 * m_E * m_Iy / (L3 * (1 + m_Phi_z));
+ m_KLocal(9, 3) = -m_G * m_J / L;
+ m_KLocal(10, 4) = (2 - m_Phi_z) * m_E * m_Iy / (L * (1 + m_Phi_z));
+ m_KLocal(10, 2) = -6 * m_E * m_Iy / (L2 * (1 + m_Phi_z)); // Anti-symmetric
+ m_KLocal(8, 4) = -m_KLocal(10, 2); // Anti-symmetric
+ m_KLocal(11, 5) = (2 - m_Phi_y) * m_E * m_Iz / (L * (1 + m_Phi_y));
+ m_KLocal(11, 1) = 6 * m_E * m_Iz / (L2 * (1 + m_Phi_y)); // Anti-symmetric
+ m_KLocal(7, 5) = -m_KLocal(11, 1); // Anti-symmetric
+
+ // Stiffness matrix node 1 / node 2 (symmetric of node 2 / node 1)
+ m_KLocal(0, 6) = m_KLocal(6, 0);
+ m_KLocal(1, 7) = m_KLocal(7, 1);
+ m_KLocal(2, 8) = m_KLocal(8, 2);
+ m_KLocal(3, 9) = m_KLocal(9, 3);
+ m_KLocal(4, 10) = m_KLocal(10, 4);
+ m_KLocal(2, 10) = m_KLocal(10, 2);
+ m_KLocal(4, 8) = m_KLocal(8, 4);
+ m_KLocal(5, 11) = m_KLocal(11, 5);
+ m_KLocal(1, 11) = m_KLocal(11, 1);
+ m_KLocal(5, 7) = m_KLocal(7, 5);
+
+ // Transformation Local -> Global
+ m_K = m_R0 * m_KLocal * m_R0.transpose();
+}
+
+void Fem1DElementBeam::computeInitialRotation(const SurgSim::Math::OdeState& state)
+{
+ // Build (i, j, k) an orthonormal frame
+ const Vector3d A = state.getPosition(m_nodeIds[0]);
+ const Vector3d B = state.getPosition(m_nodeIds[1]);
+ Vector3d i = B - A;
+ Vector3d j, k;
+
+ SURGSIM_ASSERT(SurgSim::Math::buildOrthonormalBasis(&i, &j, &k))
+ << "Invalid beam formed by extremities A=(" << A.transpose() << ") B=(" << B.transpose() << ")";
+
+ // Set up a temporary 3x3 initial rotation matrix
+ SurgSim::Math::Matrix33d rotation3x3;
+ rotation3x3.col(0) = i;
+ rotation3x3.col(1) = j;
+ rotation3x3.col(2) = k;
+
+ // Set up the 12x12 initial rotation matrix
+ m_R0.setZero();
+ setSubMatrix(rotation3x3, 0, 0, 3, 3, &m_R0);
+ setSubMatrix(rotation3x3, 1, 1, 3, 3, &m_R0);
+ setSubMatrix(rotation3x3, 2, 2, 3, 3, &m_R0);
+ setSubMatrix(rotation3x3, 3, 3, 3, 3, &m_R0);
+}
+
+SurgSim::Math::Vector Fem1DElementBeam::computeCartesianCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const
+{
+ SURGSIM_ASSERT(isValidCoordinate(naturalCoordinate)) << "naturalCoordinate must be normalized and length 2.";
+
+ Vector3d cartesianCoordinate(0.0, 0.0, 0.0);
+
+ const Vector& positions = state.getPositions();
+
+ for (int i = 0; i < 2; i++)
+ {
+ cartesianCoordinate += naturalCoordinate(i) * getSubVector(positions, m_nodeIds[i], 6).segment<3>(0);
+ }
+
+ return cartesianCoordinate;
+}
+
+SurgSim::Math::Vector Fem1DElementBeam::computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const
+{
+ SURGSIM_FAILURE() << "Function " << __FUNCTION__ << " not yet implemented.";
+ return SurgSim::Math::Vector3d::Zero();
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem1DElementBeam.h b/SurgSim/Physics/Fem1DElementBeam.h
new file mode 100644
index 0000000..e494d09
--- /dev/null
+++ b/SurgSim/Physics/Fem1DElementBeam.h
@@ -0,0 +1,208 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM1DELEMENTBEAM_H
+#define SURGSIM_PHYSICS_FEM1DELEMENTBEAM_H
+
+#include <array>
+
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// 1D FemElement based on a beam volume discretization with a fixed cross section
+///
+/// The inertia property (mass) and the stiffness matrices are derived from "Theory of Matrix Structural Analysis" from
+/// J.S. Przemieniecki. The deformation is based on linear elasticity theory and not on visco-elasticity theory;
+/// therefore, the element does not have any damping components.
+/// \note The element is considered to have a circular cross section.
+class Fem1DElementBeam : public FemElement
+{
+public:
+ /// Constructor
+ /// \param nodeIds An array of 2 node ids (A, B) defining this beam element with respect to a
+ /// DeformableRepresentaitonState which is passed to the initialize method.
+ explicit Fem1DElementBeam(std::array<size_t, 2> nodeIds);
+
+ /// Sets the beam's circular cross-section radius
+ /// \param radius The radius of the beam
+ void setRadius(double radius);
+
+ /// Gets the beam's circular cross-section radius
+ /// \return The radius of the beam
+ double getRadius() const;
+
+ /// Initializes the FemElement once everything has been set
+ /// \param state The state to initialize the FemElement with
+ /// \note We use the theory of linear elasticity, so this method pre-computes the stiffness and mass matrices
+ virtual void initialize(const SurgSim::Math::OdeState& state) override;
+
+ /// Gets the element's volume based on the input state
+ /// \param state The state to compute the volume with
+ /// \return The element's volume
+ virtual double getVolume(const SurgSim::Math::OdeState& state) const override;
+
+ /// Gets whether shearing is enabled for the element
+ /// \return True if shearing is enabled
+ bool getShearingEnabled() const;
+
+ /// Enables or disables shearing for the element
+ ///
+ /// Shearing can only be meaningfully enabled or disabled before the element has had initialize called.
+ /// \param enabled Boolean determining whether shearing is enabled
+ void setShearingEnabled(bool enabled);
+
+ /// Adds the element's force (computed for a given state) to a complete system force vector F (assembly)
+ /// \param state The state to compute the force with
+ /// \param[in,out] F The complete system force vector to add the element's force into
+ /// \param scale A factor to scale the added force with
+ /// \note The element's force is of size (getNumDofPerNode() x getNumNodes()).
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode().
+ virtual void addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ double scale = 1.0) override;
+
+ /// Adds the element's mass matrix M (computed for a given state) to a complete system mass matrix M (assembly)
+ /// \param state The state to compute the mass matrix with
+ /// \param[in,out] M The complete system mass matrix to add the element's mass-matrix into
+ /// \param scale A factor to scale the added mass matrix with
+ /// \note The element's mass matrix is a square matrix of size getNumDofPerNode() x getNumNodes().
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode()
+ virtual void addMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* M,
+ double scale = 1.0) override;
+
+ /// Adds the element's damping matrix D (= -df/dv) (computed for a given state) to a complete system damping matrix
+ /// D (assembly)
+ /// \param state The state to compute the damping matrix with
+ /// \param[in,out] D The complete system damping matrix to add the element damping matrix into
+ /// \param scale A factor to scale the added damping matrix with
+ /// \note The element's damping matrix is a square matrix of size getNumDofPerNode() x getNumNodes().
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode().
+ /// \note The beam uses linear elasticity (not visco-elasticity), so it does not have any damping.
+ virtual void addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D,
+ double scale = 1.0) override;
+
+ /// Adds the element's stiffness matrix K (= -df/dx) (computed for a given state) to a complete system stiffness
+ /// matrix K (assembly)
+ /// \param state The state to compute the stiffness matrix with
+ /// \param[in,out] K The complete system stiffness matrix to add the element stiffness matrix into
+ /// \param scale A factor to scale the added stiffness matrix with
+ /// \note The element stiffness matrix is square of size getNumDofPerNode() x getNumNodes().
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode()
+ virtual void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale = 1.0) override;
+
+ /// Adds the element's force vector, mass, stiffness and damping matrices (computed for a given state) into a
+ /// complete system data structure F, M, D, K (assembly)
+ /// \param state The state to compute everything with
+ /// \param[in,out] F The complete system force vector to add the element force into
+ /// \param[in,out] M The complete system mass matrix to add the element mass matrix into
+ /// \param[in,out] D The complete system damping matrix to add the element damping matrix into
+ /// \param[in,out] K The complete system stiffness matrix to add the element stiffness matrix into
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode().
+ virtual void addFMDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F, SurgSim::Math::Matrix* M,
+ SurgSim::Math::Matrix* D, SurgSim::Math::Matrix* K) override;
+
+ /// Adds the element's matrix-vector contribution F += (alphaM.M + alphaD.D + alphaK.K).x (computed for a given
+ /// state) into a complete system data structure F (assembly)
+ /// \param state The state to compute everything with
+ /// \param alphaM The scaling factor for the mass contribution
+ /// \param alphaD The scaling factor for the damping contribution
+ /// \param alphaK The scaling factor for the stiffness contribution
+ /// \param x A complete system vector to use as the vector in the matrix-vector multiplication
+ /// \param[in,out] F The complete system force vector to add the element matrix-vector contribution into
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode().
+ virtual void addMatVec(const SurgSim::Math::OdeState& state, double alphaM, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F);
+
+ virtual SurgSim::Math::Vector computeCartesianCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const;
+
+ virtual SurgSim::Math::Vector computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const override;
+
+protected:
+ /// Computes the beam element's initial rotation
+ /// \param state The state to compute the rotation from
+ /// \note This method stores the result in m_R0
+ void computeInitialRotation(const SurgSim::Math::OdeState& state);
+
+ /// Computes the beam's stiffness matrix
+ /// \param state The state to compute the stiffness matrix from
+ /// \param[out] k The stiffness matrix to store the result into
+ void computeStiffness(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 12, 12>* k);
+
+ /// Computes the beam's mass matrix
+ /// \param state The state to compute the stiffness matrix from
+ /// \param[out] m The mass matrix to store the result into
+ void computeMass(const SurgSim::Math::OdeState& state, Eigen::Matrix<double, 12, 12>* m);
+
+ /// The element's rest state
+ Eigen::Matrix<double, 12, 1> m_x0;
+
+ /// Initial rotation matrix for the element
+ Eigen::Matrix<double, 12, 12> m_R0;
+
+ /// Mass matrix (in global coordinate frame)
+ Eigen::Matrix<double, 12, 12> m_M;
+ /// Stiffness matrix (in local coordinate frame)
+ Eigen::Matrix<double, 12, 12> m_MLocal;
+ /// Stiffness matrix (in global coordinate frame)
+ Eigen::Matrix<double, 12, 12> m_K;
+ /// Stiffness matrix (in local coordinate frame)
+ Eigen::Matrix<double, 12, 12> m_KLocal;
+
+ /// Physical shear modulus G = E/( 2(1+mu) )
+ double m_G;
+
+ /// Rest length
+ double m_restLength;
+ /// radius for a circular Beam
+ double m_radius;
+ /// Cross sectional area = PI.radius.radius if circular
+ double m_A;
+ /// Does this beam element have shear
+ bool m_haveShear;
+ /// Shear factor (usually 5/8)
+ double m_shearFactor;
+ /// The shear area in the y and z directions (=0 => no shear) http://en.wikipedia.org/wiki/Timoshenko_beam_theory
+ double m_Asy, m_Asz;
+ /// Shear deformation parameters
+ /// Phi_y=12.E.Iz/(G.Asy.L^2) or 0 if As?=0
+ /// Phi_z=12.E.Iy/(G.Asz.L^2) or 0 if As?=0
+ double m_Phi_y, m_Phi_z;
+ /// Cross sectional moment of inertia
+ double m_Iy, m_Iz;
+ /// Polar moment of inertia
+ double m_J;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM1DELEMENTBEAM_H
diff --git a/SurgSim/Physics/Fem1DPlyReaderDelegate.cpp b/SurgSim/Physics/Fem1DPlyReaderDelegate.cpp
new file mode 100644
index 0000000..6dba519
--- /dev/null
+++ b/SurgSim/Physics/Fem1DPlyReaderDelegate.cpp
@@ -0,0 +1,96 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2014, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <array>
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Math/Valid.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+using SurgSim::DataStructures::PlyReader;
+
+Fem1DPlyReaderDelegate::Fem1DPlyReaderDelegate(std::shared_ptr<Fem1DRepresentation> fem) :
+ FemPlyReaderDelegate(fem),
+ m_radius(std::numeric_limits<double>::quiet_NaN())
+{
+}
+
+std::string Fem1DPlyReaderDelegate::getElementName() const
+{
+ return "1d_element";
+}
+
+bool Fem1DPlyReaderDelegate::registerDelegate(PlyReader* reader)
+{
+ FemPlyReaderDelegate::registerDelegate(reader);
+
+ // Radius Processing
+ reader->requestElement(
+ "radius",
+ std::bind(
+ &Fem1DPlyReaderDelegate::beginRadius, this, std::placeholders::_1, std::placeholders::_2),
+ nullptr,
+ std::bind(&Fem1DPlyReaderDelegate::endRadius, this, std::placeholders::_1));
+ reader->requestScalarProperty("radius", "value", PlyReader::TYPE_DOUBLE, 0);
+
+ return true;
+}
+
+bool Fem1DPlyReaderDelegate::fileIsAcceptable(const PlyReader& reader)
+{
+ bool result = FemPlyReaderDelegate::fileIsAcceptable(reader);
+ result = result && reader.hasProperty("radius", "value");
+
+ return result;
+}
+
+void Fem1DPlyReaderDelegate::processFemElement(const std::string& elementName)
+{
+ SURGSIM_ASSERT(m_femData.vertexCount == 2) << "Cannot process 1D Element with "
+ << m_femData.vertexCount << " vertices.";
+
+ std::array<size_t, 2> fem1DVertices;
+ std::copy(m_femData.indices, m_femData.indices + 2, fem1DVertices.begin());
+ m_fem->addFemElement(std::make_shared<Fem1DElementBeam>(fem1DVertices));
+}
+
+void* Fem1DPlyReaderDelegate::beginRadius(const std::string& elementName, size_t radiusCount)
+{
+ return &m_radius;
+}
+
+void Fem1DPlyReaderDelegate::endRadius(const std::string& elementName)
+{
+ SURGSIM_ASSERT(SurgSim::Math::isValid(m_radius)) << "No radius information processed.";
+}
+
+void Fem1DPlyReaderDelegate::endParseFile()
+{
+ for (size_t i = 0; i < m_fem->getNumFemElements(); ++i)
+ {
+ std::static_pointer_cast<Fem1DElementBeam>(m_fem->getFemElement(i))->setRadius(m_radius);
+ }
+
+ FemPlyReaderDelegate::endParseFile();
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Physics/Fem1DPlyReaderDelegate.h b/SurgSim/Physics/Fem1DPlyReaderDelegate.h
new file mode 100644
index 0000000..2dfe0c7
--- /dev/null
+++ b/SurgSim/Physics/Fem1DPlyReaderDelegate.h
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2014, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM1DPLYREADERDELEGATE_H
+#define SURGSIM_PHYSICS_FEM1DPLYREADERDELEGATE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Physics/FemPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+class Fem1DRepresentation;
+
+/// Implementation of PlyReaderDelegate for Fem1DRepresentation
+class Fem1DPlyReaderDelegate : public SurgSim::Physics::FemPlyReaderDelegate
+{
+public:
+ /// Constructor
+ /// \param fem The object that is updated when PlyReader::parseFile is called.
+ explicit Fem1DPlyReaderDelegate(std::shared_ptr<Fem1DRepresentation> fem);
+
+protected:
+ virtual std::string getElementName() const override;
+
+ virtual bool registerDelegate(SurgSim::DataStructures::PlyReader* reader) override;
+ virtual bool fileIsAcceptable(const SurgSim::DataStructures::PlyReader& reader) override;
+
+ virtual void endParseFile() override;
+ virtual void processFemElement(const std::string& elementName) override;
+
+ /// Callback function, begin the processing of radius.
+ /// \param elementName Name of the element.
+ /// \param radiusCount Number of radii.
+ /// \return memory for radius data to the reader.
+ void* beginRadius(const std::string& elementName, size_t radiusCount);
+
+ /// Callback function, end the processing of radius.
+ /// \param elementName Name of the element.
+ void endRadius(const std::string& elementName);
+
+private:
+ double m_radius;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM1DPLYREADERDELEGATE_H
\ No newline at end of file
diff --git a/SurgSim/Physics/Fem1DRepresentation.cpp b/SurgSim/Physics/Fem1DRepresentation.cpp
new file mode 100644
index 0000000..372d71d
--- /dev/null
+++ b/SurgSim/Physics/Fem1DRepresentation.cpp
@@ -0,0 +1,163 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/LinearSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/Fem1DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem1DRepresentationLocalization.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace
+{
+
+void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& transform, SurgSim::Math::Vector* x,
+ bool rotationOnly = false)
+{
+ typedef SurgSim::Math::Vector::Index IndexType;
+
+ IndexType numNodes = x->size() / 6;
+
+ SURGSIM_ASSERT(numNodes * 6 == x->size())
+ << "Unexpected number of dof in a Fem1D state vector (not a multiple of 6)";
+
+ for (IndexType nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ // Only the translational dof are transformed, rotational dof remains unchanged
+ SurgSim::Math::Vector3d xi = x->segment<3>(6 * nodeId);
+
+ x->segment<3>(6 * nodeId) = (rotationOnly) ? transform.linear() * xi : transform * xi;
+ }
+}
+
+}
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::Fem1DRepresentation, Fem1DRepresentation);
+
+Fem1DRepresentation::Fem1DRepresentation(const std::string& name) : FemRepresentation(name)
+{
+ // Reminder: m_numDofPerNode is held by DeformableRepresentation but needs to be set by all concrete derived classes
+ m_numDofPerNode = 6;
+}
+
+Fem1DRepresentation::~Fem1DRepresentation()
+{
+}
+
+RepresentationType Fem1DRepresentation::getType() const
+{
+ return REPRESENTATION_TYPE_FEM1D;
+}
+
+void Fem1DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K,
+ const SurgSim::Math::Matrix& D)
+{
+ const size_t dofPerNode = getNumDofPerNode();
+ const SurgSim::Math::Matrix::Index expectedSize = static_cast<const SurgSim::Math::Matrix::Index>(dofPerNode);
+
+ SURGSIM_ASSERT(localization != nullptr) << "Invalid localization (nullptr)";
+ SURGSIM_ASSERT(generalizedForce.size() == expectedSize) <<
+ "Generalized force has an invalid size of " << generalizedForce.size() << ". Expected " << dofPerNode;
+ SURGSIM_ASSERT(K.size() == 0 || (K.rows() == expectedSize && K.cols() == expectedSize)) <<
+ "Stiffness matrix K has an invalid size (" << K.rows() << "," << K.cols() <<
+ ") was expecting a square matrix of size " << dofPerNode;
+ SURGSIM_ASSERT(D.size() == 0 || (D.rows() == expectedSize && D.cols() == expectedSize)) <<
+ "Damping matrix D has an invalid size (" << D.rows() << "," << D.cols() <<
+ ") was expecting a square matrix of size " << dofPerNode;
+
+ std::shared_ptr<Fem1DRepresentationLocalization> localization1D =
+ std::dynamic_pointer_cast<Fem1DRepresentationLocalization>(localization);
+ SURGSIM_ASSERT(localization1D != nullptr) << "Invalid localization type (not a Fem1DRepresentationLocalization)";
+
+ const size_t elementId = localization1D->getLocalPosition().index;
+ const SurgSim::Math::Vector& coordinate = localization1D->getLocalPosition().coordinate;
+ std::shared_ptr<FemElement> element = getFemElement(elementId);
+
+ size_t index = 0;
+ for (auto nodeId : element->getNodeIds())
+ {
+ m_externalGeneralizedForce.segment(dofPerNode * nodeId, dofPerNode) += generalizedForce * coordinate[index];
+ index++;
+ }
+
+ if (K.size() != 0 || D.size() != 0)
+ {
+ size_t index1 = 0;
+ for (auto nodeId1 : element->getNodeIds())
+ {
+ size_t index2 = 0;
+ for (auto nodeId2 : element->getNodeIds())
+ {
+ if (K.size() != 0)
+ {
+ m_externalGeneralizedStiffness.block(dofPerNode * nodeId1, dofPerNode * nodeId2,
+ dofPerNode, dofPerNode) += coordinate[index1] * coordinate[index2] * K;
+ }
+ if (D.size() != 0)
+ {
+ m_externalGeneralizedDamping.block(dofPerNode * nodeId1, dofPerNode * nodeId2,
+ dofPerNode, dofPerNode) += coordinate[index1] * coordinate[index2] * D;
+ }
+ index2++;
+ }
+
+ index1++;
+ }
+ }
+}
+
+bool Fem1DRepresentation::doWakeUp()
+{
+ using SurgSim::Math::LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix;
+
+ if (!FemRepresentation::doWakeUp())
+ {
+ return false;
+ }
+
+ // Make use of a specialized linear solver for symmetric tri-diagonal block matrix of block size 6
+ m_odeSolver->setLinearSolver(std::make_shared<LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<6>>());
+
+ return true;
+}
+
+void Fem1DRepresentation::transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform)
+{
+ transformVectorByBlockOf3(transform, &state->getPositions());
+ transformVectorByBlockOf3(transform, &state->getVelocities(), true);
+}
+
+std::shared_ptr<FemPlyReaderDelegate> Fem1DRepresentation::getDelegate()
+{
+ auto thisAsSharedPtr = std::static_pointer_cast<Fem1DRepresentation>(shared_from_this());
+ auto readerDelegate = std::make_shared<Fem1DPlyReaderDelegate>(thisAsSharedPtr);
+
+ return readerDelegate;
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem1DRepresentation.h b/SurgSim/Physics/Fem1DRepresentation.h
new file mode 100644
index 0000000..0270180
--- /dev/null
+++ b/SurgSim/Physics/Fem1DRepresentation.h
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM1DREPRESENTATION_H
+#define SURGSIM_PHYSICS_FEM1DREPRESENTATION_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+SURGSIM_STATIC_REGISTRATION(Fem1DRepresentation);
+
+class FemPlyReaderDelegate;
+
+/// Finite Element Model 1D is a fem built with 1D FemElement
+///
+/// The structure of a 1D model derives into a matrix structure of tri-diagonal block.
+class Fem1DRepresentation : public FemRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name of the Fem1DRepresentation
+ explicit Fem1DRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~Fem1DRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem1DRepresentation);
+
+ virtual void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K = SurgSim::Math::Matrix(),
+ const SurgSim::Math::Matrix& D = SurgSim::Math::Matrix()) override;
+
+ /// Query the representation type
+ /// \return the RepresentationType for this representation
+ virtual RepresentationType getType() const override;
+
+protected:
+ virtual bool doWakeUp() override;
+
+ /// Transform a state using a given transformation
+ /// \param[in,out] state The state to be transformed
+ /// \param transform The transformation to apply
+ virtual void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform) override;
+
+private:
+ virtual std::shared_ptr<FemPlyReaderDelegate> getDelegate() override;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM1DREPRESENTATION_H
diff --git a/SurgSim/Physics/Fem1DRepresentationLocalization.cpp b/SurgSim/Physics/Fem1DRepresentationLocalization.cpp
new file mode 100644
index 0000000..4342ab9
--- /dev/null
+++ b/SurgSim/Physics/Fem1DRepresentationLocalization.cpp
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/Fem1DRepresentationLocalization.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem1DRepresentationLocalization::Fem1DRepresentationLocalization(
+ std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition) :
+ Localization()
+{
+ setRepresentation(representation);
+ setLocalPosition(localPosition);
+}
+
+Fem1DRepresentationLocalization::~Fem1DRepresentationLocalization()
+{
+
+}
+
+void Fem1DRepresentationLocalization::setLocalPosition(
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition)
+{
+ auto femRepresentation = std::static_pointer_cast<Fem1DRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(femRepresentation != nullptr) << "FemRepresentation is null, it was probably not" <<
+ " initialized";
+
+ SURGSIM_ASSERT(femRepresentation->isValidCoordinate(localPosition))
+ << "IndexedLocalCoordinate is invalid for Representation " << getRepresentation()->getName();
+
+ m_position = localPosition;
+}
+
+const SurgSim::DataStructures::IndexedLocalCoordinate& Fem1DRepresentationLocalization::getLocalPosition() const
+{
+ return m_position;
+}
+
+SurgSim::Math::Vector3d Fem1DRepresentationLocalization::doCalculatePosition(double time)
+{
+ using SurgSim::Math::Vector3d;
+
+ auto femRepresentation = std::static_pointer_cast<Fem1DRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(femRepresentation != nullptr) << "FemRepresentation is null, it was probably not" <<
+ " initialized";
+
+ std::shared_ptr<FemElement> femElement = femRepresentation->getFemElement(m_position.index);
+ const Vector3d currentPosition = femElement->computeCartesianCoordinate(*femRepresentation->getCurrentState(),
+ m_position.coordinate);
+ const Vector3d previousPosition = femElement->computeCartesianCoordinate(*femRepresentation->getPreviousState(),
+ m_position.coordinate);
+
+ if (time == 0.0)
+ {
+ return previousPosition;
+ }
+ else if (time == 1.0)
+ {
+ return currentPosition;
+ }
+
+ return previousPosition + time * (currentPosition - previousPosition);
+}
+
+bool Fem1DRepresentationLocalization::isValidRepresentation(std::shared_ptr<Representation> representation)
+{
+ auto femRepresentation = std::dynamic_pointer_cast<Fem1DRepresentation>(representation);
+
+ // Allows to reset the representation to nullptr ...
+ return (femRepresentation != nullptr || representation == nullptr);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem1DRepresentationLocalization.h b/SurgSim/Physics/Fem1DRepresentationLocalization.h
new file mode 100644
index 0000000..be7029a
--- /dev/null
+++ b/SurgSim/Physics/Fem1DRepresentationLocalization.h
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM1DREPRESENTATIONLOCALIZATION_H
+#define SURGSIM_PHYSICS_FEM1DREPRESENTATIONLOCALIZATION_H
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Physics/Localization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Implementation of Localization for Fem1DRepresentation
+///
+/// Fem1DRepresentationLocalization tracks the global coordinates of an IndexedLocalCoordinate associated with an
+/// Fem1DRepresentation.
+class Fem1DRepresentationLocalization : public Localization
+{
+public:
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ /// \param localCoordinate The indexed local coordinate relative to the representation.
+ Fem1DRepresentationLocalization(std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localCoordinate);
+
+ /// Destructor
+ virtual ~Fem1DRepresentationLocalization();
+
+ /// Sets the local position.
+ /// \param localCoordinate The local position to set the localization at.
+ void setLocalPosition(const SurgSim::DataStructures::IndexedLocalCoordinate& localCoordinate);
+
+ /// Gets the local position.
+ /// \return The local position set for this localization.
+ const SurgSim::DataStructures::IndexedLocalCoordinate& getLocalPosition() const;
+
+ /// Query if 'representation' is valid representation.
+ /// \param representation The representation.
+ /// \return true if valid representation, false if not.
+ virtual bool isValidRepresentation(std::shared_ptr<Representation> representation) override;
+
+private:
+ /// Calculates the global position of this localization.
+ /// \param time The time in [0..1] at which the position should be calculated.
+ /// \return The global position of the localization at the requested time.
+ /// \note time can be useful when dealing with CCD.
+ SurgSim::Math::Vector3d doCalculatePosition(double time);
+
+ /// Barycentric position in local coordinates
+ SurgSim::DataStructures::IndexedLocalCoordinate m_position;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM1DREPRESENTATIONLOCALIZATION_H
diff --git a/SurgSim/Physics/Fem2DElementTriangle.cpp b/SurgSim/Physics/Fem2DElementTriangle.cpp
new file mode 100644
index 0000000..d88a6a4
--- /dev/null
+++ b/SurgSim/Physics/Fem2DElementTriangle.cpp
@@ -0,0 +1,624 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+
+using SurgSim::Math::addSubMatrix;
+using SurgSim::Math::addSubVector;
+using SurgSim::Math::getSubMatrix;
+using SurgSim::Math::getSubVector;
+using SurgSim::Math::setSubMatrix;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+const double epsilon = 1e-8;
+const double epsilonArea = 1e-10;
+};
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem2DElementTriangle::Fem2DElementTriangle(std::array<size_t, 3> nodeIds)
+ : m_restArea(0.0),
+ m_thickness(0.0)
+{
+ // 6 dof per node (x, y, z, thetaX, thetaY, thetaZ)
+ setNumDofPerNode(6);
+
+ m_nodeIds.assign(nodeIds.cbegin(), nodeIds.cend());
+}
+
+void Fem2DElementTriangle::setThickness(double thickness)
+{
+ SURGSIM_ASSERT(thickness != 0.0) << "The thickness cannot be set to 0";
+ SURGSIM_ASSERT(thickness > 0.0) << "The thickness cannot be negative (trying to set it to " << thickness << ")";
+
+ m_thickness = thickness;
+}
+
+double Fem2DElementTriangle::getThickness() const
+{
+ return m_thickness;
+}
+
+double Fem2DElementTriangle::getVolume(const SurgSim::Math::OdeState& state) const
+{
+ const Vector3d A = state.getPosition(m_nodeIds[0]);
+ const Vector3d B = state.getPosition(m_nodeIds[1]);
+ const Vector3d C = state.getPosition(m_nodeIds[2]);
+
+ return m_thickness * (B - A).cross(C - A).norm() / 2.0;
+}
+
+void Fem2DElementTriangle::initialize(const SurgSim::Math::OdeState& state)
+{
+ // Test the validity of the physical parameters
+ FemElement::initialize(state);
+
+ SURGSIM_ASSERT(m_thickness > 0.0) << "Fem2DElementTriangle thickness should be positive and non-zero. " <<
+ "Did you call setThickness(thickness) ?";
+
+ // Store the rest state for this beam in m_x0
+ getSubVector(state.getPositions(), m_nodeIds, 6, &m_x0);
+
+ // Store the rest rotation in m_initialRotation
+ computeInitialRotation(state);
+
+ // computeShapeFunctionsParameters needs the initial rotation and
+ // is required to compute the stiffness and mass matrices
+ computeShapeFunctionsParameters(state);
+
+ // Pre-compute the mass and stiffness matrix
+ computeMass(state, &m_M);
+ computeStiffness(state, &m_K);
+}
+
+void Fem2DElementTriangle::addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F, double scale)
+{
+ Eigen::Matrix<double, 18, 1> x, f;
+
+ // K.U = F_ext
+ // K.(x - x0) = F_ext
+ // 0 = F_ext + F_int, with F_int = -K.(x - x0)
+ getSubVector(state.getPositions(), m_nodeIds, 6, &x);
+ f = -scale * (m_K * (x - m_x0));
+ addSubVector(f, m_nodeIds, 6, F);
+}
+
+void Fem2DElementTriangle::addMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* M, double scale)
+{
+ addSubMatrix(m_M * scale, m_nodeIds, 6, M);
+}
+
+void Fem2DElementTriangle::addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D,
+ double scale)
+{
+}
+
+void Fem2DElementTriangle::addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale)
+{
+ addSubMatrix(m_K * scale, getNodeIds(), 6, K);
+}
+
+void Fem2DElementTriangle::addFMDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ SurgSim::Math::Matrix* M, SurgSim::Math::Matrix* D, SurgSim::Math::Matrix* K)
+{
+ // Assemble the mass matrix
+ addMass(state, M);
+
+ // No damping matrix as we are using linear elasticity (not visco-elasticity)
+
+ // Assemble the stiffness matrix
+ addStiffness(state, K);
+
+ // Assemble the force vector
+ addForce(state, F);
+}
+
+void Fem2DElementTriangle::addMatVec(const SurgSim::Math::OdeState& state, double alphaM, double alphaD,
+ double alphaK, const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F)
+{
+ using SurgSim::Math::addSubVector;
+ using SurgSim::Math::getSubVector;
+
+ if (alphaM == 0.0 && alphaK == 0.0)
+ {
+ return;
+ }
+
+ Eigen::Matrix<double, 18, 1> extractedX, extractedResult;
+ getSubVector(x, m_nodeIds, 6, &extractedX);
+
+ // Adds the mass contribution
+ if (alphaM != 0.0)
+ {
+ extractedResult = alphaM * (m_M * extractedX);
+ addSubVector(extractedResult, m_nodeIds, 6, F);
+ }
+
+ // Adds the damping contribution (No damping)
+
+ // Adds the stiffness contribution
+ if (alphaK != 0.0)
+ {
+ extractedResult = alphaK * (m_K * extractedX);
+ addSubVector(extractedResult, m_nodeIds, 6, F);
+ }
+}
+
+void Fem2DElementTriangle::computeMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* M)
+{
+ double mass = m_rho * m_restArea * m_thickness;
+
+ m_MLocal.setIdentity();
+
+ for(size_t i = 0; i < 3; ++i)
+ {
+ // Membrane inertia matrix
+ // Przemieniecki book "Theory of Matrix Structural Analysis"
+ // Chapter 11.6, equation 11.42 for a in-plane triangle deformation
+ // m = rho.A(123).t/12.0.[2 1 1]
+ // [1 2 1]
+ // [1 1 2]
+ m_MLocal.block<3, 3>(i * 6, i * 6).setConstant(mass / 12.0);
+ m_MLocal.block<3, 3>(i * 6, i * 6).diagonal().setConstant(mass / 6.0);
+
+ // Plate inertia matrix developed from Batoz paper
+ // Interpolation of the rotational displacement over the triangle w.r.t. DOF:
+ // Uthetax(xi,neta) = z.Hx^T. U
+ // Uthetay(xi,neta) = z.Hy^T. U
+ //
+ // Which means that the shape functions for the rotational DOF are:
+ // a=(zHx^T zH^yT)
+ //
+ // Mass = \int_V rho.a^T.a dV
+ // = rho . \int_z \int_A a^T.a dA dz
+ // = rho . \int_{-h/2}^{h/2} \int_A z^2 ( Hx^t.Hx Hx^T.Hy ) dA dz
+ // ( Hy^t.Hx Hy^T.Hy )
+ //
+ // = rho . [z^3/3]_{-h/2}^{h/2} \int_0^{1} \int_0^{1-neta} 2A (Hx^T.Hx Hx^T.Hy) dxi dneta
+ // (Hy^T.Hx Hy^T.Hy)
+ //
+ // = rho . (h^3/12) . 2A . \int_0^{1} \int_0^{1-neta} (Hx^T.Hx Hx^T.Hy) dxi dneta
+ // (Hy^T.Hx Hy^T.Hy)
+ //
+ // int_0^1 \int_0^(1-neta) Hx^T.Hx dxi dneta =
+ // 1/5.( 2a4^2 + 2a5^2 + 2a6^2 - a4a5 - a4a6 - a5a6 ) +
+ // 1/20 +
+ // 4/45.( 2b4^2 + 2b5^2 + 2b6^2 + b4b5 + b4b6 + b5b6 + 2c4^2 + 2c5^2 + 2c6^2 + c4c5 + c4c6 + c5c6)
+ //
+ // int_0^1 \int_0^(1-neta) Hy^T.Hy dxi dneta =
+ // 1/5.( 2d4^2 + 2d5^2 + 2d6^2 - d4d5 - d4d6 - d5d6 ) +
+ // 1/20 +
+ // 4/45.( 2b4^2 + 2b5^2 + 2b6^2 + b4b5 + b4b6 + b5b6 + 2e4^2 + 2e5^2 + 2e6^2 + e4e5 + e4e6 + e5e6)
+ //
+ // int_0^1 \int_0^(1-neta) Hy^T.Hx dxi dneta =
+ // int_0^1 \int_0^(1-neta) Hx^T.Hy dxi dneta =
+ // 1/5 .( 2a4d4 + 2a5d5 + 2a6d6) - 1/10.( a4(d5+d6) + a5(d4+d6) + a6(d4+d5) )
+ // 4/45.( 2b4(e4+c4) + 2b5(e5+c5) + 2b6(e6+c6) )
+ // 2/45.( b4(e5+c5) + b4(e6+c6) + b5(e4+c4) + b5(e6+c6) + b6(e4+c4) + b6(e5+c5) )
+ //
+ double xx = 1.0 / 20.0;
+ xx += 1.0 / 5.0 * ( 2.0 * m_ak.squaredNorm() - m_ak[0] * m_ak[1] - m_ak[0] * m_ak[2] - m_ak[1] * m_ak[2] );
+ xx += 4.0 / 45.0 * ( 2.0 * m_bk.squaredNorm() + m_bk[0] * m_bk[1] + m_bk[0] * m_bk[2] + m_bk[1] * m_bk[2] );
+ xx += 4.0 / 45.0 * ( 2.0 * m_ck.squaredNorm() + m_ck[0] * m_ck[1] + m_ck[0] * m_ck[2] + m_ck[1] * m_ck[2] );
+ double yy = 1.0 / 20.0;
+ yy += 1.0 / 5.0 * ( 2.0 * m_dk.squaredNorm() - m_dk[0] * m_dk[1] - m_dk[0] * m_dk[2] - m_dk[1] * m_dk[2] );
+ yy += 4.0 / 45.0 * ( 2.0 * m_bk.squaredNorm() + m_bk[0] * m_bk[1] + m_bk[0] * m_bk[2] + m_bk[1] * m_bk[2] );
+ yy += 4.0 / 45.0 * ( 2.0 * m_ek.squaredNorm() + m_ek[0] * m_ek[1] + m_ek[0] * m_ek[2] + m_ek[1] * m_ek[2] );
+ double xy = 0.0;
+ xy += 1.0 / 5.0 * ( 2.0 * m_ak.dot(m_dk) );
+ xy -= 1.0 / 10.0 * ( m_ak.dot(Vector3d(m_dk[1] + m_dk[2], m_dk[0] + m_dk[2], m_dk[0] + m_dk[1])) );
+ xy += 4.0 / 45.0 * ( 2.0 * m_bk.dot(m_ek + m_ck) );
+ xy += 2.0 / 45.0 * ( m_bk[0] * (m_ek[1] + m_ck[1] + m_ek[2] + m_ck[2]) );
+ xy += 2.0 / 45.0 * ( m_bk[1] * (m_ek[0] + m_ck[0] + m_ek[2] + m_ck[2]) );
+ xy += 2.0 / 45.0 * ( m_bk[2] * (m_ek[0] + m_ck[0] + m_ek[1] + m_ck[1]) );
+
+ double coef2 = m_rho * (m_restArea * 2.0) * (m_thickness * m_thickness * m_thickness / 12.0);
+ m_MLocal(6 * i + 3, 6 * i + 3) = coef2 * xx;
+ m_MLocal(6 * i + 3, 6 * i + 4) = coef2 * xy;
+ m_MLocal(6 * i + 4, 6 * i + 3) = coef2 * xy;
+ m_MLocal(6 * i + 4, 6 * i + 4) = coef2 * yy;
+ }
+
+ // Transformation Local -> Global
+ // m_MLocal has only 3x3 block element on the diagonal, we can transform each block independently
+ m_M.setZero();
+ const SurgSim::Math::Matrix33d& rotation = m_initialRotation;
+ const SurgSim::Math::Matrix33d rotationTranspose = m_initialRotation.transpose();
+ for (size_t rowId = 0; rowId < 6; ++rowId)
+ {
+ auto MLocal3x3block = getSubMatrix(m_MLocal, rowId, rowId, 3, 3);
+ setSubMatrix(rotation * MLocal3x3block * rotationTranspose, rowId, rowId, 3, 3, &m_M);
+ }
+}
+
+void Fem2DElementTriangle::computeStiffness(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* k)
+{
+ // Membrane part from "Theory of Matrix Structural Analysis" from J.S. Przemieniecki
+ // Compute the membrane local strain-displacement matrix
+ Matrix36Type membraneStrainDisplacement;
+ membraneStrainDisplacement.setZero();
+ for(size_t i = 0; i < 3; ++i)
+ {
+ // Noting f(x,y) the membrane shape function, the displacement is:
+ // u(x,y) = f0(x,y).u0 + f1(x,y).u1 + f2(x,y).u2
+ // The strain is E=(Exx , Eyy , Exy)
+ // Exx = dux/dx = df0/dx.u0x + df1/dx.u1x + df2/dx.u2x
+ // dfi/dx = bi
+ membraneStrainDisplacement(0, 2 * i) = m_membraneShapeFunctionsParameters(i, 1);
+ // Eyy = duy/dy = df0/dy.u0y + df1/dy.u1y + df2/dy.u2y
+ // dfi/dy = ci
+ membraneStrainDisplacement(1, 2 * i + 1) = m_membraneShapeFunctionsParameters(i, 2);
+ // Exy = dux/dy + duy/dx =
+ // (df0/dy.u0x + df0/dx.u0y) + (df1/dy.u1x + df1/dx.u1y) + (df2/dy.u2x + df2/dx.u2y)
+ membraneStrainDisplacement(2, 2 * i) = m_membraneShapeFunctionsParameters(i, 2);
+ membraneStrainDisplacement(2, 2 * i + 1) = m_membraneShapeFunctionsParameters(i, 1);
+ }
+ // Membrane material stiffness coming from Hooke Law (isotropic material)
+ Matrix33Type membraneElasticMaterial;
+ membraneElasticMaterial.setIdentity();
+ membraneElasticMaterial(2, 2) = 0.5 * (1.0 - m_nu);
+ membraneElasticMaterial(0, 1) = m_nu;
+ membraneElasticMaterial(1, 0) = m_nu;
+ membraneElasticMaterial*= m_E / (1.0 - m_nu * m_nu);
+ // Membrane local stiffness matrix = integral(strain:stress)
+ Matrix66Type membraneKLocal =
+ membraneStrainDisplacement.transpose() * membraneElasticMaterial * membraneStrainDisplacement;
+ membraneKLocal *= m_thickness * m_restArea;
+
+ // Thin-plate part from "A Study Of Three-Node Triangular Plate Bending Elements", Jean-Louis Batoz
+ // Thin-Plate strain-displacement matrices using a 3 point Gauss quadrature located at the middle of the edges
+ Matrix39Type plateStrainDisplacementGaussPoint0 = batozStrainDisplacement(0.0 , 0.5);
+ Matrix39Type plateStrainDisplacementGaussPoint1 = batozStrainDisplacement(0.5 , 0.0);
+ Matrix39Type plateStrainDisplacementGaussPoint2 = batozStrainDisplacement(0.5 , 0.5);
+ // Thin-plate material stiffness coming from Hooke Law (isotropic material)
+ Matrix33Type plateElasticMaterial = membraneElasticMaterial;
+ plateElasticMaterial *= m_thickness * m_thickness * m_thickness / 12.0;
+ // Thin-plate local stiffness matrix = integral(strain:stress) using a 3 points integration
+ // rule over the parametrized triangle (exact integration because only quadratic terms)
+ // integral(over parametric triangle) Bt.Em.B = sum(gauss point (xi,neta)) wi Bt(xi,neta).Em.B(xi,neta)
+ // http://www.ams.org/journals/mcom/1959-13-068/S0025-5718-1959-0107976-5/S0025-5718-1959-0107976-5.pdf
+ // = Area(parametric triangle)/3.0 * Bt(0.5, 0.0).Em.B(0.5, 0.0)
+ // + Area(parametric triangle)/3.0 * Bt(0.0, 0.5).Em.B(0.0, 0.5)
+ // + Area(parametric triangle)/3.0 * Bt(0.5, 0.5).Em.B(0.5, 0.5)
+ double areaParametricTriangle = 1.0 / 2.0;
+ Matrix99Type plateKLocal = (areaParametricTriangle / 3.0) *
+ plateStrainDisplacementGaussPoint0.transpose() * plateElasticMaterial * plateStrainDisplacementGaussPoint0;
+ plateKLocal += (areaParametricTriangle / 3.0) *
+ plateStrainDisplacementGaussPoint1.transpose() * plateElasticMaterial * plateStrainDisplacementGaussPoint1;
+ plateKLocal += (areaParametricTriangle / 3.0) *
+ plateStrainDisplacementGaussPoint2.transpose() * plateElasticMaterial * plateStrainDisplacementGaussPoint2;
+ plateKLocal *= 2.0 * m_restArea; // Factor due to switch from cartesian coordinates to parametric coordinates
+
+ // Assemble shell stiffness as combination of membrane (Ux Uy) and plate stiffnesses (Uz ThetaX ThetaY)
+ // In the Kirchhof theory of Thin-Plate, the drilling dof (ThetaZ) is not considered.
+ // DOF are stored as follow (Ux Uy Uz ThetaX ThetaY ThetaZ)
+ m_KLocal.setIdentity();
+ for(size_t row = 0; row < 3; ++row)
+ {
+ for(size_t column = 0; column < 3; ++column)
+ {
+ // Membrane part
+ m_KLocal.block<2, 2>(6 * row, 6 * column) = membraneKLocal.block<2 ,2>(2 * row , 2 * column);
+
+ // Thin-plate part
+ m_KLocal.block(6 * row + 2, 6 * column + 2, 3, 3) = plateKLocal.block(3 * row, 3 * column, 3, 3);
+ }
+ }
+
+ // Transformation Local -> Global
+ const SurgSim::Math::Matrix33d& rotation = m_initialRotation;
+ const SurgSim::Math::Matrix33d rotationTranspose = m_initialRotation.transpose();
+ for (size_t rowBlockId = 0; rowBlockId < 6; ++rowBlockId)
+ {
+ for (size_t colBlockId = 0; colBlockId < 6; ++colBlockId)
+ {
+ auto KLocal3x3block = getSubMatrix(m_KLocal, rowBlockId, colBlockId, 3, 3);
+ setSubMatrix(rotation * KLocal3x3block * rotationTranspose, rowBlockId, colBlockId, 3, 3, &m_K);
+ }
+ }
+}
+
+void Fem2DElementTriangle::computeInitialRotation(const SurgSim::Math::OdeState& state)
+{
+ // Build (A; i, j, k) an orthonormal frame
+ // such that in the local frame, we have:
+ // A(0, 0)
+ // B(xb > 0, 0)
+ // C(xc, yc > 0)
+ const Vector3d A = state.getPosition(m_nodeIds[0]);
+ const Vector3d B = state.getPosition(m_nodeIds[1]);
+ const Vector3d C = state.getPosition(m_nodeIds[2]);
+ Vector3d i = B - A;
+ SURGSIM_ASSERT(!i.isZero())
+ << "Degenerate triangle A=B, A=(" << A.transpose() << ") B=(" << B.transpose() << ")";
+ i.normalize();
+ Vector3d j = C - A;
+ SURGSIM_ASSERT(!j.isZero())
+ << "Degenerate triangle A=C, A=(" << A.transpose() << ") C=(" << C.transpose() << ")";
+ Vector3d k = i.cross(j);
+ SURGSIM_ASSERT(!k.isZero()) << "Degenerate triangle ABC aligned or B=C, "<<
+ "A=(" << A.transpose() << ") " <<
+ "B=(" << B.transpose() << ") " <<
+ "C=(" << C.transpose() << ")";
+ k.normalize();
+ j = k.cross(i);
+ j.normalize();
+
+ // Initialize the initial rotation matrix (transform vectors from local to global coordinates)
+ m_initialRotation.col(0) = i;
+ m_initialRotation.col(1) = j;
+ m_initialRotation.col(2) = k;
+}
+
+SurgSim::Math::Vector Fem2DElementTriangle::computeCartesianCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const
+{
+ SURGSIM_ASSERT(isValidCoordinate(naturalCoordinate)) << "naturalCoordinate must be normalized and length 3.";
+
+ Vector3d cartesianCoordinate(0.0, 0.0, 0.0);
+
+ const Vector& positions = state.getPositions();
+
+ for (int i = 0; i < 3; i++)
+ {
+ cartesianCoordinate += naturalCoordinate(i) * getSubVector(positions, m_nodeIds[i], 6).segment<3>(0);
+ }
+
+ return cartesianCoordinate;
+}
+
+SurgSim::Math::Vector Fem2DElementTriangle::computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const
+{
+ SURGSIM_FAILURE() << "Function " << __FUNCTION__ << " not yet implemented.";
+ return SurgSim::Math::Vector3d::Zero();
+}
+
+void Fem2DElementTriangle::computeShapeFunctionsParameters(const SurgSim::Math::OdeState& restState)
+{
+ SURGSIM_ASSERT(m_nodeIds[0] < restState.getNumNodes()) << "Invalid nodeId[0] = " << m_nodeIds[0] <<
+ ", the number of nodes is " << restState.getNumNodes();
+ SURGSIM_ASSERT(m_nodeIds[1] < restState.getNumNodes()) << "Invalid nodeId[1] = " << m_nodeIds[1] <<
+ ", the number of nodes is " << restState.getNumNodes();
+ SURGSIM_ASSERT(m_nodeIds[2] < restState.getNumNodes()) << "Invalid nodeId[2] = " << m_nodeIds[2] <<
+ ", the number of nodes is " << restState.getNumNodes();
+
+ const SurgSim::Math::Vector3d a = restState.getPosition(m_nodeIds[0]);
+ const SurgSim::Math::Vector3d b = restState.getPosition(m_nodeIds[1]);
+ const SurgSim::Math::Vector3d c = restState.getPosition(m_nodeIds[2]);
+
+ // Transforms the 3 triangle points in 2D triangle cartesian coordinates
+ SurgSim::Math::RigidTransform3d inverseTransform;
+ inverseTransform = SurgSim::Math::makeRigidTransform(m_initialRotation, a).inverse();
+ SurgSim::Math::Vector2d a2D = (inverseTransform * a).segment(0, 2);
+ SurgSim::Math::Vector2d b2D = (inverseTransform * b).segment(0, 2);
+ SurgSim::Math::Vector2d c2D = (inverseTransform * c).segment(0, 2);
+
+ // To avoid confusion, we base all our notation on a 0-based indexing
+ // Note that Batoz paper has a 0-based indexing as well, but Przemieniecki has a 1-base indexing
+ double x0 = a2D[0];
+ double y0 = a2D[1];
+ double x1 = b2D[0];
+ double y1 = b2D[1];
+ double x2 = c2D[0];
+ double y2 = c2D[1];
+
+ // Note that by construction, we should have x0=y0=0 and y1=0
+ SURGSIM_ASSERT(std::abs(x0) < epsilon && std::abs(y0) < epsilon && std::abs(y1) < epsilon) <<
+ "Membrane local transform problem. We should have x0=y0=y1=0, but we have x0=" << x0 <<
+ " y0=" << y0 << " y1=" << y1;
+ x0=y0=y1=0.0; // Force it to exactly 0 for numerical purpose
+
+ // Also note that x1>=0 and y2>=0 by construction
+ SURGSIM_ASSERT(x1 >= 0 && y2 >= 0) <<
+ "Membrane local transform problem. We should have x1>=0 and y2>=0, but we have x1=" << x1 <<
+ " y2=" << y2;
+
+ // Note: 2Area(ABC) = 2A = (x0y1 + x1y2 + x2y0 - x0y2 - x1y0 - x2y1) =
+ // (x1-x2)(y1-y0) - (x1-x0)(y1-y2) = x12y10 - x10y12
+ // In the local frame, we have x0=y0=y1=0
+ // 2A = x1y2 (with x1>=0 and y2>=0)
+ m_restArea = x1 * y2 / 2.0;
+ SURGSIM_ASSERT(m_restArea != 0.0) << "Triangle with null area, A=(" << a.transpose() <<
+ "), B=(" << b.transpose() << "), C=(" << c.transpose() << ")";
+ SURGSIM_ASSERT(m_restArea > 0.0) << "Triangle with negtive area, Area = " << m_restArea <<
+ ", A=(" << a.transpose() << "), B=(" << b.transpose() << "), C=(" << c.transpose() << ")";
+
+ // Membrane shape functions
+ // Notation: yij = yi - yj (reminder Przemieniecki use 1-based indexing, while we use 0-based)
+ // Notation: xij = xi - xj (reminder Przemieniecki use 1-based indexing, while we use 0-based)
+ //
+ // Shape functions fi(x, y) = ai + bi.x + ci.y
+ // Let's note A the area of the triangle
+ //
+ // Identifying coefficient for f1:
+ // (a0) 1 ( x1y2-x2y1 -(x0y2-x2y0) x0y1-x1y0 )(1) 1/2A (x1y2-x2y1) ( x1y2-x2y1)
+ // (b0) = ---- ( y1-y2 -(y0-y2) y0-y1 )(0) = 1/2A y12 = 1/2A (-y21 )
+ // (c0) 2A (-(x1-x2) x0-x2 -(x0-x1) )(0) 1/2A x21 ( x21 )
+ // Similarly for f2:
+ // (a1) 1 ( x1y2-x2y1 -(x0y2-x2y0) x0y1-x1y0 )(0) 1/2A (x2y0-x0y2) (-x0y2+x2y0)
+ // (b1) = ---- ( y1-y2 -(y0-y2) y0-y1 )(1) = 1/2A y20 = 1/2A ( y20 )
+ // (c1) 2A (-(x1-x2) x0-x2 -(x0-x1) )(0) 1/2A x02 (-x20 )
+ // Similarly for f3:
+ // (a2) 1 ( x1y2-x2y1 -(x0y2-x2y0) x0y1-x1y0 )(0) 1/2A (x0y1-x1y0) ( x0y1-x1y0)
+ // (b2) = ---- ( y1-y2 -(y0-y2) y0-y1 )(0) = 1/2A y01 = 1/2A (-y10 )
+ // (c2) 2A (-(x1-x2) x0-x2 -(x0-x1) )(1) 1/2A x10 ( x10 )
+ //
+ // The above equation has been simplified in the following assignment using x0=y0=y1=0
+ // and 2A = x1y2 (cf. above for more details)
+
+ // f0(x, y) = a0 + b0.x + c0.y, store a0 b0 c0
+ m_membraneShapeFunctionsParameters(0, 0) = 1.0; // = (x1 * y2 - x2 * y1) / 2A
+ m_membraneShapeFunctionsParameters(0, 1) = -1.0 / x1; // = (y1 - y2) / 2A
+ m_membraneShapeFunctionsParameters(0, 2) = (x2 / x1 - 1.0) / y2; // = (x2 - x1) / 2A
+ // f1(x, y) = a1 + b1.x + c1.y, store a1 b1 c1
+ m_membraneShapeFunctionsParameters(1, 0) = 0.0; // = (x2 * y0 - x0 * y2) / 2A
+ m_membraneShapeFunctionsParameters(1, 1) = 1.0 / x1; // = (y2 - y0) / 2A
+ m_membraneShapeFunctionsParameters(1, 2) = -x2 / (x1 * y2); // = (x0 - x2) / 2A
+ // f2(x, y) = a2 + b2.x + c2.y, store a2 b2 c2
+ m_membraneShapeFunctionsParameters(2, 0) = 0.0; // = (x0 * y1 - x1 * y0) / 2A
+ m_membraneShapeFunctionsParameters(2, 1) = 0.0; // = (y0 - y1) / 2A
+ m_membraneShapeFunctionsParameters(2, 2) = 1.0 / y2; // = (x1 - x0) / 2A
+
+ // Thin-Plate Batoz specific data
+ m_xij[0] = x1 - x2; // xij[0] = x1 - x2
+ m_xij[1] = x2; // xij[1] = x2 - x0 but x0=0
+ m_xij[2] = -x1; // xij[2] = x0 - x1 but x0=0
+ m_yij[0] = -y2; // yij[0] = y1 - y2 but y1=0
+ m_yij[1] = y2; // yij[1] = y2 - y0 but y0=0
+ m_yij[2] = 0.0; // yij[2] = y0 - y1 but y0=y1=0
+ for(size_t k = 0; k < 3; ++k)
+ {
+ m_lij_sqr[k] = m_xij[k] * m_xij[k] + m_yij[k] * m_yij[k]; // lij_sqr = xij^2 + yij^2
+ m_ak[k] = -m_xij[k] / m_lij_sqr[k]; // ak = -xij/li^2
+ m_bk[k] = 0.75 * m_xij[k] * m_yij[k] / m_lij_sqr[k]; // bk = 3/4 xij yij/lij^2
+ m_ck[k] = (0.25 * m_xij[k] * m_xij[k] - 0.5 * m_yij[k] * m_yij[k]) / m_lij_sqr[k];
+ // ck = (1/4 xij^2 - 1/2 yij^2)/lij^2
+ m_dk[k] = -m_yij[k] / m_lij_sqr[k]; // dk = -yij/lij^2
+ m_ek[k] = (0.25 * m_yij[k] * m_yij[k] - 0.5 * m_xij[k] * m_xij[k]) / m_lij_sqr[k];
+ // ek = (1/4 yij^2 - 1/2 xij^2)/lij^2
+ //// ... and some more for the derivatives...
+ m_Pk[k] = 6.0 * m_ak[k]; // Pk = -6xij/lij^2 = 6ak
+ m_qk[k] = 4.0 * m_bk[k]; // qk = 3xijyij/lij^2 = 4bk
+ m_tk[k] = 6.0 * m_dk[k]; // tk = -6yij/lij^2 = 6dk
+ m_rk[k] = 3.0 * m_yij[k] * m_yij[k] / m_lij_sqr[k]; // rk = 3yij^2/lij^2
+ }
+}
+
+std::array<double, 9> Fem2DElementTriangle::batozDhxDxi(double xi, double neta) const
+{
+ std::array<double, 9> res;
+
+ double a = 1.0 - 2.0 * xi;
+
+ res[0] = m_Pk[2] * a + (m_Pk[1] - m_Pk[2]) * neta; // P6(1-2xi) + (P5-P6)neta
+ res[1] = m_qk[2] * a - (m_qk[1] + m_qk[2]) * neta; // q6(1-2xi) - (q5-q6)neta
+ res[2] = -4.0 + 6.0 * (xi + neta) + m_rk[2] * a - neta * (m_rk[1] + m_rk[2]); // -4 + 6(xi+neta) + r6(1-2xi) -
+ // neta(r5+r6)
+ res[3] = -m_Pk[2] * a + neta * (m_Pk[0] + m_Pk[2]); // -P6(1-2xi) + neta(P4+P6)
+ res[4] = m_qk[2] * a - neta * (m_qk[2] - m_qk[0]); // q6(1-2xi) - neta(q6-q4)
+ res[5] = -2.0 + 6.0 * xi + m_rk[2] * a + neta * (m_rk[0] - m_rk[2]); // -2 + 6xi + r6(1-2xi) + neta(r4-r6)
+
+ res[6] = -neta * (m_Pk[1] + m_Pk[0]); // -neta(P5+P4)
+ res[7] = neta * (m_qk[0] - m_qk[1]); // neta(a4-a5)
+ res[8] = -neta * (m_rk[1] - m_rk[0]); // -neta(r5-r4)
+
+ return res;
+}
+
+std::array<double, 9> Fem2DElementTriangle::batozDhxDneta(double xi, double neta) const
+{
+ std::array<double, 9> res;
+ double a = 1.0 - 2.0 * neta;
+
+ res[0] = -m_Pk[1] * a - xi * (m_Pk[2] - m_Pk[1]); // -P5(1-2neta) - xi(P6-P5)
+ res[1] = m_qk[1] * a - xi * (m_qk[1] + m_qk[2]); // q5(1-2neta) - xi(q5+q6)
+ res[2] = -4.0 + 6.0 * (xi + neta) + m_rk[1] * a - xi * (m_rk[1] + m_rk[2]); // -4 + 6(xi+neta) + r5(1-2neta) -
+ // xi(r5+r6)
+ res[3] = xi * (m_Pk[0] + m_Pk[2]); // xi(P4+P6)
+ res[4] = xi * (m_qk[0] - m_qk[2]); // xi(q4-q6)
+ res[5] = -xi * (m_rk[2] - m_rk[0]); // -xi(r6-r4)
+
+ res[6] = m_Pk[1] * a - xi * (m_Pk[0] + m_Pk[1]); // P5(1-2neta) - xi(P4+P5)
+ res[7] = m_qk[1] * a + xi * (m_qk[0] - m_qk[1]); // q5(1-2neta) + xi(q4-q5)
+ res[8] = -2.0 + 6.0 * neta + m_rk[1] * a + xi * (m_rk[0] - m_rk[1]); // -2 + 6neta + r5(1-2neta) + xi(r4-r5)
+
+ return res;
+}
+
+std::array<double, 9> Fem2DElementTriangle::batozDhyDxi(double xi, double neta) const
+{
+ std::array<double, 9> res;
+ double a = 1.0 - 2.0 * xi;
+
+ res[0] = m_tk[2] * a + neta * (m_tk[1] - m_tk[2]); // t6(1-2xi) + neta(t5-t6)
+ res[1] = 1.0 + m_rk[2] * a - neta * (m_rk[1] + m_rk[2]); // 1+r6(1-2xi) - neta(r5+r6)
+ res[2] = -m_qk[2] * a + neta * (m_qk[1] + m_qk[2]); // -q6(1-2xi) + neta(q5+q6)
+
+ res[3] = -m_tk[2] * a + neta * (m_tk[0] + m_tk[2]); // -t6(1-2xi) + neta(t4+t6)
+ res[4] = -1.0 + m_rk[2] * a + neta * (m_rk[0] - m_rk[2]); // -1 + r6(1-2xi) + neta(r4-r6)
+ res[5] = -m_qk[2] * a - neta * (m_qk[0] - m_qk[2]); // -q6(1-2xi) - neta(q4-q6)
+
+ res[6] = -neta * (m_tk[0] + m_tk[1]); // -neta(t4+t5)
+ res[7] = neta * (m_rk[0] - m_rk[1]); // neta(r4-r5)
+ res[8] = -neta * (m_qk[0] - m_qk[1]); // -neta(q4-q5)
+
+ return res;
+}
+
+std::array<double, 9> Fem2DElementTriangle::batozDhyDneta(double xi, double neta) const
+{
+ std::array<double, 9> res;
+ double a = 1.0 - 2.0 * neta;
+
+ res[0] = -m_tk[1] * a - xi * (m_tk[2] - m_tk[1]); // -t5(1-2neta) - xi(t6-t5)
+ res[1] = 1.0 + m_rk[1] * a - xi * (m_rk[1] + m_rk[2]); // 1+r5(1-2neta) - xi(r5+r6)
+ res[2] = -m_qk[1] * a + xi * (m_qk[1] + m_qk[2]); // -q5(1-2neta) + xi(q5+q6)
+
+ res[3] = xi * (m_tk[0] + m_tk[2]); // xi(t4+t6)
+ res[4] = xi * (m_rk[0] - m_rk[2]); // xi(r4-r6)
+ res[5] = -xi * (m_qk[0] - m_qk[2]); // -xi(q4-q6)
+
+ res[6] = m_tk[1] * a - xi * (m_tk[0] + m_tk[1]); // t5(1-2neta) - xi(t4+t5)
+ res[7] = -1.0 + m_rk[1] * a + xi * (m_rk[0] - m_rk[1]); // -1 + r5(1-2neta) + xi (r4-r5)
+ res[8] = -m_qk[1] * a - xi * (m_qk[0] - m_qk[1]); // -q5(1-2neta) - xi(q4-q5)
+
+ return res;
+}
+
+Fem2DElementTriangle::Matrix39Type Fem2DElementTriangle::batozStrainDisplacement(double xi, double neta)const
+{
+ Matrix39Type res;
+ std::array<double, 9> dHx_dxi, dHx_dneta, dHy_dxi, dHy_dneta;
+ double coefficient = 1.0 / (2.0 * m_restArea);
+
+ dHx_dxi = batozDhxDxi(xi, neta);
+ dHx_dneta = batozDhxDneta(xi, neta);
+ dHy_dxi = batozDhyDxi(xi, neta);
+ dHy_dneta = batozDhyDneta(xi, neta);
+
+ for(size_t i = 0; i < 9; ++i)
+ {
+ // 4 -> mid-edge 12
+ // 5 -> mid-edge 20
+ // 6 -> mid-edge 01
+ res(0, i) = coefficient * ( m_yij[1] * dHx_dxi[i] + m_yij[2] * dHx_dneta[i]);
+ res(1, i) = coefficient * (-m_xij[1] * dHy_dxi[i] - m_xij[2] * dHy_dneta[i]);
+ res(2, i) = coefficient *
+ (-m_xij[1] * dHx_dxi[i] - m_xij[2] * dHx_dneta[i] + m_yij[1] * dHy_dxi[i] + m_yij[2] * dHy_dneta[i]);
+ }
+
+ return res;
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem2DElementTriangle.h b/SurgSim/Physics/Fem2DElementTriangle.h
new file mode 100644
index 0000000..e86ade2
--- /dev/null
+++ b/SurgSim/Physics/Fem2DElementTriangle.h
@@ -0,0 +1,249 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM2DELEMENTTRIANGLE_H
+#define SURGSIM_PHYSICS_FEM2DELEMENTTRIANGLE_H
+
+#include <array>
+
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// 2D FemElement based on a triangle with a constant thickness
+///
+/// The triangle is modelled as a shell (6DOF) which is decomposed into a membrane (in-plane 2DOF (X,Y)) and
+/// a plate (bending/twisting 3DOF (Z, ThetaX,ThetaY)). The thin-plate assumption does not consider the drilling
+/// dof (ThetaZ). The system includes the DOF for completeness but does not assign any mass or stiffness to it.
+///
+/// The membrane (in-plane) equations (mass and stiffness) are following
+/// "Theory of Matrix Structural Analysis" from J.S. Przemieniecki.
+///
+/// The thin-plate (bending) equations (mass and stiffness) are following
+/// "A Study Of Three-Node Triangular Plate Bending Elements", Jean-Louis Batoz
+/// Numerical Methods in Engineering, vol 15, 1771-1812 (1980)
+/// \note The plate mass matrix is not detailed in the above paper, but the analytical equations
+/// \note have been derived from it.
+///
+/// \note The element is considered to have a constant thickness.
+class Fem2DElementTriangle : public FemElement
+{
+ typedef Eigen::Matrix<double, 3, 3> Matrix33Type;
+
+ typedef Eigen::Matrix<double, 3, 6> Matrix36Type;
+ typedef Eigen::Matrix<double, 6, 6> Matrix66Type;
+
+ typedef Eigen::Matrix<double, 3, 9> Matrix39Type;
+ typedef Eigen::Matrix<double, 9, 9> Matrix99Type;
+
+public:
+ /// Constructor
+ /// \param nodeIds An array of 3 node ids (A, B, C) defining this triangle element with respect to a
+ /// DeformableRepresentaitonState which is passed to the initialize method.
+ explicit Fem2DElementTriangle(std::array<size_t, 3> nodeIds);
+
+ /// Sets the triangle's thickness
+ /// \param thickness The thickness of the triangle
+ void setThickness(double thickness);
+
+ /// Gets the triangle's thickness
+ /// \return The thickness of the triangle
+ double getThickness() const;
+
+ /// Initializes the FemElement once everything has been set
+ /// \param state The state to initialize the FemElement with
+ /// \note We use the theory of linear elasticity, so this method pre-computes the stiffness and mass matrices
+ virtual void initialize(const SurgSim::Math::OdeState& state) override;
+
+ /// Gets the element's volume based on the input state
+ /// \param state The state to compute the volume with
+ /// \return The element's volume
+ virtual double getVolume(const SurgSim::Math::OdeState& state) const override;
+
+ /// Adds the element's force (computed for a given state) to a complete system force vector F (assembly)
+ /// \param state The state to compute the force with
+ /// \param[in,out] F The complete system force vector to add the element's force into
+ /// \param scale A factor to scale the added force with
+ /// \note The element's force is of size (getNumDofPerNode() x getNumNodes()).
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode().
+ virtual void addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ double scale = 1.0) override;
+
+ /// Adds the element's mass matrix M (computed for a given state) to a complete system mass matrix M (assembly)
+ /// \param state The state to compute the mass matrix with
+ /// \param[in,out] M The complete system mass matrix to add the element's mass-matrix into
+ /// \param scale A factor to scale the added mass matrix with
+ /// \note The element's mass matrix is a square matrix of size getNumDofPerNode() x getNumNodes().
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode()
+ virtual void addMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* M,
+ double scale = 1.0) override;
+
+ /// Adds the element's damping matrix D (= -df/dv) (computed for a given state) to a complete system damping matrix
+ /// D (assembly)
+ /// \param state The state to compute the damping matrix with
+ /// \param[in,out] D The complete system damping matrix to add the element damping matrix into
+ /// \param scale A factor to scale the added damping matrix with
+ /// \note The element's damping matrix is a square matrix of size getNumDofPerNode() x getNumNodes().
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode().
+ /// \note The beam uses linear elasticity (not visco-elasticity), so it does not have any damping.
+ virtual void addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D,
+ double scale = 1.0) override;
+
+ /// Adds the element's stiffness matrix K (= -df/dx) (computed for a given state) to a complete system stiffness
+ /// matrix K (assembly)
+ /// \param state The state to compute the stiffness matrix with
+ /// \param[in,out] K The complete system stiffness matrix to add the element stiffness matrix into
+ /// \param scale A factor to scale the added stiffness matrix with
+ /// \note The element stiffness matrix is square of size getNumDofPerNode() x getNumNodes().
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode()
+ virtual void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale = 1.0) override;
+
+ /// Adds the element's force vector, mass, stiffness and damping matrices (computed for a given state) into a
+ /// complete system data structure F, M, D, K (assembly)
+ /// \param state The state to compute everything with
+ /// \param[in,out] F The complete system force vector to add the element force into
+ /// \param[in,out] M The complete system mass matrix to add the element mass matrix into
+ /// \param[in,out] D The complete system damping matrix to add the element damping matrix into
+ /// \param[in,out] K The complete system stiffness matrix to add the element stiffness matrix into
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode().
+ virtual void addFMDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F, SurgSim::Math::Matrix* M,
+ SurgSim::Math::Matrix* D, SurgSim::Math::Matrix* K) override;
+
+ /// Adds the element's matrix-vector contribution F += (alphaM.M + alphaD.D + alphaK.K).x (computed for a given
+ /// state) into a complete system data structure F (assembly)
+ /// \param state The state to compute everything with
+ /// \param alphaM The scaling factor for the mass contribution
+ /// \param alphaD The scaling factor for the damping contribution
+ /// \param alphaK The scaling factor for the stiffness contribution
+ /// \param x A complete system vector to use as the vector in the matrix-vector multiplication
+ /// \param[in,out] F The complete system force vector to add the element matrix-vector contribution into
+ /// \note This method supposes that the incoming state contains information with the same number of dof per node as
+ /// getNumDofPerNode().
+ virtual void addMatVec(const SurgSim::Math::OdeState& state, double alphaM, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F);
+
+ virtual SurgSim::Math::Vector computeCartesianCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const;
+
+ virtual SurgSim::Math::Vector computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const override;
+
+protected:
+ /// Computes the triangle element's initial rotation
+ /// \param state The state to compute the rotation from
+ /// \note This method stores the result in m_initialRotation
+ void computeInitialRotation(const SurgSim::Math::OdeState& state);
+
+ /// Computes the triangle's stiffness matrix
+ /// \param state The state to compute the stiffness matrix from
+ /// \param[out] k The stiffness matrix to store the result into
+ void computeStiffness(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* k);
+
+ /// Computes the triangle's mass matrix
+ /// \param state The state to compute the stiffness matrix from
+ /// \param[out] m The mass matrix to store the result into
+ void computeMass(const SurgSim::Math::OdeState& state, Eigen::Matrix<double, 18, 18>* m);
+
+ /// The element's rest state
+ Eigen::Matrix<double, 18, 1> m_x0;
+
+ /// Initial rotation matrix for the element
+ SurgSim::Math::Matrix33d m_initialRotation;
+
+ /// Mass matrix (in global coordinate frame)
+ Eigen::Matrix<double, 18, 18> m_M;
+ /// Stiffness matrix (in local coordinate frame)
+ Eigen::Matrix<double, 18, 18> m_MLocal;
+ /// Stiffness matrix (in global coordinate frame)
+ Eigen::Matrix<double, 18, 18> m_K;
+ /// Stiffness matrix (in local coordinate frame)
+ Eigen::Matrix<double, 18, 18> m_KLocal;
+
+ /// The triangle rest area
+ double m_restArea;
+
+ /// Thickness of the element
+ double m_thickness;
+
+ /// Compute the various shape functions (membrane and plate deformations) parameters
+ /// \param restState the rest state to compute the shape functions paramters from
+ void computeShapeFunctionsParameters(const SurgSim::Math::OdeState& restState);
+
+ /// Membrane (in-plane) deformation. DOF simulated: (x, y)
+ /// "Theory of Matrix Structural Analysis" from J.S. Przemieniecki
+ /// Shape functions fi(x, y) = ai + bi.x + ci.y
+ SurgSim::Math::Matrix33d m_membraneShapeFunctionsParameters; //< Stores (ai, bi, ci) on each row
+
+ /// Thin-plate (bending/twisting) specific data structure
+ /// DOF simulated: (z, thetaX, thetaY)
+ /// "A Study Of Three-Node Triangular Plate Bending Elements", Jean-Louis Batoz
+ /// Numerical Methods in Engineering, vol 15, 1771-1812 (1980)
+ /// Indices are as follow:
+ /// 0 1 2 denotes triangle's points ABC:
+ /// 4 (mid-edge 12) 5 (mid-edge 20) 6 (mid-edge 01) denotes mid-edge points
+ /// Data structures having only mid-edge information are 0 based (0->4 (mid-egde 12) ; 1->5 ; 2->6)
+ SurgSim::Math::Vector3d m_xij; //< xi - xj
+ SurgSim::Math::Vector3d m_yij; //< yi - yj
+ SurgSim::Math::Vector3d m_lij_sqr; //< xij^2 + yij^2
+ SurgSim::Math::Vector3d m_ak; //< -xij/li^2
+ SurgSim::Math::Vector3d m_bk; //< 3/4 xij yij/lij2
+ SurgSim::Math::Vector3d m_ck; //< (1/4 xij^2 - 1/2 yij^2)/lij^2
+ SurgSim::Math::Vector3d m_dk; //< -yij/lij^2
+ SurgSim::Math::Vector3d m_ek; //< (1/4 yij^2 - 1/2 xij^2)/lij^2
+ //...and more variables for the derivatives
+ SurgSim::Math::Vector3d m_Pk; //< -6xij/lij^2 = 6 m_ak
+ SurgSim::Math::Vector3d m_qk; //< 3xijyij/lij^2 = 4 m_bk
+ SurgSim::Math::Vector3d m_tk; //< -6yij/lij^2 = 6 m_dk
+ SurgSim::Math::Vector3d m_rk; //< 3yij^2/lij^2
+ /// Batoz derivative dHx/dxi
+ /// \param xi, neta The parametric coordinate (in [0 1] and xi+neta<1.0)
+ /// \return The vector dHx/dxi evaluated at (xi, neta)
+ std::array<double, 9> batozDhxDxi(double xi, double neta) const;
+ /// Batoz derivative dHx/dneta
+ /// \param xi, neta The parametric coordinate (in [0 1] and xi+neta<1.0)
+ /// \return The vector dHx/dneta evaluated at (xi, neta)
+ std::array<double, 9> batozDhxDneta(double xi, double neta) const;
+ /// Batoz derivative dHy/dxi
+ /// \param xi, neta The parametric coordinate (in [0 1] and xi+neta<1.0)
+ /// \return The vector dHy/dxi evaluated at (xi, neta)
+ std::array<double, 9> batozDhyDxi(double xi, double neta) const;
+ /// Batoz derivative dHy/dneta
+ /// \param xi, neta The parametric coordinate (in [0 1] and xi+neta<1.0)
+ /// \return The vector dHy/dneta evaluated at (xi, neta)
+ std::array<double, 9> batozDhyDneta(double xi, double neta) const;
+ /// Batoz strain displacement matrix evaluated at a given point
+ /// \param xi, neta The parametric coordinate (in [0 1] and xi+neta<1.0)
+ /// \return The 3x9 strain displacement matrix evaluated at (xi, neta)
+ Matrix39Type batozStrainDisplacement(double xi, double neta) const;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM2DELEMENTTRIANGLE_H
diff --git a/SurgSim/Physics/Fem2DPlyReaderDelegate.cpp b/SurgSim/Physics/Fem2DPlyReaderDelegate.cpp
new file mode 100644
index 0000000..213ef88
--- /dev/null
+++ b/SurgSim/Physics/Fem2DPlyReaderDelegate.cpp
@@ -0,0 +1,96 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2014, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <array>
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Math/Valid.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/Fem2DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+using SurgSim::DataStructures::PlyReader;
+
+Fem2DPlyReaderDelegate::Fem2DPlyReaderDelegate(std::shared_ptr<Fem2DRepresentation> fem) :
+ FemPlyReaderDelegate(fem),
+ m_thickness(std::numeric_limits<double>::quiet_NaN())
+{
+}
+
+std::string Fem2DPlyReaderDelegate::getElementName() const
+{
+ return "2d_element";
+}
+
+bool Fem2DPlyReaderDelegate::registerDelegate(PlyReader* reader)
+{
+ FemPlyReaderDelegate::registerDelegate(reader);
+
+ // Thickness Processing
+ reader->requestElement(
+ "thickness",
+ std::bind(
+ &Fem2DPlyReaderDelegate::beginThickness, this, std::placeholders::_1, std::placeholders::_2),
+ nullptr,
+ std::bind(&Fem2DPlyReaderDelegate::endThickness, this, std::placeholders::_1));
+ reader->requestScalarProperty("thickness", "value", PlyReader::TYPE_DOUBLE, 0);
+
+ return true;
+}
+
+bool Fem2DPlyReaderDelegate::fileIsAcceptable(const PlyReader& reader)
+{
+ bool result = FemPlyReaderDelegate::fileIsAcceptable(reader);
+ result = result && reader.hasProperty("thickness", "value");
+
+ return result;
+}
+
+void Fem2DPlyReaderDelegate::processFemElement(const std::string& elementName)
+{
+ SURGSIM_ASSERT(m_femData.vertexCount == 3) << "Cannot process 2D Element with "
+ << m_femData.vertexCount << " vertices.";
+
+ std::array<size_t, 3> fem2DVertices;
+ std::copy(m_femData.indices, m_femData.indices + 3, fem2DVertices.begin());
+ m_fem->addFemElement(std::make_shared<Fem2DElementTriangle>(fem2DVertices));
+}
+
+void* Fem2DPlyReaderDelegate::beginThickness(const std::string& elementName, size_t thicknessCount)
+{
+ return &m_thickness;
+}
+
+void Fem2DPlyReaderDelegate::endThickness(const std::string& elementName)
+{
+ SURGSIM_ASSERT(SurgSim::Math::isValid(m_thickness)) << "No thickness information processed.";
+}
+
+void Fem2DPlyReaderDelegate::endParseFile()
+{
+ for (size_t i = 0; i < m_fem->getNumFemElements(); ++i)
+ {
+ std::static_pointer_cast<Fem2DElementTriangle>(m_fem->getFemElement(i))->setThickness(m_thickness);
+ }
+
+ FemPlyReaderDelegate::endParseFile();
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Physics/Fem2DPlyReaderDelegate.h b/SurgSim/Physics/Fem2DPlyReaderDelegate.h
new file mode 100644
index 0000000..a0c6db9
--- /dev/null
+++ b/SurgSim/Physics/Fem2DPlyReaderDelegate.h
@@ -0,0 +1,63 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2014, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM2DPLYREADERDELEGATE_H
+#define SURGSIM_PHYSICS_FEM2DPLYREADERDELEGATE_H
+
+#include <memory>
+
+#include "SurgSim/Physics/FemPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+class Fem2DRepresentation;
+
+/// Implementation of PlyReaderDelegate for Fem2DRepresentation
+class Fem2DPlyReaderDelegate : public SurgSim::Physics::FemPlyReaderDelegate
+{
+public:
+ /// Constructor
+ /// \param fem The object that is updated when PlyReader::parseFile is called.
+ explicit Fem2DPlyReaderDelegate(std::shared_ptr<Fem2DRepresentation> fem);
+
+protected:
+ virtual std::string getElementName() const override;
+
+ virtual bool registerDelegate(SurgSim::DataStructures::PlyReader* reader) override;
+ virtual bool fileIsAcceptable(const SurgSim::DataStructures::PlyReader& reader) override;
+
+ virtual void endParseFile() override;
+ virtual void processFemElement(const std::string& elementName) override;
+
+ /// Callback function, begin the processing of thickness.
+ /// \param elementName Name of the element.
+ /// \param thicknessCount Number of thicknesses.
+ /// \return memory for thickness data to the reader.
+ void* beginThickness(const std::string& elementName, size_t thicknessCount);
+
+ /// Callback function, end the processing of thickness.
+ /// \param elementName Name of the element.
+ void endThickness(const std::string& elementName);
+
+private:
+ double m_thickness;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM2DPLYREADERDELEGATE_H
\ No newline at end of file
diff --git a/SurgSim/Physics/Fem2DRepresentation.cpp b/SurgSim/Physics/Fem2DRepresentation.cpp
new file mode 100644
index 0000000..77ced0e
--- /dev/null
+++ b/SurgSim/Physics/Fem2DRepresentation.cpp
@@ -0,0 +1,145 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/Fem2DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem2DRepresentationLocalization.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace
+{
+void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& transform, SurgSim::Math::Vector* x,
+ bool rotationOnly = false)
+{
+ typedef SurgSim::Math::Vector::Index IndexType;
+
+ IndexType numNodes = x->size() / 6;
+
+ SURGSIM_ASSERT(numNodes * 6 == x->size())
+ << "Unexpected number of dof in a Fem2D state vector (not a multiple of 6)";
+
+ for (IndexType nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ // Only the translational dof are transformed, rotational dof remains unchanged
+ SurgSim::Math::Vector3d xi = x->segment<3>(6 * nodeId);
+
+ x->segment<3>(6 * nodeId) = (rotationOnly) ? transform.linear() * xi : transform * xi;
+ }
+}
+}
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::Fem2DRepresentation, Fem2DRepresentation);
+
+Fem2DRepresentation::Fem2DRepresentation(const std::string& name) : FemRepresentation(name)
+{
+ // Reminder: m_numDofPerNode is held by DeformableRepresentation but needs to be set by all concrete derived classes
+ m_numDofPerNode = 6;
+}
+
+Fem2DRepresentation::~Fem2DRepresentation()
+{
+}
+
+void Fem2DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K,
+ const SurgSim::Math::Matrix& D)
+{
+ const size_t dofPerNode = getNumDofPerNode();
+ const SurgSim::Math::Matrix::Index expectedSize = static_cast<const SurgSim::Math::Matrix::Index>(dofPerNode);
+
+ SURGSIM_ASSERT(localization != nullptr) << "Invalid localization (nullptr)";
+ SURGSIM_ASSERT(generalizedForce.size() == expectedSize) <<
+ "Generalized force has an invalid size of " << generalizedForce.size() << ". Expected " << dofPerNode;
+ SURGSIM_ASSERT(K.size() == 0 || (K.rows() == expectedSize && K.cols() == expectedSize)) <<
+ "Stiffness matrix K has an invalid size (" << K.rows() << "," << K.cols() <<
+ ") was expecting a square matrix of size " << dofPerNode;
+ SURGSIM_ASSERT(D.size() == 0 || (D.rows() == expectedSize && D.cols() == expectedSize)) <<
+ "Damping matrix D has an invalid size (" << D.rows() << "," << D.cols() <<
+ ") was expecting a square matrix of size " << dofPerNode;
+
+ std::shared_ptr<Fem2DRepresentationLocalization> localization2D =
+ std::dynamic_pointer_cast<Fem2DRepresentationLocalization>(localization);
+ SURGSIM_ASSERT(localization2D != nullptr) << "Invalid localization type (not a Fem2DRepresentationLocalization)";
+
+ const size_t elementId = localization2D->getLocalPosition().index;
+ const SurgSim::Math::Vector& coordinate = localization2D->getLocalPosition().coordinate;
+ std::shared_ptr<FemElement> element = getFemElement(elementId);
+
+ size_t index = 0;
+ for (auto nodeId : element->getNodeIds())
+ {
+ m_externalGeneralizedForce.segment(dofPerNode * nodeId, dofPerNode) += generalizedForce * coordinate[index];
+ index++;
+ }
+
+ if (K.size() != 0 || D.size() != 0)
+ {
+ size_t index1 = 0;
+ for (auto nodeId1 : element->getNodeIds())
+ {
+ size_t index2 = 0;
+ for (auto nodeId2 : element->getNodeIds())
+ {
+ if (K.size() != 0)
+ {
+ m_externalGeneralizedStiffness.block(dofPerNode * nodeId1, dofPerNode * nodeId2,
+ dofPerNode, dofPerNode) += coordinate[index1] * coordinate[index2] * K;
+ }
+ if (D.size() != 0)
+ {
+ m_externalGeneralizedDamping.block(dofPerNode * nodeId1, dofPerNode * nodeId2,
+ dofPerNode, dofPerNode) += coordinate[index1] * coordinate[index2] * D;
+ }
+ index2++;
+ }
+
+ index1++;
+ }
+ }
+}
+
+RepresentationType Fem2DRepresentation::getType() const
+{
+ return REPRESENTATION_TYPE_FEM2D;
+}
+
+std::shared_ptr<FemPlyReaderDelegate> Fem2DRepresentation::getDelegate()
+{
+ auto thisAsSharedPtr = std::static_pointer_cast<Fem2DRepresentation>(shared_from_this());
+ auto readerDelegate = std::make_shared<Fem2DPlyReaderDelegate>(thisAsSharedPtr);
+
+ return readerDelegate;
+}
+
+void Fem2DRepresentation::transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform)
+{
+ transformVectorByBlockOf3(transform, &state->getPositions());
+ transformVectorByBlockOf3(transform, &state->getVelocities(), true);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem2DRepresentation.h b/SurgSim/Physics/Fem2DRepresentation.h
new file mode 100644
index 0000000..ddb3517
--- /dev/null
+++ b/SurgSim/Physics/Fem2DRepresentation.h
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM2DREPRESENTATION_H
+#define SURGSIM_PHYSICS_FEM2DREPRESENTATION_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+SURGSIM_STATIC_REGISTRATION(Fem2DRepresentation);
+
+class Fem2DPlyReaderDelegate;
+
+/// Finite Element Model 2D is a fem built with 2D FemElement
+class Fem2DRepresentation : public FemRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name of the Fem2DRepresentation
+ explicit Fem2DRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~Fem2DRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem2DRepresentation);
+
+ virtual void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K = SurgSim::Math::Matrix(),
+ const SurgSim::Math::Matrix& D = SurgSim::Math::Matrix()) override;
+
+ virtual RepresentationType getType() const override;
+
+protected:
+ virtual void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform) override;
+
+private:
+ virtual std::shared_ptr<FemPlyReaderDelegate> getDelegate() override;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM2DREPRESENTATION_H
diff --git a/SurgSim/Physics/Fem2DRepresentationLocalization.cpp b/SurgSim/Physics/Fem2DRepresentationLocalization.cpp
new file mode 100644
index 0000000..6807610
--- /dev/null
+++ b/SurgSim/Physics/Fem2DRepresentationLocalization.cpp
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/Fem2DRepresentationLocalization.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem2DRepresentationLocalization::Fem2DRepresentationLocalization(
+ std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition) :
+ Localization()
+{
+ setRepresentation(representation);
+ setLocalPosition(localPosition);
+}
+
+Fem2DRepresentationLocalization::~Fem2DRepresentationLocalization()
+{
+
+}
+
+void Fem2DRepresentationLocalization::setLocalPosition(
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition)
+{
+ auto femRepresentation = std::static_pointer_cast<Fem2DRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(femRepresentation != nullptr) << "FemRepresentation is null, it was probably not" <<
+ " initialized";
+
+ SURGSIM_ASSERT(femRepresentation->isValidCoordinate(localPosition))
+ << "IndexedLocalCoordinate is invalid for Representation " << getRepresentation()->getName();
+
+ m_position = localPosition;
+}
+
+const SurgSim::DataStructures::IndexedLocalCoordinate& Fem2DRepresentationLocalization::getLocalPosition() const
+{
+ return m_position;
+}
+
+SurgSim::Math::Vector3d Fem2DRepresentationLocalization::doCalculatePosition(double time)
+{
+ using SurgSim::Math::Vector3d;
+
+ auto femRepresentation = std::static_pointer_cast<Fem2DRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(femRepresentation != nullptr) << "FemRepresentation is null, it was probably not" <<
+ " initialized";
+
+ std::shared_ptr<FemElement> femElement = femRepresentation->getFemElement(m_position.index);
+ const Vector3d currentPosition = femElement->computeCartesianCoordinate(*femRepresentation->getCurrentState(),
+ m_position.coordinate);
+ const Vector3d previousPosition = femElement->computeCartesianCoordinate(*femRepresentation->getPreviousState(),
+ m_position.coordinate);
+
+ if (time == 0.0)
+ {
+ return previousPosition;
+ }
+ else if (time == 1.0)
+ {
+ return currentPosition;
+ }
+
+ return previousPosition + time * (currentPosition - previousPosition);
+}
+
+bool Fem2DRepresentationLocalization::isValidRepresentation(std::shared_ptr<Representation> representation)
+{
+ auto femRepresentation = std::dynamic_pointer_cast<Fem2DRepresentation>(representation);
+
+ // Allows to reset the representation to nullptr ...
+ return (femRepresentation != nullptr || representation == nullptr);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem2DRepresentationLocalization.h b/SurgSim/Physics/Fem2DRepresentationLocalization.h
new file mode 100644
index 0000000..a43b427
--- /dev/null
+++ b/SurgSim/Physics/Fem2DRepresentationLocalization.h
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM2DREPRESENTATIONLOCALIZATION_H
+#define SURGSIM_PHYSICS_FEM2DREPRESENTATIONLOCALIZATION_H
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Physics/Localization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Implementation of Localization for Fem2DRepresentation
+///
+/// Fem2DRepresentationLocalization tracks the global coordinates of an IndexedLocalCoordinate associated with an
+/// Fem2DRepresentation.
+class Fem2DRepresentationLocalization : public Localization
+{
+public:
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ /// \param localCoordinate The indexed local coordinate relative to the representation.
+ Fem2DRepresentationLocalization(std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localCoordinate);
+
+ /// Destructor
+ virtual ~Fem2DRepresentationLocalization();
+
+ /// Sets the local position.
+ /// \param localCoordinate The local position to set the localization at.
+ void setLocalPosition(const SurgSim::DataStructures::IndexedLocalCoordinate& localCoordinate);
+
+ /// Gets the local position.
+ /// \return The local position set for this localization.
+ const SurgSim::DataStructures::IndexedLocalCoordinate& getLocalPosition() const;
+
+ /// Query if 'representation' is valid representation.
+ /// \param representation The representation.
+ /// \return true if valid representation, false if not.
+ virtual bool isValidRepresentation(std::shared_ptr<Representation> representation) override;
+
+private:
+ /// Calculates the global position of this localization.
+ /// \param time The time in [0..1] at which the position should be calculated.
+ /// \return The global position of the localization at the requested time.
+ /// \note time can be useful when dealing with CCD.
+ SurgSim::Math::Vector3d doCalculatePosition(double time);
+
+ /// Barycentric position in local coordinates
+ SurgSim::DataStructures::IndexedLocalCoordinate m_position;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM2DREPRESENTATIONLOCALIZATION_H
diff --git a/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.cpp b/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.cpp
new file mode 100644
index 0000000..69be499
--- /dev/null
+++ b/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.cpp
@@ -0,0 +1,268 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h"
+
+using SurgSim::Math::addSubMatrix;
+using SurgSim::Math::addSubVector;
+using SurgSim::Math::getSubMatrix;
+using SurgSim::Math::getSubVector;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem3DElementCorotationalTetrahedron::Fem3DElementCorotationalTetrahedron(std::array<size_t, 4> nodeIds)
+ : Fem3DElementTetrahedron(nodeIds)
+{
+}
+
+void Fem3DElementCorotationalTetrahedron::initialize(const SurgSim::Math::OdeState& state)
+{
+ // Initialize the linear tetrahedron element (this computes the linear stiffness matrix)
+ Fem3DElementTetrahedron::initialize(state);
+
+ // The co-rotational frame is set to identity at first
+ m_rotation.setIdentity();
+
+ // The initial co-rotated stiffness matrix is equal to the local stiffness matrix
+ m_corotationalStiffnessMatrix = m_K;
+
+ // Pre-compute the matrix V^-1 with V the matrix of the undeformed tetrahedron points in homogeneous coordinates
+ SurgSim::Math::Matrix44d m_V;
+ m_V.col(0).segment<3>(0) = state.getPosition(m_nodeIds[0]);
+ m_V.col(1).segment<3>(0) = state.getPosition(m_nodeIds[1]);
+ m_V.col(2).segment<3>(0) = state.getPosition(m_nodeIds[2]);
+ m_V.col(3).segment<3>(0) = state.getPosition(m_nodeIds[3]);
+ m_V.row(3).setOnes();
+ double determinant;
+ bool invertible;
+ m_V.computeInverseAndDetWithCheck(m_Vinverse, determinant, invertible);
+ SURGSIM_ASSERT(invertible) << "Trying to initialize an invalid co-rotational tetrahedron." <<
+ " Matrix V not invertible." << std::endl <<
+ "More likely the tetrahedron is degenerated:" << std::endl <<
+ " A = (" << state.getPosition(m_nodeIds[0]).transpose() << ")" << std::endl <<
+ " B = (" << state.getPosition(m_nodeIds[1]).transpose() << ")" << std::endl <<
+ " C = (" << state.getPosition(m_nodeIds[2]).transpose() << ")" << std::endl <<
+ " D = (" << state.getPosition(m_nodeIds[3]).transpose() << ")" << std::endl;
+}
+
+void Fem3DElementCorotationalTetrahedron::addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ double scale)
+{
+ Eigen::Matrix<double, 12, 1> x, R_x0, f;
+
+ // R.K.(R^-1.x - x0) = Fext
+ // 0 = Fext + Fint with Fint = -R.K.R^-1.(x - R.x0)
+ getSubVector(state.getPositions(), m_nodeIds, 3, &x);
+ for (size_t nodeId = 0; nodeId < 4; ++nodeId)
+ {
+ getSubVector(R_x0, nodeId, 3) = m_rotation * getSubVector(m_x0, nodeId, 3);
+ }
+ f = (- scale) * m_corotationalStiffnessMatrix * (x - R_x0);
+ addSubVector(f, m_nodeIds, 3, F);
+}
+
+void Fem3DElementCorotationalTetrahedron::addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale)
+{
+ addSubMatrix(m_corotationalStiffnessMatrix * scale, getNodeIds(), 3, K);
+}
+
+void Fem3DElementCorotationalTetrahedron::addMatVec(const SurgSim::Math::OdeState& state,
+ double alphaM, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& vector, SurgSim::Math::Vector* result)
+{
+ if (alphaM == 0.0 && alphaK == 0.0)
+ {
+ return;
+ }
+
+ Eigen::Matrix<double, 12, 1> vectorLocal, resultLocal;
+ getSubVector(vector, m_nodeIds, 3, &vectorLocal);
+
+ // Adds the mass contribution
+ if (alphaM != 0.0)
+ {
+ resultLocal = alphaM * (m_M * vectorLocal);
+ addSubVector(resultLocal, m_nodeIds, 3, result);
+ }
+
+ // Adds the stiffness contribution
+ if (alphaK != 0.0)
+ {
+ resultLocal = alphaK * (m_corotationalStiffnessMatrix * vectorLocal);
+ addSubVector(resultLocal, m_nodeIds, 3, result);
+ }
+}
+
+bool Fem3DElementCorotationalTetrahedron::update(const SurgSim::Math::OdeState& state)
+{
+ using SurgSim::Math::makeSkewSymmetricMatrix;
+ using SurgSim::Math::skew;
+ using SurgSim::Math::Vector3d;
+
+ // The update does two things:
+ // 1) Recompute the element's rotation
+ // 2) Update the element's stiffness matrix based on the new rotation
+
+ // 1) Recompute the element's rotation
+ Eigen::Matrix<double, 12, 1> x;
+ getSubVector(state.getPositions(), m_nodeIds, 3, &x);
+
+ // Matrix P is the matrix of the deformed tetrahedron points in homogenous coordinates.
+ SurgSim::Math::Matrix44d P;
+ P.col(0).segment<3>(0) = getSubVector(x, 0, 3);
+ P.col(1).segment<3>(0) = getSubVector(x, 1, 3);
+ P.col(2).segment<3>(0) = getSubVector(x, 2, 3);
+ P.col(3).segment<3>(0) = getSubVector(x, 3, 3);
+ P.row(3).setOnes();
+
+ // Matrix V is the matrix of the undeformed tetrahedron points in homogenous coordinates.
+ // F = P.V^1 is an affine transformation (deformation gradient) measuring the transformation
+ // between the undeformed and deformed configurations.
+ Eigen::Transform<double, 3, Eigen::Affine> F(P * m_Vinverse);
+ // F is of the form (B t) where t contains the translation part and B the rotation and stretching.
+ // (0 1)
+ // c.f. "Interactive Virtual Materials", Muller, Gross. Graphics Interface 2004
+
+ // http://en.wikipedia.org/wiki/Polar_decomposition
+ // Compute the polar decomposition of F to extract the rotation and the scaling parts.
+ SurgSim::Math::Matrix33d scaling;
+ F.computeRotationScaling(&m_rotation, &scaling);
+
+ if (std::abs(m_rotation.determinant() - 1.0) > 1e-8)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "Rotation has an invalid determinant of " << m_rotation.determinant();
+ return false;
+ }
+
+ // Build a 12x12 rotation matrix, duplicating the 3x3 rotation on the diagonal blocks
+ Eigen::Matrix<double, 12, 12> R12x12 = Eigen::Matrix<double, 12, 12>::Zero();
+ for (size_t nodeId = 0; nodeId < 4; ++nodeId)
+ {
+ getSubMatrix(R12x12, nodeId, nodeId, 3, 3) = m_rotation;
+ }
+
+ // 2) Update the element's stiffness matrix based on the new rotation
+ // F = -RKe(R^t.x - x0) with Ke the element linear stiffness matrix, R the 12x12 rotation matrix
+ // K = -dF/dx
+ // K = sum[dR/dxl.Ke.(R^t.x-x0)]_l + R.Ke.R^t + [R.Ke.(dR/dxl)^T.x]_l
+ // c.f. "Exact Corotational Linear FEM Stiffness Matrix", Jernej Barbic. Technical Report USC 2012.
+ // with
+ // xl the l^th component of the dof vector x
+ // [v(l)]_l is a 12x12 matrix whose l^th column is defined by the column vector v(l)
+ // Each column of the 2 matrices [dR/dxl.Ke.(R^t.x-x0)]_l + [R.Ke.(dR/dxl)^T.x]_l
+ // can be calculated independently by differentiating R with respect to x, so the entire matrices are defined
+ // with a sum over the 12 degrees of freedom, to define each column one by one.
+
+ // Here is the rotated element stiffness matrix part
+ Eigen::Matrix<double, 12, 12> RK = R12x12 * m_K;
+ m_corotationalStiffnessMatrix = RK * R12x12.transpose();
+
+ // Now we compute some useful matrices for the next step
+ double determinant;
+ bool invertible;
+ SurgSim::Math::Matrix33d G, Ginv;
+ G = (scaling.trace() * SurgSim::Math::Matrix33d::Identity() - scaling) * m_rotation.transpose();
+ G.computeInverseAndDetWithCheck(Ginv, determinant, invertible);
+ if (!invertible)
+ {
+ SURGSIM_LOG(SurgSim::Framework::Logger::getDefaultLogger(), WARNING) <<
+ "Corotational element has a singular G matrix";
+ return false;
+ }
+
+ // dR/dx = dR/dF . dF/dx with dF/dx constant over time.
+ // Here, we compute the various dR/d(F[i][j]).
+ std::array<std::array<SurgSim::Math::Matrix33d, 3>, 3> dRdF;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ for (size_t j = 0; j < 3; ++j)
+ {
+ // Compute wij, the vector solution of G.wij = 2.skew(R^t.ei.ej^t)
+ // wij is a rotation vector by nature
+ Vector3d wij = Ginv * 2.0 * skew((m_rotation.transpose() *
+ (Vector3d::Unit(i) * Vector3d::Unit(j).transpose())).eval());
+
+ // Compute dR/dFij = [wij].R
+ dRdF[i][j] = makeSkewSymmetricMatrix(wij) * m_rotation;
+ }
+ }
+
+ // dR/dx = dR/dF . dF/dx
+ // dF/dx is block sparse and constant over time, so we develop the matrix multiplication to avoid unecessary
+ // calculation. Nevertheless, to allow the user to follow this calculation, we relate it to the notation in
+ // the paper:
+ // A3x3 being a 3x3 matrix, asVector(A3x3) = (A00 A01 A02 A10 A11 A12 A20 A21 A22)
+ // dR/dF becomes a single 9x9 matrix where the 3x3 matrix dR/dFij is stored asVector in the (3*i+j)th row
+ // dF/dx becomes a single 9x12 matrix of the form (n1 0 0 n2 0 0 n3 0 0 n4 0 0)
+ // (0 n1 0 0 n2 0 0 n3 0 0 n4 0)
+ // (0 0 n1 0 0 n2 0 0 n3 0 0 n4)
+ // ni being the first 3 entries of the i^th row of V^1 (m_Vinverse)
+ //
+ // dR/dx = (asVector(dRdF00)).(n1x 0 0 n2x 0 0 n3x 0 0 n4x 0 0)
+ // (asVector(dRdF01)) (n1y 0 0 n2y 0 0 n3y 0 0 n4y 0 0)
+ // ( ... ) ( ... )
+ // (asVector(dRdF22)) (0 0 n1z 0 0 n2z 0 0 n3z 0 0 n4z)
+ // dR/dxl is a 3x3 matrix stored asVector in the l^th column of the above resulting matrix
+ std::array<SurgSim::Math::Matrix33d, 12> dRdX;
+ for(int nodeId = 0; nodeId < 4; ++nodeId)
+ {
+ Vector3d ni(m_Vinverse.row(nodeId).segment<3>(0));
+
+ for(int axis = 0; axis < 3; ++axis)
+ {
+ size_t dofId = 3 * nodeId + axis;
+
+ // Let's define the 3x3 matrix dR/d(x[dofId])
+ for (int i = 0; i < 3; ++i)
+ {
+ for (int j = 0; j < 3; ++j)
+ {
+ dRdX[dofId](i, j) = dRdF[i][j].row(axis).dot(ni);
+ }
+ }
+ }
+ }
+
+ // Now that we have dR/dx, we can add the 2 extra terms to the stiffness matrix:
+ // K += [dR/dxl.Ke.(R^t.x-x0]_l + [R.Ke.(dR/dxl)^T.x]_l
+ // with
+ // xl the l^th component of the dof vector x
+ // [v(l)]_l a 12x12 matrix whose l^th column is defined by the column vector v(l)
+ Eigen::Matrix<double, 12, 1> KTimesRx_x0 = m_K * (R12x12.transpose() * x - m_x0);
+ Eigen::Matrix<double, 12, 12> dRdxl12x12 = Eigen::Matrix<double, 12, 12>::Zero();
+ for (size_t dofId = 0; dofId < 12; ++dofId)
+ {
+ for (size_t nodeId = 0; nodeId < 4; ++nodeId)
+ {
+ getSubMatrix(dRdxl12x12, nodeId, nodeId, 3, 3) = dRdX[dofId];
+ }
+
+ m_corotationalStiffnessMatrix.col(dofId) += dRdxl12x12 * KTimesRx_x0 + (RK * dRdxl12x12.transpose()) * x;
+ }
+
+ return true;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h b/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h
new file mode 100644
index 0000000..7cd764d
--- /dev/null
+++ b/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h
@@ -0,0 +1,83 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM3DELEMENTCOROTATIONALTETRAHEDRON_H
+#define SURGSIM_PHYSICS_FEM3DELEMENTCOROTATIONALTETRAHEDRON_H
+
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Fem Element 3D co-rotational based on a tetrahedron volume discretization
+/// \note This class derives from the linear version of the FEM 3D element tetrahedron, adding a rigid frame
+/// \note attached to the tetrahedron element to follow its rigid motion. Therefore it computes deformation in the
+/// \note local coordinate system of the tetrahedron. This class is based on 2 papers:
+/// \note "Interactive Virtual Materials", Muller, Gross. Graphics Interface 2004
+/// \note "Exact Corotational Linear FEM Stiffness Matrix", Jernej Barbic. Technical Report USC 2012.
+/// \note Only the force and stiffness calculation are different, involving some changes as well in addMatVec
+/// \note (it uses the updated stiffness matrix).
+/// \note The update method takes care of extracting the rigid motion of the element.
+/// \note This element is updating its stiffness matrix at each new time step, which means that it cannot
+/// \note be used with any OdeSolverLinearXXX, it needs an ode solver that recomputes the data at each iteration.
+class Fem3DElementCorotationalTetrahedron : public Fem3DElementTetrahedron
+{
+public:
+ /// Constructor
+ /// \param nodeIds An array of 4 node (A, B, C, D) ids defining this tetrahedron element in a overall mesh
+ /// \note It is required that the triangle ABC is CCW looking from D (i.e. dot(cross(AB, AC), AD) > 0)
+ /// \note This is required from the signed volume calculation method getVolume()
+ /// \note A warning will be logged when the initialize function is called if this condition is not met, but the
+ /// simulation will keep running. Behavior will be undefined because of possible negative volume terms.
+ explicit Fem3DElementCorotationalTetrahedron(std::array<size_t, 4> nodeIds);
+
+ virtual void initialize(const SurgSim::Math::OdeState& state) override;
+
+ virtual void addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F, double scale = 1.0) override;
+
+ virtual void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale = 1.0) override;
+
+ virtual void addMatVec(const SurgSim::Math::OdeState& state,
+ double alphaM, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& vector, SurgSim::Math::Vector* result) override;
+
+ /// Update the element co-rotational frame. Updating as well the stiffness matrix.
+ /// \param state The state from which the element rigid transformation needs to be computed
+ /// \return True if the update was successful, false otherwise, in which case the representation should be
+ /// deactivated (invalid data).
+ virtual bool update(const SurgSim::Math::OdeState& state) override;
+
+protected:
+ /// The element rigid rotation
+ SurgSim::Math::Matrix33d m_rotation;
+
+ /// The co-rotational stiffness matrix
+ Eigen::Matrix<double, 12, 12> m_corotationalStiffnessMatrix;
+
+ /// The constant inverse matrix of the undeformed tetrahedron homogeneous 4 points coordinates.
+ /// This is useful to compute the deformation gradient from which the element rotation is extracted.
+ SurgSim::Math::Matrix44d m_Vinverse;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM3DELEMENTCOROTATIONALTETRAHEDRON_H
diff --git a/SurgSim/Physics/Fem3DElementCube.cpp b/SurgSim/Physics/Fem3DElementCube.cpp
new file mode 100644
index 0000000..f12dd2f
--- /dev/null
+++ b/SurgSim/Physics/Fem3DElementCube.cpp
@@ -0,0 +1,469 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+
+using SurgSim::Math::addSubMatrix;
+using SurgSim::Math::addSubVector;
+using SurgSim::Math::getSubVector;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem3DElementCube::Fem3DElementCube(std::array<size_t, 8> nodeIds)
+{
+ // Set the number of dof per node (3 in this case)
+ setNumDofPerNode(3);
+
+ // Set the shape functions coefficients
+ // Ni(epsilon, eta, mu) = (1 + epsilon * sign(epsilon_i))(1 + eta * sign(eta_i))(1 + mu * sign(mu_i))/8
+ std::array<double, 8> tmpEpsilon = {{-1.0, +1.0, +1.0, -1.0, -1.0, +1.0, +1.0, -1.0}};
+ std::array<double, 8> tmpEta = {{-1.0, -1.0, +1.0, +1.0, -1.0, -1.0, +1.0, +1.0}};
+ std::array<double, 8> tmpMu = {{-1.0, -1.0, -1.0, -1.0, +1.0, +1.0, +1.0, +1.0}};
+ m_shapeFunctionsEpsilonSign = tmpEpsilon;
+ m_shapeFunctionsEtaSign = tmpEta;
+ m_shapeFunctionsMuSign = tmpMu;
+
+ m_nodeIds.assign( nodeIds.cbegin(), nodeIds.cend());
+}
+
+void Fem3DElementCube::initialize(const SurgSim::Math::OdeState& state)
+{
+ // Test the validity of the physical parameters
+ FemElement::initialize(state);
+
+ // Store the 8 nodeIds in order
+ for (auto nodeId = m_nodeIds.cbegin(); nodeId != m_nodeIds.cend(); ++nodeId)
+ {
+ SURGSIM_ASSERT(*nodeId >= 0 && *nodeId < state.getNumNodes()) <<
+ "Invalid nodeId " << *nodeId << " expected in range [0.." << state.getNumNodes()-1 << "]";
+ }
+
+ // Store the rest state for this cube in m_elementRestPosition
+ getSubVector(state.getPositions(), m_nodeIds, 3, &m_elementRestPosition);
+
+ // Compute the cube rest volume
+ m_restVolume = getVolume(state);
+
+ // Pre-compute the mass and stiffness matrix
+ computeMass(state, &m_mass);
+ computeStiffness(state, &m_strain, &m_stress, &m_stiffness);
+}
+
+void Fem3DElementCube::addForce(const SurgSim::Math::OdeState& state,
+ const Eigen::Matrix<double, 24, 24>& k, SurgSim::Math::Vector* F, double scale)
+{
+ Eigen::Matrix<double, 24, 1> x, f;
+
+ // K.U = Fext
+ // K.(x - x0) = Fext
+ // 0 = Fext + Fint with Fint = -K.(x - x0)
+ getSubVector(state.getPositions(), m_nodeIds, 3, &x);
+ f = (- scale) * k * (x - m_elementRestPosition);
+ addSubVector(f, m_nodeIds, 3, F);
+}
+
+void Fem3DElementCube::addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F, double scale)
+{
+ addForce(state, m_stiffness, F, scale);
+}
+
+void Fem3DElementCube::computeMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 24, 24>* M)
+{
+ using SurgSim::Math::gaussQuadrature2Points;
+
+ // From the internal documentation on cube mass matrix calculation, we have:
+ // M = rho * integration{over volume} {phi^T.phi} dV
+ // with phi = (N0 0 0 N1 0 0...)
+ // ( 0 N0 0 0 N1 0...)
+ // ( 0 0 N0 0 0 N1...)
+ // a 3x24 matrix filled with shape functions evaluation at a given point.
+ // Using the Gauss-Legendre quadrature to evaluate this integral, we have:
+ // M = sum{i=1..2} sum{j=1..2} sum{k=1..2}(w_i * w_j * _w_k * det(J) * rho * phi^T.phi)
+
+ // Zero out the mass matrix
+ M->setZero();
+
+ // Build up the mass matrix using a 2-points Gauss-Legendre quadrature
+ for (int i = 0; i < 2; ++i)
+ {
+ for (int j = 0; j < 2; ++j)
+ {
+ for (int k = 0; k < 2; ++k)
+ {
+ addMassMatrixAtPoint(state,
+ gaussQuadrature2Points[i], gaussQuadrature2Points[j], gaussQuadrature2Points[k], M);
+ }
+ }
+ }
+}
+
+void Fem3DElementCube::addMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* M, double scale)
+{
+ addSubMatrix(m_mass * scale, m_nodeIds, 3, M);
+}
+
+void Fem3DElementCube::addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D, double scale)
+{
+}
+
+void Fem3DElementCube::computeStiffness(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 6, 24>* strain,
+ Eigen::Matrix<double, 6, 24>* stress,
+ Eigen::Matrix<double, 24, 24>* stiffness)
+{
+ using SurgSim::Math::gaussQuadrature2Points;
+
+ // Zero out the stiffness matrix
+ stiffness->setZero();
+
+ // Build up the stiffness matrix using a 2-points Gauss-Legendre quadrature
+ for (int i = 0; i < 2; ++i)
+ {
+ for (int j = 0; j < 2; ++j)
+ {
+ for (int k = 0; k < 2; ++k)
+ {
+ addStrainStressStiffnessAtPoint(state,
+ gaussQuadrature2Points[i], gaussQuadrature2Points[j], gaussQuadrature2Points[k],
+ strain, stress, stiffness);
+ }
+ }
+ }
+}
+
+void Fem3DElementCube::evaluateJ(const SurgSim::Math::OdeState& state, double epsilon, double eta, double mu,
+ SurgSim::Math::Matrix33d *J,
+ SurgSim::Math::Matrix33d *Jinv,
+ double *detJ) const
+{
+ using SurgSim::Framework::Logger;
+
+ SURGSIM_ASSERT(J != nullptr) << "Trying to evaluate J with a nullptr for matrix J";
+
+ Vector3d p[8];
+ for (size_t index = 0; index < 8; index++)
+ {
+ p[index] = state.getPosition(m_nodeIds[index]);
+ }
+
+ // Zero out the matrix J
+ J->setZero();
+
+ // Compute J = d(x,y,z)/d(epsilon,eta,mu)
+ // Note that (x,y,z) = for(i in {0..7}){ (x,y,z) += (xi,yi,zi).Ni(epsilon,eta,mu)}
+ for (size_t index = 0; index < 8; ++index)
+ {
+ for(size_t axis = 0; axis < 3; ++axis)
+ {
+ (*J)(0, axis) += p[index][axis] * dShapeFunctiondepsilon(index, epsilon, eta, mu);
+ (*J)(1, axis) += p[index][axis] * dShapeFunctiondeta (index, epsilon, eta, mu);
+ (*J)(2, axis) += p[index][axis] * dShapeFunctiondmu (index, epsilon, eta, mu);
+ }
+ }
+
+ if (Jinv != nullptr && detJ != nullptr)
+ {
+ bool invertible;
+ J->computeInverseAndDetWithCheck(*Jinv, *detJ, invertible);
+
+ SURGSIM_ASSERT(invertible) <<
+ "Found a non invertible matrix J\n" << *J << "\ndet(J)=" << *detJ <<
+ ") while computing Fem3DElementCube stiffness matrix\n";
+ SURGSIM_LOG_IF(*detJ <= 1e-8 && *detJ >= -1e-8, Logger::getLogger("Physics"), WARNING) <<
+ "Found an invalid matrix J\n" << *J << "\ninvertible, but det(J)=" << *detJ <<
+ ") while computing Fem3DElementCube stiffness matrix\n";
+ }
+}
+
+void Fem3DElementCube::evaluateStrainDisplacement(double epsilon, double eta, double mu,
+ const SurgSim::Math::Matrix33d& Jinv,
+ Eigen::Matrix<double, 6, 24> *B) const
+{
+ SURGSIM_ASSERT(B != nullptr) << "Trying to evaluate the strain-displacmenet with a nullptr";
+
+ // Zero out the strain-displacement
+ B->setZero();
+
+ // Set non-zero entries of the strain-displacement
+ for (size_t index = 0; index < 8; ++index)
+ {
+ // Compute dNi/d(x,y,z) = dNi/d(epsilon,eta,mu) d(epsilon,eta,mu)/d(x,y,z)
+ // = J^{-1}.dNi/d(epsilon,eta,mu)
+ Vector3d dNidEpsilonEtaMu(
+ dShapeFunctiondepsilon(index, epsilon, eta, mu),
+ dShapeFunctiondeta(index, epsilon, eta, mu),
+ dShapeFunctiondmu(index, epsilon, eta, mu)
+ );
+ Vector3d dNidxyz = Jinv * dNidEpsilonEtaMu;
+
+ // B = (dNi/dx 0 0)
+ // ( 0 dNi/dy 0)
+ // ( 0 0 dNi/dz)
+ // (dNi/dy dNi/dx 0)
+ // ( 0 dNi/dz dNi/dy)
+ // (dNi/dz 0 dNi/dx)
+ // c.f. http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch11.d/AFEM.Ch11.pdf
+ (*B)(0, getNumDofPerNode() * index ) = dNidxyz[0];
+ (*B)(1, getNumDofPerNode() * index + 1) = dNidxyz[1];
+ (*B)(2, getNumDofPerNode() * index + 2) = dNidxyz[2];
+ (*B)(3, getNumDofPerNode() * index ) = dNidxyz[1];
+ (*B)(3, getNumDofPerNode() * index + 1) = dNidxyz[0];
+ (*B)(4, getNumDofPerNode() * index + 1) = dNidxyz[2];
+ (*B)(4, getNumDofPerNode() * index + 2) = dNidxyz[1];
+ (*B)(5, getNumDofPerNode() * index ) = dNidxyz[2];
+ (*B)(5, getNumDofPerNode() * index + 2) = dNidxyz[0];
+ }
+}
+
+void Fem3DElementCube::buildConstitutiveMaterialMatrix(
+ Eigen::Matrix<double, 6, 6>* constitutiveMatrix)
+{
+ // Compute the elasticity material matrix
+ // which is commonly based on the Lame coefficients (1st = lambda, 2nd = mu = shear modulus):
+ double lambda = m_E * m_nu / ((1.0 + m_nu) * (1.0 - 2.0 * m_nu));
+ double mu = m_E / (2.0 * (1 + m_nu));
+ constitutiveMatrix->setZero();
+ (*constitutiveMatrix)(0, 0) = (*constitutiveMatrix)(1, 1) = (*constitutiveMatrix)(2, 2) = 2.0 * mu + lambda;
+ (*constitutiveMatrix)(0, 1) = (*constitutiveMatrix)(0, 2) = (*constitutiveMatrix)(1, 0) = lambda;
+ (*constitutiveMatrix)(1, 2) = (*constitutiveMatrix)(2, 0) = (*constitutiveMatrix)(2, 1) = lambda;
+ (*constitutiveMatrix)(3, 3) = (*constitutiveMatrix)(4, 4) = (*constitutiveMatrix)(5, 5) = mu;
+}
+
+void Fem3DElementCube::addStrainStressStiffnessAtPoint(const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::gaussQuadraturePoint& epsilon,
+ const SurgSim::Math::gaussQuadraturePoint& eta,
+ const SurgSim::Math::gaussQuadraturePoint& mu,
+ Eigen::Matrix<double, 6, 24>* strain,
+ Eigen::Matrix<double, 6, 24>* stress,
+ Eigen::Matrix<double, 24, 24>* k)
+{
+ SurgSim::Math::Matrix33d J, Jinv;
+ double detJ;
+ Eigen::Matrix<double, 6, 24> B;
+
+ evaluateJ(state, epsilon.point, eta.point, mu.point, &J, &Jinv, &detJ);
+
+ evaluateStrainDisplacement(epsilon.point, eta.point, mu.point, Jinv, &B);
+
+ buildConstitutiveMaterialMatrix(&m_constitutiveMaterial);
+
+ *strain += (epsilon.weight * eta.weight * mu.weight * detJ) * B;
+ *stress += (epsilon.weight * eta.weight * mu.weight * detJ) * m_constitutiveMaterial * B;
+ *k += (epsilon.weight * eta.weight * mu.weight * detJ) * B.transpose() * m_constitutiveMaterial * B;
+}
+
+void Fem3DElementCube::addMassMatrixAtPoint(const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::gaussQuadraturePoint& epsilon,
+ const SurgSim::Math::gaussQuadraturePoint& eta,
+ const SurgSim::Math::gaussQuadraturePoint& mu,
+ Eigen::Matrix<double, 24, 24>* m)
+{
+ // This helper method hels to compute:
+ // M = rho * integration{over volume} {phi^T.phi} dV
+ // = sum{i=1..2} sum{j=1..2} sum{k=1..2} (w_i * w_j * w_k * det(J) * rho * phi^T.phi)
+ // with phi = (N0 0 0 N1 0 0...) a 3x24 matrix filled with shape functions
+ // ( 0 N0 0 0 N1 0...) evaluation at a given point.
+ // ( 0 0 N0 0 0 N1...)
+ // by computing the term inside the sums: (w_i * w_j * w_k * det(J) * rho * phi^T.phi)
+ SurgSim::Math::Matrix33d J, Jinv;
+ SurgSim::Math::Matrix phi(3, 24);
+ double detJ;
+
+ evaluateJ(state, epsilon.point, eta.point, mu.point, &J, &Jinv, &detJ);
+
+ phi.setZero();
+ for (size_t index = 0; index < 8; ++index)
+ {
+ double weightPerIndex = shapeFunction(index, epsilon.point, eta.point, mu.point);
+ phi(0, getNumDofPerNode() * index ) += weightPerIndex;
+ phi(1, getNumDofPerNode() * index + 1) += weightPerIndex;
+ phi(2, getNumDofPerNode() * index + 2) += weightPerIndex;
+ }
+
+ *m += (epsilon.weight * eta.weight * mu.weight * detJ * m_rho) * phi.transpose() * phi;
+}
+
+void Fem3DElementCube::addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K, double scale)
+{
+ addSubMatrix(m_stiffness * scale, getNodeIds(), 3, K);
+}
+
+void Fem3DElementCube::addFMDK(const SurgSim::Math::OdeState& state,
+ SurgSim::Math::Vector* F,
+ SurgSim::Math::Matrix* M,
+ SurgSim::Math::Matrix* D,
+ SurgSim::Math::Matrix* K)
+{
+ // Assemble the mass matrix
+ addMass(state, M);
+
+ // No damping matrix as we are using linear elasticity (not visco-elasticity)
+
+ // Assemble the stiffness matrix
+ addStiffness(state, K);
+
+ // Assemble the force vector
+ addForce(state, F);
+}
+
+void Fem3DElementCube::addMatVec(const SurgSim::Math::OdeState& state,
+ double alphaM, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F)
+{
+ using SurgSim::Math::addSubVector;
+ using SurgSim::Math::getSubVector;
+
+ if (alphaM == 0.0 && alphaK == 0.0)
+ {
+ return;
+ }
+
+ Eigen::Matrix<double, 24, 1> xElement, fElement;
+ getSubVector(x, m_nodeIds, 3, &xElement);
+
+ // Adds the mass contribution
+ if (alphaM)
+ {
+ fElement = alphaM * (m_mass * xElement);
+ addSubVector(fElement, m_nodeIds, 3, F);
+ }
+
+ // Adds the damping contribution (No damping)
+
+ // Adds the stiffness contribution
+ if (alphaK)
+ {
+ fElement = alphaK * (m_stiffness * xElement);
+ addSubVector(fElement, m_nodeIds, 3, F);
+ }
+}
+
+double Fem3DElementCube::getVolume(const SurgSim::Math::OdeState& state) const
+{
+ using SurgSim::Math::gaussQuadrature2Points;
+
+ double v = 0.0;
+
+ // Compute the volume:
+ // V = integration{over volume} dV
+ // Using a 2-points Gauss-Legendre quadrature:
+ // V = for{i in {0..1}} for{j in {0..1}} for{k in {0..1}}
+ // V += weightEpsilon[i] * weightEta[j] * weightMu[k] * det(J(epsilon[i], eta[j], mu[k]))
+ for (int i = 0; i < 2; ++i)
+ {
+ const double &epsilon = gaussQuadrature2Points[i].point;
+ const double &weightEpsilon = gaussQuadrature2Points[i].weight;
+
+ for (int j = 0; j < 2; ++j)
+ {
+ const double &eta= gaussQuadrature2Points[j].point;
+ const double &weightEta = gaussQuadrature2Points[j].weight;
+
+ for (int k = 0; k < 2; ++k)
+ {
+ const double &mu= gaussQuadrature2Points[k].point;
+ const double &weightMu = gaussQuadrature2Points[k].weight;
+
+ SurgSim::Math::Matrix33d J, Jinv;
+ double detJ;
+
+ evaluateJ(state, epsilon, eta, mu, &J, &Jinv, &detJ);
+
+ v += weightEpsilon * weightEta * weightMu * detJ;
+ }
+ }
+ }
+
+ SURGSIM_ASSERT(v > 1e-12) << "Fem3DElementCube ill-defined, its volume is " << v << std::endl <<
+ "Please make sure the element is not degenerate and " <<
+ "check the node ordering of your element formed by node ids " <<
+ m_nodeIds[0] << " " << m_nodeIds[1] << " " << m_nodeIds[2] << " " << m_nodeIds[3] << " " <<
+ m_nodeIds[4] << " " << m_nodeIds[5] << " " << m_nodeIds[6] << " " << m_nodeIds[7] << std::endl;
+
+ return v;
+}
+
+double Fem3DElementCube::shapeFunction(size_t i, double epsilon, double eta, double mu) const
+{
+ return 1.0 / 8.0 *
+ (1 + epsilon * m_shapeFunctionsEpsilonSign[i]) *
+ (1 + eta * m_shapeFunctionsEtaSign[i]) *
+ (1 + mu * m_shapeFunctionsMuSign[i]);
+}
+
+double Fem3DElementCube::dShapeFunctiondepsilon(size_t i, double epsilon, double eta, double mu) const
+{
+ return 1.0 / 8.0 *
+ m_shapeFunctionsEpsilonSign[i] *
+ (1 + eta * m_shapeFunctionsEtaSign[i]) *
+ (1 + mu * m_shapeFunctionsMuSign[i]);
+}
+
+double Fem3DElementCube::dShapeFunctiondeta(size_t i, double epsilon, double eta, double mu) const
+{
+ return 1.0 / 8.0 *
+ (1 + epsilon * m_shapeFunctionsEpsilonSign[i]) *
+ m_shapeFunctionsEtaSign[i] *
+ (1 + mu * m_shapeFunctionsMuSign[i]);
+}
+
+double Fem3DElementCube::dShapeFunctiondmu(size_t i, double epsilon, double eta, double mu) const
+{
+ return 1.0 / 8.0 *
+ (1 + epsilon * m_shapeFunctionsEpsilonSign[i]) *
+ (1 + eta * m_shapeFunctionsEtaSign[i]) *
+ m_shapeFunctionsMuSign[i];
+}
+
+SurgSim::Math::Vector Fem3DElementCube::computeCartesianCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const
+{
+ SURGSIM_ASSERT(isValidCoordinate(naturalCoordinate))
+ << "naturalCoordinate must be normalized and length 8 within [0 1].";
+
+ Vector3d cartesianCoordinate(0.0, 0.0, 0.0);
+
+ const Vector& positions = state.getPositions();
+
+ for (int i = 0; i < 8; i++)
+ {
+ cartesianCoordinate += naturalCoordinate(i) * getSubVector(positions, m_nodeIds[i], 3);
+ }
+
+ return cartesianCoordinate;
+}
+
+SurgSim::Math::Vector Fem3DElementCube::computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const
+{
+ SURGSIM_FAILURE() << "Function " << __FUNCTION__ << " not yet implemented.";
+ return SurgSim::Math::Vector3d::Zero();
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem3DElementCube.h b/SurgSim/Physics/Fem3DElementCube.h
new file mode 100644
index 0000000..6a0e8bf
--- /dev/null
+++ b/SurgSim/Physics/Fem3DElementCube.h
@@ -0,0 +1,308 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM3DELEMENTCUBE_H
+#define SURGSIM_PHYSICS_FEM3DELEMENTCUBE_H
+
+#include <array>
+
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Math/GaussLegendreQuadrature.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Class for Fem Element 3D based on a cube volume discretization
+/// \note The stiffness property of the cube is derived from
+/// \note http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch11.d/AFEM.Ch11.pdf
+/// \note The mass property of the cube is derived from the kinetic energy computed on the cube's volume
+/// \note (c.f. internal documentation on cube mass matrix computation for details).
+/// \note Integral terms over the volume are evaluated using the Gauss-Legendre 2-points quadrature.
+/// \note http://en.wikipedia.org/wiki/Gaussian_quadrature
+/// \note Note that this technique is accurate for any polynomial evaluation up to degree 3.
+/// \note In our case, the shape functions \f$N_i\f$ are linear (of degree 1). So for exmaple,
+/// \note in the mass matrix we have integral terms like \f$\int_V N_i.N_j dV\f$, which are of degree 2.
+class Fem3DElementCube : public FemElement
+{
+public:
+ /// Constructor
+ /// \param nodeIds An array of 8 node ids defining this cube element in an overall mesh
+ /// \note It is required that getVolume() is positive, to do so, it needs (looking at the cube from
+ /// \note the exterior, face normal 'n' pointing outward):
+ /// \note the 1st 4 nodeIds (ABCD) should define any face CW i.e. (AB^AC or AB^AD or AC^AD).n < 0
+ /// \note the last 4 nodeIds (EFGH) should define the opposite face CCW i.e. (EF^EG or EF^EH or EG^EH).n > 0
+ /// \note A warning will be logged when the initialize function is called if this condition is not met, but the
+ /// \note simulation will keep running. Behavior will be undefined because of possible negative volume terms.
+ explicit Fem3DElementCube(std::array<size_t, 8> nodeIds);
+
+ /// Initializes the element once everything has been set
+ /// \param state The state to initialize the FemElement with
+ /// \note We use the theory of linear elasticity, so this method precomputes the stiffness and mass matrices
+ /// \note The 8 node ids must be valid in the given state, if they aren't an ASSERT will be raised
+ /// \note The 8 node ids must define a cube with positive volume, if they don't an ASSERT will be raised
+ /// \note In order to do so (looking at the cube from the exterior, face normal 'n' pointing outward),
+ /// \note the 1st 4 nodeIds (ABCD) should define any face CW i.e. (AB^AC or AB^AD or AC^AD).n < 0
+ /// \note the last 4 nodeIds (EFGH) should define the opposite face CCW i.e. (EF^EG or EF^EH or EG^EH).n > 0
+ /// \note A warning will be logged in if this condition is not met, but the simulation will keep running.
+ /// \note Behavior will be undefined because of possible negative volume terms.
+ virtual void initialize(const SurgSim::Math::OdeState& state) override;
+
+ /// Gets the element volume based on the input state
+ /// \param state The state to compute the volume with
+ virtual double getVolume(const SurgSim::Math::OdeState& state) const override;
+
+ /// Adds the element force (computed for a given state) to a complete system force vector F (assembly)
+ /// \param state The state to compute the force with
+ /// \param[in,out] F The complete system force vector to add the element force into
+ /// \param scale A factor to scale the added force with
+ /// \note The element force is of size (getNumDofPerNode() x getNumNodes())
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ virtual void addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ double scale = 1.0) override;
+
+ /// Adds the element mass matrix M (computed for a given state) to a complete system mass matrix M (assembly)
+ /// \param state The state to compute the mass matrix with
+ /// \param[in,out] M The complete system mass matrix to add the element mass-matrix into
+ /// \param scale A factor to scale the added mass matrix with
+ /// \note The element mass matrix is square of size getNumDofPerNode() x getNumNodes()
+ /// \note This method supposes that the incoming state contains information with the same number of
+ /// \note dof per node as getNumDofPerNode()
+ virtual void addMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* M,
+ double scale = 1.0) override;
+
+ /// Adds the element damping matrix D (= -df/dv) (computed for a given state)
+ /// to a complete system damping matrix D (assembly)
+ /// \param state The state to compute the damping matrix with
+ /// \param[in,out] D The complete system damping matrix to add the element damping matrix into
+ /// \param scale A factor to scale the added damping matrix with
+ /// \note The element damping matrix is square of size getNumDofPerNode() x getNumNodes()
+ /// \note This method supposes that the incoming state contains information with the same number of
+ /// \note dof per node as getNumDofPerNode()
+ /// \note Fem3DElementCube uses linear elasticity (not visco-elasticity), so it does not give any damping.
+ virtual void addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D,
+ double scale = 1.0) override;
+
+ /// Adds the element stiffness matrix K (= -df/dx) (computed for a given state)
+ /// to a complete system stiffness matrix K (assembly)
+ /// \param state The state to compute the stiffness matrix with
+ /// \param[in,out] K The complete system stiffness matrix to add the element stiffness matrix into
+ /// \param scale A factor to scale the added stiffness matrix with
+ /// \note The element stiffness matrix is square of size getNumDofPerNode() x getNumNodes()
+ /// \note This method supposes that the incoming state contains information with the same number of
+ /// \note dof per node as getNumDofPerNode()
+ virtual void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale = 1.0) override;
+
+ /// Adds the element force vector, mass, stiffness and damping matrices (computed for a given state)
+ /// into a complete system data structure F, M, D, K (assembly)
+ /// \param state The state to compute everything with
+ /// \param[in,out] F The complete system force vector to add the element force into
+ /// \param[in,out] M The complete system mass matrix to add the element mass matrix into
+ /// \param[in,out] D The complete system damping matrix to add the element damping matrix into
+ /// \param[in,out] K The complete system stiffness matrix to add the element stiffness matrix into
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ virtual void addFMDK(const SurgSim::Math::OdeState& state,
+ SurgSim::Math::Vector* F,
+ SurgSim::Math::Matrix* M,
+ SurgSim::Math::Matrix* D,
+ SurgSim::Math::Matrix* K) override;
+
+ /// Adds the element matrix-vector contribution F += (alphaM.M + alphaD.D + alphaK.K).x (computed for a given state)
+ /// into a complete system data structure F (assembly)
+ /// \param state The state to compute everything with
+ /// \param alphaM The scaling factor for the mass contribution
+ /// \param alphaD The scaling factor for the damping contribution
+ /// \param alphaK The scaling factor for the stiffness contribution
+ /// \param x A complete system vector to use as the vector in the matrix-vector multiplication
+ /// \param[in,out] F The complete system force vector to add the element matrix-vector contribution into
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ virtual void addMatVec(const SurgSim::Math::OdeState& state,
+ double alphaM, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F) override;
+
+ virtual SurgSim::Math::Vector computeCartesianCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const override;
+
+ virtual SurgSim::Math::Vector computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const override;
+
+protected:
+ /// Build the constitutive material 6x6 matrix
+ /// \param[out] constitutiveMatrix The 6x6 constitutive material matrix
+ void buildConstitutiveMaterialMatrix(Eigen::Matrix<double, 6, 6>* constitutiveMatrix);
+
+ /// Computes the cube stiffness matrix along with the strain and stress matrices
+ /// \param state The state to compute the stiffness matrix from
+ /// \param[out] strain, stress, k The strain, stress and stiffness matrices to store the result into
+ void computeStiffness(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 6, 24>* strain,
+ Eigen::Matrix<double, 6, 24>* stress,
+ Eigen::Matrix<double, 24, 24>* k);
+
+ /// Computes the cube mass matrix
+ /// \param state The state to compute the mass matrix from
+ /// \param[out] m The mass matrix to store the result into
+ void computeMass(const SurgSim::Math::OdeState& state, Eigen::Matrix<double, 24, 24>* m);
+
+ /// Adds the element force (computed for a given state) to a complete system force vector F (assembly)
+ /// This method relies on a given stiffness matrix and does not evaluate it from the state
+ /// \param state The state to compute the force with
+ /// \param k The given element stiffness matrix
+ /// \param[in,out] F The complete system force vector to add the element force into
+ /// \param scale A factor to scale the added force with
+ /// \note The element force is of size (getNumDofPerNode() x getNumNodes())
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ void addForce(const SurgSim::Math::OdeState& state, const Eigen::Matrix<double, 24, 24>& k,
+ SurgSim::Math::Vector* F, double scale = 1.0);
+
+ /// Helper method to evaluate strain-stress and stiffness integral terms with a discrete sum using
+ /// a Gauss quadrature rule
+ /// \param state The state to compute the evaluation with
+ /// \param epsilon, eta, mu The Gauss quadrature points to evaluate the data at
+ /// \param[out] strain, stress, k The matrices in which to add the evaluations
+ void addStrainStressStiffnessAtPoint(const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::gaussQuadraturePoint& epsilon,
+ const SurgSim::Math::gaussQuadraturePoint& eta,
+ const SurgSim::Math::gaussQuadraturePoint& mu,
+ Eigen::Matrix<double, 6, 24>* strain,
+ Eigen::Matrix<double, 6, 24>* stress,
+ Eigen::Matrix<double, 24, 24>* k);
+
+ /// Helper method to evaluate mass integral terms with a discrete sum using a Gauss quadrature rule
+ /// \param state The state to compute the evaluation with
+ /// \param epsilon, eta, mu The Gauss quadrature points to evaluate the data at
+ /// \param[out] m The matrix in which to add the evaluations
+ void addMassMatrixAtPoint(const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::gaussQuadraturePoint& epsilon,
+ const SurgSim::Math::gaussQuadraturePoint& eta,
+ const SurgSim::Math::gaussQuadraturePoint& mu,
+ Eigen::Matrix<double, 24, 24>* m);
+
+ /// Helper method to evaluate matrix J = d(x,y,z)/d(epsilon,eta,mu) at a given 3D parametric location
+ /// J expresses the 3D space coordinate frames variation w.r.t. parametric coordinates
+ /// \param state The state to compute the evaluation with
+ /// \param epsilon, eta, mu The 3D parametric coordinates to evaluate the data at (within \f$[-1 +1]\f$)
+ /// \param[out] J, Jinv, detJ The J matrix with its inverse and determinant evaluated at (epsilon, eta, mu)
+ void evaluateJ(const SurgSim::Math::OdeState& state, double epsilon, double eta, double mu,
+ SurgSim::Math::Matrix33d *J,
+ SurgSim::Math::Matrix33d *Jinv,
+ double *detJ) const;
+
+ /// Helper method to evaluate the strain-displacement matrix at a given 3D parametric location
+ /// c.f. http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch11.d/AFEM.Ch11.pdf for more details
+ /// \param epsilon, eta, mu The 3D parametric coordinates to evaluate the data at (within \f$[-1 +1]\f$)
+ /// \param Jinv The inverse of matrix J (3D global coords to 3D parametric coords)
+ /// \param[out] B The strain-displacement matrix
+ void evaluateStrainDisplacement(double epsilon, double eta, double mu, const SurgSim::Math::Matrix33d& Jinv,
+ Eigen::Matrix<double, 6, 24> *B) const;
+
+ /// Cube rest volume
+ double m_restVolume;
+
+ ///@{
+ /// Shape functions parameters
+ /// \f$N_i(\epsilon, \eta, \mu) = (1\pm\epsilon)(1\pm\eta)(1\pm\mu)/8
+ /// = (1+\epsilon.sign(\epsilon_i))(1+\eta.sign(\eta_i))(1+\mu.sign(\mu_i))/8
+ /// \textbf{ with } (\epsilon, \eta, \mu) \in [-1 +1]^3\f$
+ ///
+ /// We choose to only store the sign of epsilon, eta and mu for each shape functions.
+ /// \sa N
+ std::array<double, 8> m_shapeFunctionsEpsilonSign;
+ std::array<double, 8> m_shapeFunctionsEtaSign;
+ std::array<double, 8> m_shapeFunctionsMuSign;
+ ///@}
+
+ /// Shape functions \f$N_i(\epsilon, \eta, \mu) = (1\pm\epsilon)(1\pm\eta)(1\pm\mu)/8\f$
+ ///
+ /*! \f$
+ * \begin{array}{r | r r r}
+ * i & sign(\epsilon) & sign(\eta) & sign(\mu) \\
+ * \hline
+ * 0 & -1 & -1 & -1 \\
+ * 1 & +1 & -1 & -1 \\
+ * 2 & +1 & +1 & -1 \\
+ * 3 & -1 & +1 & -1 \\
+ * 4 & -1 & -1 & +1 \\
+ * 5 & +1 & -1 & +1 \\
+ * 6 & +1 & +1 & +1 \\
+ * 7 & -1 & +1 & +1
+ * \end{array}
+ * \f$
+ */
+ /// \param i The node id (w.r.t. local element) to evaluate at
+ /// \param epsilon, eta, mu The 3D parametric coordinates each within \f$[-1 +1]\f$
+ /// \return Ni(epsilon, eta, mu)
+ /// \note A check is performed on the nodeId i but not on the 3D parametric coordinates range
+ /// \sa m_shapeFunctionsEpsilonSign, m_shapeFunctionsEtaSign, m_shapeFunctionsMuSign
+ /// \sa dNdepsilon, dNdeta, dNdmu
+ double shapeFunction(size_t i, double epsilon, double eta, double mu) const;
+
+ /// Shape functions derivative \f$dN_i/d\epsilon(\epsilon, \eta, \mu) = \pm(1\pm\eta)(1\pm\mu)/8\f$
+ /// \param i The node id (w.r.t. local element) to evaluate at
+ /// \param epsilon, eta, mu The 3D parametric coordinates each within \f$[-1 +1]\f$
+ /// \return dNi/depsilon(epsilon, eta, mu)
+ /// \note A check is performed on the nodeId i but not on the 3D parametric coordinates range
+ /// \sa N
+ /// \sa m_shapeFunctionsEpsilonSign, m_shapeFunctionsEtaSign, m_shapeFunctionsMuSign
+ double dShapeFunctiondepsilon(size_t i, double epsilon, double eta, double mu) const;
+
+ /// Shape functions derivative \f$dN_i/d\eta(\epsilon, \eta, \mu) = \pm(1\pm\epsilon)(1\pm\mu)/8\f$
+ /// \param i The node id (w.r.t. local element) to evaluate at
+ /// \param epsilon, eta, mu The 3D parametric coordinates each within \f$[-1 +1]\f$
+ /// \return dNi/depsilon(epsilon, eta, mu)
+ /// \note A check is performed on the nodeId i but not on the 3D parametric coordinates range
+ /// \sa N
+ /// \sa m_shapeFunctionsEpsilonSign, m_shapeFunctionsEtaSign, m_shapeFunctionsMuSign
+ double dShapeFunctiondeta(size_t i, double epsilon, double eta, double mu) const;
+
+ /// Shape functions derivative \f$dN_i/d\mu(\epsilon, \eta, \mu) = \pm(1\pm\epsilon)(1\pm\eta)/8\f$
+ /// \param i The node id (w.r.t. local element) to evaluate at
+ /// \param epsilon, eta, mu The 3D parametric coordinates each within \f$[-1 +1]\f$
+ /// \return dNi/depsilon(epsilon, eta, mu)
+ /// \note A check is performed on the nodeId i but not on the 3D parametric coordinates range
+ /// \sa N
+ /// \sa m_shapeFunctionsEpsilonSign, m_shapeFunctionsEtaSign, m_shapeFunctionsMuSign
+ double dShapeFunctiondmu(size_t i, double epsilon, double eta, double mu) const;
+
+ /// The cube rest state (nodes ordered by m_nodeIds)
+ Eigen::Matrix<double, 24, 1> m_elementRestPosition;
+
+ /// Strain matrix (usually noted \f$\epsilon\f$)
+ Eigen::Matrix<double, 6, 24> m_strain;
+ /// Stress matrix (usually noted \f$\sigma\f$)
+ Eigen::Matrix<double, 6, 24> m_stress;
+ /// Constitutive material matrix (Hooke's law in this case) defines the relationship between stress and strain
+ Eigen::Matrix<double, 6, 6> m_constitutiveMaterial;
+
+ /// %Mass matrix (usually noted \f$M\f$)
+ Eigen::Matrix<double, 24, 24> m_mass;
+ /// Stiffness matrix (usually noted \f$K\f$)
+ Eigen::Matrix<double, 24, 24> m_stiffness;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM3DELEMENTCUBE_H
diff --git a/SurgSim/Physics/Fem3DElementTetrahedron.cpp b/SurgSim/Physics/Fem3DElementTetrahedron.cpp
new file mode 100644
index 0000000..c756674
--- /dev/null
+++ b/SurgSim/Physics/Fem3DElementTetrahedron.cpp
@@ -0,0 +1,447 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+
+using SurgSim::Math::getSubVector;
+using SurgSim::Math::getSubMatrix;
+using SurgSim::Math::addSubVector;
+using SurgSim::Math::addSubMatrix;
+
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+
+/// Computes the determinant of 3 vectors
+/// \param a, b, c The 3 vectors to compute the determinant from
+/// \return |a b c|, The determinant of the 3 vectors a, b and c
+double det(const Vector3d& a, const Vector3d& b, const Vector3d& c)
+{
+ return a[0]*b[1]*c[2] + a[2]*b[0]*c[1] + a[1]*b[2]*c[0] - a[2]*b[1]*c[0] - a[1]*b[0]*c[2] - a[0]*b[2]*c[1];
+}
+
+};
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem3DElementTetrahedron::Fem3DElementTetrahedron(std::array<size_t, 4> nodeIds)
+{
+ setNumDofPerNode(3); // 3 dof per node (x, y, z)
+
+ m_nodeIds.assign(std::begin(nodeIds), std::end(nodeIds));
+}
+
+void Fem3DElementTetrahedron::initialize(const SurgSim::Math::OdeState& state)
+{
+ // Test the validity of the physical parameters
+ FemElement::initialize(state);
+
+ for (auto nodeId = m_nodeIds.cbegin(); nodeId != m_nodeIds.cend(); nodeId++)
+ {
+ SURGSIM_ASSERT(*nodeId >= 0 && *nodeId < state.getNumNodes())
+ << "Invalid nodeId " << *nodeId << " expected in range [0.." << state.getNumNodes() - 1 << "]";
+ }
+
+ // Compute the fem tetrahedron shape functions Ni(x,y,z) = 1/6V ( ai + x.bi + y.ci + z.di )
+ computeShapeFunctions(state, &m_restVolume, &m_ai, &m_bi, &m_ci, &m_di);
+
+ // Store the rest state for this tetrahedron in m_x0
+ getSubVector(state.getPositions(), m_nodeIds, 3, &m_x0);
+
+ // Verify the Counter clock-wise condition
+ auto A = getSubVector(m_x0, 0, 3);
+ auto B = getSubVector(m_x0, 1, 3);
+ auto C = getSubVector(m_x0, 2, 3);
+ auto D = getSubVector(m_x0, 3, 3);
+ SurgSim::Math::Vector3d AB = B - A;
+ SurgSim::Math::Vector3d AC = C - A;
+ SurgSim::Math::Vector3d AD = D - A;
+ SURGSIM_LOG_IF(AB.cross(AC).dot(AD) < 0, SurgSim::Framework::Logger::getDefaultLogger(), WARNING)
+ << "Tetrahedron ill-defined (ABC defined counter clock viewed from D) with node ids[" << m_nodeIds[0] << ", "
+ << m_nodeIds[1] << ", " << m_nodeIds[2] << ", " << m_nodeIds[3] << "]";
+
+ // Pre-compute the mass and stiffness matrix
+ computeMass(state, &m_M);
+ computeStiffness(state, &m_K);
+}
+
+void Fem3DElementTetrahedron::addForce(const SurgSim::Math::OdeState& state,
+ const Eigen::Matrix<double, 12, 12>& k, SurgSim::Math::Vector* F, double scale)
+{
+ Eigen::Matrix<double, 12, 1> x, f;
+
+ // K.U = Fext
+ // K.(x - x0) = Fext
+ // 0 = Fext + Fint with Fint = -K.(x - x0)
+ getSubVector(state.getPositions(), m_nodeIds, 3, &x);
+ f = (- scale) * k * (x - m_x0);
+ addSubVector(f, m_nodeIds, 3, F);
+}
+
+void Fem3DElementTetrahedron::addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ double scale)
+{
+ addForce(state, m_K, F, scale);
+}
+
+void Fem3DElementTetrahedron::computeMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 12, 12>* M)
+{
+ // From Przemieniecki book
+ // -> section 11 "Inertia properties of structural elements
+ // -> subsection 8 "Solid Tetrahedron"
+ //
+ // On each axis (x, y, z), the mass matrix is
+ // m = rho * volume / 20 * (2 1 1 1)
+ // (1 2 1 1)
+ // (1 1 2 1)
+ // (1 1 1 2)
+ //
+ // x1 y1 z1 x2 y2 z2 x3 y3 z3 x4 y4 z4
+ // x1 ( 2 1 1 1 )
+ // y1 ( 2 1 1 1 )
+ // z1 ( 2 1 1 1 )
+ // x2 ( 1 2 1 1 )
+ // y2 ( 1 2 1 1 )
+ // M = z2 ( 1 2 1 1 ) * rho * volume / 20
+ // x3 ( 1 1 2 1 )
+ // y3 ( 1 1 2 1 )
+ // z3 ( 1 1 2 1 )
+ // x4 ( 1 1 1 2 )
+ // y4 ( 1 1 1 2 )
+ // z4 ( 1 1 1 2 )
+ double coef = getVolume(state) * m_rho / 20.0;
+ for (size_t rowNodeId = 0; rowNodeId < 4; rowNodeId++)
+ {
+ for (size_t colNodeId = 0; colNodeId < 4; colNodeId++)
+ {
+ auto Mii = getSubMatrix(*M, rowNodeId, colNodeId, 3, 3);
+
+ if (rowNodeId == colNodeId)
+ {
+ Mii = SurgSim::Math::Matrix33d::Identity() * (2.0 * coef);
+ }
+ else
+ {
+ Mii = SurgSim::Math::Matrix33d::Identity() * coef;
+ }
+ }
+ }
+}
+
+
+void Fem3DElementTetrahedron::addMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* M, double scale)
+{
+ addSubMatrix(m_M * scale, m_nodeIds, 3, M);
+}
+
+void Fem3DElementTetrahedron::addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D, double scale)
+{
+}
+
+void Fem3DElementTetrahedron::computeStiffness(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 12, 12>* k)
+{
+ m_Em.setZero();
+ m_strain.setZero();
+
+ // Compute the strain matrix
+ double coef = 1.0 / (6.0 * m_restVolume);
+ for(int i = 0; i < 4; i++)
+ {
+ m_strain(0, 3 * i ) = coef * m_bi[i];
+ m_strain(1, 3 * i + 1) = coef * m_ci[i];
+ m_strain(2, 3 * i + 2) = coef * m_di[i];
+ m_strain(3, 3 * i ) = coef * m_ci[i];
+ m_strain(3, 3 * i + 1) = coef * m_bi[i];
+ m_strain(4 ,3 * i + 1) = coef * m_di[i];
+ m_strain(4, 3 * i + 2) = coef * m_ci[i];
+ m_strain(5 ,3 * i ) = coef * m_di[i];
+ m_strain(5, 3 * i + 2) = coef * m_bi[i];
+ }
+
+ // Compute the elasticity material matrix
+ // which is commonly based on the Lame coefficients (1st = lambda, 2nd = mu = shear modulus):
+ double lambda = m_E * m_nu / ((1.0 + m_nu) * (1.0 - 2.0 * m_nu));
+ double mu = m_E / (2.0 * (1 + m_nu));
+ m_Em(0, 0) = m_Em(1, 1) = m_Em(2, 2) = 2.0 * mu + lambda;
+ m_Em(0, 1) = m_Em(0, 2) = m_Em(1, 0) = m_Em(1, 2) = m_Em(2, 0) = m_Em(2, 1) = lambda;
+ m_Em(3, 3) = m_Em(4, 4) = m_Em(5, 5) = mu;
+
+ // Compute the stress matrix
+ m_stress = m_Em * m_strain;
+
+ // Compute the stiffness matrix
+ *k = m_restVolume * m_strain.transpose() * m_stress;
+
+ // Ke is symmetric but numerical computation might introduce epsilon, we force the symmetry here
+ *k = ((*k) + (*k).transpose()) * 0.5;
+}
+
+void Fem3DElementTetrahedron::addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale)
+{
+ addSubMatrix(m_K * scale, getNodeIds(), 3, K);
+}
+
+void Fem3DElementTetrahedron::addFMDK(const SurgSim::Math::OdeState& state,
+ SurgSim::Math::Vector* F,
+ SurgSim::Math::Matrix* M,
+ SurgSim::Math::Matrix* D,
+ SurgSim::Math::Matrix* K)
+{
+ // Assemble the mass matrix
+ addMass(state, M);
+
+ // No damping matrix as we are using linear elasticity (not visco-elasticity)
+
+ // Assemble the stiffness matrix
+ addStiffness(state, K);
+
+ // Assemble the force vector
+ addForce(state, F);
+}
+
+void Fem3DElementTetrahedron::addMatVec(const SurgSim::Math::OdeState& state,
+ double alphaM, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F)
+{
+ using SurgSim::Math::addSubVector;
+ using SurgSim::Math::getSubVector;
+
+ if (alphaM == 0.0 && alphaK == 0.0)
+ {
+ return;
+ }
+
+ Eigen::Matrix<double, 12, 1> xLoc, resLoc;
+ getSubVector(x, m_nodeIds, 3, &xLoc);
+
+ // Adds the mass contribution
+ if (alphaM)
+ {
+ resLoc = alphaM * (m_M * xLoc);
+ addSubVector(resLoc, m_nodeIds, 3, F);
+ }
+
+ // Adds the damping contribution (No damping)
+
+ // Adds the stiffness contribution
+ if (alphaK)
+ {
+ resLoc = alphaK * (m_K * xLoc);
+ addSubVector(resLoc, m_nodeIds, 3, F);
+ }
+}
+
+double Fem3DElementTetrahedron::getVolume(const SurgSim::Math::OdeState& state) const
+{
+ /// Computes the tetrahedron volume 1/6 * | 1 p0x p0y p0z |
+ /// | 1 p1x p1y p1z |
+ /// | 1 p2x p2y p2z |
+ /// | 1 p3x p3y p3z |
+ const Vector& x = state.getPositions();
+ auto p0 = getSubVector(x, m_nodeIds[0], 3);
+ auto p1 = getSubVector(x, m_nodeIds[1], 3);
+ auto p2 = getSubVector(x, m_nodeIds[2], 3);
+ auto p3 = getSubVector(x, m_nodeIds[3], 3);
+
+ // fabs is necessary if we don't pay attention to the indexing !
+ // If the tetrahedron verify ABC counter clock wise viewed from D, this determinant is always positive = 6V
+ // i.e. dot( cross(AB, AC), AD ) > 0
+ // Otherwise, it can happen that this determinant is negative = -6V !!
+ return (det(p1, p2, p3) - det(p0, p2, p3) + det(p0, p1, p3) - det(p0, p1, p2)) / 6.0;
+}
+
+void Fem3DElementTetrahedron::computeShapeFunctions(const SurgSim::Math::OdeState& state,
+ double* volume,
+ std::array<double, 4>* ai,
+ std::array<double, 4>* bi,
+ std::array<double, 4>* ci,
+ std::array<double, 4>* di) const
+{
+ // The tetrahedron nodes 3D position {a,b,c,d}
+ Vector3d a = getSubVector(state.getPositions(), m_nodeIds[0], 3);
+ Vector3d b = getSubVector(state.getPositions(), m_nodeIds[1], 3);
+ Vector3d c = getSubVector(state.getPositions(), m_nodeIds[2], 3);
+ Vector3d d = getSubVector(state.getPositions(), m_nodeIds[3], 3);
+
+ *volume = getVolume(state);
+
+ // See http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf for more details.
+ // Relationship between the notations in this source code and the document mentioned above:
+ // a(x1 y1 z1) b(x2 y2 z2) c(x3 y3 z3) d(x4 y4 z4)
+
+ // Shape functions link the 3D space (x, y, z) to the natural parametrization (sigmai) of the shape.
+ // (i.e. sigmai are the barycentric coordinates for the respective 4 nodes of the tetrahedon)
+ // (1) ( 1 1 1 1) (sigma1)
+ // (x) = (x1 x2 x3 x4) (sigma2)
+ // (y) (y1 y2 y3 y4) (sigma3)
+ // (z) (z1 z2 z3 z4) (sigma4)
+ //
+ // The shape functions Ni(x, y, z) are given by the inverse relationship:
+ // (sigma1) ( 1 1 1 1)^-1 (1) (a[0] b[0] c[0] d[0]) (1) | 1 1 1 1|
+ // (sigma2) = (x1 x2 x3 x4) (x) = 1/6V (a[1] b[1] c[1] d[1]) (x) where |x1 x2 x3 x4| = 6V
+ // (sigma3) (y1 y2 y3 y4) (y) (a[2] b[2] c[2] d[2]) (y) |y1 y2 y3 y4|
+ // (sigma4) (z1 z2 z3 z4) (z) (a[3] b[3] c[3] d[3]) (z) |z1 z2 z3 z4|
+
+ // Computes the shape functions parameters m_ai (noted 6V0i in the document mentioned above, eq 9.12)
+ // m_ai[0] = 6V01 = 6V(origin,b,c,d) = x2(y3z4 - y4z3) + x3(y4z2 - y2z4) + x4(y2z3 - y3z2) = |b c d|
+ // m_ai[1] = 6V02 = 6V(origin,c,d,a) = x1(y4z3 - y3z4) + x3(y1z4 - y4z1) + x4(y3z1 - y1z3) = -|a c d|
+ // m_ai[2] = 6V03 = 6V(origin,d,a,b) = x1(y2z4 - y4z2) + x2(y4z1 - y1z4) + x4(y1z2 - y2z1) = |a b d|
+ // m_ai[3] = 6V04 = 6V(origin,a,b,c) = x1(y3z2 - y2z3) + x2(y1z3 - y3z1) + x3(y2z1 - y1z2) = -|a b c|
+ (*ai)[0] = det(b, c, d);
+ (*ai)[1] = -det(a, c, d);
+ (*ai)[2] = det(a, b, d);
+ (*ai)[3] = -det(a, b, c);
+
+ // Computes the shape function parameters m_bi (noted ai in the document mentioned above, eq 9.11)
+ // m_bi[0] = y42z32 - y32z42 = (y4-y2)(z3-z2) - (y3-y2)(z4-z2) = |1 y2 z2| = |1 by bz|
+ // -|1 y3 z3| -|1 cy cz|
+ // |1 y4 z4| |1 dy dz|
+ //
+ // m_bi[1] = y31z43 - y34z13 = (y3-y1)(z4-z3) - (y3-y4)(z1-z3) = |1 y1 z1| = |1 ay az|
+ // |1 y3 z3| |1 cy cz|
+ // |1 y4 z4| |1 dy dz|
+ //
+ // m_bi[2] = y24z14 - y14z24 = (y2-y4)(z1-z4) - (y1-y4)(z2-z4) = |1 y1 z1| = |1 ay az|
+ // -|1 y2 z2| -|1 by bz|
+ // |1 y4 z4| |1 dy dz|
+ //
+ // m_bi[3] = y13z21 - y12z31 = (y1-y3)(z2-z1) - (y1-y2)(z3-z1) = |1 y1 z1| = |1 ay az|
+ // |1 y2 z2| |1 by bz|
+ // |1 y3 z3| |1 cy cz|
+ {
+ Vector3d atilde(1, a[1], a[2]);
+ Vector3d btilde(1, b[1], b[2]);
+ Vector3d ctilde(1, c[1], c[2]);
+ Vector3d dtilde(1, d[1], d[2]);
+ (*bi)[0] = -det(btilde, ctilde, dtilde);
+ (*bi)[1] = det(atilde, ctilde, dtilde);
+ (*bi)[2] = -det(atilde, btilde, dtilde);
+ (*bi)[3] = det(atilde, btilde, ctilde);
+ }
+
+ // Computes the shape function parameters m_ci (noted bi in the document mentioned above, eq 9.11)
+ // m_ci[0] = x32z42 - x42z32 = (x3-x2)(z4-z2) - (x4-x2)(z3-z2) = |1 x2 z2| = |1 bx bz|
+ // |1 x3 z3| |1 cx cz|
+ // |1 x4 z4| |1 dx dz|
+ //
+ // m_ci[1] = x43z31 - x13z34 = (x4-x3)(z3-z1) - (x1-x3)(z3-z4) = |1 x1 z1| = |1 ax az|
+ // -|1 x3 z3| -|1 cx cz|
+ // |1 x4 z4| |1 dx dz|
+ //
+ // m_ci[2] = x14z24 - x24z14 = (x1-x4)(z2-z4) - (x2-x4)(z1-z4) = |1 x1 z1| = |1 ax az|
+ // |1 x2 z2| |1 bx bz|
+ // |1 x4 z4| |1 dx dz|
+ //
+ // m_ci[3] = x21z13 - x31z12 = (x2-x1)(z1-z3) - (x3-x1)(z1-z2) = |1 x1 z1| = |1 ax az|
+ // -|1 x2 z2| -|1 bx bz|
+ // |1 x3 z3| |1 cx cz|
+ {
+ Vector3d atilde(1, a[0], a[2]);
+ Vector3d btilde(1, b[0], b[2]);
+ Vector3d ctilde(1, c[0], c[2]);
+ Vector3d dtilde(1, d[0], d[2]);
+ (*ci)[0] = det(btilde, ctilde, dtilde);
+ (*ci)[1] = -det(atilde, ctilde, dtilde);
+ (*ci)[2] = det(atilde, btilde, dtilde);
+ (*ci)[3] = -det(atilde, btilde, ctilde);
+ }
+
+ // Computes the shape function parameters m_di (noted ci in the document mentioned above, eq 9.11)
+ // m_di[0] = x42y32 - x32y42 = (x4-x2)(y3-y2) - (x3-x2)(y4-y2) = |1 x2 y2| = |1 bx by|
+ // -|1 x3 y3| -|1 cx cy|
+ // |1 x4 y4| |1 dx dy|
+ //
+ // m_di[1] = x31y43 - x34y13 = (x3-x1)(y4-y3) - (x3-x4)(y1-y3) = |1 x1 y1| = |1 ax ay|
+ // |1 x3 y3| |1 cx cy|
+ // |1 x4 y4| |1 dx dy|
+ //
+ // m_di[2] = x24y14 - x14y24 = (x2-x4)(y1-y4) - (x1-x4)(y2-y4) = |1 x1 y1| = |1 ax ay|
+ // -|1 x2 y2| -|1 bx by|
+ // |1 x4 y4| |1 dx dy|
+ //
+ // m_di[3] = x13y21 - x12y31 = (x1-x3)(y2-y1) - (x1-x2)(y3-y1) = |1 x1 y1| = |1 ax ay|
+ // |1 x2 y2| |1 bx by|
+ // |1 x3 y3| |1 cx cy|
+ {
+ Vector3d atilde(1, a[0], a[1]);
+ Vector3d btilde(1, b[0], b[1]);
+ Vector3d ctilde(1, c[0], c[1]);
+ Vector3d dtilde(1, d[0], d[1]);
+ (*di)[0] = -det(btilde, ctilde, dtilde);
+ (*di)[1] = det(atilde, ctilde, dtilde);
+ (*di)[2] = -det(atilde, btilde, dtilde);
+ (*di)[3] = det(atilde, btilde, ctilde);
+ }
+}
+
+SurgSim::Math::Vector Fem3DElementTetrahedron::computeCartesianCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const
+{
+ SURGSIM_ASSERT(isValidCoordinate(naturalCoordinate))
+ << "naturalCoordinate must be normalized and length 4.";
+
+ const Vector& x = state.getPositions();
+ Vector3d p0 = getSubVector(x, m_nodeIds[0], 3);
+ Vector3d p1 = getSubVector(x, m_nodeIds[1], 3);
+ Vector3d p2 = getSubVector(x, m_nodeIds[2], 3);
+ Vector3d p3 = getSubVector(x, m_nodeIds[3], 3);
+
+ return naturalCoordinate(0) * p0
+ + naturalCoordinate(1) * p1
+ + naturalCoordinate(2) * p2
+ + naturalCoordinate(3) * p3;
+}
+
+SurgSim::Math::Vector Fem3DElementTetrahedron::computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state, const SurgSim::Math::Vector& cartesianCoordinate) const
+{
+ SURGSIM_ASSERT(cartesianCoordinate.size() == 3) << "globalCoordinate must be length 3.";
+
+ double volume;
+ std::array<double, 4> ai;
+ std::array<double, 4> bi;
+ std::array<double, 4> ci;
+ std::array<double, 4> di;
+ computeShapeFunctions(state, &volume, &ai, &bi, &ci, &di);
+
+ SurgSim::Math::Vector4d result;
+
+ for (size_t i = 0; i < 4; ++i)
+ {
+ result[i] = ai[i] + bi[i] * cartesianCoordinate[0]
+ + ci[i] * cartesianCoordinate[1]
+ + di[i] * cartesianCoordinate[2];
+ }
+ result /= 6.0 * volume;
+
+ return result;
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem3DElementTetrahedron.h b/SurgSim/Physics/Fem3DElementTetrahedron.h
new file mode 100644
index 0000000..f7fe767
--- /dev/null
+++ b/SurgSim/Physics/Fem3DElementTetrahedron.h
@@ -0,0 +1,206 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM3DELEMENTTETRAHEDRON_H
+#define SURGSIM_PHYSICS_FEM3DELEMENTTETRAHEDRON_H
+
+#include <array>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Class for Fem Element 3D based on a tetrahedron volume discretization
+/// \note The inertia property (mass) of the tetrahedron is derived from
+/// \note "Theory of Matrix Structural Analysis" from J.S. Przemieniecki
+/// \note The force and stiffness matrix of the tetrahedron is derived from
+/// \note http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf
+/// \note The deformation is based on the linear elasticity theory and not on the visco-elasticity theory.
+/// \note Therefore the element does not have any damping component.
+class Fem3DElementTetrahedron : public FemElement
+{
+public:
+ /// Constructor
+ /// \param nodeIds An array of 4 node (A, B, C, D) ids defining this tetrahedron element in a overall mesh
+ /// \note It is required that the triangle ABC is CCW looking from D (i.e. dot(cross(AB, AC), AD) > 0)
+ /// \note This is required from the signed volume calculation method getVolume()
+ /// \note A warning will be logged when the initialize function is called if this condition is not met, but the
+ /// \note simulation will keep running. Behavior will be undefined because of possible negative volume terms.
+ explicit Fem3DElementTetrahedron(std::array<size_t, 4> nodeIds);
+
+ /// Initialize the FemElement once everything has been set
+ /// \param state The state to initialize the FemElement with
+ /// \note We use the theory of linear elasticity, so this method pre-compute the stiffness and mass matrices
+ /// \note It is required that the triangle ABC is CCW looking from D (i.e. dot(cross(AB, AC), AD) > 0)
+ /// \note This is required from the signed volume calculation method getVolume()
+ /// \note A warning will be logged in if this condition is not met, but the simulation will keep running. Behavior
+ /// will be undefined because of possible negative volume terms.
+ virtual void initialize(const SurgSim::Math::OdeState& state) override;
+
+ /// Get the element volume based on the input state
+ /// \param state The state to compute the volume with
+ virtual double getVolume(const SurgSim::Math::OdeState& state) const override;
+
+ /// Adds the element force (computed for a given state) to a complete system force vector F (assembly)
+ /// \param state The state to compute the force with
+ /// \param[in,out] F The complete system force vector to add the element force into
+ /// \param scale A factor to scale the added force with
+ /// \note The element force is of size (getNumDofPerNode() x getNumNodes())
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ virtual void addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ double scale = 1.0) override;
+
+ /// Adds the element mass matrix M (computed for a given state) to a complete system mass matrix M (assembly)
+ /// \param state The state to compute the mass matrix with
+ /// \param[in,out] M The complete system mass matrix to add the element mass-matrix into
+ /// \param scale A factor to scale the added mass matrix with
+ /// \note The element mass matrix is square of size getNumDofPerNode() x getNumNodes()
+ /// \note This method supposes that the incoming state contains information with the same number of
+ /// \note dof per node as getNumDofPerNode()
+ virtual void addMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* M,
+ double scale = 1.0) override;
+
+ /// Adds the element damping matrix D (= -df/dv) (comuted for a given state)
+ /// to a complete system damping matrix D (assembly)
+ /// \param state The state to compute the damping matrix with
+ /// \param[in,out] D The complete system damping matrix to add the element damping matrix into
+ /// \param scale A factor to scale the added damping matrix with
+ /// \note The element damping matrix is square of size getNumDofPerNode() x getNumNodes()
+ /// \note This method supposes that the incoming state contains information with the same number of
+ /// \note dof per node as getNumDofPerNode()
+ /// \note Fem3DElementTetrahedron uses linear elasticity (not visco-elasticity), so it does not give any damping.
+ virtual void addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D,
+ double scale = 1.0) override;
+
+ /// Adds the element stiffness matrix K (= -df/dx) (computed for a given state)
+ /// to a complete system stiffness matrix K (assembly)
+ /// \param state The state to compute the stiffness matrix with
+ /// \param[in,out] K The complete system stiffness matrix to add the element stiffness matrix into
+ /// \param scale A factor to scale the added stiffness matrix with
+ /// \note The element stiffness matrix is square of size getNumDofPerNode() x getNumNodes()
+ /// \note This method supposes that the incoming state contains information with the same number of
+ /// \note dof per node as getNumDofPerNode()
+ virtual void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale = 1.0) override;
+
+ /// Adds the element force vector, mass, stiffness and damping matrices (computed for a given state)
+ /// into a complete system data structure F, M, D, K (assembly)
+ /// \param state The state to compute everything with
+ /// \param[in,out] F The complete system force vector to add the element force into
+ /// \param[in,out] M The complete system mass matrix to add the element mass matrix into
+ /// \param[in,out] D The complete system damping matrix to add the element damping matrix into
+ /// \param[in,out] K The complete system stiffness matrix to add the element stiffness matrix into
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ virtual void addFMDK(const SurgSim::Math::OdeState& state,
+ SurgSim::Math::Vector* F,
+ SurgSim::Math::Matrix* M,
+ SurgSim::Math::Matrix* D,
+ SurgSim::Math::Matrix* K) override;
+
+ /// Adds the element matrix-vector contribution F += (alphaM.M + alphaD.D + alphaK.K).x (computed for a given state)
+ /// into a complete system data structure F (assembly)
+ /// \param state The state to compute everything with
+ /// \param alphaM The scaling factor for the mass contribution
+ /// \param alphaD The scaling factor for the damping contribution
+ /// \param alphaK The scaling factor for the stiffness contribution
+ /// \param x A complete system vector to use as the vector in the matrix-vector multiplication
+ /// \param[in,out] F The complete system force vector to add the element matrix-vector contribution into
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ virtual void addMatVec(const SurgSim::Math::OdeState& state,
+ double alphaM, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F) override;
+
+ virtual SurgSim::Math::Vector computeCartesianCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const override;
+
+ virtual SurgSim::Math::Vector computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const override;
+
+protected:
+ /// Computes the tetrahedron shape functions
+ /// \param state The deformable rest state to compute the shape function from
+ /// \param[out] volume the volume calculated with the given state
+ /// \param[out] ai from the shape function, Ni(x, y, z) = 1/6*volume (ai + bi.x + ci.y + di.z)
+ /// \param[out] bi from the shape function, Ni(x, y, z) = 1/6*volume (ai + bi.x + ci.y + di.z)
+ /// \param[out] ci from the shape function, Ni(x, y, z) = 1/6*volume (ai + bi.x + ci.y + di.z)
+ /// \param[out] di from the shape function, Ni(x, y, z) = 1/6*volume (ai + bi.x + ci.y + di.z)
+ void computeShapeFunctions(const SurgSim::Math::OdeState& state,
+ double* volume,
+ std::array<double, 4>* ai,
+ std::array<double, 4>* bi,
+ std::array<double, 4>* ci,
+ std::array<double, 4>* di) const;
+
+ /// Computes the tetrahedron stiffness matrix
+ /// \param state The state to compute the stiffness matrix from
+ /// \param[out] k The stiffness matrix to store the result into
+ void computeStiffness(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 12, 12>* k);
+
+ /// Computes the tetrahedron mass matrix
+ /// \param state The state to compute the mass matrix from
+ /// \param[out] m The mass matrix to store the result into
+ void computeMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 12, 12>* m);
+
+ /// Adds the element force (computed for a given state) to a complete system force vector F (assembly)
+ /// This method relies on a given stiffness matrix and does not evaluate it from the state
+ /// \param state The state to compute the force with
+ /// \param k The given element stiffness matrix
+ /// \param[in,out] F The complete system force vector to add the element force into
+ /// \param scale A factor to scale the added force with
+ /// \note The element force is of size (getNumDofPerNode() x getNumNodes())
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ void addForce(const SurgSim::Math::OdeState& state, const Eigen::Matrix<double, 12, 12>& k,
+ SurgSim::Math::Vector* F, double scale = 1.0);
+
+ /// Shape functions: Tetrahedron rest volume
+ double m_restVolume;
+ /// Shape functions coefficients Ni(x,y,z) = 1/6V ( ai + x.bi + y.ci + z.di )
+ std::array<double, 4> m_ai, m_bi, m_ci, m_di;
+
+ /// The tetrahedon rest state
+ Eigen::Matrix<double, 12, 1> m_x0;
+
+ /// Elasticity material matrix (contains the elastic properties of the material)
+ Eigen::Matrix<double, 6, 6> m_Em;
+ /// Strain matrix
+ Eigen::Matrix<double, 6, 12> m_strain;
+ /// Stress matrix
+ Eigen::Matrix<double, 6, 12> m_stress;
+
+ /// Mass matrix
+ Eigen::Matrix<double, 12, 12> m_M;
+ /// Stiffness matrix
+ Eigen::Matrix<double, 12, 12> m_K;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM3DELEMENTTETRAHEDRON_H
diff --git a/SurgSim/Physics/Fem3DPlyReaderDelegate.cpp b/SurgSim/Physics/Fem3DPlyReaderDelegate.cpp
new file mode 100644
index 0000000..f249c6e
--- /dev/null
+++ b/SurgSim/Physics/Fem3DPlyReaderDelegate.cpp
@@ -0,0 +1,61 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2014, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <array>
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/Fem3DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+
+using SurgSim::DataStructures::PlyReader;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+Fem3DPlyReaderDelegate::Fem3DPlyReaderDelegate(std::shared_ptr<Fem3DRepresentation> fem)
+ : FemPlyReaderDelegate(fem)
+{
+}
+
+std::string Fem3DPlyReaderDelegate::getElementName() const
+{
+ return "3d_element";
+}
+
+void Fem3DPlyReaderDelegate::processFemElement(const std::string& elementName)
+{
+ SURGSIM_ASSERT(4 == m_femData.vertexCount || 8 == m_femData.vertexCount) <<
+ "Cannot process 3D element with " << m_femData.vertexCount << " vertices.";
+
+ if (4 == m_femData.vertexCount)
+ {
+ std::array<size_t, 4> fem3DVertices;
+ std::copy(m_femData.indices, m_femData.indices + 4, fem3DVertices.begin());
+ m_fem->addFemElement(std::make_shared<Fem3DElementTetrahedron>(fem3DVertices));
+ }
+ else
+ {
+ std::array<size_t, 8> fem3DVertices;
+ std::copy(m_femData.indices, m_femData.indices + 8, fem3DVertices.begin());
+ m_fem->addFemElement(std::make_shared<Fem3DElementCube>(fem3DVertices));
+ }
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Physics/Fem3DPlyReaderDelegate.h b/SurgSim/Physics/Fem3DPlyReaderDelegate.h
new file mode 100644
index 0000000..4e3ceb4
--- /dev/null
+++ b/SurgSim/Physics/Fem3DPlyReaderDelegate.h
@@ -0,0 +1,46 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2014, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM3DPLYREADERDELEGATE_H
+#define SURGSIM_PHYSICS_FEM3DPLYREADERDELEGATE_H
+
+#include <memory>
+
+#include "SurgSim/Physics/FemPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+class Fem3DRepresentation;
+
+/// Implementation of PlyReaderDelegate for Fem3DRepresentation
+class Fem3DPlyReaderDelegate : public FemPlyReaderDelegate
+{
+public:
+ /// Constructor
+ /// \param fem The object that is updated when PlyReader::parseFile is called.
+ explicit Fem3DPlyReaderDelegate(std::shared_ptr<Fem3DRepresentation> fem);
+
+protected:
+ virtual std::string getElementName() const override;
+
+ virtual void processFemElement(const std::string& elementName) override;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM3DPLYREADERDELEGATE_H
\ No newline at end of file
diff --git a/SurgSim/Physics/Fem3DRepresentation.cpp b/SurgSim/Physics/Fem3DRepresentation.cpp
new file mode 100644
index 0000000..9987cfe
--- /dev/null
+++ b/SurgSim/Physics/Fem3DRepresentation.cpp
@@ -0,0 +1,301 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Valid.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+#include "SurgSim/Physics/Fem3DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentationLocalization.h"
+#include "SurgSim/Physics/FemElement.h"
+
+using SurgSim::Framework::Logger;
+
+namespace
+{
+void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& transform,
+ SurgSim::Math::Vector* x, bool rotationOnly = false)
+{
+ typedef SurgSim::Math::Vector::Index IndexType;
+
+ IndexType numNodes = x->size() / 3;
+ SURGSIM_ASSERT(numNodes * 3 == x->size()) <<
+ "Unexpected number of dof in a Fem3D state vector (not a multiple of 3)";
+
+ for (IndexType nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ SurgSim::Math::Vector3d xi = SurgSim::Math::getSubVector(*x, nodeId, 3);
+ SurgSim::Math::Vector3d xiTransformed;
+ if (rotationOnly)
+ {
+ xiTransformed = transform.linear() * xi;
+ }
+ else
+ {
+ xiTransformed = transform * xi;
+ }
+ SurgSim::Math::setSubVector(xiTransformed, nodeId, 3, x);
+ }
+}
+}
+
+namespace SurgSim
+{
+namespace Physics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::Fem3DRepresentation, Fem3DRepresentation);
+
+Fem3DRepresentation::Fem3DRepresentation(const std::string& name) :
+ FemRepresentation(name)
+{
+ // Reminder: m_numDofPerNode is held by DeformableRepresentation
+ // but needs to be set by all concrete derived classes
+ m_numDofPerNode = 3;
+}
+
+Fem3DRepresentation::~Fem3DRepresentation()
+{
+}
+
+RepresentationType Fem3DRepresentation::getType() const
+{
+ return REPRESENTATION_TYPE_FEM3D;
+}
+
+void Fem3DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K,
+ const SurgSim::Math::Matrix& D)
+{
+ const size_t dofPerNode = getNumDofPerNode();
+ const SurgSim::Math::Matrix::Index expectedSize = static_cast<const SurgSim::Math::Matrix::Index>(dofPerNode);
+
+ SURGSIM_ASSERT(localization != nullptr) << "Invalid localization (nullptr)";
+ SURGSIM_ASSERT(generalizedForce.size() == expectedSize) <<
+ "Generalized force has an invalid size of " << generalizedForce.size() << ". Expected " << dofPerNode;
+ SURGSIM_ASSERT(K.size() == 0 || (K.rows() == expectedSize && K.cols() == expectedSize)) <<
+ "Stiffness matrix K has an invalid size (" << K.rows() << "," << K.cols() <<
+ ") was expecting a square matrix of size " << dofPerNode;
+ SURGSIM_ASSERT(D.size() == 0 || (D.rows() == expectedSize && D.cols() == expectedSize)) <<
+ "Damping matrix D has an invalid size (" << D.rows() << "," << D.cols() <<
+ ") was expecting a square matrix of size " << dofPerNode;
+
+ std::shared_ptr<Fem3DRepresentationLocalization> localization3D =
+ std::dynamic_pointer_cast<Fem3DRepresentationLocalization>(localization);
+ SURGSIM_ASSERT(localization3D != nullptr) << "Invalid localization type (not a Fem3DRepresentationLocalization)";
+
+ const size_t elementId = localization3D->getLocalPosition().index;
+ const SurgSim::Math::Vector& coordinate = localization3D->getLocalPosition().coordinate;
+ std::shared_ptr<FemElement> element = getFemElement(elementId);
+
+ size_t index = 0;
+ for (auto nodeId : element->getNodeIds())
+ {
+ m_externalGeneralizedForce.segment(dofPerNode * nodeId, dofPerNode) += generalizedForce * coordinate[index];
+ index++;
+ }
+
+ if (K.size() != 0 || D.size() != 0)
+ {
+ size_t index1 = 0;
+ for (auto nodeId1 : element->getNodeIds())
+ {
+ size_t index2 = 0;
+ for (auto nodeId2 : element->getNodeIds())
+ {
+ if (K.size() != 0)
+ {
+ m_externalGeneralizedStiffness.block(dofPerNode * nodeId1,
+ dofPerNode * nodeId2,
+ dofPerNode, dofPerNode)
+ += coordinate[index1] * coordinate[index2] * K;
+ }
+ if (D.size() != 0)
+ {
+ m_externalGeneralizedDamping.block(dofPerNode * nodeId1,
+ dofPerNode * nodeId2,
+ dofPerNode, dofPerNode)
+ += coordinate[index1] * coordinate[index2] * D;
+ }
+ index2++;
+ }
+
+ index1++;
+ }
+ }
+}
+
+std::shared_ptr<FemPlyReaderDelegate> Fem3DRepresentation::getDelegate()
+{
+ auto thisAsSharedPtr = std::static_pointer_cast<Fem3DRepresentation>(shared_from_this());
+ auto readerDelegate = std::make_shared<Fem3DPlyReaderDelegate>(thisAsSharedPtr);
+
+ return readerDelegate;
+}
+
+std::unordered_map<size_t, size_t> Fem3DRepresentation::createTriangleIdToElementIdMap(
+ const SurgSim::DataStructures::TriangleMesh& mesh)
+{
+ std::unordered_map<size_t, size_t> result;
+
+ // An Fem3DElementCube/Fem3DElementTetrahedron element has 8/4 nodes.
+ // A triangle has 3 nodes.
+ // If all the nodes of a triangle are present in a Fem3DElement*** node, then a row in the map is created.
+ // The nodes are identified using their ids.
+ // std::includes(...) is used to find whether a given list of triangle node ids are present in the supplied list
+ // of femElement node ids. This function requires the lists of node ids to be sorted.
+
+ // Get the list of fem elements with their node ids.
+ std::vector<std::vector<size_t>> femElements;
+ femElements.reserve(getNumFemElements());
+ for (size_t i = 0; i < getNumFemElements(); ++i)
+ {
+ auto elementNodeIds = getFemElement(i)->getNodeIds();
+ std::sort(elementNodeIds.begin(), elementNodeIds.end());
+ femElements.push_back(elementNodeIds);
+ }
+
+ std::array<size_t, 3> triangleSorted;
+ auto doesIncludeTriangle = [&triangleSorted](const std::vector<size_t>& femElementSorted)
+ {
+ return std::includes(femElementSorted.begin(), femElementSorted.end(),
+ triangleSorted.begin(), triangleSorted.end());
+ };
+
+ auto& meshTriangles = mesh.getTriangles();
+ for (auto triangle = meshTriangles.cbegin(); triangle != meshTriangles.cend(); ++triangle)
+ {
+ if (! triangle->isValid)
+ {
+ continue;
+ }
+ triangleSorted = triangle->verticesId;
+ std::sort(triangleSorted.begin(), triangleSorted.end());
+
+ // Find the femElement that contains all the node ids of this triangle.
+ std::vector<std::vector<size_t>>::iterator foundFemElement =
+ std::find_if(femElements.begin(), femElements.end(), doesIncludeTriangle);
+
+ // Assert to make sure that a triangle doesn't end up not having a femElement mapped to it.
+ SURGSIM_ASSERT(foundFemElement != femElements.end())
+ << "A triangle in the given mesh of an Fem3DRepresentation does not have a corresponding"
+ << " femElement.";
+
+ // Add a row to the mapping (triangleId, elementId).
+ // std::distance gives the index of the iterator within the container (by finding the distance
+ // from the beginning of the container).
+ result[std::distance(meshTriangles.begin(), triangle)] = std::distance(femElements.begin(), foundFemElement);
+ }
+
+ return result;
+}
+
+bool Fem3DRepresentation::doWakeUp()
+{
+ if (!FemRepresentation::doWakeUp())
+ {
+ return false;
+ }
+
+ auto deformableCollisionRepresentation
+ = std::dynamic_pointer_cast<DeformableCollisionRepresentation>(m_collisionRepresentation);
+
+ if (deformableCollisionRepresentation != nullptr)
+ {
+ m_triangleIdToElementIdMap = createTriangleIdToElementIdMap(*deformableCollisionRepresentation->getMesh());
+ }
+
+ return true;
+}
+
+std::shared_ptr<Localization> Fem3DRepresentation::createLocalization(const SurgSim::DataStructures::Location& location)
+{
+ SURGSIM_ASSERT(location.meshLocalCoordinate.hasValue())
+ << "Localization cannot be created if the triangle ID is not available.";
+
+ SURGSIM_ASSERT(location.meshLocalCoordinate.getValue().coordinate.size() == 3)
+ << "Localization has incorrect size for the barycentric coordinates.";
+
+ auto deformableCollisionRepresentation
+ = std::dynamic_pointer_cast<DeformableCollisionRepresentation>(m_collisionRepresentation);
+
+ SURGSIM_ASSERT(deformableCollisionRepresentation != nullptr)
+ << "Localization cannot be created if the DeformableCollisionRepresentation is not correctly set.";
+
+ // Find the vertex ids of the triangle.
+ size_t triangleId = location.meshLocalCoordinate.getValue().index;
+ auto triangleVertices = deformableCollisionRepresentation->getMesh()->getTriangle(triangleId).verticesId;
+
+ // Find the vertex ids of the corresponding FemNode.
+ // Get FemElement id from the triangle id.
+ SURGSIM_ASSERT(m_triangleIdToElementIdMap.count(triangleId) == 1) << "Triangle must be mapped to an fem element.";
+
+ size_t elementId = m_triangleIdToElementIdMap[triangleId];
+ std::shared_ptr<FemElement> element = getFemElement(elementId);
+
+ auto elementVertices = element->getNodeIds();
+
+ // Find the mapping between triangleVertices and elementVertices.
+ std::vector<size_t> indices;
+ indices.reserve(elementVertices.size());
+ for (size_t i = 0; i < elementVertices.size(); ++i)
+ {
+ indices.push_back(3);
+ for (int j = 0; j < 3; ++j)
+ {
+ if (triangleVertices[j] == elementVertices[i])
+ {
+ indices[i] = j;
+ break;
+ }
+ }
+ }
+
+ // Create the natural coordinate.
+ SurgSim::Math::Vector4d barycentricCoordinate(location.meshLocalCoordinate.getValue().coordinate[0],
+ location.meshLocalCoordinate.getValue().coordinate[1],
+ location.meshLocalCoordinate.getValue().coordinate[2],
+ 0.0);
+ SurgSim::DataStructures::IndexedLocalCoordinate coordinate;
+ coordinate.index = elementId;
+ coordinate.coordinate.resize(elementVertices.size());
+ for (size_t i = 0; i < elementVertices.size(); ++i)
+ {
+ coordinate.coordinate[i] = barycentricCoordinate[indices[i]];
+ }
+
+ // Fem3DRepresentationLocalization will verify the coordinate (2nd parameter) based on
+ // the Fem3DRepresentation passed as 1st parameter.
+ auto result = std::make_shared<Fem3DRepresentationLocalization>(
+ std::static_pointer_cast<SurgSim::Physics::Representation>(getSharedPtr()), coordinate);
+
+ return result;
+}
+
+void Fem3DRepresentation::transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform)
+{
+ transformVectorByBlockOf3(transform, &state->getPositions());
+ transformVectorByBlockOf3(transform, &state->getVelocities(), true);
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem3DRepresentation.h b/SurgSim/Physics/Fem3DRepresentation.h
new file mode 100644
index 0000000..c59350d
--- /dev/null
+++ b/SurgSim/Physics/Fem3DRepresentation.h
@@ -0,0 +1,92 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM3DREPRESENTATION_H
+#define SURGSIM_PHYSICS_FEM3DREPRESENTATION_H
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+class TriangleMesh;
+}
+
+namespace Physics
+{
+SURGSIM_STATIC_REGISTRATION(Fem3DRepresentation);
+
+class FemPlyReaderDelegate;
+
+/// Finite Element Model 3D is a fem built with 3D FemElement
+class Fem3DRepresentation : public FemRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name of the Fem3DRepresentation
+ explicit Fem3DRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~Fem3DRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem3DRepresentation);
+
+ /// Query the representation type
+ /// \return the RepresentationType for this representation
+ virtual RepresentationType getType() const override;
+
+ virtual void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K = SurgSim::Math::Matrix(),
+ const SurgSim::Math::Matrix& D = SurgSim::Math::Matrix()) override;
+
+ virtual std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location&) override;
+
+protected:
+ virtual bool doWakeUp() override;
+
+ /// Transform a state using a given transformation
+ /// \param[in,out] state The state to be transformed
+ /// \param transform The transformation to apply
+ virtual void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform) override;
+
+private:
+ virtual std::shared_ptr<FemPlyReaderDelegate> getDelegate() override;
+
+ /// Produces a mapping from the provided mesh's triangle ids to this object's fem element ids. The mesh's vertices
+ /// must be identical to this object's fem element nodes.
+ /// \param mesh The mesh used to produce the mapping.
+ /// \return A map from the mesh's triangle ids to this object's fem elements.
+ std::unordered_map<size_t, size_t> createTriangleIdToElementIdMap(
+ const SurgSim::DataStructures::TriangleMesh& mesh);
+
+ /// Mapping from collision triangle's id to fem element id.
+ std::unordered_map<size_t, size_t> m_triangleIdToElementIdMap;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM3DREPRESENTATION_H
diff --git a/SurgSim/Physics/Fem3DRepresentationBilateral3D.cpp b/SurgSim/Physics/Fem3DRepresentationBilateral3D.cpp
new file mode 100644
index 0000000..af64409
--- /dev/null
+++ b/SurgSim/Physics/Fem3DRepresentationBilateral3D.cpp
@@ -0,0 +1,128 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentationBilateral3D.h"
+#include "SurgSim/Physics/Fem3DRepresentationLocalization.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/Localization.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem3DRepresentationBilateral3D::Fem3DRepresentationBilateral3D()
+{
+}
+
+Fem3DRepresentationBilateral3D::~Fem3DRepresentationBilateral3D()
+{
+}
+
+void Fem3DRepresentationBilateral3D::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+ std::shared_ptr<Fem3DRepresentation> fem3d
+ = std::static_pointer_cast<Fem3DRepresentation>(localization->getRepresentation());
+
+ if (!fem3d->isActive())
+ {
+ return;
+ }
+
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+ const SurgSim::DataStructures::IndexedLocalCoordinate& coord
+ = std::static_pointer_cast<Fem3DRepresentationLocalization>(localization)->getLocalPosition();
+
+ Vector3d globalPosition = localization->calculatePosition();
+
+ // Fixed point constraint in MCLP
+ // p(t) is defined as the point before motion
+ // s is defined as position to be constrained to
+ // u is defined as the displacement needed to enforce the constraint
+ //
+ // The equation is
+ // u + (p(t) - s) = 0
+ //
+ // Using backward-euler integration,
+ // u = dt.v(t + dt)
+ //
+ // The constraint (p(t) - s) exists in 3-space, but we must modify the velocity of coordinates in (n * 3) space. The
+ // transform from (n * 3) velocity space -> 3 translational space is denoted by H, which we construct here.
+ //
+ // The constructing principal of FEM is that nodes must be placed close enough such that the value of a function
+ // within an FEM can be linearly interpolated by the values at the nodes of the FEM. The interpolation weights are
+ // given by barycentric coordinates which linearly transform the nodes from (n * 3) -> 3 space (and vice versa):
+ // sum(n_i * b_i) = n_1 * b_1 + n_2 * b_i ... n_n * b_n
+ // where v_i are in 3 space.
+ //
+ // So the transform from node-velocity to constraint space is
+ // dt * sum(v_i * b_i)
+ //
+ // See RigidRepresentationBilateral3D for more implementation details.
+
+ // Update b with new violation: P(free motion)
+ mlcp->b.segment<3>(indexOfConstraint) += globalPosition * scale;
+
+ // m_newH is a SparseVector, so resizing is cheap. The object's memory also gets cleared.
+ m_newH.resize(fem3d->getNumDof());
+ // m_newH is a member variable, so 'reserve' only needs to allocate memory on the first run.
+ size_t numNodeToConstrain = (coord.coordinate.array() != 0.0).count();
+ m_newH.reserve(3 * numNodeToConstrain);
+
+ std::shared_ptr<FemElement> femElement = fem3d->getFemElement(coord.index);
+ size_t numNodes = femElement->getNumNodes();
+ for (size_t axis = 0; axis < 3; axis++)
+ {
+ m_newH.setZero();
+ for (size_t index = 0; index < numNodes; index++)
+ {
+ if (coord.coordinate[index] != 0.0)
+ {
+ size_t nodeIndex = femElement->getNodeId(index);
+ m_newH.insert(3 * nodeIndex + axis) = coord.coordinate[index] * (dt * scale);
+ }
+ }
+ mlcp->updateConstraint(m_newH, fem3d->getComplianceMatrix(), indexOfRepresentation, indexOfConstraint + axis);
+ }
+}
+
+SurgSim::Math::MlcpConstraintType Fem3DRepresentationBilateral3D::getMlcpConstraintType() const
+{
+ return SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT;
+}
+
+SurgSim::Physics::RepresentationType Fem3DRepresentationBilateral3D::getRepresentationType() const
+{
+ return REPRESENTATION_TYPE_FEM3D;
+}
+
+size_t Fem3DRepresentationBilateral3D::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/Fem3DRepresentationBilateral3D.h b/SurgSim/Physics/Fem3DRepresentationBilateral3D.h
new file mode 100644
index 0000000..a4dfa27
--- /dev/null
+++ b/SurgSim/Physics/Fem3DRepresentationBilateral3D.h
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM3DREPRESENTATIONBILATERAL3D_H
+#define SURGSIM_PHYSICS_FEM3DREPRESENTATIONBILATERAL3D_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Fem3DRepresentation bilateral 3d constraint implementation.
+///
+/// The family of bilateral3D constraints enforce equality between two points.
+class Fem3DRepresentationBilateral3D : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ Fem3DRepresentationBilateral3D();
+
+ /// Destructor
+ virtual ~Fem3DRepresentationBilateral3D();
+
+ /// Gets the Mixed Linear Complementarity Problem constraint type for this ConstraintImplementation
+ /// \return The MLCP constraint type corresponding to this constraint implementation
+ SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+
+ /// Gets the Type of representation that this implementation is concerned with
+ /// \return RepresentationType for this implementation
+ virtual RepresentationType getRepresentationType() const override;
+
+private:
+ /// Gets the number of degree of freedom.
+ /// \return 3 A bilateral 3d constraint enforces equality in the x, y, and z dimensions between 2 points.
+ size_t doGetNumDof() const override;
+
+ /// Builds the subset of an Mlcp physics problem associated to this implementation.
+ /// \param dt The time step.
+ /// \param data The data associated to the constraint.
+ /// \param localization The localization for the representation.
+ /// \param [in, out] mlcp The Mixed LCP physics problem to fill up.
+ /// \param indexOfRepresentation The index of the representation (associated to this implementation) in the mlcp.
+ /// \param indexOfConstraint The index of the constraint in the mlcp.
+ /// \param sign The sign of this implementation in the constraint (positive or negative side).
+ /// \note Empty for a Fixed Representation
+ void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) override;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM3DREPRESENTATIONBILATERAL3D_H
diff --git a/SurgSim/Physics/Fem3DRepresentationContact.cpp b/SurgSim/Physics/Fem3DRepresentationContact.cpp
new file mode 100644
index 0000000..5412c24
--- /dev/null
+++ b/SurgSim/Physics/Fem3DRepresentationContact.cpp
@@ -0,0 +1,130 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentationContact.h"
+#include "SurgSim/Physics/Fem3DRepresentationLocalization.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem3DRepresentationContact::Fem3DRepresentationContact()
+{
+}
+
+Fem3DRepresentationContact::~Fem3DRepresentationContact()
+{
+}
+
+void Fem3DRepresentationContact::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+ using SurgSim::Math::Vector3d;
+
+ auto fem3d = std::static_pointer_cast<Fem3DRepresentation>(localization->getRepresentation());
+
+ if (!fem3d->isActive())
+ {
+ return;
+ }
+
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+
+ const ContactConstraintData& contactData = static_cast<const ContactConstraintData&>(data);
+ const SurgSim::Math::Vector3d& n = contactData.getNormal();
+ const double d = contactData.getDistance();
+
+ const SurgSim::DataStructures::IndexedLocalCoordinate& coord
+ = std::static_pointer_cast<Fem3DRepresentationLocalization>(localization)->getLocalPosition();
+
+ // FRICTIONLESS CONTACT in a LCP
+ // (n, d) defines the plane of contact
+ // p(t) the point of contact (usually after free motion)
+ // p(free) is the point of contact after free motion
+ // u is the displacement needed to verify the constraint
+ // note that u = sum ui * baryCoord[i]
+ //
+ // The constraint equation for a plane is
+ // n^t.[p(free)+u] + d >= 0
+ // n^t.p(free) + n^t.u + d >= 0
+ // n^t.u + (n^t.p(free) + d) >= 0
+ //
+ // For implicit integration, u = dt.v(t+dt)
+
+ // Update b with new violation
+ Vector3d globalPosition = localization->calculatePosition();
+ double violation = n.dot(globalPosition) + d;
+
+ mlcp->b[indexOfConstraint] += violation * scale;
+
+ // m_newH is a SparseVector, so resizing is cheap. The object's memory also gets cleared.
+ m_newH.resize(fem3d->getNumDof());
+ // m_newH is a member variable, so 'reserve' only needs to allocate memory on the first run.
+ std::shared_ptr<FemElement> femElement = fem3d->getFemElement(coord.index);
+ size_t numNodes = femElement->getNumNodes();
+ size_t numNodeToConstrain = 0;
+ for (size_t index = 0; index < numNodes; index++)
+ {
+ if (coord.coordinate[index] != 0.0)
+ {
+ numNodeToConstrain++;
+ }
+ }
+ m_newH.reserve(3 * numNodeToConstrain);
+
+ for (size_t index = 0; index < numNodes; index++)
+ {
+ if (coord.coordinate[index] != 0.0)
+ {
+ size_t nodeIndex = femElement->getNodeId(index);
+ m_newH.insert(3 * nodeIndex + 0) = coord.coordinate[index] * n[0] * scale * dt;
+ m_newH.insert(3 * nodeIndex + 1) = coord.coordinate[index] * n[1] * scale * dt;
+ m_newH.insert(3 * nodeIndex + 2) = coord.coordinate[index] * n[2] * scale * dt;
+ }
+ }
+
+ mlcp->updateConstraint(m_newH, fem3d->getComplianceMatrix(), indexOfRepresentation, indexOfConstraint);
+}
+
+SurgSim::Math::MlcpConstraintType Fem3DRepresentationContact::getMlcpConstraintType() const
+{
+ return SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
+}
+
+SurgSim::Physics::RepresentationType Fem3DRepresentationContact::getRepresentationType() const
+{
+ return REPRESENTATION_TYPE_FEM3D;
+}
+
+size_t Fem3DRepresentationContact::doGetNumDof() const
+{
+ return 1;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/Fem3DRepresentationContact.h b/SurgSim/Physics/Fem3DRepresentationContact.h
new file mode 100644
index 0000000..79e5c45
--- /dev/null
+++ b/SurgSim/Physics/Fem3DRepresentationContact.h
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM3DREPRESENTATIONCONTACT_H
+#define SURGSIM_PHYSICS_FEM3DREPRESENTATIONCONTACT_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Fem3DRepresentation frictionless contact implementation.
+class Fem3DRepresentationContact : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ explicit Fem3DRepresentationContact();
+
+ /// Destructor
+ virtual ~Fem3DRepresentationContact();
+
+ /// Gets the Mixed Linear Complementarity Problem constraint type for this ConstraintImplementation
+ /// \return The MLCP constraint type corresponding to this constraint implementation
+ SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+
+ /// Gets the Type of representation that this implementation is concerned with
+ /// \return RepresentationType for this implementation
+ virtual RepresentationType getRepresentationType() const override;
+
+private:
+ /// Gets the number of degree of freedom.
+ /// \return 1 as a frictionless contact is formed of 1 equation of constraint (along the normal direction).
+ size_t doGetNumDof() const override;
+
+ /// Builds the subset of an Mlcp physics problem associated to this implementation.
+ /// \param dt The time step.
+ /// \param data The data associated to the constraint.
+ /// \param localization The localization for the representation.
+ /// \param [in, out] mlcp The Mixed LCP physics problem to fill up.
+ /// \param indexOfRepresentation The index of the representation (associated to this implementation) in the mlcp.
+ /// \param indexOfConstraint The index of the constraint in the mlcp.
+ /// \param sign The sign of this implementation in the constraint (positive or negative side).
+ /// \note Empty for a Fixed Representation
+ void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) override;
+
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM3DREPRESENTATIONCONTACT_H
diff --git a/SurgSim/Physics/Fem3DRepresentationLocalization.cpp b/SurgSim/Physics/Fem3DRepresentationLocalization.cpp
new file mode 100644
index 0000000..469c49d
--- /dev/null
+++ b/SurgSim/Physics/Fem3DRepresentationLocalization.cpp
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/Fem3DRepresentationLocalization.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem3DRepresentationLocalization::Fem3DRepresentationLocalization(
+ std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition) :
+ Localization()
+{
+ setRepresentation(representation);
+ setLocalPosition(localPosition);
+}
+
+Fem3DRepresentationLocalization::~Fem3DRepresentationLocalization()
+{
+
+}
+
+void Fem3DRepresentationLocalization::setLocalPosition(
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition)
+{
+ auto femRepresentation = std::static_pointer_cast<Fem3DRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(femRepresentation != nullptr) << "FemRepresentation is null, it was probably not" <<
+ " initialized";
+
+ SURGSIM_ASSERT(femRepresentation->isValidCoordinate(localPosition))
+ << "IndexedLocalCoordinate is invalid for Representation " << getRepresentation()->getName();
+
+ m_position = localPosition;
+}
+
+const SurgSim::DataStructures::IndexedLocalCoordinate& Fem3DRepresentationLocalization::getLocalPosition() const
+{
+ return m_position;
+}
+
+SurgSim::Math::Vector3d Fem3DRepresentationLocalization::doCalculatePosition(double time)
+{
+ using SurgSim::Math::Vector3d;
+
+ auto femRepresentation = std::static_pointer_cast<Fem3DRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(femRepresentation != nullptr) << "FemRepresentation is null, it was probably not" <<
+ " initialized";
+
+ std::shared_ptr<FemElement> femElement = femRepresentation->getFemElement(m_position.index);
+ const Vector3d currentPosition = femElement->computeCartesianCoordinate(*femRepresentation->getCurrentState(),
+ m_position.coordinate);
+ const Vector3d previousPosition = femElement->computeCartesianCoordinate(*femRepresentation->getPreviousState(),
+ m_position.coordinate);
+
+ if (time == 0.0)
+ {
+ return previousPosition;
+ }
+ else if (time == 1.0)
+ {
+ return currentPosition;
+ }
+
+ return previousPosition + time * (currentPosition - previousPosition);
+}
+
+bool Fem3DRepresentationLocalization::isValidRepresentation(std::shared_ptr<Representation> representation)
+{
+ auto femRepresentation = std::dynamic_pointer_cast<Fem3DRepresentation>(representation);
+
+ // Allows to reset the representation to nullptr ...
+ return (femRepresentation != nullptr || representation == nullptr);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem3DRepresentationLocalization.h b/SurgSim/Physics/Fem3DRepresentationLocalization.h
new file mode 100644
index 0000000..5c26fc6
--- /dev/null
+++ b/SurgSim/Physics/Fem3DRepresentationLocalization.h
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEM3DREPRESENTATIONLOCALIZATION_H
+#define SURGSIM_PHYSICS_FEM3DREPRESENTATIONLOCALIZATION_H
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Physics/Localization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Implementation of Localization for Fem3DRepresentation
+///
+/// Fem3DRepresentationLocalization tracks the global coordinates of an IndexedLocalCoordinate associated with an
+/// Fem3DRepresentation. It is used, for example, as a helper class for filling out the MlcpPhysicsProblem in
+/// Fem3DRepresentationContact::doBuild, which constrains the motion of Fem3DRepresentation at a frictionless contact.
+class Fem3DRepresentationLocalization : public Localization
+{
+public:
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ /// \param localPosition The local position to set the localization at.
+ Fem3DRepresentationLocalization(std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition);
+
+ /// Destructor
+ virtual ~Fem3DRepresentationLocalization();
+
+ /// Sets the local position.
+ /// \param localPosition The local position to set the localization at.
+ void setLocalPosition(const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition);
+
+ /// Gets the local position.
+ /// \return The local position set for this localization.
+ const SurgSim::DataStructures::IndexedLocalCoordinate& getLocalPosition() const;
+
+ /// Query if 'representation' is valid representation.
+ /// \param representation The representation.
+ /// \return true if valid representation, false if not.
+ virtual bool isValidRepresentation(std::shared_ptr<Representation> representation) override;
+
+private:
+ /// Calculates the global position of this localization.
+ /// \param time The time in [0..1] at which the position should be calculated.
+ /// \return The global position of the localization at the requested time.
+ /// \note time can be useful when dealing with CCD.
+ SurgSim::Math::Vector3d doCalculatePosition(double time);
+
+ /// Barycentric position in local coordinates
+ SurgSim::DataStructures::IndexedLocalCoordinate m_position;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM3DREPRESENTATIONLOCALIZATION_H
diff --git a/SurgSim/Physics/FemElement.cpp b/SurgSim/Physics/FemElement.cpp
new file mode 100644
index 0000000..408d89d
--- /dev/null
+++ b/SurgSim/Physics/FemElement.cpp
@@ -0,0 +1,119 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FemElement::FemElement() : m_numDofPerNode(0), m_rho(0.0), m_E(0.0), m_nu(0.0)
+{}
+
+FemElement::~FemElement()
+{}
+
+void FemElement::initialize(const SurgSim::Math::OdeState& state)
+{
+ SURGSIM_ASSERT(m_rho != 0.0) << "Mass density is not set. Did you call setMassDensity() ?";
+ SURGSIM_ASSERT(m_nu != 0.0) << "Poisson ratio is not set. Did you call setPoissonRatio() ?";
+ SURGSIM_ASSERT(m_E != 0.0) << "Young modulus is not set. Did you call setYoungModulus() ?";
+ SURGSIM_ASSERT(m_rho > 0.0) << "Mass density ("<<m_rho<<") is invalid, it should be positive";
+ SURGSIM_ASSERT(m_nu > 0.0 && m_nu < 0.5) << "Poisson ratio ("<<m_nu<<") is invalid, it should be within [0 0.5)";
+ SURGSIM_ASSERT(m_E > 0.0) << "Young modulus ("<<m_E<<") is invalid, it should be positive";
+}
+
+size_t FemElement::getNumDofPerNode() const
+{
+ return m_numDofPerNode;
+}
+
+void FemElement::setNumDofPerNode(size_t numDofPerNode)
+{
+ m_numDofPerNode = numDofPerNode;
+}
+
+size_t FemElement::getNumNodes() const
+{
+ return m_nodeIds.size();
+}
+
+size_t FemElement::getNodeId(size_t elementNodeId) const
+{
+ return m_nodeIds[elementNodeId];
+}
+
+const std::vector<size_t>& FemElement::getNodeIds() const
+{
+ return m_nodeIds;
+}
+
+void FemElement::setYoungModulus(double E)
+{
+ m_E = E;
+}
+
+double FemElement::getYoungModulus() const
+{
+ return m_E;
+}
+
+void FemElement::setPoissonRatio(double nu)
+{
+ m_nu = nu;
+}
+
+double FemElement::getPoissonRatio() const
+{
+ return m_nu;
+}
+
+void FemElement::setMassDensity(double rho)
+{
+ m_rho = rho;
+}
+
+double FemElement::getMassDensity() const
+{
+ return m_rho;
+}
+
+double FemElement::getMass(const SurgSim::Math::OdeState& state) const
+{
+ return getVolume(state) * m_rho;
+}
+
+bool FemElement::update(const SurgSim::Math::OdeState& state)
+{
+ return true;
+}
+
+bool FemElement::isValidCoordinate(const SurgSim::Math::Vector& naturalCoordinate) const
+{
+ return (std::abs(naturalCoordinate.sum() - 1.0) < SurgSim::Math::Geometry::ScalarEpsilon)
+ && (naturalCoordinate.size() >= 0)
+ && (static_cast<size_t>(naturalCoordinate.size()) == getNumNodes())
+ && (-SurgSim::Math::Geometry::ScalarEpsilon <= naturalCoordinate.minCoeff() &&
+ naturalCoordinate.maxCoeff() <= 1.0 + SurgSim::Math::Geometry::ScalarEpsilon);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/FemElement.h b/SurgSim/Physics/FemElement.h
new file mode 100644
index 0000000..a19f835
--- /dev/null
+++ b/SurgSim/Physics/FemElement.h
@@ -0,0 +1,224 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEMELEMENT_H
+#define SURGSIM_PHYSICS_FEMELEMENT_H
+
+#include <vector>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class OdeState;
+};
+
+namespace Physics
+{
+
+/// Base class for all Fem Element (1D, 2D, 3D)
+/// It handles the node ids to which it is connected and requires all derived classes to compute the element
+/// mass matrix and the force vector along with the derivatives (the stiffness and damping matrices).
+/// A extra method also exist to compute all of them at once for performance purposes.
+/// It holds on to the actual computed values (m_f, m_M, m_D, m_K) as its size is not predefined from outside
+/// and would requires intensive (de)allocation or a temporary variable anyway.
+/// It contains the linear elasticity parameter (Young modulus and Poisson ratio) as well as mass density
+class FemElement
+{
+public:
+ /// Constructor
+ FemElement();
+
+ /// Virtual destructor
+ virtual ~FemElement();
+
+ /// Initialize the FemElement once everything has been set
+ /// \param state The state to initialize the FemElement with
+ virtual void initialize(const SurgSim::Math::OdeState& state);
+
+ /// Gets the number of degree of freedom per node
+ /// \return The number of dof per node
+ size_t getNumDofPerNode() const;
+
+ /// Gets the number of nodes connected by this element
+ /// \return The number of nodes
+ size_t getNumNodes() const;
+
+ /// Gets the elementNodeId-th node id
+ /// \return The requested node id
+ size_t getNodeId(size_t elementNodeId) const;
+
+ /// Gets the node ids for this element
+ /// \return A vector containing the node ids on which the element is defined
+ const std::vector<size_t>& getNodeIds() const;
+
+ /// Sets the Young modulus (in N.m-2)
+ /// \param E The Young modulus
+ void setYoungModulus(double E);
+ /// Gets the Young modulus (in N.m-2)
+ /// \return The Young modulus
+ double getYoungModulus() const;
+
+ /// Sets the Poisson ratio (unitless)
+ /// \param nu The Poisson ratio
+ void setPoissonRatio(double nu);
+ /// Gets the Poisson ratio (unitless)
+ /// \return The Poisson ratio
+ double getPoissonRatio() const;
+
+ /// Sets the mass density (in Kg.m-3)
+ /// \param rho The mass density
+ void setMassDensity(double rho);
+ /// Gets the mass density (in Kg.m-3)
+ /// \return The mass density
+ double getMassDensity() const;
+
+ /// Gets the element mass based on the input state (in Kg)
+ /// \param state The state to compute the mass with
+ /// \return The mass of this element (in Kg)
+ double getMass(const SurgSim::Math::OdeState& state) const;
+
+ /// Gets the element volume based on the input state (in m-3)
+ /// \param state The state to compute the volume with
+ /// \return The volume of this element (in m-3)
+ virtual double getVolume(const SurgSim::Math::OdeState& state) const = 0;
+
+ /// Adds the element force (computed for a given state) to a complete system force vector F (assembly)
+ /// \param state The state to compute the force with
+ /// \param[in,out] F The complete system force vector to add the element force into
+ /// \param scale A factor to scale the added force with
+ /// \note The element force is of size (getNumDofPerNode() x getNumNodes())
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ virtual void addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F, double scale = 1.0) = 0;
+
+ /// Adds the element mass matrix M (computed for a given state) to a complete system mass matrix M (assembly)
+ /// \param state The state to compute the mass matrix with
+ /// \param[in,out] M The complete system mass matrix to add the element mass-matrix into
+ /// \param scale A factor to scale the added mass matrix with
+ /// \note The element mass matrix is square of size getNumDofPerNode() x getNumNodes()
+ /// \note This method supposes that the incoming state contains information with the same number of
+ /// \note dof per node as getNumDofPerNode()
+ virtual void addMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* M, double scale = 1.0) = 0;
+
+ /// Adds the element damping matrix D (= -df/dv) (comuted for a given state)
+ /// to a complete system damping matrix D (assembly)
+ /// \param state The state to compute the damping matrix with
+ /// \param[in,out] D The complete system damping matrix to add the element damping matrix into
+ /// \param scale A factor to scale the added damping matrix with
+ /// \note The element damping matrix is square of size getNumDofPerNode() x getNumNodes()
+ /// \note This method supposes that the incoming state contains information with the same number of
+ /// \note dof per node as getNumDofPerNode()
+ virtual void addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D,
+ double scale = 1.0) = 0;
+
+ /// Adds the element stiffness matrix K (= -df/dx) (computed for a given state)
+ /// to a complete system stiffness matrix K (assembly)
+ /// \param state The state to compute the stiffness matrix with
+ /// \param[in,out] K The complete system stiffness matrix to add the element stiffness matrix into
+ /// \param scale A factor to scale the added stiffness matrix with
+ /// \note The element stiffness matrix is square of size getNumDofPerNode() x getNumNodes()
+ /// \note This method supposes that the incoming state contains information with the same number of
+ /// \note dof per node as getNumDofPerNode()
+ virtual void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale = 1.0) = 0;
+
+ /// Adds the element force vector, mass, stiffness and damping matrices (computed for a given state)
+ /// into a complete system data structure F, M, D, K (assembly)
+ /// \param state The state to compute everything with
+ /// \param[in,out] F The complete system force vector to add the element force into
+ /// \param[in,out] M The complete system mass matrix to add the element mass matrix into
+ /// \param[in,out] D The complete system damping matrix to add the element damping matrix into
+ /// \param[in,out] K The complete system stiffness matrix to add the element stiffness matrix into
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ virtual void addFMDK(const SurgSim::Math::OdeState& state,
+ SurgSim::Math::Vector* F,
+ SurgSim::Math::Matrix* M,
+ SurgSim::Math::Matrix* D,
+ SurgSim::Math::Matrix* K) = 0;
+
+ /// Adds the element matrix-vector contribution F += (alphaM.M + alphaD.D + alphaK.K).x (computed for a given state)
+ /// into a complete system data structure F (assembly)
+ /// \param state The state to compute everything with
+ /// \param alphaM The scaling factor for the mass contribution
+ /// \param alphaD The scaling factor for the damping contribution
+ /// \param alphaK The scaling factor for the stiffness contribution
+ /// \param x A complete system vector to be used as the vector in the matrix-vector multiplication
+ /// \param[in,out] F The complete system force vector to add the element matrix-vector contribution into
+ /// \note This method supposes that the incoming state contains information with the same number of dof
+ /// \note per node as getNumDofPerNode()
+ virtual void addMatVec(const SurgSim::Math::OdeState& state, double alphaM, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F) = 0;
+
+ /// Update the element based on a given state
+ /// \param state The state to compute the update from
+ /// \return True if the update is successful, False otherwise, in which case the element behavior
+ /// becomes undefined. The representation should get deactivated/reset in this case.
+ virtual bool update(const SurgSim::Math::OdeState& state);
+
+ /// Determines whether a given natural coordinate is valid
+ /// \param naturalCoordinate Coordinate to check
+ /// \return True if valid
+ bool isValidCoordinate(const SurgSim::Math::Vector& naturalCoordinate) const;
+
+ /// Computes a given natural coordinate in cartesian coordinates
+ /// \param state The state at which to transform coordinates
+ /// \param naturalCoordinate The coordinates to transform
+ /// \return The resultant cartesian coordinates
+ virtual SurgSim::Math::Vector computeCartesianCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const = 0;
+
+ /// Computes a natural coordinate given a global coordinate
+ /// \param state The state at which to transform coordinates
+ /// \param cartesianCoordinate The coordinates to transform
+ /// \return The resultant natural coordinates
+ virtual SurgSim::Math::Vector computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const = 0;
+
+protected:
+ /// Sets the number of degrees of freedom per node
+ /// \param numDofPerNode The number of dof per node
+ /// \note Protected to be accessible only to derived classes which should be the only
+ /// \note ones able to set this parameter
+ void setNumDofPerNode(size_t numDofPerNode);
+
+ /// Number of degree of freedom per node for this element
+ size_t m_numDofPerNode;
+
+ /// Node ids connected by this element
+ std::vector<size_t> m_nodeIds;
+
+ /// Mass density (in Kg.m-3)
+ double m_rho;
+
+ /// Young modulus (in N.m-2)
+ double m_E;
+
+ /// Poisson ratio (unitless)
+ double m_nu;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEMELEMENT_H
diff --git a/SurgSim/Physics/FemPlyReaderDelegate.cpp b/SurgSim/Physics/FemPlyReaderDelegate.cpp
new file mode 100644
index 0000000..907df6a
--- /dev/null
+++ b/SurgSim/Physics/FemPlyReaderDelegate.cpp
@@ -0,0 +1,205 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2014, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/FemPlyReaderDelegate.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+using SurgSim::DataStructures::PlyReader;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+FemPlyReaderDelegate::FemPlyReaderDelegate(std::shared_ptr<FemRepresentation> fem) :
+ m_hasBoundaryConditions(false),
+ m_boundaryConditionData(std::numeric_limits<size_t>::quiet_NaN()),
+ m_vertexIterator(nullptr),
+ m_fem(fem)
+{
+}
+
+FemPlyReaderDelegate::ElementData::ElementData() : indices(nullptr), vertexCount(0)
+{
+}
+
+bool FemPlyReaderDelegate::registerDelegate(PlyReader* reader)
+{
+ // Vertex processing
+ reader->requestElement(
+ "vertex",
+ std::bind(
+ &FemPlyReaderDelegate::beginVertices, this, std::placeholders::_1, std::placeholders::_2),
+ std::bind(&FemPlyReaderDelegate::processVertex, this, std::placeholders::_1),
+ std::bind(&FemPlyReaderDelegate::endVertices, this, std::placeholders::_1));
+ reader->requestScalarProperty("vertex", "x", PlyReader::TYPE_DOUBLE, 0 * sizeof(m_vertexData[0]));
+ reader->requestScalarProperty("vertex", "y", PlyReader::TYPE_DOUBLE, 1 * sizeof(m_vertexData[0]));
+ reader->requestScalarProperty("vertex", "z", PlyReader::TYPE_DOUBLE, 2 * sizeof(m_vertexData[0]));
+
+ // Element Processing
+ reader->requestElement(
+ getElementName(),
+ std::bind(&FemPlyReaderDelegate::beginFemElements,
+ this,
+ std::placeholders::_1,
+ std::placeholders::_2),
+ std::bind(&FemPlyReaderDelegate::processFemElement, this, std::placeholders::_1),
+ std::bind(&FemPlyReaderDelegate::endFemElements, this, std::placeholders::_1));
+ reader->requestListProperty(getElementName(),
+ "vertex_indices",
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(ElementData, indices),
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(ElementData, vertexCount));
+
+ // Boundary Condition Processing
+ if (m_hasBoundaryConditions)
+ {
+ reader->requestElement(
+ "boundary_condition",
+ std::bind(&FemPlyReaderDelegate::beginBoundaryConditions,
+ this,
+ std::placeholders::_1,
+ std::placeholders::_2),
+ std::bind(&FemPlyReaderDelegate::processBoundaryCondition, this, std::placeholders::_1),
+ nullptr);
+ reader->requestScalarProperty("boundary_condition", "vertex_index", PlyReader::TYPE_UNSIGNED_INT, 0);
+ }
+
+ // Material Processing
+ reader->requestElement(
+ "material",
+ std::bind(
+ &FemPlyReaderDelegate::beginMaterials, this, std::placeholders::_1, std::placeholders::_2),
+ nullptr,
+ std::bind(&FemPlyReaderDelegate::endMaterials, this, std::placeholders::_1));
+ reader->requestScalarProperty(
+ "material", "mass_density", PlyReader::TYPE_DOUBLE, offsetof(MaterialData, massDensity));
+ reader->requestScalarProperty(
+ "material", "poisson_ratio", PlyReader::TYPE_DOUBLE, offsetof(MaterialData, poissonRatio));
+ reader->requestScalarProperty(
+ "material", "young_modulus", PlyReader::TYPE_DOUBLE, offsetof(MaterialData, youngModulus));
+
+ reader->setStartParseFileCallback(std::bind(&FemPlyReaderDelegate::startParseFile, this));
+ reader->setEndParseFileCallback(std::bind(&FemPlyReaderDelegate::endParseFile, this));
+
+ return true;
+}
+
+bool FemPlyReaderDelegate::fileIsAcceptable(const PlyReader& reader)
+{
+ bool result = true;
+
+ // Shortcut test if one fails ...
+ result = result && reader.hasProperty("vertex", "x");
+ result = result && reader.hasProperty("vertex", "y");
+ result = result && reader.hasProperty("vertex", "z");
+
+ result = result && reader.hasProperty(getElementName(), "vertex_indices");
+ result = result && !reader.isScalar(getElementName(), "vertex_indices");
+
+ result = result && reader.hasProperty("material", "mass_density");
+ result = result && reader.hasProperty("material", "poisson_ratio");
+ result = result && reader.hasProperty("material", "young_modulus");
+
+ m_hasBoundaryConditions = reader.hasProperty("boundary_condition", "vertex_index");
+
+ return result;
+}
+
+void FemPlyReaderDelegate::startParseFile()
+{
+ SURGSIM_ASSERT(nullptr != m_fem) << "The FemRepresentation cannot be nullptr.";
+ SURGSIM_ASSERT(0 == m_fem->getNumFemElements()) <<
+ "The FemRepresentation already contains fem elements, so it cannot be initialized.";
+ SURGSIM_ASSERT(nullptr == m_fem->getInitialState()) << "The FemRepresentation already has an initial state";
+
+ m_state = std::make_shared<SurgSim::Math::OdeState>();
+}
+
+void FemPlyReaderDelegate::endParseFile()
+{
+ for (size_t i = 0; i < m_fem->getNumFemElements(); ++i)
+ {
+ m_fem->getFemElement(i)->setMassDensity(m_materialData.massDensity);
+ m_fem->getFemElement(i)->setPoissonRatio(m_materialData.poissonRatio);
+ m_fem->getFemElement(i)->setYoungModulus(m_materialData.youngModulus);
+ }
+
+ m_fem->setInitialState(m_state);
+}
+
+void* FemPlyReaderDelegate::beginVertices(const std::string& elementName, size_t vertexCount)
+{
+ m_state->setNumDof(m_fem->getNumDofPerNode(), vertexCount);
+ m_vertexIterator = m_state->getPositions().data();
+
+ return m_vertexData.data();
+}
+
+void FemPlyReaderDelegate::processVertex(const std::string& elementName)
+{
+ std::copy(std::begin(m_vertexData), std::end(m_vertexData), m_vertexIterator);
+ m_vertexIterator += m_fem->getNumDofPerNode();
+}
+
+void FemPlyReaderDelegate::endVertices(const std::string& elementName)
+{
+ m_vertexIterator = nullptr;
+}
+
+void* FemPlyReaderDelegate::beginFemElements(const std::string& elementName, size_t elementCount)
+{
+ m_femData.overrun = 0l;
+ return &m_femData;
+}
+
+void FemPlyReaderDelegate::endFemElements(const std::string& elementName)
+{
+ SURGSIM_ASSERT(m_femData.overrun == 0) <<
+ "There was an overrun while reading the element structures, it is likely that data " <<
+ "has become corrupted.";
+ m_femData.indices = nullptr;
+}
+
+void* FemPlyReaderDelegate::beginMaterials(const std::string& elementName, size_t materialCount)
+{
+ m_materialData.overrun = 0l;
+ return &m_materialData;
+}
+
+void FemPlyReaderDelegate::endMaterials(const std::string& elementName)
+{
+ SURGSIM_ASSERT(m_materialData.overrun == 0) <<
+ "There was an overrun while reading the material structures, it is likely that data " <<
+ "has become corrupted.";
+}
+
+void* FemPlyReaderDelegate::beginBoundaryConditions(const std::string& elementName,
+ size_t boundaryConditionCount)
+{
+ return &m_boundaryConditionData;
+}
+
+void FemPlyReaderDelegate::processBoundaryCondition(const std::string& elementName)
+{
+ m_state->addBoundaryCondition(m_boundaryConditionData);
+}
+
+} // namespace SurgSim
+} // namespace DataStructures
diff --git a/SurgSim/Physics/FemPlyReaderDelegate.h b/SurgSim/Physics/FemPlyReaderDelegate.h
new file mode 100644
index 0000000..bcc247e
--- /dev/null
+++ b/SurgSim/Physics/FemPlyReaderDelegate.h
@@ -0,0 +1,149 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2014, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEMPLYREADERDELEGATE_H
+#define SURGSIM_PHYSICS_FEMPLYREADERDELEGATE_H
+
+#include <array>
+#include <memory>
+
+#include "SurgSim/DataStructures/PlyReaderDelegate.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class OdeState;
+};
+
+namespace Physics
+{
+class FemRepresentation;
+
+/// Common part of implementation of PlyReaderDelegate for FemRepresentations.
+/// This is an abstract class and needs to be inherited.
+/// Methods 'registerDelegate()' and 'fileIsAcceptable()' need to be overridden.
+class FemPlyReaderDelegate : public SurgSim::DataStructures::PlyReaderDelegate
+{
+public:
+ /// Constructor
+ /// \param fem The object that is updated when PlyReader::parseFile is called.
+ explicit FemPlyReaderDelegate(std::shared_ptr<FemRepresentation> fem);
+
+protected:
+ // \return Name of the element (1/2/3D), which this delegate processes.
+ virtual std::string getElementName() const = 0;
+
+ virtual bool registerDelegate(SurgSim::DataStructures::PlyReader* reader) override;
+ virtual bool fileIsAcceptable(const SurgSim::DataStructures::PlyReader& reader) override;
+
+ /// Callback for beginning of PlyReader::parseFile.
+ void startParseFile();
+
+ /// Callback for end of PlyReader::parseFile.
+ virtual void endParseFile();
+
+ /// Callback function, begin the processing of vertices.
+ /// \param elementName Name of the element.
+ /// \param vertexCount Number of vertices.
+ /// \return memory for vertex data to the reader.
+ void* beginVertices(const std::string& elementName, size_t vertexCount);
+
+ /// Callback function to process one vertex.
+ /// \param elementName Name of the element.
+ void processVertex(const std::string& elementName);
+
+ /// Callback function to finalize processing of vertices.
+ /// \param elementName Name of the element.
+ void endVertices(const std::string& elementName);
+
+ /// Callback function, begin the processing of FemElements.
+ /// \param elementName Name of the element.
+ /// \param elementCount Number of elements.
+ /// \return memory for FemElement data to the reader.
+ void* beginFemElements(const std::string& elementName, size_t elementCount);
+
+ /// Callback function to process one FemElement.
+ /// \param elementName Name of the element.
+ virtual void processFemElement(const std::string& elementName) = 0;
+
+ /// Callback function to finalize processing of FemElements.
+ /// \param elementName Name of the element.
+ void endFemElements(const std::string& elementName);
+
+ /// Callback function, begin the processing of materials.
+ /// \param elementName Name of the element.
+ /// \param materialCount Number of materials.
+ /// \return memory for material data to the reader.
+ void* beginMaterials(const std::string& elementName, size_t materialCount);
+
+ /// Callback function, end the processing of materials.
+ /// \param elementName Name of the element.
+ void endMaterials(const std::string& elementName);
+
+ /// Callback function, begin the processing of boundary conditions.
+ /// \param elementName Name of the element.
+ /// \param boundaryConditionCount Number of boundary conditions.
+ /// \return memory for boundary conditions data to the reader.
+ void* beginBoundaryConditions(const std::string& elementName, size_t boundaryConditionCount);
+
+ /// Callback function to process one boundary condition.
+ /// \param elementName Name of the element.
+ void processBoundaryCondition(const std::string& elementName);
+
+protected:
+ /// Flag indicating if the associated file has boundary conditions
+ bool m_hasBoundaryConditions;
+
+ /// Internal data to receive the "boundary_condition" element
+ size_t m_boundaryConditionData;
+
+ /// Internal iterator to save the "vertex" element
+ double* m_vertexIterator;
+
+ /// Internal data to receive the "vertex" element
+ std::array<double, 3> m_vertexData;
+
+ /// The fem that will be created by loading
+ std::shared_ptr<FemRepresentation> m_fem;
+
+ /// The state that will be created by loading
+ std::shared_ptr<SurgSim::Math::OdeState> m_state;
+
+ /// Internal data to receive the "material" data
+ struct MaterialData
+ {
+ double massDensity;
+ double poissonRatio;
+ double youngModulus;
+ int64_t overrun; ///< Used to check for buffer overruns
+ } m_materialData;
+
+ /// Internal data to receive the fem element
+ struct ElementData
+ {
+ ElementData();
+
+ unsigned int* indices;
+ unsigned int vertexCount;
+ int64_t overrun; ///< Used to check for buffer overruns
+ } m_femData;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEMPLYREADERDELEGATE_H
\ No newline at end of file
diff --git a/SurgSim/Physics/FemRepresentation.cpp b/SurgSim/Physics/FemRepresentation.cpp
new file mode 100644
index 0000000..325f676
--- /dev/null
+++ b/SurgSim/Physics/FemRepresentation.cpp
@@ -0,0 +1,434 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/FemPlyReaderDelegate.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FemRepresentation::FemRepresentation(const std::string& name) :
+ DeformableRepresentation(name)
+{
+ m_rayleighDamping.massCoefficient = 0.0;
+ m_rayleighDamping.stiffnessCoefficient = 0.0;
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(FemRepresentation, std::string, Filename, getFilename, setFilename);
+}
+
+FemRepresentation::~FemRepresentation()
+{
+}
+
+void FemRepresentation::setFilename(const std::string& filename)
+{
+ m_filename = filename;
+}
+
+const std::string& FemRepresentation::getFilename() const
+{
+ return m_filename;
+}
+
+bool FemRepresentation::loadFile()
+{
+ using SurgSim::Framework::Logger;
+
+ bool result = true;
+ if (m_filename.empty())
+ {
+ SURGSIM_LOG_WARNING(Logger::getDefaultLogger()) << __FUNCTION__ << "Filename is empty";
+ result = false;
+ }
+ else
+ {
+ std::string filePath = getRuntime()->getApplicationData()->findFile(m_filename);
+ if (filePath.empty())
+ {
+ SURGSIM_LOG_WARNING(Logger::getDefaultLogger()) << __FUNCTION__ <<
+ "File " << m_filename << " can not be found.";
+ result = false;
+ }
+
+ auto reader = std::make_shared<SurgSim::DataStructures::PlyReader>(filePath);
+ if (result && !reader->isValid())
+ {
+ SURGSIM_LOG_WARNING(Logger::getDefaultLogger()) << __FUNCTION__ <<
+ "File " << m_filename << " is invalid.";
+ result = false;
+ }
+
+ if (result && !reader->parseWithDelegate(getDelegate()))
+ {
+ SURGSIM_LOG_WARNING(Logger::getDefaultLogger()) << __FUNCTION__ << "Failed to load file " << m_filename;
+ result = false;
+ }
+ }
+
+ return result;
+}
+
+bool FemRepresentation::doInitialize()
+{
+ if (!m_filename.empty() && !loadFile())
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__ <<
+ "Failed to initialize from file " << m_filename;
+ return false;
+ }
+
+ SURGSIM_ASSERT(m_initialState != nullptr) << "You must set the initial state before calling Initialize";
+
+ // Initialize the FemElements
+ for (auto element = std::begin(m_femElements); element != std::end(m_femElements); element++)
+ {
+ (*element)->initialize(*m_initialState);
+ }
+
+ // Allocate the vector m_massPerNode
+ if (m_massPerNode.size() == 0 || m_massPerNode.size() < m_initialState->getNumNodes())
+ {
+ m_massPerNode.resize(m_initialState->getNumNodes());
+ }
+
+ // Compute the entries of m_massPerNode from the FemElements mass information
+ for (auto element = std::begin(m_femElements); element != std::end(m_femElements); element++)
+ {
+ double mass = (*element)->getMass(*m_initialState);
+ for (auto nodeId = std::begin((*element)->getNodeIds()); nodeId != std::end((*element)->getNodeIds()); nodeId++)
+ {
+ m_massPerNode[*nodeId] += mass / (*element)->getNumNodes();
+ }
+ }
+
+ return true;
+}
+
+void FemRepresentation::addFemElement(const std::shared_ptr<FemElement> femElement)
+{
+ m_femElements.push_back(femElement);
+}
+
+size_t FemRepresentation::getNumFemElements() const
+{
+ return m_femElements.size();
+}
+
+std::shared_ptr<FemElement> FemRepresentation::getFemElement(size_t femElementId)
+{
+ SURGSIM_ASSERT(femElementId < getNumFemElements()) << "Invalid femElement id";
+ return m_femElements[femElementId];
+}
+
+bool FemRepresentation::isValidCoordinate(const SurgSim::DataStructures::IndexedLocalCoordinate& coordinate) const
+{
+ return (coordinate.index < m_femElements.size())
+ && m_femElements[coordinate.index]->isValidCoordinate(coordinate.coordinate);
+}
+
+double FemRepresentation::getTotalMass() const
+{
+ double mass = 0.0;
+ for (auto it = std::begin(m_femElements); it != std::end(m_femElements); it++)
+ {
+ mass += (*it)->getMass(*m_currentState);
+ }
+ return mass;
+}
+
+double FemRepresentation::getRayleighDampingStiffness() const
+{
+ return m_rayleighDamping.stiffnessCoefficient;
+}
+
+double FemRepresentation::getRayleighDampingMass() const
+{
+ return m_rayleighDamping.massCoefficient;
+}
+
+void FemRepresentation::setRayleighDampingStiffness(double stiffnessCoef)
+{
+ m_rayleighDamping.stiffnessCoefficient = stiffnessCoef;
+}
+
+void FemRepresentation::setRayleighDampingMass(double massCoef)
+{
+ m_rayleighDamping.massCoefficient = massCoef;
+}
+
+void FemRepresentation::beforeUpdate(double dt)
+{
+ DeformableRepresentation::beforeUpdate(dt);
+
+ SURGSIM_ASSERT(getNumFemElements())
+ << "No fem element specified yet, call addFemElement() prior to running the simulation";
+ SURGSIM_ASSERT(getNumDof())
+ << "State has not been initialized yet, call setInitialState() prior to running the simulation";
+}
+
+void FemRepresentation::afterUpdate(double dt)
+{
+ DeformableRepresentation::afterUpdate(dt);
+
+ // Update the elements with the final state
+ std::for_each(m_femElements.begin(), m_femElements.end(),
+ [this](std::shared_ptr<FemElement> element)
+ {
+ if (!element->update(*m_finalState))
+ {
+ SURGSIM_LOG(SurgSim::Framework::Logger::getDefaultLogger(), DEBUG)
+ << getName() << " deactivated :" << std::endl
+ << "position=(" << m_currentState->getPositions().transpose() << ")" << std::endl
+ << "velocity=(" << m_currentState->getVelocities().transpose() << ")" << std::endl;
+
+ setLocalActive(false);
+ return;
+ }
+ }
+ );
+}
+
+SurgSim::Math::Vector& FemRepresentation::computeF(const SurgSim::Math::OdeState& state)
+{
+ // Make sure the force vector has been properly allocated and zeroed out
+ m_f.resize(state.getNumDof());
+ m_f.setZero();
+
+ addGravityForce(&m_f, state);
+ addRayleighDampingForce(&m_f, state);
+ addFemElementsForce(&m_f, state);
+
+ // Add external generalized force
+ m_f += m_externalGeneralizedForce;
+
+ return m_f;
+}
+
+const SurgSim::Math::Matrix& FemRepresentation::computeM(const SurgSim::Math::OdeState& state)
+{
+ // Make sure the mass matrix has been properly allocated and zeroed out
+ m_M.resize(state.getNumDof(), state.getNumDof());
+ m_M.setZero();
+
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ (*femElement)->addMass(state, &m_M);
+ }
+
+ return m_M;
+}
+
+const SurgSim::Math::Matrix& FemRepresentation::computeD(const SurgSim::Math::OdeState& state)
+{
+ const double& rayleighStiffness = m_rayleighDamping.stiffnessCoefficient;
+ const double& rayleighMass = m_rayleighDamping.massCoefficient;
+
+ // Make sure the damping matrix has been properly allocated and zeroed out
+ m_D.resize(state.getNumDof(), state.getNumDof());
+ m_D.setZero();
+
+ // D += rayleighMass.M
+ if (rayleighMass != 0.0)
+ {
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ (*femElement)->addMass(state, &m_D, rayleighMass);
+ }
+ }
+
+ // D += rayleighStiffness.K
+ if (rayleighStiffness != 0.0)
+ {
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ (*femElement)->addStiffness(state, &m_D, rayleighStiffness);
+ }
+ }
+
+ // D += FemElements damping matrix
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ (*femElement)->addDamping(state, &m_D);
+ }
+
+ // Add external generalized damping
+ m_D += m_externalGeneralizedDamping;
+
+ return m_D;
+}
+
+const SurgSim::Math::Matrix& FemRepresentation::computeK(const SurgSim::Math::OdeState& state)
+{
+ // Make sure the stiffness matrix has been properly allocated and zeroed out
+ m_K.resize(state.getNumDof(), state.getNumDof());
+ m_K.setZero();
+
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ (*femElement)->addStiffness(state, &m_K);
+ }
+
+ // Add external generalized stiffness
+ m_K += m_externalGeneralizedStiffness;
+
+ return m_K;
+}
+
+void FemRepresentation::computeFMDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector** f,
+ SurgSim::Math::Matrix** M, SurgSim::Math::Matrix** D, SurgSim::Math::Matrix** K)
+{
+ // Make sure the force vector has been properly allocated and zeroed out
+ m_f.resize(state.getNumDof());
+ m_f.setZero();
+
+ // Make sure the mass matrix has been properly allocated and zeroed out
+ m_M.resize(state.getNumDof(), state.getNumDof());
+ m_M.setZero();
+
+ // Make sure the damping matrix has been properly allocated and zeroed out
+ m_D.resize(state.getNumDof(), state.getNumDof());
+ m_D.setZero();
+
+ // Make sure the stiffness matrix has been properly allocated and zeroed out
+ m_K.resize(state.getNumDof(), state.getNumDof());
+ m_K.setZero();
+
+ // Add all the FemElement contribution to f, M, D, K
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ (*femElement)->addFMDK(state, &m_f, &m_M, &m_D, &m_K);
+ }
+
+ // Add the Rayleigh damping matrix
+ if (m_rayleighDamping.massCoefficient)
+ {
+ m_D += m_M * m_rayleighDamping.massCoefficient;
+ }
+ if (m_rayleighDamping.stiffnessCoefficient)
+ {
+ m_D += m_K * m_rayleighDamping.stiffnessCoefficient;
+ }
+
+ // Add the gravity to m_f
+ addGravityForce(&m_f, state);
+
+ // Add the Rayleigh damping force to m_f
+ addRayleighDampingForce(&m_f, state, true, true);
+
+ // Add external generalized force, stiffness and damping
+ m_f += m_externalGeneralizedForce;
+ m_K += m_externalGeneralizedStiffness;
+ m_D += m_externalGeneralizedDamping;
+
+ *f = &m_f;
+ *M = &m_M;
+ *D = &m_D;
+ *K = &m_K;
+}
+
+void FemRepresentation::addRayleighDampingForce(
+ SurgSim::Math::Vector* force, const SurgSim::Math::OdeState& state,
+ bool useGlobalStiffnessMatrix, bool useGlobalMassMatrix, double scale)
+{
+ // Temporary variables for convenience
+ double& rayleighMass = m_rayleighDamping.massCoefficient;
+ double& rayleighStiffness = m_rayleighDamping.stiffnessCoefficient;
+ const SurgSim::Math::Vector& v = state.getVelocities();
+
+ // Rayleigh damping mass: F = -rayleighMass.M.v(t)
+ if (rayleighMass != 0.0)
+ {
+ // If we have the mass matrix, we can compute directly F = -rayleighMass.M.v(t)
+ if (useGlobalMassMatrix)
+ {
+ *force -= (scale * rayleighMass) * (m_M * v);
+ }
+ else
+ {
+ // Otherwise, we loop through each fem element to compute its contribution
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ (*femElement)->addMatVec(state, - scale * rayleighMass, 0.0, 0.0, v, force);
+ }
+ }
+ }
+
+ // Rayleigh damping stiffness: F = - rayleighStiffness.K.v(t)
+ // K is not diagonal and links all dof of the N connected nodes
+ if (rayleighStiffness != 0.0)
+ {
+ if (useGlobalStiffnessMatrix)
+ {
+ *force -= scale * rayleighStiffness * (m_K * v);
+ }
+ else
+ {
+ // Otherwise, we loop through each fem element to compute its contribution
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ (*femElement)->addMatVec(state, 0.0, 0.0, - scale * rayleighStiffness, v, force);
+ }
+ }
+ }
+}
+
+void FemRepresentation::addFemElementsForce(SurgSim::Math::Vector* force,
+ const SurgSim::Math::OdeState& state,
+ double scale)
+{
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ (*femElement)->addForce(state, force, scale);
+ }
+}
+
+void FemRepresentation::addGravityForce(SurgSim::Math::Vector* f,
+ const SurgSim::Math::OdeState& state,
+ double scale)
+{
+ using SurgSim::Math::addSubVector;
+
+ SURGSIM_ASSERT(m_massPerNode.size() == state.getNumNodes()) <<
+ "Mass per node has not been properly allocated. Did you call Initialize() ?";
+
+ // Prepare a gravity vector of the proper size
+ SurgSim::Math::Vector gravitynD = SurgSim::Math::Vector::Zero(getNumDofPerNode());
+ gravitynD.segment(0, 3) = getGravity();
+
+ if (isGravityEnabled())
+ {
+ for (size_t nodeId = 0; nodeId < state.getNumNodes(); nodeId++)
+ {
+ // F = mg
+ addSubVector(gravitynD * (scale * m_massPerNode[nodeId]), nodeId, getNumDofPerNode(), f);
+ }
+ }
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/FemRepresentation.h b/SurgSim/Physics/FemRepresentation.h
new file mode 100644
index 0000000..a42572e
--- /dev/null
+++ b/SurgSim/Physics/FemRepresentation.h
@@ -0,0 +1,202 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEMREPRESENTATION_H
+#define SURGSIM_PHYSICS_FEMREPRESENTATION_H
+
+#include <memory>
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/DeformableRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+class FemElement;
+class FemPlyReaderDelegate;
+
+/// Finite Element Model (a.k.a. fem) is a deformable model (a set of nodes connected by FemElement).
+/// \note A fem is a DeformableRepresentation (Physics::Representation and Math::OdeEquation)
+/// \note Therefore, it defines a dynamic system M.a=F(x,v)
+/// \note The model handles damping through the Rayleigh damping (where damping is a combination of mass and stiffness)
+class FemRepresentation : public DeformableRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name of the FemRepresentation
+ explicit FemRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~FemRepresentation();
+
+ /// Sets the name of the file to be loaded
+ /// \param filename The name of the file to be loaded
+ void setFilename(const std::string& filename);
+
+ /// Gets the name of the file to be loaded
+ /// \return filename The name of the file to be loaded
+ const std::string& getFilename() const;
+
+ /// Loads the file
+ /// \return true if successful
+ bool loadFile();
+
+ /// Adds a FemElement
+ /// \param element The FemElement to add to the representation
+ void addFemElement(const std::shared_ptr<FemElement> element);
+
+ /// Gets the number of FemElement
+ /// \return the number of FemElement
+ size_t getNumFemElements() const;
+
+ /// Retrieves a given FemElement from its id
+ /// \param femElementId The FemElement id for which the FemElement is requested
+ /// \return The FemElement for the given femElementId
+ /// \note The FemElement is returned with read/write access
+ /// \note Out of range femElementId will raise an exception
+ std::shared_ptr<FemElement> getFemElement(size_t femElementId);
+
+ /// Gets the total mass of the fem
+ /// \return The total mass of the fem (in Kg)
+ double getTotalMass() const;
+
+ /// Gets the Rayleigh stiffness parameter
+ /// \return The Rayleigh stiffness parameter
+ double getRayleighDampingStiffness() const;
+
+ /// Gets the Rayleigh mass parameter
+ /// \return The Rayleigh mass parameter
+ double getRayleighDampingMass() const;
+
+ /// Sets the Rayleigh stiffness parameter
+ /// \param stiffnessCoef The Rayleigh stiffness parameter
+ void setRayleighDampingStiffness(double stiffnessCoef);
+
+ /// Sets the Rayleigh mass parameter
+ /// \param massCoef The Rayleigh mass parameter
+ void setRayleighDampingMass(double massCoef);
+
+ /// Determines whether the associated coordinate is valid
+ /// \param coordinate Coordinate to check
+ /// \return True if coordinate is valid
+ bool isValidCoordinate(const SurgSim::DataStructures::IndexedLocalCoordinate &coordinate) const;
+
+ /// Preprocessing done before the update call
+ /// \param dt The time step (in seconds)
+ virtual void beforeUpdate(double dt) override;
+
+ /// Postprocessing done after the update call
+ /// \param dt The time step (in seconds)
+ /// \note This method will update all FemElement with the final state
+ /// \note and potentially deactivate/reset the representation if necessary.
+ virtual void afterUpdate(double dt) override;
+
+ /// Evaluation of the RHS function f(x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the function f(x,v) with
+ /// \return The vector containing f(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeF() or computeFMDK()
+ virtual SurgSim::Math::Vector& computeF(const SurgSim::Math::OdeState& state) override;
+
+ /// Evaluation of the LHS matrix M(x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the matrix M(x,v) with
+ /// \return The matrix M(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeM() or computeFMDK()
+ virtual const SurgSim::Math::Matrix& computeM(const SurgSim::Math::OdeState& state) override;
+
+ /// Evaluation of D = -df/dv (x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the Jacobian matrix with
+ /// \return The matrix D = -df/dv(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeD() or computeFMDK()
+ virtual const SurgSim::Math::Matrix& computeD(const SurgSim::Math::OdeState& state) override;
+
+ /// Evaluation of K = -df/dx (x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the Jacobian matrix with
+ /// \return The matrix K = -df/dx(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeK() or computeFMDK()
+ virtual const SurgSim::Math::Matrix& computeK(const SurgSim::Math::OdeState& state) override;
+
+ /// Evaluation of f(x,v), M(x,v), D = -df/dv(x,v), K = -df/dx(x,v)
+ /// When all the terms are needed, this method can perform optimization in evaluating everything together
+ /// \param state (x, v) the current position and velocity to evaluate the various terms with
+ /// \param[out] f The RHS f(x,v)
+ /// \param[out] M The matrix M(x,v)
+ /// \param[out] D The matrix D = -df/dv(x,v)
+ /// \param[out] K The matrix K = -df/dx(x,v)
+ /// \note Returns pointers, the internal data will remain unchanged until the next call to computeFMDK() or
+ /// \note computeF(), computeM(), computeD(), computeK()
+ virtual void computeFMDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector** f,
+ SurgSim::Math::Matrix** M, SurgSim::Math::Matrix** D, SurgSim::Math::Matrix** K) override;
+
+protected:
+ /// Adds the Rayleigh damping forces
+ /// \param[in,out] f The force vector to cumulate the Rayleigh damping force into
+ /// \param state The state vector containing positions and velocities
+ /// \param useGlobalMassMatrix, useGlobalStiffnessMatrix True indicates that the global mass and stiffness matrices
+ /// should be used (F = -c.M.v - d.K.v)
+ /// \param scale A scaling factor to apply on the damping force
+ /// \note Damping matrix D = c.M + d.K (Rayleigh damping definition)
+ /// \note F = - D.v = -c.M.v - d.K.v
+ /// \note If {useGlobalMassMatrix | useGlobalStiffnessMatrix} is True, {M | K} will be used
+ /// \note If {useGlobalMassMatrix | useGlobalStiffnessMatrix} is False
+ /// \note the {mass|stiffness} component will be computed FemElement by FemElement
+ void addRayleighDampingForce(SurgSim::Math::Vector* f, const SurgSim::Math::OdeState& state,
+ bool useGlobalMassMatrix = false, bool useGlobalStiffnessMatrix = false, double scale = 1.0);
+
+ /// Adds the FemElements forces to f (given a state)
+ /// \param[in,out] f The force vector to cumulate the FemElements forces into
+ /// \param state The state vector containing positions and velocities
+ /// \param scale A scaling factor to scale the FemElements forces with
+ void addFemElementsForce(SurgSim::Math::Vector* f, const SurgSim::Math::OdeState& state, double scale = 1.0);
+
+ /// Adds the gravity force to f (given a state)
+ /// \param[in,out] f The force vector to cumulate the gravity force into
+ /// \param state The state vector containing positions and velocities
+ /// \param scale A scaling factor to scale the gravity force with
+ /// \note This method does not do anything if gravity is disabled
+ void addGravityForce(SurgSim::Math::Vector *f, const SurgSim::Math::OdeState& state, double scale = 1.0);
+
+ virtual bool doInitialize() override;
+
+ /// Useful information per node
+ std::vector<double> m_massPerNode; //< Useful in setting up the gravity force F=mg
+
+ /// Filename for loading the fem representation.
+ std::string m_filename;
+
+private:
+ /// To be implemented by derived classes.
+ /// \return The delegate to load the corresponding derived class.
+ virtual std::shared_ptr<FemPlyReaderDelegate> getDelegate() = 0;
+
+ /// FemElements
+ std::vector<std::shared_ptr<FemElement>> m_femElements;
+
+ /// Rayleigh damping parameters (massCoefficient and stiffnessCoefficient)
+ /// D = massCoefficient.M + stiffnessCoefficient.K
+ /// Matrices: D = damping, M = mass, K = stiffness
+ struct {
+ double massCoefficient;
+ double stiffnessCoefficient;
+ } m_rayleighDamping;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEMREPRESENTATION_H
diff --git a/SurgSim/Physics/FemRepresentationParameters.cpp b/SurgSim/Physics/FemRepresentationParameters.cpp
new file mode 100644
index 0000000..8629325
--- /dev/null
+++ b/SurgSim/Physics/FemRepresentationParameters.cpp
@@ -0,0 +1,198 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/FemRepresentationParameters.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FemRepresentationParameters::FemRepresentationParameters()
+ : m_boundaryConditionsMass(0.0), m_boundaryConditionsInverseMass(0.0),
+ m_rho(0.0), m_rayleighDampingMass(0.0), m_rayleighDampingStiffness(0.0),
+ m_youngModulus(0.0), m_poissonRatio(0.0), m_isValid(false)
+{
+}
+
+FemRepresentationParameters::~FemRepresentationParameters()
+{
+}
+
+bool FemRepresentationParameters::operator ==(const FemRepresentationParameters &p) const
+{
+ return ( m_boundaryConditions == p.m_boundaryConditions &&
+ m_boundaryConditionsMass == p.m_boundaryConditionsMass &&
+ m_boundaryConditionsInverseMass == p.m_boundaryConditionsInverseMass &&
+ m_rho == p.m_rho &&
+ m_rayleighDampingMass == p.m_rayleighDampingMass &&
+ m_rayleighDampingStiffness == p.m_rayleighDampingStiffness &&
+ m_youngModulus == p.m_youngModulus &&
+ m_poissonRatio == p.m_poissonRatio &&
+ m_isValid == p.m_isValid);
+}
+
+bool FemRepresentationParameters::operator !=(const FemRepresentationParameters &p) const
+{
+ return ! ((*this) == p);
+}
+
+bool FemRepresentationParameters::addBoundaryCondition(size_t nodeId)
+{
+ auto found = std::find(m_boundaryConditions.begin(), m_boundaryConditions.end(), nodeId);
+ if (found == m_boundaryConditions.end())
+ {
+ m_boundaryConditions.push_back(nodeId);
+ return true;
+ }
+ return false;
+}
+
+bool FemRepresentationParameters::removeBoundaryCondition(size_t nodeId)
+{
+ auto found = std::find(m_boundaryConditions.begin(), m_boundaryConditions.end(), nodeId);
+ if (found != m_boundaryConditions.end())
+ {
+ m_boundaryConditions.erase(found);
+ return true;
+ }
+ return false;
+}
+
+size_t FemRepresentationParameters::addBoundaryConditions(const std::vector<size_t>& boundaryConditions)
+{
+ size_t count = 0u;
+
+ for(auto it = boundaryConditions.begin(); it != boundaryConditions.end(); it++)
+ {
+ auto found = std::find(m_boundaryConditions.begin(), m_boundaryConditions.end(), *it);
+ if (found == m_boundaryConditions.end())
+ {
+ m_boundaryConditions.push_back(*it);
+ count++;
+ }
+ }
+ return count;
+}
+
+void FemRepresentationParameters::clearBoundaryConditions()
+{
+ m_boundaryConditions.clear();
+}
+
+const std::vector<size_t>& FemRepresentationParameters::getBoundaryConditions() const
+{
+ return m_boundaryConditions;
+}
+
+void FemRepresentationParameters::setBoundaryConditionMass(double mass)
+{
+ m_boundaryConditionsMass = mass;
+}
+
+double FemRepresentationParameters::getBoundaryConditionMass() const
+{
+ return m_boundaryConditionsMass;
+}
+
+void FemRepresentationParameters::setBoundaryConditionInverseMass(double invMass)
+{
+ m_boundaryConditionsInverseMass = invMass;
+}
+
+double FemRepresentationParameters::getBoundaryConditionInverseMass() const
+{
+ return m_boundaryConditionsInverseMass;
+}
+
+void FemRepresentationParameters::setDensity(double rho)
+{
+ m_rho = rho;
+ checkValidity();
+}
+
+double FemRepresentationParameters::getDensity() const
+{
+ return m_rho;
+}
+
+void FemRepresentationParameters::setRayleighDampingMass(double massCoef)
+{
+ m_rayleighDampingMass = massCoef;
+ checkValidity();
+}
+
+double FemRepresentationParameters::getRayleighDampingMass() const
+{
+ return m_rayleighDampingMass;
+}
+
+void FemRepresentationParameters::setRayleighDampingStiffness(double stiffnessCoef)
+{
+ m_rayleighDampingStiffness = stiffnessCoef;
+ checkValidity();
+}
+
+double FemRepresentationParameters::getRayleighDampingStiffness() const
+{
+ return m_rayleighDampingStiffness;
+}
+
+void FemRepresentationParameters::setYoungModulus(double E)
+{
+ m_youngModulus = E;
+ checkValidity();
+}
+
+double FemRepresentationParameters::getYoungModulus() const
+{
+ return m_youngModulus;
+}
+
+void FemRepresentationParameters::setPoissonRatio(double nu)
+{
+ m_poissonRatio = nu;
+ checkValidity();
+}
+
+double FemRepresentationParameters::getPoissonRatio() const
+{
+ return m_poissonRatio;
+}
+
+bool FemRepresentationParameters::isValid() const
+{
+ return m_isValid;
+}
+
+void FemRepresentationParameters::checkValidity()
+{
+ // Valid if mass density and Young modulus are strictly positive and
+ // Poisson ratio in valid range and Rayleigh parameters positives or nulls
+ if (m_rho > 0.0 && m_youngModulus > 0.0 && m_poissonRatio > -1.0 && m_poissonRatio < 0.5 &&
+ m_rayleighDampingMass >= 0.0 && m_rayleighDampingStiffness >= 0.0)
+ {
+ m_isValid = true;
+ }
+ else
+ {
+ m_isValid = false;
+ }
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FemRepresentationParameters.h b/SurgSim/Physics/FemRepresentationParameters.h
new file mode 100644
index 0000000..ab7386a
--- /dev/null
+++ b/SurgSim/Physics/FemRepresentationParameters.h
@@ -0,0 +1,171 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FEMREPRESENTATIONPARAMETERS_H
+#define SURGSIM_PHYSICS_FEMREPRESENTATIONPARAMETERS_H
+
+#include <algorithm>
+#include <vector>
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// The FemRepresentationParameters class defines the physical parameters for all Finite Element Model (1D, 2D, 3D)
+class FemRepresentationParameters
+{
+public:
+ /// Default constructor
+ FemRepresentationParameters();
+
+ /// Destructor
+ virtual ~FemRepresentationParameters();
+
+ /// Comparison operator (equality test)
+ /// \param p A FemRepresentationParameters to compare it to
+ /// \return True if the 2 parameters set are equals, False otherwise
+ bool operator ==(const FemRepresentationParameters &p) const;
+
+ /// Comparison operator (difference test)
+ /// \param p A FemRepresentationParameters to compare it to
+ /// \return False if the 2 parameters set are equals, True otherwise
+ bool operator !=(const FemRepresentationParameters &p) const;
+
+ /// Add a boundary condition
+ /// \param nodeId The nodeId of the Fem to be fixed
+ /// \return True if the boundary condition has been added, False otherwise
+ bool addBoundaryCondition(size_t nodeId);
+
+ /// Remove a boundary condition
+ /// \param nodeId The nodeId of the Fem to be removed from the boundary conditions list
+ /// \return True if the boundary condition has been removed, False otherwise
+ bool removeBoundaryCondition(size_t nodeId);
+
+ /// Add boundary conditions
+ /// \param boundaryConditions The vector of all boundary conditions to be added (nodeIdx)
+ /// \return The number of boundary conditions actually added
+ size_t addBoundaryConditions(const std::vector<size_t>& boundaryConditions);
+
+ /// Remove all boundary conditions
+ void clearBoundaryConditions();
+
+ /// Get all boundary conditions
+ /// \return The vector of all boundary conditions (nodeIds)
+ const std::vector<size_t>& getBoundaryConditions() const;
+
+ /// Set the boundary condition mass property
+ /// \param mass The mass to be assigned to boundary condition nodes
+ void setBoundaryConditionMass(double mass);
+
+ /// Get the boundary condition mass property
+ /// \return The mass assigned to boundary condition nodes
+ double getBoundaryConditionMass() const;
+
+ /// Set the boundary condition inverse mass property
+ /// \param invMass The inverse mass to be assigned to boundary condition nodes
+ void setBoundaryConditionInverseMass(double invMass);
+
+ /// Get the boundary condition inverse mass property
+ /// \return The inverse mass assigned to boundary condition nodes
+ double getBoundaryConditionInverseMass() const;
+
+ /// Set the mass density of the fem
+ /// \param rho The mass density (in Kg.m-3)
+ void setDensity(double rho);
+
+ /// Get the mass density of the fem
+ /// \return The density if it has been provided, 0 otherwise (in Kg.m-3)
+ double getDensity() const;
+
+ /// Set the Rayleigh damping mass parameter
+ /// \param massCoef The Rayleigh damping mass parameter (in s-1)
+ void setRayleighDampingMass(double massCoef);
+
+ /// Get the Rayleigh damping mass parameter
+ /// \return The Rayleigh damping mass parameter (in s-1)
+ double getRayleighDampingMass() const;
+
+ /// Set the Rayleigh damping stiffness parameter
+ /// \param stiffnessCoef The Rayleigh damping stiffness parameter (in s)
+ void setRayleighDampingStiffness(double stiffnessCoef);
+
+ /// Get the Rayleigh damping stiffness parameter
+ /// \return The Rayleigh damping stiffness parameter (in s)
+ double getRayleighDampingStiffness() const;
+
+ /// Set the Young modulus of the material
+ /// \param E The Young modulus of the material (in N.m-2)
+ void setYoungModulus(double E);
+
+ /// Get the material Young modulus
+ /// \return The Young modulus of the material (in N.m-2)
+ double getYoungModulus() const;
+
+ /// Set the Poisson ratio of the material
+ /// \param nu The Poisson ratio of the material (unitless)
+ void setPoissonRatio(double nu);
+
+ /// Get the material Poisson ratio
+ /// \return The Poisson ratio of the material (unitless)
+ double getPoissonRatio() const;
+
+ /// Test if the the parameters are fully set and ready
+ /// \return True if the set of parameters is valid
+ /// \note Valid if mass density and Young modulus strictly positive
+ /// \note and Poisson ratio in ]-1, 0.5[ and both Rayleigh parameters positive or null
+ bool isValid() const;
+
+private:
+ /// Check the validity of the parameters and set the flag m_isValid accordingly
+ void checkValidity();
+
+ /// Boundary conditions (vector of node indices to fix)
+ std::vector<size_t> m_boundaryConditions;
+
+ /// Boundary conditions mass property (useful to build the system matrix)
+ double m_boundaryConditionsMass;
+
+ /// Boundary conditions mass property (useful to build the system matrix inverse)
+ /// Note that m_boundaryConditionsInverseMass can be different than 1.0/m_boundaryConditionsMass
+ double m_boundaryConditionsInverseMass;
+
+ /// Density of the object (in Kg.m-3)
+ double m_rho;
+
+ /// Rayleigh damping, mass parameter (in s-1)
+ double m_rayleighDampingMass;
+
+ /// Rayleigh damping, stiffness parameter (in s)
+ double m_rayleighDampingStiffness;
+
+ /// Young modulus (in N.m-2 or Pa or Kg.m-1.s-2)
+ double m_youngModulus;
+
+ /// Poisson ratio (unit less)
+ /// Theoretically within (-1, 0.5) with 0.5 for incompressible material
+ /// In general within [ 0, 0.5)
+ double m_poissonRatio;
+
+ /// Validity of the set of parameters
+ bool m_isValid;
+};
+
+}; // Physics
+
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_FEMREPRESENTATIONPARAMETERS_H
diff --git a/SurgSim/Physics/FixedRepresentation.cpp b/SurgSim/Physics/FixedRepresentation.cpp
new file mode 100644
index 0000000..b055e9d
--- /dev/null
+++ b/SurgSim/Physics/FixedRepresentation.cpp
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/FixedRepresentation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::FixedRepresentation, FixedRepresentation);
+
+FixedRepresentation::FixedRepresentation(const std::string& name) :
+ RigidRepresentationBase(name)
+{
+}
+
+FixedRepresentation::~FixedRepresentation()
+{
+}
+
+
+RepresentationType FixedRepresentation::getType() const
+{
+ return REPRESENTATION_TYPE_FIXED;
+}
+
+void FixedRepresentation::updateGlobalInertiaMatrices(const RigidRepresentationState& state)
+{
+ // Do Nothing it is a fixed object
+}
+
+void FixedRepresentation::update(double dt)
+{
+ m_currentState.setPose(getPose());
+}
+
+}; // Physics
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Physics/FixedRepresentation.h b/SurgSim/Physics/FixedRepresentation.h
new file mode 100644
index 0000000..4a38126
--- /dev/null
+++ b/SurgSim/Physics/FixedRepresentation.h
@@ -0,0 +1,55 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FIXEDREPRESENTATION_H
+#define SURGSIM_PHYSICS_FIXEDREPRESENTATION_H
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Physics/RigidRepresentationBase.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+class RigidRepresentationState;
+
+SURGSIM_STATIC_REGISTRATION(FixedRepresentation);
+
+/// The FixedRepresentation class represents a physics entity without any motion nor
+/// compliance against which others physics entities can interact
+class FixedRepresentation : public RigidRepresentationBase
+{
+public:
+ /// Constructor
+ /// \param name The fixed representation's name
+ explicit FixedRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~FixedRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::FixedRepresentation);
+
+ virtual RepresentationType getType() const override;
+
+ virtual void updateGlobalInertiaMatrices(const RigidRepresentationState& state) override;
+
+ virtual void update(double dt) override;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_FIXEDREPRESENTATION_H
diff --git a/SurgSim/Physics/FixedRepresentationBilateral3D.cpp b/SurgSim/Physics/FixedRepresentationBilateral3D.cpp
new file mode 100644
index 0000000..14866de
--- /dev/null
+++ b/SurgSim/Physics/FixedRepresentationBilateral3D.cpp
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/FixedRepresentationBilateral3D.h"
+#include "SurgSim/Physics/FixedRepresentationLocalization.h"
+#include "SurgSim/Physics/Localization.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FixedRepresentationBilateral3D::FixedRepresentationBilateral3D()
+{
+}
+
+FixedRepresentationBilateral3D::~FixedRepresentationBilateral3D()
+{
+}
+
+void FixedRepresentationBilateral3D::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+ std::shared_ptr<Representation> representation = localization->getRepresentation();
+
+ if (!representation->isActive())
+ {
+ return;
+ }
+
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+
+ Vector3d globalPosition = localization->calculatePosition();
+
+ // Fill up b with the constraint equation...
+ mlcp->b.segment<3>(indexOfConstraint) += globalPosition * scale;
+}
+
+SurgSim::Math::MlcpConstraintType FixedRepresentationBilateral3D::getMlcpConstraintType() const
+{
+ return SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT;
+}
+
+SurgSim::Physics::RepresentationType FixedRepresentationBilateral3D::getRepresentationType() const
+{
+ return REPRESENTATION_TYPE_FIXED;
+}
+
+size_t FixedRepresentationBilateral3D::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FixedRepresentationBilateral3D.h b/SurgSim/Physics/FixedRepresentationBilateral3D.h
new file mode 100644
index 0000000..bbd69be
--- /dev/null
+++ b/SurgSim/Physics/FixedRepresentationBilateral3D.h
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FIXEDREPRESENTATIONBILATERAL3D_H
+#define SURGSIM_PHYSICS_FIXEDREPRESENTATIONBILATERAL3D_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// FixedRepresentation bilateral 3d constraint implementation.
+///
+/// The family of bilateral3D constraints enforce equality between two points.
+class FixedRepresentationBilateral3D : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ FixedRepresentationBilateral3D();
+
+ /// Destructor
+ virtual ~FixedRepresentationBilateral3D();
+
+ /// Gets the Mixed Linear Complementarity Problem constraint type for this ConstraintImplementation
+ /// \return The MLCP constraint type corresponding to this constraint implementation
+ SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+
+ /// Gets the Type of representation that this implementation is concerned with
+ /// \return RepresentationType for this implementation
+ virtual RepresentationType getRepresentationType() const override;
+
+private:
+ /// Gets the number of degree of freedom.
+ /// \return 3 A bilateral 3d constraint enforces equality in the x, y, and z dimensions between 2 points.
+ size_t doGetNumDof() const override;
+
+ /// Builds the subset of an Mlcp physics problem associated to this implementation.
+ /// \param dt The time step.
+ /// \param data The data associated to the constraint.
+ /// \param localization The localization for the representation.
+ /// \param [in, out] mlcp The Mixed LCP physics problem to fill up.
+ /// \param indexOfRepresentation The index of the representation (associated to this implementation) in the mlcp.
+ /// \param indexOfConstraint The index of the constraint in the mlcp.
+ /// \param sign The sign of this implementation in the constraint (positive or negative side).
+ /// \note Empty for a Fixed Representation
+ void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) override;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FIXEDREPRESENTATIONBILATERAL3D_H
diff --git a/SurgSim/Physics/FixedRepresentationContact.cpp b/SurgSim/Physics/FixedRepresentationContact.cpp
new file mode 100644
index 0000000..3cd6db7
--- /dev/null
+++ b/SurgSim/Physics/FixedRepresentationContact.cpp
@@ -0,0 +1,84 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/FixedRepresentationContact.h"
+#include "SurgSim/Physics/FixedRepresentationLocalization.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FixedRepresentationContact::FixedRepresentationContact()
+{
+}
+
+FixedRepresentationContact::~FixedRepresentationContact()
+{
+}
+
+void FixedRepresentationContact::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+ MlcpPhysicsProblem::Vector& b = mlcp->b;
+
+ std::shared_ptr<Representation> representation = localization->getRepresentation();
+ std::shared_ptr<FixedRepresentation> fixed = std::static_pointer_cast<FixedRepresentation>(representation);
+
+ if (! fixed->isActive())
+ {
+ return;
+ }
+
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE ? 1.0 : -1.0);
+ const ContactConstraintData& contactData = static_cast<const ContactConstraintData&>(data);
+ const SurgSim::Math::Vector3d& n = contactData.getNormal();
+ const double d = contactData.getDistance();
+
+ SurgSim::Math::Vector3d globalPosition = localization->calculatePosition();
+
+ // Fill up b with the constraint equation...
+ double violation = n.dot(globalPosition) + d;
+ b[indexOfConstraint] += violation * scale;
+}
+
+SurgSim::Math::MlcpConstraintType FixedRepresentationContact::getMlcpConstraintType() const
+{
+ return SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
+}
+
+SurgSim::Physics::RepresentationType FixedRepresentationContact::getRepresentationType() const
+{
+ return REPRESENTATION_TYPE_FIXED;
+}
+
+size_t FixedRepresentationContact::doGetNumDof() const
+{
+ return 1;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FixedRepresentationContact.h b/SurgSim/Physics/FixedRepresentationContact.h
new file mode 100644
index 0000000..a98f026
--- /dev/null
+++ b/SurgSim/Physics/FixedRepresentationContact.h
@@ -0,0 +1,76 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FIXEDREPRESENTATIONCONTACT_H
+#define SURGSIM_PHYSICS_FIXEDREPRESENTATIONCONTACT_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// FixedRepresentation frictionless contact implementation.
+class FixedRepresentationContact : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ explicit FixedRepresentationContact();
+
+ /// Destructor
+ virtual ~FixedRepresentationContact();
+
+
+ /// Gets the Mixed Linear Complementarity Problem constraint type for this ConstraintImplementation
+ /// \return The MLCP constraint type corresponding to this constraint implementation
+ SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+
+ /// Gets the Type of representation that this implementation is concerned with
+ /// \return RepresentationType for this implementation
+ virtual RepresentationType getRepresentationType() const override;
+
+private:
+ /// Gets the number of degree of freedom.
+ /// \return 1 as a frictionless contact is formed of 1 equation of constraint (along the normal direction).
+ size_t doGetNumDof() const override;
+
+ /// Builds the subset of an Mlcp physics problem associated to this implementation.
+ /// \param dt The time step.
+ /// \param data The data associated to the constraint.
+ /// \param localization The localization for the representation.
+ /// \param [in, out] mlcp The Mixed LCP physics problem to fill up.
+ /// \param indexOfRepresentation The index of the representation (associated to this implementation) in the mlcp.
+ /// \param indexOfConstraint The index of the constraint in the mlcp.
+ /// \param sign The sign of this implementation in the constraint (positive or negative side).
+ /// \note Empty for a Fixed Representation
+ void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) override;
+
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FIXEDREPRESENTATIONCONTACT_H
diff --git a/SurgSim/Physics/FixedRepresentationLocalization.h b/SurgSim/Physics/FixedRepresentationLocalization.h
new file mode 100644
index 0000000..04b0d6d
--- /dev/null
+++ b/SurgSim/Physics/FixedRepresentationLocalization.h
@@ -0,0 +1,105 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FIXEDREPRESENTATIONLOCALIZATION_H
+#define SURGSIM_PHYSICS_FIXEDREPRESENTATIONLOCALIZATION_H
+
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// This class implement the localization on a FixedRepresentation, as a local position
+class FixedRepresentationLocalization: public Localization
+{
+public:
+ /// Default constructor
+ FixedRepresentationLocalization() :
+ Localization()
+ {
+ }
+
+ /// Constructor
+ /// \param representation The representation to assign to this localization
+ explicit FixedRepresentationLocalization(std::shared_ptr<Representation> representation) :
+ Localization(representation)
+ {
+ std::shared_ptr<FixedRepresentation> fixed = std::dynamic_pointer_cast<FixedRepresentation>(representation);
+ SURGSIM_ASSERT(fixed != nullptr) << "Unexpected representation type" << std::endl;
+ }
+
+ /// Destructor
+ virtual ~FixedRepresentationLocalization()
+ {
+ }
+
+ /// Sets the local position
+ /// \param p The local position to set the localization at
+ void setLocalPosition(const SurgSim::Math::Vector3d& p)
+ {
+ m_position = p;
+ }
+
+ /// Gets the local position
+ /// \return The local position set for this localization
+ const SurgSim::Math::Vector3d& getLocalPosition() const
+ {
+ return m_position;
+ }
+
+private:
+ /// Calculates the global position of this localization
+ /// \param time The time in [0..1] at which the position should be calculated
+ /// \return The global position of the localization at the requested time
+ /// \note time can useful when dealing with CCD
+ SurgSim::Math::Vector3d doCalculatePosition(double time)
+ {
+ std::shared_ptr<FixedRepresentation> fixed = std::static_pointer_cast<FixedRepresentation>(getRepresentation());
+ const SurgSim::Math::RigidTransform3d& currentPose = fixed->getCurrentState().getPose();
+ const SurgSim::Math::RigidTransform3d& previousPose = fixed->getPreviousState().getPose();
+
+ if (time == 0.0)
+ {
+ return previousPose * m_position;
+ }
+ else if (time == 1.0)
+ {
+ return currentPose * m_position;
+ }
+ else if (currentPose.isApprox(previousPose))
+ {
+ return currentPose * m_position;
+ }
+
+ SurgSim::Math::RigidTransform3d pose = SurgSim::Math::interpolate(previousPose, currentPose, time);
+ return pose * m_position;
+ }
+
+ /// 3D position in local coordinates
+ SurgSim::Math::Vector3d m_position;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FIXEDREPRESENTATIONLOCALIZATION_H
diff --git a/SurgSim/Physics/FreeMotion.cpp b/SurgSim/Physics/FreeMotion.cpp
new file mode 100644
index 0000000..10b3b85
--- /dev/null
+++ b/SurgSim/Physics/FreeMotion.cpp
@@ -0,0 +1,61 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <memory>
+#include <vector>
+
+#include "SurgSim/Physics/FreeMotion.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+
+FreeMotion::FreeMotion(bool doCopyState) : Computation(doCopyState)
+{
+
+}
+
+FreeMotion::~FreeMotion()
+{
+
+}
+
+std::shared_ptr<PhysicsManagerState> FreeMotion::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ // Copy state to new state
+ std::shared_ptr<PhysicsManagerState> result = state;
+ std::vector<std::shared_ptr<Representation>> representations = result->getRepresentations();
+
+ auto const itEnd = representations.end();
+ for (auto it = representations.begin(); it != itEnd; ++it)
+ {
+ (*it)->update(dt);
+ }
+
+ result->setRepresentations(representations);
+
+ return result;
+}
+
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/FreeMotion.h b/SurgSim/Physics/FreeMotion.h
new file mode 100644
index 0000000..32d11e3
--- /dev/null
+++ b/SurgSim/Physics/FreeMotion.h
@@ -0,0 +1,56 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_FREEMOTION_H
+#define SURGSIM_PHYSICS_FREEMOTION_H
+
+#include <memory>
+#include <vector>
+
+
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class Representation;
+
+/// Apply the Freemotion calcluation to all physics representations
+class FreeMotion : public Computation
+{
+public:
+
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit FreeMotion(bool doCopyState = false);
+
+ /// Destructor
+ ~FreeMotion();
+
+protected:
+
+ /// Override doUpdate from superclass
+ virtual std::shared_ptr<PhysicsManagerState> doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state) override;
+
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Physics/LinearSpring.cpp b/SurgSim/Physics/LinearSpring.cpp
new file mode 100644
index 0000000..92c0939
--- /dev/null
+++ b/SurgSim/Physics/LinearSpring.cpp
@@ -0,0 +1,268 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+using SurgSim::Math::addSubMatrix;
+using SurgSim::Math::addSubVector;
+using SurgSim::Math::getSubVector;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+LinearSpring::LinearSpring(size_t nodeId0, size_t nodeId1) :
+ Spring(), m_restLength(0.0), m_stiffness(0.0), m_damping(0.0)
+{
+ m_nodeIds.push_back(nodeId0);
+ m_nodeIds.push_back(nodeId1);
+}
+
+void LinearSpring::setStiffness(double stiffness)
+{
+ m_stiffness = stiffness;
+}
+
+double LinearSpring::getStiffness() const
+{
+ return m_stiffness;
+}
+
+void LinearSpring::setDamping(double damping)
+{
+ m_damping = damping;
+}
+
+double LinearSpring::getDamping() const
+{
+ return m_damping;
+}
+
+void LinearSpring::setRestLength(double restLength)
+{
+ m_restLength = restLength;
+}
+
+double LinearSpring::getRestLength() const
+{
+ return m_restLength;
+}
+
+void LinearSpring::addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F, double scale)
+{
+ const Vector& x = state.getPositions();
+ const Vector& v = state.getVelocities();
+ const Vector& x0 = getSubVector(x, m_nodeIds[0], 3);
+ const Vector& x1 = getSubVector(x, m_nodeIds[1], 3);
+ const Vector& v0 = getSubVector(v, m_nodeIds[0], 3);
+ const Vector& v1 = getSubVector(v, m_nodeIds[1], 3);
+
+ Vector3d u = x1 - x0;
+ double length = u.norm();
+ if (length < SurgSim::Math::Geometry::DistanceEpsilon)
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "Spring (initial length = " << m_restLength << ") became degenerated with 0 length => no force generated";
+ return;
+ }
+ u /= length;
+ double elongationPosition = length - m_restLength;
+ double elongationVelocity = (v1 - v0).dot(u);
+ const Vector3d f = scale * (m_stiffness* elongationPosition + m_damping * elongationVelocity) * u;
+
+ // Assembly stage in F
+ addSubVector( f, m_nodeIds[0], 3, F);
+ addSubVector(-f, m_nodeIds[1], 3, F);
+}
+
+void LinearSpring::addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D, double scale)
+{
+ const Vector& x = state.getPositions();
+ Vector3d u = getSubVector(x, m_nodeIds[1], 3) - getSubVector(x, m_nodeIds[0], 3);
+ double length = u.norm();
+ if (length < SurgSim::Math::Geometry::DistanceEpsilon)
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "Spring (initial length = " << m_restLength << ") became degenerated with 0 length => no force generated";
+ return;
+ }
+ u /= length;
+ Matrix33d D00 = scale * m_damping * (u * u.transpose());
+
+ // Assembly stage in D
+ addSubMatrix( D00, m_nodeIds[0], m_nodeIds[0], 3, 3, D);
+ addSubMatrix(-D00, m_nodeIds[0], m_nodeIds[1], 3, 3, D);
+ addSubMatrix(-D00, m_nodeIds[1], m_nodeIds[0], 3, 3, D);
+ addSubMatrix( D00, m_nodeIds[1], m_nodeIds[1], 3, 3, D);
+}
+
+void LinearSpring::addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K, double scale)
+{
+ const Vector& x = state.getPositions();
+ const Vector& v = state.getVelocities();
+ const Vector& x0 = getSubVector(x, m_nodeIds[0], 3);
+ const Vector& x1 = getSubVector(x, m_nodeIds[1], 3);
+ const Vector& v0 = getSubVector(v, m_nodeIds[0], 3);
+ const Vector& v1 = getSubVector(v, m_nodeIds[1], 3);
+ Vector3d u = x1 - x0;
+ double length = u.norm();
+ if (length < SurgSim::Math::Geometry::DistanceEpsilon)
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "Spring (initial length = " << m_restLength << ") became degenerated with 0 length => no force generated";
+ return;
+ }
+ u /= length;
+ double lRatio = (length - m_restLength) / length;
+ double vRatio = (v1 -v0).dot(u) / length;
+
+ Matrix33d K00 = Matrix33d::Identity() * (m_stiffness * lRatio + m_damping * vRatio);
+ K00 -= (u * u.transpose()) * (m_stiffness * (lRatio - 1.0) + 2.0 * m_damping * vRatio);
+ K00 += m_damping * (u * (v1 -v0).transpose()) / length;
+ K00 *= scale;
+
+ // Assembly stage in K
+ addSubMatrix( K00, m_nodeIds[0], m_nodeIds[0], 3, 3, K);
+ addSubMatrix(-K00, m_nodeIds[0], m_nodeIds[1], 3, 3, K);
+ addSubMatrix(-K00, m_nodeIds[1], m_nodeIds[0], 3, 3, K);
+ addSubMatrix( K00, m_nodeIds[1], m_nodeIds[1], 3, 3, K);
+}
+
+void LinearSpring::addFDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ SurgSim::Math::Matrix* D, SurgSim::Math::Matrix* K)
+{
+ const Vector& x = state.getPositions();
+ const Vector& v = state.getVelocities();
+ const Vector& x0 = getSubVector(x, m_nodeIds[0], 3);
+ const Vector& x1 = getSubVector(x, m_nodeIds[1], 3);
+ const Vector& v0 = getSubVector(v, m_nodeIds[0], 3);
+ const Vector& v1 = getSubVector(v, m_nodeIds[1], 3);
+ Vector3d u = x1 - x0;
+ double length = u.norm();
+ if (length < SurgSim::Math::Geometry::DistanceEpsilon)
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "Spring (initial length = " << m_restLength << ") became degenerated with 0 length => no force generated";
+ return;
+ }
+ u /= length;
+ double elongationPosition = length - m_restLength;
+ double elongationVelocity = (v1 - v0).dot(u);
+ double lRatio = elongationPosition / length;
+ double vRatio = (v1 -v0).dot(u) / length;
+
+ Matrix33d K00 = Matrix33d::Identity() * (m_stiffness * lRatio + m_damping * vRatio);
+ K00 -= (u * u.transpose()) * (m_stiffness * (lRatio - 1.0) + 2.0 * m_damping * vRatio);
+ K00 += m_damping * (u * (v1 -v0).transpose()) / length;
+ addSubMatrix( K00, m_nodeIds[0], m_nodeIds[0], 3, 3, K);
+ addSubMatrix(-K00, m_nodeIds[0], m_nodeIds[1], 3, 3, K);
+ addSubMatrix(-K00, m_nodeIds[1], m_nodeIds[0], 3, 3, K);
+ addSubMatrix( K00, m_nodeIds[1], m_nodeIds[1], 3, 3, K);
+
+ Matrix33d D00 = m_damping * (u * u.transpose());
+ addSubMatrix( D00, m_nodeIds[0], m_nodeIds[0], 3, 3, D);
+ addSubMatrix(-D00, m_nodeIds[0], m_nodeIds[1], 3, 3, D);
+ addSubMatrix(-D00, m_nodeIds[1], m_nodeIds[0], 3, 3, D);
+ addSubMatrix( D00, m_nodeIds[1], m_nodeIds[1], 3, 3, D);
+
+ const Vector3d f = u * (m_stiffness* elongationPosition + m_damping * elongationVelocity);
+ addSubVector( f, m_nodeIds[0], 3, F);
+ addSubVector(-f, m_nodeIds[1], 3, F);
+}
+
+void LinearSpring::addMatVec(const SurgSim::Math::OdeState& state, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& vector, SurgSim::Math::Vector* F)
+{
+ // Premature return if both factors are zero
+ if (alphaK == 0.0 && alphaD == 0.0)
+ {
+ return;
+ }
+
+ // Shared data: the 6D vector to multiply the 6x6 matrix with
+ Eigen::Matrix<double, 6, 1> vector6D;
+ getSubVector(vector, m_nodeIds, 3, &vector6D);
+
+ // Shared data: spring direction and length
+ const Vector& xState = state.getPositions();
+ const Vector& x0 = getSubVector(xState, m_nodeIds[0], 3);
+ const Vector& x1 = getSubVector(xState, m_nodeIds[1], 3);
+ Vector3d u = x1 - x0;
+ double length = u.norm();
+ if (length < SurgSim::Math::Geometry::DistanceEpsilon)
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "Spring (initial length = " << m_restLength << ") became degenerated with 0 length => no force generated";
+ return;
+ }
+ u /= length;
+
+ if (alphaD != 0.0)
+ {
+ Matrix33d D00 = m_damping * (u * u.transpose());
+
+ const Vector3d force = alphaD * (D00 * (vector6D.segment(0, 3) - vector6D.segment(3, 3)));
+ addSubVector( force, m_nodeIds[0], 3, F);
+ addSubVector(-force, m_nodeIds[1], 3, F);
+ }
+
+ if (alphaK != 0.0)
+ {
+ const Vector& v = state.getVelocities();
+ const Vector& v0 = getSubVector(v, m_nodeIds[0], 3);
+ const Vector& v1 = getSubVector(v, m_nodeIds[1], 3);
+
+ double elongationPosition = length - m_restLength;
+ double elongationVelocity = (v1 - v0).dot(u);
+ double lRatio = elongationPosition / length;
+ double vRatio = elongationVelocity / length;
+
+ Matrix33d K00 = Matrix33d::Identity() * (m_stiffness * lRatio + m_damping * vRatio);
+ K00 -= (u * u.transpose()) * (m_stiffness * (lRatio - 1.0) + 2.0 * m_damping * vRatio);
+ K00 += m_damping * (u * (v1 - v0).transpose()) / length;
+
+ const Vector3d force = alphaK * (K00 * (vector6D.segment(0, 3) - vector6D.segment(3, 3)));
+ addSubVector( force, m_nodeIds[0], 3, F);
+ addSubVector(-force, m_nodeIds[1], 3, F);
+ }
+}
+
+bool LinearSpring::operator ==(const Spring& spring) const
+{
+ const LinearSpring *ls = dynamic_cast<const LinearSpring*>(&spring);
+ if (! ls)
+ {
+ return false;
+ }
+ return m_nodeIds == ls->m_nodeIds &&
+ m_restLength == ls->m_restLength && m_stiffness == ls->m_stiffness && m_damping == ls->m_damping;
+}
+
+bool LinearSpring::operator !=(const Spring& spring) const
+{
+ return ! ((*this) == spring);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/LinearSpring.h b/SurgSim/Physics/LinearSpring.h
new file mode 100644
index 0000000..a474108
--- /dev/null
+++ b/SurgSim/Physics/LinearSpring.h
@@ -0,0 +1,128 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_LINEARSPRING_H
+#define SURGSIM_PHYSICS_LINEARSPRING_H
+
+#include "SurgSim/Physics/Spring.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Linear spring connecting 2 nodes with a viscous term
+class LinearSpring : public Spring
+{
+public:
+ /// Constructor
+ /// \param nodeId0, nodeId1 The node ids on which the spring is attached
+ LinearSpring(size_t nodeId0, size_t nodeId1);
+
+ /// Sets the spring stiffness parameter
+ /// \param stiffness The stiffness to assign to the spring (in N.m-1)
+ void setStiffness(double stiffness);
+
+ /// Gets the spring stiffness parameter
+ /// \return The stiffness assigned to the spring (in N.m-1)
+ double getStiffness() const;
+
+ /// Sets the spring damping parameter
+ /// \param damping The damping to assign to the spring (in N.s.m-1)
+ void setDamping(double damping);
+
+ /// Gets the spring damping parameter
+ /// \return The damping assigned to the spring (in N.s.m-1)
+ double getDamping() const;
+
+ /// Sets the rest length of the spring
+ /// \param restLength The rest length to assign to the spring (in m)
+ void setRestLength(double restLength);
+
+ /// Gets the rest length of the spring
+ /// \return The rest length assigned to the spring (in m)
+ double getRestLength() const;
+
+ /// Adds the spring force (computed for a given state) to a complete system force vector F (assembly)
+ /// \param state The state to compute the force with
+ /// \param[in,out] F The complete system force vector to add the spring force into
+ /// \param scale A factor to scale the added force with
+ virtual void addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ double scale = 1.0) override;
+
+ /// Adds the spring damping matrix D (= -df/dv) (computed for a given state) to a complete system damping matrix
+ /// D (assembly)
+ /// \param state The state to compute the damping matrix with
+ /// \param[in,out] D The complete system damping matrix to add the spring damping matrix into
+ /// \param scale A factor to scale the added damping matrix with
+ virtual void addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D,
+ double scale = 1.0) override;
+
+ /// Adds the spring stiffness matrix K (= -df/dx) (computed for a given state) to a complete system stiffness
+ /// matrix K (assembly)
+ /// \param state The state to compute the stiffness matrix with
+ /// \param[in,out] K The complete system stiffness matrix to add the spring stiffness matrix into
+ /// \param scale A factor to scale the added stiffness matrix with
+ virtual void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale = 1.0) override;
+
+ /// Adds the spring force vector, mass, stiffness and damping matrices (computed for a given state) into a
+ /// complete system data structure F, D, K (assembly)
+ /// \param state The state to compute everything with
+ /// \param[in,out] F The complete system force vector to add the spring force into
+ /// \param[in,out] D The complete system damping matrix to add the spring damping matrix into
+ /// \param[in,out] K The complete system stiffness matrix to add the spring stiffness matrix into
+ virtual void addFDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ SurgSim::Math::Matrix* D, SurgSim::Math::Matrix* K) override;
+
+ /// Adds the spring matrix-vector contribution F += (alphaD.D + alphaK.K).x (computed for a given
+ /// state) into a complete system data structure F (assembly)
+ /// \param state The state to compute everything with
+ /// \param alphaD The scaling factor for the damping contribution
+ /// \param alphaK The scaling factor for the stiffness contribution
+ /// \param vector A complete system vector to use as the vector in the matrix-vector multiplication
+ /// \param[in,out] F The complete system force vector to add the element matrix-vector contribution into
+ virtual void addMatVec(const SurgSim::Math::OdeState& state, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& vector, SurgSim::Math::Vector* F) override;
+
+ /// Comparison operator (equality)
+ /// \param spring Spring to compare it to
+ /// \return True if the 2 springs contains the same information, false otherwise
+ /// \note Comparison is based on spring type, rest length, stiffness and damping coefficients ONLY
+ bool operator ==(const Spring& spring) const;
+
+ /// Comparison operator (inequality)
+ /// \param spring Spring to compare it to
+ /// \return False if the 2 springs contains the same information, true otherwise
+ /// \note Comparison is based on spring type, rest length, stiffness and damping coefficients ONLY
+ bool operator !=(const Spring& spring) const;
+
+private:
+ /// Rest length (in m)
+ double m_restLength;
+
+ /// Stiffness parameters (in N.m-1)
+ double m_stiffness;
+
+ /// Damping parameters (in N.s.m-1)
+ double m_damping;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_LINEARSPRING_H
diff --git a/SurgSim/Physics/Localization.cpp b/SurgSim/Physics/Localization.cpp
new file mode 100644
index 0000000..6a19dfc
--- /dev/null
+++ b/SurgSim/Physics/Localization.cpp
@@ -0,0 +1,38 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/Representation.h"
+
+using SurgSim::Physics::Localization;
+using SurgSim::Physics::Representation;
+
+Localization::Localization()
+{
+}
+Localization::Localization(std::shared_ptr<Representation> representation) :
+m_representation(representation)
+{
+}
+
+Localization::~Localization()
+{
+}
+
+bool SurgSim::Physics::Localization::isValidRepresentation(std::shared_ptr<Representation> representation)
+{
+ // Localization base class does not care about the type
+ return true;
+}
diff --git a/SurgSim/Physics/Localization.h b/SurgSim/Physics/Localization.h
new file mode 100644
index 0000000..2402cbf
--- /dev/null
+++ b/SurgSim/Physics/Localization.h
@@ -0,0 +1,92 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_LOCALIZATION_H
+#define SURGSIM_PHYSICS_LOCALIZATION_H
+
+#include <memory>
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+class Representation;
+
+/// This class localize a point on a representation (representation specific)
+class Localization
+{
+public:
+ /// Default constructor
+ Localization();
+
+ /// Constructor
+ /// \param representation The representation on which the localization is defined
+ explicit Localization(std::shared_ptr<Representation> representation);
+
+ /// Destructor
+ virtual ~Localization();
+
+ /// Sets the representation
+ /// \param representation The representation on which the localization is defined
+ void setRepresentation(std::shared_ptr<Representation> representation)
+ {
+ if (isValidRepresentation(representation))
+ {
+ m_representation = representation;
+ }
+ else
+ {
+ SURGSIM_ASSERT(false) << "Unexpected representation type" << std::endl;
+ }
+ }
+
+ /// Gets the representation
+ /// \return The representation on which the localization is defined, nullptr if none has been defined
+ std::shared_ptr<Representation> getRepresentation() const
+ {
+ return m_representation;
+ }
+
+ /// Calculates the global position of this localization
+ /// \param time The time in [0..1] at which the position should be calculated
+ /// \return The global position of the localization at the requested time
+ /// \note time can useful when dealing with CCD
+ SurgSim::Math::Vector3d calculatePosition(double time = 1.0)
+ {
+ return doCalculatePosition(time);
+ }
+
+ virtual bool isValidRepresentation(std::shared_ptr<Representation> representation);
+
+private:
+ /// Calculates the global position of this localization
+ /// \param time The time in [0..1] at which the position should be calculated
+ /// \return The global position of the localization at the requested time
+ /// \note time can useful when dealing with CCD
+ virtual SurgSim::Math::Vector3d doCalculatePosition(double time) = 0;
+ /// The representation on which the localization is defined
+ std::shared_ptr<Representation> m_representation;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_LOCALIZATION_H
diff --git a/SurgSim/Physics/Mass.h b/SurgSim/Physics/Mass.h
new file mode 100644
index 0000000..6968c01
--- /dev/null
+++ b/SurgSim/Physics/Mass.h
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_MASS_H
+#define SURGSIM_PHYSICS_MASS_H
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+// Class to handle a mass of a MassSpring node
+class Mass
+{
+public:
+ /// Constructor
+ /// \param mass The mass (in Kg) default value is 0
+ explicit Mass(double mass = 0.0) : m_mass(mass)
+ {
+ }
+
+ /// Sets the mass
+ /// \param mass The mass to be stored (in Kg)
+ void setMass(double mass)
+ {
+ m_mass = mass;
+ }
+
+ /// Gets the mass
+ /// \return The mass stored (in Kg)
+ double getMass() const
+ {
+ return m_mass;
+ }
+
+ /// Comparison operator (equality)
+ /// \param m Mass to compare it to
+ /// \return True if the 2 Mass contains the same information, false otherwise
+ bool operator ==(const Mass& m) const
+ {
+ return (m_mass == m.m_mass);
+ }
+
+ /// Comparison operator (inequality)
+ /// \param m Mass to compare it to
+ /// \return False if the 2 Mass contains the same information, True otherwise
+ bool operator !=(const Mass& m) const
+ {
+ return ! ((*this) == m);
+ }
+
+protected:
+ /// Mass (in kg)
+ double m_mass;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_MASS_H
diff --git a/SurgSim/Physics/MassSpringRepresentation.cpp b/SurgSim/Physics/MassSpringRepresentation.cpp
new file mode 100644
index 0000000..2a19922
--- /dev/null
+++ b/SurgSim/Physics/MassSpringRepresentation.cpp
@@ -0,0 +1,469 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+#include "SurgSim/Physics/MassSpringRepresentationLocalization.h"
+
+using SurgSim::Math::Vector;
+using SurgSim::Math::Matrix;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+MassSpringRepresentation::MassSpringRepresentation(const std::string& name) :
+ DeformableRepresentation(name)
+{
+ m_rayleighDamping.massCoefficient = 0.0;
+ m_rayleighDamping.stiffnessCoefficient = 0.0;
+
+ // Reminder: m_numDofPerNode is held by DeformableRepresentation
+ // but needs to be set by all concrete derived classes
+ m_numDofPerNode = 3;
+}
+
+MassSpringRepresentation::~MassSpringRepresentation()
+{
+}
+
+void MassSpringRepresentation::addMass(const std::shared_ptr<Mass> mass)
+{
+ m_masses.push_back(mass);
+}
+
+void MassSpringRepresentation::addSpring(const std::shared_ptr<Spring> spring)
+{
+ m_springs.push_back(spring);
+}
+
+size_t MassSpringRepresentation::getNumMasses() const
+{
+ return m_masses.size();
+}
+
+size_t MassSpringRepresentation::getNumSprings() const
+{
+ return m_springs.size();
+}
+
+std::shared_ptr<Mass> MassSpringRepresentation::getMass(size_t nodeId)
+{
+ SURGSIM_ASSERT(nodeId < getNumMasses()) << "Invalid node id to request a mass from";
+ return m_masses[nodeId];
+}
+
+std::shared_ptr<Spring> MassSpringRepresentation::getSpring(size_t springId)
+{
+ SURGSIM_ASSERT(springId < getNumSprings()) << "Invalid spring id";
+ return m_springs[springId];
+}
+
+double MassSpringRepresentation::getTotalMass() const
+{
+ double mass = 0.0;
+ for (auto it = std::begin(m_masses); it != std::end(m_masses); it++)
+ {
+ mass += (*it)->getMass();
+ }
+ return mass;
+}
+
+double MassSpringRepresentation::getRayleighDampingStiffness() const
+{
+ return m_rayleighDamping.stiffnessCoefficient;
+}
+
+double MassSpringRepresentation::getRayleighDampingMass() const
+{
+ return m_rayleighDamping.massCoefficient;
+}
+
+void MassSpringRepresentation::setRayleighDampingStiffness(double stiffnessCoef)
+{
+ m_rayleighDamping.stiffnessCoefficient = stiffnessCoef;
+}
+
+void MassSpringRepresentation::setRayleighDampingMass(double massCoef)
+{
+ m_rayleighDamping.massCoefficient = massCoef;
+}
+
+RepresentationType MassSpringRepresentation::getType() const
+{
+ return REPRESENTATION_TYPE_MASSSPRING;
+}
+
+void MassSpringRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K,
+ const SurgSim::Math::Matrix& D)
+{
+ std::shared_ptr<MassSpringRepresentationLocalization> localization3D =
+ std::dynamic_pointer_cast<MassSpringRepresentationLocalization>(localization);
+ SURGSIM_ASSERT(localization3D != nullptr) <<
+ "Invalid localization type (not a MassSpringRepresentationLocalization)";
+
+ const size_t dofPerNode = getNumDofPerNode();
+
+ const size_t nodeId = localization3D->getLocalNode();
+ SURGSIM_ASSERT(nodeId >= 0 && nodeId < getNumMasses()) << "Invalid nodeId " << nodeId <<
+ ". Valid range is {0.." << getNumMasses() << "}";
+
+ m_externalGeneralizedForce.segment(dofPerNode * nodeId, dofPerNode) += generalizedForce;
+ m_externalGeneralizedStiffness.block(dofPerNode * nodeId, dofPerNode * nodeId, dofPerNode, dofPerNode) += K;
+ m_externalGeneralizedDamping.block(dofPerNode * nodeId, dofPerNode * nodeId, dofPerNode, dofPerNode) += D;
+}
+
+void MassSpringRepresentation::beforeUpdate(double dt)
+{
+ // Call the DeformableRepresentation implementation
+ DeformableRepresentation::beforeUpdate(dt);
+
+ if (! isActive())
+ {
+ return;
+ }
+
+ SURGSIM_ASSERT(3 * getNumMasses() == getNumDof()) <<
+ "Mismatch between the number of masses ("<<getNumMasses()<<") and the number of dof ("<<getNumDof()<<")";
+ SURGSIM_ASSERT(getNumMasses()) << "No masses specified yet, call addMass() prior to running the simulation";
+ SURGSIM_ASSERT(getNumSprings()) << "No springs specified yet, call addSpring() prior to running the simulation";
+ SURGSIM_ASSERT(getNumDof()) <<
+ "State has not been initialized yet, call setInitialState() prior to running the simulation";
+}
+
+Vector& MassSpringRepresentation::computeF(const SurgSim::Math::OdeState& state)
+{
+ // Make sure the force vector has been properly allocated and zeroed out
+ m_f.resize(state.getNumDof());
+ m_f.setZero();
+
+ addGravityForce(&m_f, state);
+ addRayleighDampingForce(&m_f, state);
+ addSpringsForce(&m_f, state);
+
+ // Add external generalized force
+ m_f += m_externalGeneralizedForce;
+
+ // Apply boundary conditions globally
+ for (auto boundaryCondition = std::begin(state.getBoundaryConditions());
+ boundaryCondition != std::end(state.getBoundaryConditions());
+ boundaryCondition++)
+ {
+ m_f[*boundaryCondition] = 0.0;
+ }
+
+ return m_f;
+}
+
+const Matrix& MassSpringRepresentation::computeM(const SurgSim::Math::OdeState& state)
+{
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::setSubVector;
+
+ // Make sure the mass matrix has been properly allocated
+ m_M.resize(state.getNumDof(), state.getNumDof());
+ m_M.setZero();
+
+ Eigen::MatrixBase<Matrix>::DiagonalReturnType diagonal = m_M.diagonal();
+
+ for (size_t massId = 0; massId < getNumMasses(); massId++)
+ {
+ setSubVector(Vector3d::Ones() * getMass(massId)->getMass(), massId, 3, &diagonal);
+ }
+
+ // Apply boundary conditions globally
+ for (auto boundaryCondition = std::begin(state.getBoundaryConditions());
+ boundaryCondition != std::end(state.getBoundaryConditions());
+ boundaryCondition++)
+ {
+ diagonal[*boundaryCondition] = 1e9;
+ }
+
+ return m_M;
+}
+
+const Matrix& MassSpringRepresentation::computeD(const SurgSim::Math::OdeState& state)
+{
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::setSubVector;
+ using SurgSim::Math::addSubMatrix;
+
+ const double& rayleighStiffness = m_rayleighDamping.stiffnessCoefficient;
+ const double& rayleighMass = m_rayleighDamping.massCoefficient;
+
+ // Make sure the damping matrix has been properly allocated and zeroed out
+ m_D.resize(state.getNumDof(), state.getNumDof());
+ m_D.setZero();
+
+ // D += rayleighMass.M
+ if (rayleighMass != 0.0)
+ {
+ for (size_t massId = 0; massId < getNumMasses(); massId++)
+ {
+ double coef = rayleighMass * getMass(massId)->getMass();
+ Eigen::MatrixBase<Matrix>::DiagonalReturnType Ddiagonal = m_D.diagonal();
+ setSubVector(Vector3d::Ones() * coef, massId, 3, &Ddiagonal);
+ }
+ }
+
+ // D += rayleighStiffness.K
+ if (rayleighStiffness != 0.0)
+ {
+ for (auto spring = std::begin(m_springs); spring != std::end(m_springs); spring++)
+ {
+ (*spring)->addStiffness(state, &m_D, rayleighStiffness);
+ }
+ }
+
+ // D += Springs damping matrix
+ for (auto spring = std::begin(m_springs); spring != std::end(m_springs); spring++)
+ {
+ (*spring)->addDamping(state, &m_D);
+ }
+
+ // Add external generalized damping
+ m_D += m_externalGeneralizedDamping;
+
+ // Apply boundary conditions globally
+ for (auto boundaryCondition = std::begin(state.getBoundaryConditions());
+ boundaryCondition != std::end(state.getBoundaryConditions());
+ boundaryCondition++)
+ {
+ m_D.block(*boundaryCondition, 0, 1, getNumDof()).setZero();
+ m_D.block(0, *boundaryCondition, getNumDof(), 1).setZero();
+ m_D(*boundaryCondition, *boundaryCondition) = 1e9;
+ }
+
+ return m_D;
+}
+
+const Matrix& MassSpringRepresentation::computeK(const SurgSim::Math::OdeState& state)
+{
+ using SurgSim::Math::addSubMatrix;
+
+ // Make sure the stiffness matrix has been properly allocated and zeroed out
+ m_K.resize(state.getNumDof(), state.getNumDof());
+ m_K.setZero();
+
+ for (auto spring = std::begin(m_springs); spring != std::end(m_springs); spring++)
+ {
+ (*spring)->addStiffness(state, &m_K);
+ }
+
+ // Add external generalized stiffness
+ m_K += m_externalGeneralizedStiffness;
+
+ // Apply boundary conditions globally
+ for (auto boundaryCondition = std::begin(state.getBoundaryConditions());
+ boundaryCondition != std::end(state.getBoundaryConditions());
+ boundaryCondition++)
+ {
+ m_K.block(*boundaryCondition, 0, 1, getNumDof()).setZero();
+ m_K.block(0, *boundaryCondition, getNumDof(), 1).setZero();
+ m_K(*boundaryCondition, *boundaryCondition) = 1e9;
+ }
+
+ return m_K;
+}
+
+void MassSpringRepresentation::computeFMDK(const SurgSim::Math::OdeState& state,
+ Vector** f, Matrix** M, Matrix** D, Matrix** K)
+{
+ using SurgSim::Math::addSubVector;
+ using SurgSim::Math::addSubMatrix;
+
+ // Make sure the force vector has been properly allocated and zeroed out
+ m_f.resize(state.getNumDof());
+ m_f.setZero();
+
+ // Make sure the mass matrix has been properly allocated
+ m_M.resize(state.getNumDof(), state.getNumDof());
+ m_M.setZero();
+
+ // Make sure the damping matrix has been properly allocated and zeroed out
+ m_D.resize(state.getNumDof(), state.getNumDof());
+ m_D.setZero();
+
+ // Make sure the stiffness matrix has been properly allocated and zeroed out
+ m_K.resize(state.getNumDof(), state.getNumDof());
+ m_K.setZero();
+
+ // Computes the mass matrix m_M
+ computeM(state);
+
+ // Computes the stiffness matrix m_K
+ // Add the springs damping matrix to m_D
+ // Add the springs force to m_f
+ for (auto spring = std::begin(m_springs); spring != std::end(m_springs); spring++)
+ {
+ (*spring)->addFDK(state, &m_f, &m_D, &m_K);
+ }
+
+ // Add the Rayleigh damping matrix
+ if (m_rayleighDamping.massCoefficient)
+ {
+ m_D.diagonal() += m_M.diagonal() * m_rayleighDamping.massCoefficient;
+ }
+ if (m_rayleighDamping.stiffnessCoefficient)
+ {
+ m_D += m_K * m_rayleighDamping.stiffnessCoefficient;
+ }
+
+ // Add the gravity to m_f
+ addGravityForce(&m_f, state);
+
+ // Add the Rayleigh damping force to m_f (using the damping matrix)
+ addRayleighDampingForce(&m_f, state, true, true);
+
+ // Add external generalized force, stiffness and damping
+ m_f += m_externalGeneralizedForce;
+ m_K += m_externalGeneralizedStiffness;
+ m_D += m_externalGeneralizedDamping;
+
+ // Apply boundary conditions globally
+ for (auto boundaryCondition = std::begin(state.getBoundaryConditions());
+ boundaryCondition != std::end(state.getBoundaryConditions());
+ boundaryCondition++)
+ {
+ m_M.diagonal()[*boundaryCondition] = 1e9;
+
+ m_D.block(*boundaryCondition, 0, 1, getNumDof()).setZero();
+ m_D.block(0, *boundaryCondition, getNumDof(), 1).setZero();
+ m_D(*boundaryCondition, *boundaryCondition) = 1e9;
+
+ m_K.block(*boundaryCondition, 0, 1, getNumDof()).setZero();
+ m_K.block(0, *boundaryCondition, getNumDof(), 1).setZero();
+ m_K(*boundaryCondition, *boundaryCondition) = 1e9;
+
+ m_f[*boundaryCondition] = 0.0;
+ }
+
+ *f = &m_f;
+ *M = &m_M;
+ *D = &m_D;
+ *K = &m_K;
+}
+
+void MassSpringRepresentation::addRayleighDampingForce(Vector* force, const SurgSim::Math::OdeState& state,
+ bool useGlobalStiffnessMatrix, bool useGlobalMassMatrix, double scale)
+{
+ using SurgSim::Math::getSubVector;
+ using SurgSim::Math::addSubVector;
+ using SurgSim::Math::getSubMatrix;
+
+ // Temporary variables for convenience
+ double& rayleighMass = m_rayleighDamping.massCoefficient;
+ double& rayleighStiffness = m_rayleighDamping.stiffnessCoefficient;
+ const Vector& v = state.getVelocities();
+
+ // Rayleigh damping mass: F = - rayleighMass.M.v(t)
+ if (rayleighMass != 0.0)
+ {
+ // If we have the mass matrix, we can compute directly F = -rayleighMass.M.v(t)
+ if (useGlobalMassMatrix)
+ {
+ // M is diagonal, take advantage of it...
+ *force -= (scale * rayleighMass) * (m_M.diagonal().cwiseProduct(v));
+ }
+ else
+ {
+ for (size_t nodeID = 0; nodeID < getNumMasses(); nodeID++)
+ {
+ double mass = getMass(nodeID)->getMass();
+ SurgSim::Math::Vector3d f = - scale * rayleighMass * mass * getSubVector(v, nodeID, 3);
+ addSubVector(f, nodeID, 3, force);
+ }
+ }
+ }
+
+ // Rayleigh damping stiffness: F = - rayleighStiffness.K.v(t)
+ if (rayleighStiffness != 0.0)
+ {
+ if (useGlobalStiffnessMatrix)
+ {
+ *force -= scale * rayleighStiffness * (m_K * v);
+ }
+ else
+ {
+ // Otherwise, we loop through each fem element to compute its contribution
+ for (auto spring = std::begin(m_springs); spring != std::end(m_springs); ++spring)
+ {
+ (*spring)->addMatVec(state, 0.0, - scale * rayleighStiffness, v, force);
+ }
+ }
+ }
+}
+
+void MassSpringRepresentation::addSpringsForce(Vector *force, const SurgSim::Math::OdeState& state, double scale)
+{
+ for (auto spring = std::begin(m_springs); spring != std::end(m_springs); spring++)
+ {
+ (*spring)->addForce(state, force, scale);
+ }
+}
+
+void MassSpringRepresentation::addGravityForce(Vector *f, const SurgSim::Math::OdeState& state, double scale)
+{
+ using SurgSim::Math::addSubVector;
+
+ if (isGravityEnabled())
+ {
+ for (size_t massId = 0; massId < getNumMasses(); massId++)
+ {
+ addSubVector(getGravity() * getMass(massId)->getMass(), massId, 3, f);
+ }
+ }
+}
+
+static void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& transform,
+ Vector* x, bool rotationOnly = false)
+{
+ size_t numNodes = x->size() / 3;
+ SURGSIM_ASSERT(static_cast<ptrdiff_t>(numNodes * 3) == x->size()) <<
+ "Unexpected number of dof in a MassSpring state vector (not a multiple of 3)";
+
+ for (size_t nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ SurgSim::Math::Vector3d xi = SurgSim::Math::getSubVector(*x, nodeId, 3);
+ SurgSim::Math::Vector3d xiTransformed;
+ if (rotationOnly)
+ {
+ xiTransformed = transform.linear() * xi;
+ }
+ else
+ {
+ xiTransformed = transform * xi;
+ }
+ SurgSim::Math::setSubVector(xiTransformed, nodeId, 3, x);
+ }
+}
+
+void MassSpringRepresentation::transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform)
+{
+ transformVectorByBlockOf3(transform, &state->getPositions());
+ transformVectorByBlockOf3(transform, &state->getVelocities(), true);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/MassSpringRepresentation.h b/SurgSim/Physics/MassSpringRepresentation.h
new file mode 100644
index 0000000..bd147a5
--- /dev/null
+++ b/SurgSim/Physics/MassSpringRepresentation.h
@@ -0,0 +1,202 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_MASSSPRINGREPRESENTATION_H
+#define SURGSIM_PHYSICS_MASSSPRINGREPRESENTATION_H
+
+#include <memory>
+
+#include "SurgSim/Physics/DeformableRepresentation.h"
+#include "SurgSim/Physics/Mass.h"
+#include "SurgSim/Physics/Spring.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// MassSpring model is a deformable model (a set of masses connected by springs).
+/// \note A MassSpring is a DeformableRepresentation (Physics::Representation and Math::OdeEquation)
+/// \note Therefore, it defines a dynamic system M.a=F(x,v) with the particularity that M is diagonal
+/// \note The model handles damping through the Rayleigh damping (where damping is a combination of mass and stiffness)
+class MassSpringRepresentation : public DeformableRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name of the MassSpringRepresentation
+ explicit MassSpringRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~MassSpringRepresentation();
+
+ /// Adds a mass
+ /// \param mass The mass to add to the representation
+ /// \note Masses are kept in an ordered list, giving them an index
+ /// \note This mass will be associated with the node of same index in any associated OdeState
+ void addMass(const std::shared_ptr<Mass> mass);
+
+ /// Adds a spring
+ /// \param spring The spring to add to the representation
+ void addSpring(const std::shared_ptr<Spring> spring);
+
+ /// Gets the number of masses
+ /// \return the number of masses
+ size_t getNumMasses() const;
+
+ /// Gets the number of springs
+ /// \return the number of springs
+ size_t getNumSprings() const;
+
+ /// Retrieves the mass of a given node
+ /// \param nodeId The node id for which the mass is requested
+ /// \return the mass attribute of a node
+ /// \note The mass is returned with read/write access
+ /// \note Out of range nodeId will raise an exception
+ std::shared_ptr<Mass> getMass(size_t nodeId);
+
+ /// Retrieves a given spring from its id
+ /// \param springId The spring id for which the spring is requested
+ /// \return the spring for the given springId
+ /// \note The spring is returned with read/write access
+ /// \note Out of range springId will raise an exception
+ std::shared_ptr<Spring> getSpring(size_t springId);
+
+ /// Gets the total mass of the mass spring
+ /// \return The total mass of the mass spring (in Kg)
+ double getTotalMass() const;
+
+ /// Gets the Rayleigh stiffness parameter
+ /// \return The Rayleigh stiffness parameter
+ double getRayleighDampingStiffness() const;
+
+ /// Gets the Rayleigh mass parameter
+ /// \return The Rayleigh mass parameter
+ double getRayleighDampingMass() const;
+
+ /// Sets the Rayleigh stiffness parameter
+ /// \param stiffnessCoef The Rayleigh stiffness parameter
+ void setRayleighDampingStiffness(double stiffnessCoef);
+
+ /// Sets the Rayleigh mass parameter
+ /// \param massCoef The Rayleigh mass parameter
+ void setRayleighDampingMass(double massCoef);
+
+ /// Query the representation type
+ /// \return the RepresentationType for this representation
+ virtual RepresentationType getType() const override;
+
+ virtual void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K = SurgSim::Math::Matrix(),
+ const SurgSim::Math::Matrix& D = SurgSim::Math::Matrix()) override;
+
+ /// Preprocessing done before the update call
+ /// \param dt The time step (in seconds)
+ virtual void beforeUpdate(double dt) override;
+
+ /// Evaluation of the RHS function f(x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the function f(x,v) with
+ /// \return The vector containing f(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeF() or computeFMDK()
+ virtual SurgSim::Math::Vector& computeF(const SurgSim::Math::OdeState& state) override;
+
+ /// Evaluation of the LHS matrix M(x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the matrix M(x,v) with
+ /// \return The matrix M(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeM() or computeFMDK()
+ virtual const SurgSim::Math::Matrix& computeM(const SurgSim::Math::OdeState& state) override;
+
+ /// Evaluation of D = -df/dv (x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the Jacobian matrix with
+ /// \return The matrix D = -df/dv(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeD() or computeFMDK()
+ virtual const SurgSim::Math::Matrix& computeD(const SurgSim::Math::OdeState& state) override;
+
+ /// Evaluation of K = -df/dx (x,v) for a given state
+ /// \param state (x, v) the current position and velocity to evaluate the Jacobian matrix with
+ /// \return The matrix K = -df/dx(x,v)
+ /// \note Returns a reference, its values will remain unchanged until the next call to computeK() or computeFMDK()
+ virtual const SurgSim::Math::Matrix& computeK(const SurgSim::Math::OdeState& state) override;
+
+ /// Evaluation of f(x,v), M(x,v), D = -df/dv(x,v), K = -df/dx(x,v)
+ /// When all the terms are needed, this method can perform optimization in evaluating everything together
+ /// \param state (x, v) the current position and velocity to evaluate the various terms with
+ /// \param[out] f The RHS f(x,v)
+ /// \param[out] M The matrix M(x,v)
+ /// \param[out] D The matrix D = -df/dv(x,v)
+ /// \param[out] K The matrix K = -df/dx(x,v)
+ /// \note Returns pointers, the internal data will remain unchanged until the next call to computeFMDK() or
+ /// \note computeF(), computeM(), computeD(), computeK()
+ virtual void computeFMDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector** f,
+ SurgSim::Math::Matrix** M, SurgSim::Math::Matrix** D, SurgSim::Math::Matrix** K) override;
+
+protected:
+ /// Add the Rayleigh damping forces
+ /// \param[in,out] f The force vector to cumulate the Rayleigh damping force into
+ /// \param state The state vector containing positions and velocities
+ /// \param useGlobalMassMatrix, useGlobalStiffnessMatrix True indicates that the global mass and stiffness matrices
+ /// should be used (F = -c.M.v - d.K.v)
+ /// \param scale A scaling factor to apply on the damping force
+ /// \note Damping matrix D = c.M + d.K (Rayleigh damping definition)
+ /// \note F = - D.v = -c.M.v - d.K.v
+ /// \note If {useGlobalMassMatrix | useGlobalStiffnessMatrix} is True, {M | K} will be used, otherwise
+ /// \note the {mass|stiffness} component will be computed FemElement by FemElement
+ void addRayleighDampingForce(SurgSim::Math::Vector* f, const SurgSim::Math::OdeState& state,
+ bool useGlobalStiffnessMatrix = false, bool useGlobalMassMatrix = false, double scale = 1.0);
+
+ /// Add the springs force to f (given a state)
+ /// \param[in,out] f The force vector to cumulate the spring forces into
+ /// \param state The state vector containing positions and velocities
+ /// \param scale A scaling factor to scale the spring forces with
+ void addSpringsForce(SurgSim::Math::Vector* f, const SurgSim::Math::OdeState& state, double scale = 1.0);
+
+ /// Add the gravity force to f (given a state)
+ /// \param[in,out] f The force vector to cumulate the gravity force into
+ /// \param state The state vector containing positions and velocities
+ /// \param scale A scaling factor to scale the gravity force with
+ /// \note This method does not do anything if gravity is disabled
+ void addGravityForce(SurgSim::Math::Vector *f, const SurgSim::Math::OdeState& state, double scale = 1.0);
+
+ /// Transform a state using a given transformation
+ /// \param[in,out] state The state to be transformed
+ /// \param transform The transformation to apply
+ void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform);
+
+private:
+ /// Masses
+ std::vector<std::shared_ptr<Mass>> m_masses;
+
+ /// Springs
+ std::vector<std::shared_ptr<Spring>> m_springs;
+
+ /// Rayleigh damping parameters (massCoefficient and stiffnessCoefficient)
+ /// D = massCoefficient.M + stiffnessCoefficient.K
+ /// Matrices: D = damping, M = mass, K = stiffness
+ struct {
+ double massCoefficient;
+ double stiffnessCoefficient;
+ } m_rayleighDamping;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_MASSSPRINGREPRESENTATION_H
diff --git a/SurgSim/Physics/MassSpringRepresentationContact.cpp b/SurgSim/Physics/MassSpringRepresentationContact.cpp
new file mode 100644
index 0000000..8ab3a97
--- /dev/null
+++ b/SurgSim/Physics/MassSpringRepresentationContact.cpp
@@ -0,0 +1,109 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Physics/MassSpringRepresentationContact.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/MassSpringRepresentationLocalization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+MassSpringRepresentationContact::MassSpringRepresentationContact()
+{
+
+}
+
+MassSpringRepresentationContact::~MassSpringRepresentationContact()
+{
+
+}
+
+void MassSpringRepresentationContact::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+ using SurgSim::Math::Vector3d;
+
+ auto massSpring = std::static_pointer_cast<MassSpringRepresentation>(localization->getRepresentation());
+
+ if ( !massSpring->isActive())
+ {
+ return;
+ }
+
+ size_t nodeId = std::static_pointer_cast<MassSpringRepresentationLocalization>(localization)->getLocalNode();
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+
+ auto& contactData = static_cast<const ContactConstraintData&>(data);
+ const Vector3d& n = contactData.getNormal();
+ const double d = contactData.getDistance();
+
+ // FRICTIONLESS CONTACT in a LCP
+ // (n, d) defines the plane of contact
+ // p(t) the point of contact (usually after free motion)
+ //
+ // The constraint equation for a plane is
+ // U(t) = n^t.p(t) + d >= 0
+ //
+ // dU/dt = H.dp/dt
+ // => H = n^t
+
+ // Update b with new violation U
+ Vector3d globalPosition = localization->calculatePosition();
+ double violation = n.dot(globalPosition) + d;
+
+ mlcp->b[indexOfConstraint] += violation * scale;
+
+ // m_newH is a SparseVector, so resizing is cheap. The object's memory also gets cleared.
+ m_newH.resize(massSpring->getNumDof());
+ // m_newH is a member variable, so 'reserve' only needs to allocate memory on the first run.
+ m_newH.reserve(3);
+ m_newH.insert(3 * nodeId + 0) = n[0] * scale;
+ m_newH.insert(3 * nodeId + 1) = n[1] * scale;
+ m_newH.insert(3 * nodeId + 2) = n[2] * scale;
+
+ mlcp->updateConstraint(m_newH, massSpring->getComplianceMatrix(), indexOfRepresentation, indexOfConstraint);
+}
+
+SurgSim::Math::MlcpConstraintType MassSpringRepresentationContact::getMlcpConstraintType() const
+{
+ return SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
+}
+
+SurgSim::Physics::RepresentationType MassSpringRepresentationContact::getRepresentationType() const
+{
+ return REPRESENTATION_TYPE_MASSSPRING;
+}
+
+size_t MassSpringRepresentationContact::doGetNumDof() const
+{
+ return 1;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Physics/MassSpringRepresentationContact.h b/SurgSim/Physics/MassSpringRepresentationContact.h
new file mode 100644
index 0000000..9ca376f
--- /dev/null
+++ b/SurgSim/Physics/MassSpringRepresentationContact.h
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_MASSSPRINGREPRESENTATIONCONTACT_H
+#define SURGSIM_PHYSICS_MASSSPRINGREPRESENTATIONCONTACT_H
+
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+#include "SurgSim/Physics/Localization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// MassSpring frictionless contact implementation.
+///
+/// MassSpringRepresentationContact implements the frictionless contact constraint for the MassSpringRepresentation,
+/// which prevents nodes from passing through a surface. See MassSpringRepresentationContact::doBuild for more
+/// information.
+class MassSpringRepresentationContact : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ MassSpringRepresentationContact();
+
+ /// Destructor
+ virtual ~MassSpringRepresentationContact();
+
+ /// Gets the Mixed Linear Complementarity Problem constraint type for this ConstraintImplementation
+ /// \return SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
+ virtual SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+
+ /// Gets the Type of representation that this implementation is concerned with
+ /// \return SurgSim::Physics::REPRESENTATION_TYPE_MASSSPRING
+ virtual RepresentationType getRepresentationType() const override;
+
+private:
+ /// Gets the number of degrees of freedom for a frictionless contact.
+ /// \return 1, as a frictionless contact only has 1 equation of constraint (along the normal direction).
+ virtual size_t doGetNumDof() const override;
+
+ /// Adds a mass-spring frictionless contact constraint to an MlcpPhysicsProblem.
+ /// \param dt The time step.
+ /// \param data [ContactConstraintData] Plane defining the constraint.
+ /// \param localization [MassSpringRepresentationLocalization] Location and Representation to be constrained.
+ /// \param [in, out] mlcp The Mixed LCP physics problem to fill up.
+ /// \param indexOfRepresentation The index of the representation (associated to this implementation) in the mlcp.
+ /// \param indexOfConstraint The index of the constraint in the mlcp.
+ /// \param sign The sign of this implementation in the constraint (positive or negative side).
+ virtual void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) override;
+
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_MASSSPRINGREPRESENTATIONCONTACT_H
diff --git a/SurgSim/Physics/MassSpringRepresentationLocalization.cpp b/SurgSim/Physics/MassSpringRepresentationLocalization.cpp
new file mode 100644
index 0000000..1ae8c03
--- /dev/null
+++ b/SurgSim/Physics/MassSpringRepresentationLocalization.cpp
@@ -0,0 +1,88 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/MassSpringRepresentationLocalization.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+MassSpringRepresentationLocalization::MassSpringRepresentationLocalization()
+{
+
+}
+
+MassSpringRepresentationLocalization::MassSpringRepresentationLocalization(
+ std::shared_ptr<Representation> representation) :
+ Localization()
+{
+ setRepresentation(representation);
+}
+
+MassSpringRepresentationLocalization::~MassSpringRepresentationLocalization()
+{
+
+}
+
+void MassSpringRepresentationLocalization::setLocalNode(size_t nodeID)
+{
+ m_nodeID = nodeID;
+}
+
+const size_t& MassSpringRepresentationLocalization::getLocalNode() const
+{
+ return m_nodeID;
+}
+
+SurgSim::Math::Vector3d MassSpringRepresentationLocalization::doCalculatePosition(double time)
+{
+ std::shared_ptr<MassSpringRepresentation> massSpringRepresentation =
+ std::static_pointer_cast<MassSpringRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(massSpringRepresentation != nullptr) << "MassSpringRepresentation is null, it was probably not" <<
+ " initialized";
+ SURGSIM_ASSERT((0.0 <= time) && (time <= 1.0)) << "Time must be between 0.0 and 1.0 inclusive";
+
+ if (time == 0.0)
+ {
+ return massSpringRepresentation->getPreviousState()->getPosition(m_nodeID);
+ }
+ else if (time == 1.0)
+ {
+ return massSpringRepresentation->getCurrentState()->getPosition(m_nodeID);
+ }
+
+ const SurgSim::Math::Vector3d& currentPoint = massSpringRepresentation->getCurrentState()->getPosition(m_nodeID);
+ const SurgSim::Math::Vector3d& previousPoint = massSpringRepresentation->getPreviousState()->getPosition(m_nodeID);
+
+ return SurgSim::Math::interpolate(previousPoint, currentPoint, time);
+}
+
+bool MassSpringRepresentationLocalization::isValidRepresentation(std::shared_ptr<Representation> representation)
+{
+
+ std::shared_ptr<MassSpringRepresentation> massSpringRepresentation =
+ std::dynamic_pointer_cast<MassSpringRepresentation>(representation);
+
+ // Allows to reset the representation to nullptr ...
+ return (massSpringRepresentation != nullptr || representation == nullptr);
+}
+
+}; // Physics
+}; // SurgSim
+
+
diff --git a/SurgSim/Physics/MassSpringRepresentationLocalization.h b/SurgSim/Physics/MassSpringRepresentationLocalization.h
new file mode 100644
index 0000000..2a2b1b9
--- /dev/null
+++ b/SurgSim/Physics/MassSpringRepresentationLocalization.h
@@ -0,0 +1,81 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_MASSSPRINGREPRESENTATIONLOCALIZATION_H
+#define SURGSIM_PHYSICS_MASSSPRINGREPRESENTATIONLOCALIZATION_H
+
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Implementation of Localization for MassSpringRepresentation
+///
+/// MassSpringRepresentationLocalization tracks the global coordinates of a node contained in an associated
+/// MassSpringRepresentation. It is used, for example, as a helper class for filling out the MlcpPhysicsProblem in
+/// MassSpringRepresentationContact::doBuild, which constrains the motion of MassSpringRepresentation at a frictionless
+/// contact.
+///
+/// MassSpringRepresentationLocalization stores a pointer to a MassSpringRepresentation in an abstract Representation
+/// object. It tracks the ID of a node contained within the associated MassSpringRepresentation, and it provides a
+/// helper function MassSpringRepresentationLocalization::calculatePosition to find the node's position in global
+/// coordinates in the current state.
+class MassSpringRepresentationLocalization: public Localization
+{
+public:
+ /// Default constructor
+ MassSpringRepresentationLocalization();
+
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ explicit MassSpringRepresentationLocalization(std::shared_ptr<Representation> representation);
+
+ /// Destructor
+ virtual ~MassSpringRepresentationLocalization();
+
+ /// Sets the local node.
+ /// \param nodeID Node set for this localization.
+ void setLocalNode(size_t nodeID);
+
+ /// Gets the local node.
+ /// \return Node set for this localization.
+ const size_t& getLocalNode() const;
+
+ /// Queries whether Representation can be assigned to this class.
+ /// \param representation Representation to check.
+ /// \return true if Representation is valid.
+ virtual bool isValidRepresentation(std::shared_ptr<Representation> representation) override;
+
+private:
+ /// Calculates the global position of this localization.
+ /// \param time Interpolation parameter [0..1] for calcuting position between the previous state (0.0) and current
+ /// state (1.0).
+ /// \return The global position of the localization using an interpolation between the previous and current states.
+ /// \note The time parameter can useful when dealing with Continuous Collision Detection.
+ virtual SurgSim::Math::Vector3d doCalculatePosition(double time) override;
+
+ /// Node defining the localization.
+ size_t m_nodeID;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_MASSSPRINGREPRESENTATIONLOCALIZATION_H
diff --git a/SurgSim/Physics/MlcpMapping.h b/SurgSim/Physics/MlcpMapping.h
new file mode 100644
index 0000000..c15aa27
--- /dev/null
+++ b/SurgSim/Physics/MlcpMapping.h
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_MLCPMAPPING_H
+#define SURGSIM_PHYSICS_MLCPMAPPING_H
+
+#include <unordered_map>
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+template <class T>
+class MlcpMapping
+{
+public:
+ MlcpMapping(){}
+
+ /// Clear the mapping
+ void clear()
+ {
+ m_indexMapping.clear();
+ }
+
+ /// Sets the key/value (add an entry if the key is not found, change the value otherwise)
+ void setValue(const T* key, size_t value)
+ {
+ typename std::unordered_map<const T*, ptrdiff_t>::iterator found = m_indexMapping.find(key);
+ if (found == m_indexMapping.end())
+ {
+ m_indexMapping.insert(std::make_pair(key, value));
+ }
+ else
+ {
+ (*found).second = value;
+ }
+ }
+
+ /// Gets the value from a given key
+ ptrdiff_t getValue(const T* key) const
+ {
+ typename std::unordered_map<const T*, ptrdiff_t>::const_iterator returnValue = m_indexMapping.find(key);
+ return (returnValue == m_indexMapping.end() ? -1 : (*returnValue).second);
+ }
+
+private:
+
+ /// The index mapping data structure
+ std::unordered_map<const T*, ptrdiff_t> m_indexMapping;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_MLCPMAPPING_H
diff --git a/SurgSim/Physics/MlcpPhysicsProblem-inl.h b/SurgSim/Physics/MlcpPhysicsProblem-inl.h
new file mode 100644
index 0000000..cc3df97
--- /dev/null
+++ b/SurgSim/Physics/MlcpPhysicsProblem-inl.h
@@ -0,0 +1,57 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_INL_H
+#define SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_INL_H
+
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+template <typename SubCDerivedType>
+void MlcpPhysicsProblem::updateConstraint(
+ const Eigen::SparseVector<double>& newSubH,
+ const Eigen::MatrixBase<SubCDerivedType>& subC,
+ size_t indexSubC,
+ size_t indexNewSubH)
+{
+ using SurgSim::Math::Vector;
+
+ // Update H, CHt, and HCHt with newSubH, denoted H'.
+ //
+ // Note that updates are linear for H and CHt, but not for HCHt:
+ // (H+H') = H+H'
+ // => H += H';
+ //
+ // C(H+H')t = CHt + CH't
+ // => CHt += CH't;
+ //
+ // (H+H')C(H+H')t = HCHt + HCH't + H'C(H+H')t
+ // => HCHt += H(CH't) + H'[C(H+H')t];
+
+ Vector newCHt = subC * newSubH;
+ A.col(indexNewSubH) += H.middleCols(indexSubC, subC.rows()) * newCHt;
+ H.block(indexNewSubH, indexSubC, 1, subC.rows()) += newSubH.transpose();
+ CHt.block(indexSubC, indexNewSubH, subC.rows(), 1) += newCHt;
+ A.row(indexNewSubH) += newSubH.transpose() * CHt.middleRows(indexSubC, subC.rows());
+}
+
+}
+}
+
+#endif // SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_INL_H
diff --git a/SurgSim/Physics/MlcpPhysicsProblem.cpp b/SurgSim/Physics/MlcpPhysicsProblem.cpp
new file mode 100644
index 0000000..d309532
--- /dev/null
+++ b/SurgSim/Physics/MlcpPhysicsProblem.cpp
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+MlcpPhysicsProblem::~MlcpPhysicsProblem()
+{
+}
+
+void MlcpPhysicsProblem::setZero(size_t numDof, size_t numConstraintDof, size_t numConstraints)
+{
+ MlcpProblem::setZero(numDof, numConstraintDof, numConstraints);
+
+ H.resize(numConstraintDof, numDof);
+ H.setZero();
+ CHt.resize(numDof, numConstraintDof);
+ CHt.setZero();
+}
+
+MlcpPhysicsProblem MlcpPhysicsProblem::Zero(size_t numDof, size_t numConstraintDof, size_t numConstraints)
+{
+ MlcpPhysicsProblem result;
+ result.setZero(numDof, numConstraintDof, numConstraints);
+
+ return result;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/MlcpPhysicsProblem.h b/SurgSim/Physics/MlcpPhysicsProblem.h
new file mode 100644
index 0000000..3af2e0b
--- /dev/null
+++ b/SurgSim/Physics/MlcpPhysicsProblem.h
@@ -0,0 +1,94 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_H
+#define SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_H
+
+#include "SurgSim/Math/MlcpProblem.h"
+#include <Eigen/SparseCore>
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+/// A description of a physical mixed LCP system to be solved.
+///
+
+/// This extends \ref SurgSim::Math::MlcpProblem "the pure mathematical MLCP problem" by storing the intermediate
+/// matrices \ref H and \ref CHt that are necessary to physically interpret the solution.
+///
+/// Note that the matrix \f$\mathbf{A}\f$ used in the MlcpProblem is computed in the physical problem as
+/// \f$\mathbf{H\;C\;H^T}\f$, where \f$\mathbf{C}\f$ is the compliance matrix. For contact constraints, \f$b\f$ is
+/// the initial signed displacements between the colliding representations, \f$b_i \lt 0\f$ when the representations
+/// interpenetrate, \f$x\f$ is the forces to apply at each contact to prevent penetration, and \f$c\f$ is the signed
+/// displacements after the forces are applied.
+/// \note The solution to the MLCP will only address the constraints that were provided, and application of \f$x\f$ to
+/// the representations in the scene may cause new collisions for constraints that were not originally incorporated in
+/// the MLCP.
+///
+/// \sa SurgSim::Math::MlcpProblem
+struct MlcpPhysicsProblem : public SurgSim::Math::MlcpProblem
+{
+ /// Destructor
+ virtual ~MlcpPhysicsProblem() override;
+
+ /// The matrix \f$\mathbf{H}\f$, which is a matrix of size \f$c\times n\f$ that converts from
+ /// the \f$n\f$ degrees of freedom in the system (i.e., the sum of all the DOF over all the representations in the
+ /// scene), to the
+ /// \f$c\f$ degrees of freedom summed over all the constraints being applied to the system.
+ /// It is used to convert the vector of \f$n\f$ displacements of each degree of freedom of the system to the vector
+ /// of \f$c\f$ displacements of each degree of freedom of the constraints.
+ /// Given a set of constraints \f$\mathbf{G}(t, \mathbf{x})\f$, then
+ /// \f$\mathbf{H} = \frac{d \mathbf{G}}{d \mathbf{x}}\f$ (i.e., the constraints' tangential space).
+ Matrix H;
+
+ /// The matrix \f$\mathbf{C\;H^T}\f$, which is a matrix of size \f$n\times c\f$ that is used to convert the
+ /// vector of \f$c\f$ constraint forces to the \f$n\f$ displacements of each degree of freedom of the system.
+ Matrix CHt;
+
+ /// Applies a new constraint to a specific Representation
+ /// \param newSubH New constraint to be added to H
+ /// \param subC Compliance matrix associated with the Representation
+ /// \param indexSubC Index of the Representation's compliance matrix
+ /// \param indexNewSubH Index of the new constraint within H
+ /// \tparam SubCDerivedType the CRTP derived type of the passed subC matrix, which usually can be deduced
+ template <typename SubCDerivedType>
+ void updateConstraint(
+ const Eigen::SparseVector<double>& newSubH,
+ const Eigen::MatrixBase<SubCDerivedType>& subC,
+ size_t indexSubC,
+ size_t indexNewSubH);
+
+ /// Resize an MlcpPhysicsProblem and set to zero.
+ /// \param numDof the total degrees of freedom.
+ /// \param numConstraintDof the total constrained degrees of freedom.
+ /// \param numConstraints the number of constraints.
+ virtual void setZero(size_t numDof, size_t numConstraintDof, size_t numConstraints) override;
+
+ /// Initialize an MlcpPhysicsProblem with zero values.
+ /// \param numDof the total degrees of freedom for the MlcpPhysicsProblem to be constructed.
+ /// \param numConstraintDof the total constrained degrees of freedom for the MlcpPhysicsProblem to be constructed.
+ /// \param numConstraints the number of constraints for the MlcpPhysicsProblem to be constructed.
+ /// \return An MlcpPhysicsProblem appropriately sized and initialized to zero.
+ static MlcpPhysicsProblem Zero(size_t numDof, size_t numConstraintDof, size_t numConstraints);
+};
+
+}; // namespace Physics
+}; // namespace SurgSim
+
+#include "SurgSim/Physics/MlcpPhysicsProblem-inl.h"
+
+#endif // SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_H
diff --git a/SurgSim/Physics/MlcpPhysicsSolution.h b/SurgSim/Physics/MlcpPhysicsSolution.h
new file mode 100644
index 0000000..c85013d
--- /dev/null
+++ b/SurgSim/Physics/MlcpPhysicsSolution.h
@@ -0,0 +1,43 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_MLCPPHYSICSSOLUTION_H
+#define SURGSIM_PHYSICS_MLCPPHYSICSSOLUTION_H
+
+#include "SurgSim/Math/MlcpSolution.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+/// The description of a solution to a \ref MlcpPhysicsProblem "physical MLCP problem".
+///
+/// The solution consists of a \ref SurgSim::Math::MlcpSolution "mathematical solution for the MLCP" and
+/// the vector \ref dofCorrection containing the displacements (corrections) for each degree of freedom
+/// present in the system.
+///
+/// \sa SurgSim::Math::MlcpSolution, MlcpPhysicsProblem, SurgSim::Math::MlcpSolver
+
+struct MlcpPhysicsSolution: public SurgSim::Math::MlcpSolution
+{
+ /// Corrections to all of the degrees of freedom needed to satisfy the system equations.
+ Vector dofCorrection;
+};
+
+}; // namespace Physics
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_MLCPPHYSICSSOLUTION_H
diff --git a/SurgSim/Physics/PerformanceTests/CMakeLists.txt b/SurgSim/Physics/PerformanceTests/CMakeLists.txt
new file mode 100644
index 0000000..b8306ae
--- /dev/null
+++ b/SurgSim/Physics/PerformanceTests/CMakeLists.txt
@@ -0,0 +1,45 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+ ${OSG_INCLUDE_DIR}
+ ${OPENTHREADS_INCLUDE_DIR}
+)
+
+set(UNIT_TEST_SOURCES
+ Fem3DPerformanceTest.cpp
+)
+
+set(UNIT_TEST_HEADERS
+)
+
+set(LIBS
+ SurgSimInput
+ SurgSimPhysics
+)
+
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+surgsim_add_unit_tests(SurgSimPhysicsPerformanceTest)
+
+set_target_properties(SurgSimPhysicsPerformanceTest PROPERTIES FOLDER "Physics")
diff --git a/SurgSim/Physics/PerformanceTests/Data/Fem3DPerformanceTest/wound_deformable.ply b/SurgSim/Physics/PerformanceTests/Data/Fem3DPerformanceTest/wound_deformable.ply
new file mode 100644
index 0000000..b4de671
--- /dev/null
+++ b/SurgSim/Physics/PerformanceTests/Data/Fem3DPerformanceTest/wound_deformable.ply
@@ -0,0 +1,2389 @@
+ply
+format ascii 1.0
+comment This file is a part of the OpenSurgSim project.
+comment Copyright 2013, SimQuest Solutions Inc.
+comment
+comment Licensed under the Apache License, Version 2.0 (the "License");
+comment you may not use this file except in compliance with the License.
+comment You may obtain a copy of the License at
+comment
+comment http://www.apache.org/licenses/LICENSE-2.0
+comment
+comment Unless required by applicable law or agreed to in writing, software
+comment distributed under the License is distributed on an "AS IS" BASIS,
+comment WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+comment See the License for the specific language governing permissions and
+comment limitations under the License.
+comment
+comment This file provides the geometry which describes a tetrahedral volume
+comment mesh and triangular surface mesh of an arm wound.
+comment
+element vertex 391
+property double x
+property double y
+property double z
+element face 407
+property list uint uint vertex_indices
+element 3d_element 1476
+property list uint uint vertex_indices
+element boundary_condition 79
+property uint vertex_index
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+-0.017638 0.001427 -0.012363
+-0.018984 0.002069 -0.016867
+-0.014913 -0.001358 -0.015090
+-0.018090 -0.001832 -0.017359
+0.012643 -0.002455 0.001226
+0.009376 -0.006324 0.003409
+0.015479 -0.002289 0.003265
+0.014119 -0.002367 0.007046
+-0.000236 -0.006438 -0.015323
+-0.003732 -0.004010 -0.013609
+0.002155 -0.000546 -0.013612
+0.001262 -0.001523 -0.018049
+0.019667 -0.017785 0.008674
+0.029299 -0.005262 0.004546
+0.046990 -0.020879 0.007375
+0.037332 -0.020768 -0.005328
+0.033586 -0.016523 0.018450
+0.034101 -0.002031 0.016309
+0.033392 -0.002474 0.009623
+0.025349 -0.003774 0.014192
+0.019145 0.004850 0.012264
+0.020216 0.004879 0.011113
+0.020968 0.001399 0.010219
+0.018687 0.001314 0.008476
+-0.007352 0.001018 0.005860
+-0.007333 -0.005722 0.014291
+-0.013652 -0.000173 0.002866
+-0.007113 -0.008223 -0.002577
+-0.006477 -0.004144 -0.015932
+-0.010957 -0.004967 -0.018490
+-0.007629 -0.006377 -0.018801
+-0.009906 -0.007928 -0.014976
+0.014105 -0.020914 -0.009294
+0.009112 -0.006286 -0.008604
+0.006268 -0.009189 -0.014543
+0.002300 -0.008261 -0.009596
+0.006635 0.000026 -0.006193
+0.010115 -0.000309 -0.007852
+0.005102 -0.004418 -0.008996
+-0.006609 0.001420 -0.025080
+-0.011057 -0.007486 -0.026293
+-0.008811 0.000184 -0.034044
+-0.005338 -0.005470 -0.024596
+0.001259 0.002603 0.004418
+0.000123 0.002377 -0.000362
+0.002998 0.002713 0.001375
+0.002769 -0.002124 0.003090
+-0.020005 -0.002711 -0.020006
+-0.021803 0.001971 -0.019803
+-0.021059 0.001058 -0.014582
+0.014631 0.000524 0.027783
+0.022184 -0.002620 0.021520
+0.018739 -0.011699 0.030717
+0.015337 -0.003630 0.015467
+0.016049 0.003671 -0.002811
+0.018898 0.003898 -0.000351
+0.014287 0.000683 -0.000350
+0.019319 -0.000205 -0.003032
+0.046990 -0.007619 0.007375
+0.040075 -0.002131 -0.002012
+0.037332 -0.007509 -0.005328
+-0.011140 0.001039 -0.002776
+-0.014033 -0.006140 -0.001762
+-0.016684 0.000633 -0.005670
+-0.011809 -0.002190 -0.009170
+0.024204 0.005929 0.015670
+0.026606 0.006410 0.017058
+0.023576 0.006924 0.019679
+0.024566 0.001831 0.017759
+0.013743 0.004124 0.008547
+0.011935 0.004109 0.005502
+0.014670 0.004333 0.007344
+0.012704 0.000753 0.008033
+-0.003034 -0.009956 -0.020825
+-0.006038 -0.007999 -0.012282
+0.014736 0.003042 -0.006304
+0.014286 -0.003192 -0.009813
+0.014065 0.002151 -0.010793
+0.019818 -0.002939 -0.008113
+0.036755 0.006575 0.019271
+0.036907 0.001084 0.018946
+0.033263 0.001097 0.023070
+0.033376 0.002597 0.017918
+0.022543 0.001069 0.006601
+0.020245 -0.001947 0.007184
+0.019727 0.001041 0.004292
+0.023642 -0.002413 0.005541
+0.029219 0.001517 -0.003210
+0.022506 -0.007309 -0.002980
+0.013584 0.001132 0.004599
+0.015737 0.001218 0.006218
+0.015374 -0.005687 -0.005108
+0.019027 -0.006775 -0.014839
+0.030475 0.001257 0.015552
+0.027297 -0.001544 0.019319
+-0.006877 -0.002740 -0.012407
+-0.009683 -0.005474 -0.007552
+-0.008963 -0.000571 -0.011342
+-0.009123 -0.005772 -0.011405
+0.006753 0.004254 0.022359
+0.010408 0.004267 0.016270
+0.015903 0.005679 0.020105
+0.017492 0.000471 0.018668
+-0.018916 -0.006283 -0.013171
+-0.013552 -0.004660 -0.011679
+-0.015191 -0.008193 -0.008642
+-0.014454 -0.008852 -0.015804
+0.016241 0.004334 0.010184
+0.017210 0.004543 0.009049
+0.012207 0.000507 -0.002006
+0.015357 0.000245 -0.003791
+0.007154 -0.001854 0.005752
+0.011373 -0.002515 0.009976
+0.009600 -0.002702 0.003200
+0.022251 -0.002063 0.015960
+0.021468 -0.001789 0.012242
+0.017030 0.000365 0.013057
+0.020308 0.001792 0.013974
+0.013639 -0.003166 -0.002562
+0.011924 -0.003711 -0.006304
+-0.014146 -0.020429 -0.047689
+-0.014146 -0.007170 -0.047689
+-0.024140 -0.017967 -0.040188
+-0.012259 -0.006676 -0.033836
+0.002537 -0.008403 -0.037874
+-0.001499 -0.000057 -0.029561
+-0.000610 -0.002936 -0.039936
+0.031048 0.003384 0.005728
+0.031021 0.004616 0.009841
+0.029058 -0.002228 0.009819
+-0.017817 -0.005609 -0.020805
+-0.021077 -0.009144 -0.020053
+-0.019782 -0.005521 -0.017072
+-0.021431 -0.005952 -0.022335
+0.028581 0.004545 0.037841
+0.022848 0.002598 0.033651
+0.026518 0.008415 0.028378
+-0.008942 -0.024739 -0.008176
+-0.020167 -0.001455 -0.000035
+-0.019937 -0.006669 -0.033633
+-0.011533 -0.022757 -0.029013
+0.025681 0.001454 0.013298
+0.021352 0.005330 0.013823
+0.022555 0.005159 0.012575
+-0.018697 -0.003565 -0.023866
+-0.003741 -0.007976 -0.007476
+-0.004960 -0.004029 -0.006795
+0.035352 0.005531 0.016102
+0.033038 0.004995 0.012719
+0.036201 0.003833 0.011247
+-0.014781 0.002213 -0.020373
+-0.016880 -0.002824 -0.021921
+-0.016398 0.001977 -0.024375
+-0.013707 -0.002037 -0.021513
+-0.001967 0.002359 -0.017593
+-0.005030 0.002252 -0.019030
+-0.000406 0.001476 -0.021158
+-0.003015 -0.003057 -0.018944
+0.005527 0.000425 -0.000868
+0.005663 0.003045 0.003152
+0.003921 0.003399 0.000160
+-0.015527 -0.005152 -0.022539
+-0.015560 -0.006666 -0.026900
+-0.014996 -0.002896 -0.024296
+-0.013917 -0.007956 -0.024021
+0.004887 -0.021029 0.001820
+0.002057 -0.006870 -0.003212
+0.022552 -0.001675 0.009108
+-0.024489 0.000741 -0.016918
+-0.024391 0.001936 -0.022406
+-0.024601 -0.004739 -0.018778
+0.006850 -0.006797 -0.004277
+0.004032 -0.004089 -0.000551
+0.012333 0.004106 0.011509
+0.005130 0.003275 0.012820
+0.007317 0.003436 0.008224
+-0.004058 0.001878 0.001264
+-0.014203 0.001575 -0.029189
+-0.010570 -0.002455 -0.021860
+-0.012199 0.002106 -0.022621
+-0.025827 -0.009187 -0.021112
+-0.021601 -0.004637 -0.026044
+-0.017290 -0.008529 -0.023047
+-0.013967 -0.002130 -0.018341
+-0.014398 -0.004396 -0.019489
+-0.016250 -0.002623 -0.019238
+0.002921 -0.006003 -0.021793
+0.009600 0.002834 -0.010048
+0.012195 0.002939 -0.008156
+0.014492 -0.021349 -0.029300
+0.026117 -0.020765 -0.017965
+0.014492 -0.008090 -0.029300
+0.002621 -0.007830 -0.029082
+0.008423 0.000639 0.001153
+0.030678 0.007333 0.019744
+0.026656 0.007758 0.022058
+0.020949 0.007122 0.023900
+0.024451 0.002704 0.023354
+-0.023157 -0.024020 -0.019768
+0.029177 0.003608 0.018712
+0.012003 0.005360 0.025917
+0.004293 -0.015189 0.020860
+0.002537 -0.021663 -0.037874
+0.017564 -0.002515 -0.026912
+0.011421 -0.002833 -0.031689
+0.011215 0.000322 -0.020415
+0.002635 0.003021 -0.012333
+0.001235 0.002490 -0.015739
+0.004272 0.002632 -0.013690
+-0.002997 0.002057 -0.002458
+-0.002614 -0.004314 -0.002919
+0.003125 -0.022994 -0.021280
+-0.005395 -0.000823 -0.013764
+-0.008954 -0.001258 -0.015629
+-0.003834 0.002652 -0.016057
+-0.007047 -0.000971 -0.018908
+0.010433 -0.002408 -0.000327
+0.007502 -0.002301 -0.002415
+0.009372 0.000321 -0.004108
+0.025152 0.001195 0.009053
+0.016960 0.000956 0.001995
+0.019875 -0.003369 -0.000014
+0.030751 0.008190 0.022981
+0.006170 -0.006701 0.007879
+0.004293 -0.001930 0.020860
+-0.000653 -0.004919 0.008637
+-0.014173 -0.021266 0.011121
+-0.017277 -0.006019 -0.017796
+0.023544 -0.000939 0.002037
+-0.009768 -0.009134 -0.022027
+-0.009580 -0.002948 -0.014062
+-0.011797 -0.001691 -0.017176
+-0.010204 0.002309 -0.009891
+-0.011598 0.001574 -0.008061
+-0.008708 0.001627 -0.006285
+0.034635 0.001794 0.002947
+0.039472 0.002062 0.008665
+-0.015907 -0.007981 -0.018944
+-0.020598 0.001813 -0.025718
+-0.045621 -0.006163 -0.019740
+-0.040582 -0.008149 -0.025424
+-0.042936 -0.004004 -0.023210
+-0.038274 -0.003561 -0.017712
+-0.025030 -0.004697 -0.013625
+-0.023215 -0.000003 -0.009663
+-0.021118 -0.002997 -0.014161
+-0.004112 0.000695 0.015891
+-0.016389 -0.004557 -0.019826
+-0.003547 -0.002715 -0.010257
+-0.001840 -0.000524 -0.011726
+-0.000903 -0.002832 -0.008443
+-0.001161 -0.006198 -0.011121
+0.026165 -0.000632 0.016453
+0.025651 0.005503 0.014190
+-0.045621 -0.011579 -0.019740
+-0.040582 -0.021408 -0.025424
+-0.032217 -0.008366 -0.016728
+0.000124 -0.006312 0.001727
+0.017517 -0.002166 0.004909
+0.011949 -0.006639 -0.000263
+0.023286 0.001530 0.011808
+-0.013427 -0.006320 -0.020900
+0.005159 0.001722 -0.017378
+0.006008 -0.003221 -0.013176
+0.044680 -0.002154 0.004195
+-0.000907 0.002148 0.009167
+0.018182 0.005343 0.015586
+0.043927 0.001725 0.025605
+-0.012854 0.002261 -0.012050
+-0.014517 0.001574 -0.010230
+-0.011668 -0.001006 -0.012914
+-0.008316 0.002150 -0.020675
+-0.010664 0.002390 -0.018678
+-0.021013 -0.010291 0.007951
+-0.014146 -0.001754 -0.047689
+-0.008422 -0.002312 -0.044596
+-0.015684 0.000035 -0.039467
+-0.034331 -0.001228 -0.022520
+-0.031662 -0.007124 -0.023091
+-0.031224 -0.014703 0.002956
+-0.022628 -0.011995 -0.002126
+-0.025924 -0.004114 -0.023568
+0.027872 -0.000761 0.013734
+-0.034591 -0.000064 -0.031338
+-0.029145 0.000494 -0.024321
+-0.028017 -0.005001 -0.028032
+0.027902 0.001681 0.011326
+0.025087 -0.001165 0.011230
+-0.021752 0.000419 -0.042010
+-0.026528 0.000997 -0.038365
+-0.024140 -0.004708 -0.040188
+-0.020978 0.001288 -0.033343
+-0.040551 -0.026143 -0.010199
+-0.023197 -0.010713 -0.012877
+-0.040551 -0.012883 -0.010199
+0.004485 -0.000007 -0.025484
+0.010626 0.000849 0.002651
+-0.028255 0.000295 -0.019377
+0.013332 0.003514 -0.005048
+0.012798 0.000333 -0.005951
+-0.015803 0.002187 -0.014369
+-0.013341 -0.003209 -0.016395
+0.004675 -0.002400 -0.004481
+0.002779 0.000374 -0.003048
+0.026117 -0.007505 -0.017965
+0.028859 -0.001994 -0.015040
+-0.018952 -0.009957 -0.016014
+-0.017184 -0.003861 -0.014796
+0.020301 0.003439 -0.001538
+0.046716 -0.013576 0.023724
+0.055055 -0.021059 0.018567
+0.055055 -0.007800 0.018567
+-0.007396 0.002390 -0.007873
+0.001875 -0.002670 -0.006494
+0.038019 -0.008659 0.029998
+-0.008029 -0.002626 -0.003881
+-0.005500 -0.000144 -0.009081
+-0.016373 -0.003539 -0.018074
+-0.010554 -0.001307 0.012690
+0.049300 -0.002253 0.010554
+0.038759 -0.002166 0.014632
+0.005684 -0.003039 -0.035812
+0.038019 0.004600 0.029998
+0.038019 0.010016 0.029998
+-0.026286 -0.002491 -0.003954
+0.017414 0.003222 -0.004081
+0.044485 0.002423 0.015009
+0.040181 0.004389 0.016007
+-0.000017 0.000235 -0.005153
+-0.033970 -0.003806 -0.010656
+0.009082 0.001920 -0.014465
+0.017757 0.000902 -0.014705
+-0.030192 -0.001153 -0.014581
+0.025315 0.003889 0.003730
+0.024009 0.004399 0.004815
+-0.000443 0.002808 -0.014194
+0.005274 0.003135 -0.010676
+0.006826 0.002728 -0.011961
+0.008126 0.003261 -0.008819
+0.003879 -0.000228 -0.008173
+0.032342 0.006310 0.016942
+0.023610 0.001317 -0.009141
+0.028043 0.004252 0.006706
+0.028016 -0.000536 0.005381
+0.017259 0.006519 0.029649
+-0.032441 -0.018179 -0.033098
+0.022512 0.003646 0.000819
+0.027741 0.001325 0.014450
+0.028451 0.005830 0.015287
+-0.017863 -0.003577 0.009446
+0.021215 0.004148 0.001990
+0.025358 0.002875 -0.000357
+-0.025580 0.001252 -0.027579
+-0.032441 -0.004919 -0.033098
+-0.038228 -0.001462 -0.027638
+-0.019666 0.002058 -0.021535
+0.028581 -0.008715 0.037841
+0.010757 0.003389 -0.006914
+0.019618 0.002475 -0.006065
+-0.031224 -0.009287 0.002956
+-0.031224 -0.027962 0.002956
+0.034589 -0.002055 -0.008644
+-0.002831 -0.000029 -0.007070
+-0.005830 0.001763 -0.004361
+-0.038103 -0.007971 -0.006212
+0.031060 0.005869 0.013961
+-0.030292 0.001057 -0.034857
+-0.024162 -0.006173 0.006457
+-0.042999 -0.006964 -0.014187
+0.001834 0.002718 0.019362
+0.049505 -0.002359 0.021843
+0.011040 0.003784 0.006750
+-0.001834 0.002801 -0.003863
+0.001083 0.003102 -0.001764
+0.022848 0.008014 0.033651
+0.023375 -0.002185 -0.020891
+0.028581 0.009961 0.037841
+0.006652 0.003669 0.002024
+0.009164 0.003881 0.003676
+0.008207 0.003356 0.004901
+0.049505 0.003057 0.021843
+0.055055 -0.002384 0.018567
+0.001013 -0.000353 -0.009972
+0.029329 0.005348 0.010717
+-0.004550 0.002514 -0.005831
+-0.007071 0.002482 -0.017312
+0.046366 0.005522 0.023902
+-0.045621 -0.024839 -0.019740
+0.041488 0.008760 0.027307
+0.033206 0.010046 0.034162
+0.026563 0.004882 0.007661
+3 43 45 44
+3 65 67 66
+3 69 71 70
+3 99 101 100
+3 61 63 138
+3 65 143 142
+3 147 149 148
+3 154 156 155
+3 159 160 45
+3 168 48 169
+3 173 175 174
+3 43 44 176
+3 187 188 77
+3 67 196 195
+3 66 67 195
+3 150 179 152
+3 203 204 205
+3 206 208 207
+3 176 44 209
+3 232 233 234
+3 235 127 236
+3 48 49 1
+3 239 242 241
+3 253 143 65
+3 87 235 59
+3 152 179 177
+3 41 39 125
+3 179 272 271
+3 274 276 275
+3 283 277 284
+3 266 107 173
+3 288 289 291
+3 48 168 49
+3 208 262 207
+3 156 262 295
+3 297 169 284
+3 142 67 65
+3 188 298 75
+3 268 300 269
+3 300 0 269
+3 312 232 234
+3 1 49 0
+3 236 127 149
+3 233 63 61
+3 265 24 318
+3 319 264 236
+3 321 126 125
+3 79 222 323
+3 276 288 291
+3 107 71 69
+3 149 327 326
+3 269 63 233
+3 49 244 0
+3 77 331 330
+3 332 168 297
+3 268 269 233
+3 142 20 266
+3 325 54 55
+3 262 330 205
+3 0 244 63
+3 195 136 222
+3 336 338 337
+3 79 340 194
+3 246 174 265
+3 195 222 194
+3 26 138 349
+3 333 346 350
+3 351 333 127
+3 169 352 284
+3 325 55 308
+3 244 332 324
+3 241 242 277
+3 354 241 277
+3 39 271 155
+3 59 235 264
+3 169 355 238
+3 205 204 295
+3 20 108 107
+3 335 154 214
+3 87 341 358
+3 358 351 87
+3 107 69 173
+3 359 324 329
+3 41 177 39
+3 326 319 236
+3 100 173 174
+3 359 329 364
+3 214 154 155
+3 154 335 207
+3 283 354 277
+3 142 143 21
+3 289 366 291
+3 348 253 66
+3 138 367 349
+3 368 329 242
+3 361 305 341
+3 77 341 331
+3 232 268 233
+3 338 187 337
+3 174 369 100
+3 100 369 99
+3 43 265 174
+3 265 43 176
+3 127 333 342
+3 154 207 156
+3 196 200 344
+3 188 187 357
+3 188 75 77
+3 173 371 175
+3 340 348 194
+3 372 44 373
+3 147 340 79
+3 333 350 334
+3 126 41 125
+3 330 262 208
+3 291 366 352
+3 374 376 136
+3 351 346 333
+3 377 379 378
+3 331 375 205
+3 355 152 238
+3 380 381 326
+3 234 233 61
+3 367 324 359
+3 196 101 200
+3 342 128 127
+3 188 357 298
+3 379 371 378
+3 358 325 308
+3 383 148 128
+3 177 291 238
+3 138 324 367
+3 136 344 374
+3 173 101 266
+3 168 169 297
+3 149 326 236
+3 209 372 384
+3 108 71 107
+3 337 187 330
+3 149 127 128
+3 357 187 338
+3 373 45 160
+3 173 100 101
+3 265 318 246
+3 101 99 200
+3 21 108 20
+3 24 265 176
+3 55 346 308
+3 371 70 378
+3 152 177 238
+3 351 127 235
+3 168 244 49
+3 26 61 138
+3 305 375 331
+3 266 101 196
+3 156 295 125
+3 168 332 244
+3 308 346 351
+3 366 283 284
+3 329 332 242
+3 244 138 63
+3 177 179 39
+3 136 196 344
+3 361 87 59
+3 77 358 341
+3 373 44 45
+3 327 79 386
+3 207 262 156
+3 148 149 128
+3 55 350 346
+3 222 79 194
+3 332 297 277
+3 39 155 156
+3 147 148 365
+3 242 332 277
+3 187 77 330
+3 1 0 300
+3 372 209 44
+3 246 369 174
+3 26 349 318
+3 26 318 24
+3 332 329 324
+3 155 271 385
+3 156 125 39
+3 271 272 385
+3 364 329 368
+3 175 379 159
+3 348 66 194
+3 383 365 148
+3 136 389 222
+3 342 390 383
+3 326 327 386
+3 126 275 41
+3 323 222 389
+3 277 297 284
+3 142 266 67
+3 173 69 371
+3 75 298 54
+3 380 326 386
+3 381 319 326
+3 388 79 323
+3 308 351 358
+3 352 169 238
+3 386 79 388
+3 136 376 389
+3 239 368 242
+3 208 337 330
+3 358 75 325
+3 77 75 358
+3 253 65 66
+3 0 63 269
+3 361 341 87
+3 204 321 295
+3 266 20 107
+3 147 365 340
+3 43 174 175
+3 75 54 325
+3 128 342 383
+3 150 272 179
+3 266 196 67
+3 175 159 43
+3 312 363 384
+3 351 235 87
+3 355 150 152
+3 206 336 208
+3 214 155 385
+3 147 79 327
+3 208 336 337
+3 291 177 41
+3 363 234 61
+3 363 312 234
+3 371 69 70
+3 274 288 276
+3 159 377 160
+3 66 195 194
+3 330 331 205
+3 321 125 295
+3 342 334 390
+3 176 363 61
+3 26 176 61
+3 375 203 205
+3 379 175 371
+3 341 305 331
+3 291 41 276
+3 24 176 26
+3 262 205 295
+3 209 363 176
+3 276 41 275
+3 159 45 43
+3 335 206 207
+3 20 142 21
+3 366 284 352
+3 147 327 149
+3 39 179 271
+3 209 384 363
+3 379 377 159
+3 264 235 236
+3 333 334 342
+3 195 196 136
+3 244 324 138
+3 291 352 238
+3 1 2 3
+3 21 22 23
+3 47 48 3
+3 108 23 90
+3 48 1 3
+3 253 141 143
+3 268 270 2
+3 2 300 268
+3 97 232 312
+3 97 312 316
+3 23 22 167
+3 48 47 169
+3 6 90 258
+3 317 47 3
+3 193 4 216
+3 95 97 316
+3 4 193 296
+3 4 89 6
+3 282 347 93
+3 348 347 253
+3 316 250 248
+3 89 90 6
+3 372 373 303
+3 2 230 301
+3 143 141 260
+3 70 89 296
+3 377 378 296
+3 301 317 2
+3 282 141 347
+3 328 302 313
+3 141 253 347
+3 71 90 89
+3 328 384 372
+3 108 90 71
+3 328 303 302
+3 217 158 216
+3 373 160 158
+3 21 23 108
+3 378 70 296
+3 22 287 167
+3 312 362 316
+3 89 4 296
+3 143 260 21
+3 328 313 362
+3 1 300 2
+3 270 268 232
+3 22 260 287
+3 316 362 250
+3 84 90 23
+3 71 89 70
+3 141 282 287
+3 230 2 270
+3 193 216 158
+3 362 313 250
+3 270 232 97
+3 348 340 93
+3 84 258 90
+3 312 384 362
+3 377 296 193
+3 193 158 160
+3 373 158 303
+3 377 193 160
+3 317 3 2
+3 95 230 97
+3 230 270 97
+3 372 303 328
+3 217 303 158
+3 328 362 384
+3 248 95 316
+3 217 302 303
+3 84 23 167
+3 260 22 21
+3 260 141 287
+3 347 348 93
+3 54 56 55
+3 83 85 84
+3 56 54 109
+3 212 214 213
+3 216 218 217
+3 219 83 167
+3 230 213 231
+3 248 250 249
+3 56 220 55
+3 286 219 287
+3 85 220 258
+3 185 317 183
+3 150 183 272
+3 83 334 85
+3 336 339 338
+3 301 230 231
+3 95 248 212
+3 183 301 231
+3 109 216 4
+3 282 286 287
+3 169 47 355
+3 338 339 36
+3 335 214 212
+3 4 220 56
+3 339 313 302
+3 85 334 350
+3 47 317 185
+3 357 36 218
+3 382 206 335
+3 250 313 382
+3 334 83 219
+3 231 213 272
+3 218 36 217
+3 219 286 383
+3 220 4 6
+3 286 282 93
+3 183 317 301
+3 213 214 385
+3 167 83 84
+3 357 109 298
+3 36 357 338
+3 272 213 385
+3 55 85 350
+3 85 258 84
+3 336 382 339
+3 336 206 382
+3 249 250 382
+3 230 95 213
+3 47 185 355
+3 340 365 93
+3 248 249 212
+3 383 286 365
+3 219 383 390
+3 249 382 335
+3 218 216 109
+3 219 167 287
+3 286 93 365
+3 217 36 302
+3 54 298 109
+3 357 218 109
+3 185 150 355
+3 183 231 272
+3 36 339 302
+3 313 339 382
+3 334 219 390
+3 185 183 150
+3 85 55 220
+3 213 95 212
+3 258 220 6
+3 56 109 4
+3 335 212 249
+3 14 310 309
+4 0 1 2 3
+4 4 5 6 7
+4 8 9 10 11
+4 12 13 14 15
+4 16 17 18 19
+4 20 21 22 23
+4 24 25 26 27
+4 28 29 30 31
+4 32 33 34 35
+4 36 37 33 38
+4 39 40 41 42
+4 43 44 45 46
+4 47 3 48 49
+4 50 51 52 53
+4 54 55 56 57
+4 58 59 60 13
+4 61 62 63 64
+4 65 66 67 68
+4 69 70 71 72
+4 73 8 30 74
+4 75 76 77 78
+4 79 80 81 82
+4 83 84 85 86
+4 87 88 60 13
+4 89 7 90 72
+4 91 78 88 92
+4 93 82 94 17
+4 95 96 97 98
+4 99 100 101 102
+4 103 104 105 106
+4 107 108 23 90
+4 56 109 54 110
+4 111 7 112 113
+4 114 115 116 117
+4 91 118 110 119
+4 120 121 122 123
+4 124 125 126 123
+4 127 18 128 129
+4 130 131 132 133
+4 134 135 136 81
+4 105 27 137 74
+4 61 138 63 62
+4 122 139 140 123
+4 65 141 142 143
+4 47 144 130 133
+4 27 145 96 146
+4 147 148 149 82
+4 150 151 152 153
+4 154 155 156 157
+4 158 159 160 45
+4 161 162 163 164
+4 165 145 27 166
+4 167 12 84 86
+4 168 169 48 170
+4 5 171 166 172
+4 173 174 175 112
+4 43 176 44 46
+4 177 178 179 163
+4 180 181 182 139
+4 183 184 185 153
+4 8 186 157 11
+4 187 77 188 37
+4 173 116 112 72
+4 189 190 191 92
+4 192 186 125 42
+4 159 158 193 46
+4 79 81 194 82
+4 67 195 196 197
+4 122 198 140 182
+4 66 195 67 199
+4 150 152 179 153
+4 200 50 99 102
+4 12 201 52 53
+4 15 14 58 13
+4 202 120 140 123
+4 191 203 204 205
+4 152 151 163 153
+4 206 207 208 10
+4 176 209 44 210
+4 137 35 211 74
+4 212 213 214 215
+4 216 217 218 119
+4 219 167 83 86
+4 118 220 221 56
+4 195 199 222 197
+4 174 223 224 225
+4 137 226 165 27
+4 47 132 130 227
+4 88 228 221 86
+4 131 170 132 133
+4 106 31 137 229
+4 230 231 213 29
+4 232 233 97 234
+4 235 236 127 18
+4 48 3 1 49
+4 214 154 212 157
+4 130 237 182 161
+4 114 68 141 117
+4 125 123 192 42
+4 238 181 177 144
+4 239 240 241 242
+4 103 243 244 245
+4 193 111 159 46
+4 246 224 25 225
+4 184 161 247 151
+4 248 249 250 251
+4 94 199 93 252
+4 253 141 65 143
+4 254 255 240 256
+4 257 210 176 46
+4 258 221 12 259
+4 22 115 260 117
+4 260 115 114 117
+4 94 82 81 17
+4 96 146 145 98
+4 178 163 261 153
+4 262 263 208 10
+4 264 13 58 18
+4 24 265 25 225
+4 87 235 60 59
+4 152 177 179 163
+4 116 101 102 266
+4 34 35 33 38
+4 24 257 176 225
+4 41 125 39 42
+4 261 164 40 229
+4 94 17 16 19
+4 267 81 79 80
+4 268 269 2 270
+4 95 74 248 98
+4 179 271 272 178
+4 178 184 261 29
+4 25 27 273 62
+4 274 121 275 276
+4 277 256 242 278
+4 273 226 279 280
+4 210 145 250 166
+4 181 133 47 281
+4 282 17 94 19
+4 56 55 220 57
+4 283 284 277 285
+4 182 162 161 164
+4 286 287 219 129
+4 266 173 107 116
+4 174 224 246 225
+4 282 18 93 17
+4 51 114 94 19
+4 288 289 290 291
+4 223 257 165 225
+4 205 34 191 92
+4 47 48 168 49
+4 292 293 256 294
+4 208 207 262 10
+4 156 295 262 11
+4 149 80 147 82
+4 223 46 257 225
+4 296 111 193 113
+4 165 257 223 5
+4 297 284 169 281
+4 117 142 67 65
+4 188 75 298 299
+4 85 258 220 221
+4 170 133 180 281
+4 178 215 30 42
+4 270 104 230 64
+4 2 268 300 269
+4 184 106 301 227
+4 302 210 172 303
+4 106 237 261 229
+4 221 118 91 259
+4 300 0 2 269
+4 304 88 305 78
+4 182 131 198 306
+4 3 307 49 245
+4 55 308 220 57
+4 132 243 103 245
+4 14 309 310 311
+4 145 74 96 98
+4 178 29 183 153
+4 44 210 303 172
+4 97 312 232 234
+4 302 166 313 38
+4 1 49 3 0
+4 174 111 223 225
+4 94 81 314 17
+4 73 31 30 229
+4 177 139 291 123
+4 140 198 137 106
+4 211 192 73 186
+4 236 149 127 18
+4 100 224 174 112
+4 176 210 24 315
+4 261 29 184 237
+4 269 104 270 64
+4 292 294 280 293
+4 97 316 312 234
+4 250 145 248 251
+4 23 167 22 115
+4 135 51 50 197
+4 61 315 26 62
+4 48 169 47 170
+4 234 315 61 64
+4 178 229 40 42
+4 137 145 165 35
+4 233 61 63 64
+4 185 183 317 184
+4 46 113 5 172
+4 177 181 291 139
+4 6 258 90 7
+4 135 81 51 197
+4 265 24 25 318
+4 319 236 264 320
+4 181 162 177 144
+4 321 126 124 125
+4 8 10 263 11
+4 322 79 222 323
+4 244 103 324 243
+4 54 110 325 57
+4 73 34 211 186
+4 114 116 53 102
+4 189 211 191 192
+4 51 68 94 252
+4 317 3 47 132
+4 290 276 288 291
+4 282 94 93 252
+4 106 227 237 306
+4 91 299 110 76
+4 261 247 184 161
+4 294 292 280 279
+4 179 178 272 153
+4 150 272 183 153
+4 193 216 4 113
+4 107 90 69 71
+4 12 53 84 7
+4 33 171 302 38
+4 198 293 137 306
+4 149 326 327 320
+4 269 233 63 64
+4 49 0 244 245
+4 6 221 258 259
+4 166 171 35 38
+4 47 247 317 227
+4 291 139 276 123
+4 44 209 328 210
+4 168 170 47 245
+4 191 190 304 92
+4 181 285 291 139
+4 329 243 293 256
+4 95 316 97 96
+4 96 62 315 64
+4 60 88 15 13
+4 77 330 331 76
+4 293 103 105 306
+4 182 181 180 133
+4 137 27 165 145
+4 50 197 51 102
+4 332 297 168 170
+4 106 29 301 31
+4 34 76 33 92
+4 40 162 140 164
+4 320 80 149 17
+4 137 106 198 306
+4 30 157 73 42
+4 140 182 198 306
+4 268 233 269 64
+4 106 307 301 227
+4 142 266 20 117
+4 83 333 85 334
+4 47 170 169 281
+4 313 166 35 38
+4 325 55 54 57
+4 30 8 73 157
+4 262 205 330 263
+4 0 63 244 103
+4 125 186 156 42
+4 3 132 317 307
+4 12 112 53 7
+4 250 35 313 166
+4 94 114 51 252
+4 195 222 136 197
+4 335 10 249 11
+4 18 17 129 19
+4 111 46 223 225
+4 336 337 338 339
+4 266 68 101 102
+4 156 186 157 42
+4 13 86 12 19
+4 18 129 16 19
+4 294 255 254 256
+4 293 243 180 256
+4 137 73 140 229
+4 4 296 193 113
+4 79 194 340 82
+4 295 186 262 11
+4 305 78 341 92
+4 157 9 8 11
+4 342 286 219 343
+4 140 306 106 229
+4 196 135 344 197
+4 93 199 94 82
+4 198 182 345 180
+4 329 293 294 256
+4 5 46 111 113
+4 163 161 261 153
+4 242 254 240 256
+4 293 131 180 170
+4 216 259 4 113
+4 346 333 85 228
+4 207 10 335 11
+4 40 164 140 229
+4 246 265 174 225
+4 195 194 222 199
+4 66 347 348 199
+4 26 349 138 62
+4 333 346 85 350
+4 301 231 230 29
+4 347 199 68 252
+4 351 127 333 343
+4 95 212 248 9
+4 140 164 182 229
+4 73 192 40 42
+4 240 242 239 254
+4 114 68 51 252
+4 213 29 231 215
+4 248 145 250 146
+4 182 237 261 161
+4 52 51 12 53
+4 2 270 269 104
+4 258 6 5 7
+4 184 151 185 153
+4 81 199 94 197
+4 219 129 287 86
+4 97 96 316 64
+4 248 74 95 9
+4 183 231 301 29
+4 169 284 352 281
+4 314 80 320 17
+4 104 64 96 98
+4 217 5 216 172
+4 286 343 127 129
+4 292 198 255 256
+4 33 37 36 119
+4 121 276 290 139
+4 290 285 353 139
+4 130 131 237 227
+4 349 273 138 62
+4 169 181 238 144
+4 325 308 55 57
+4 33 76 37 119
+4 152 163 179 153
+4 111 46 193 113
+4 8 263 34 186
+4 244 324 332 243
+4 104 106 103 307
+4 109 4 216 118
+4 295 192 204 186
+4 241 242 240 277
+4 240 354 241 277
+4 204 192 191 186
+4 39 155 271 215
+4 36 37 218 119
+4 110 118 91 57
+4 353 122 290 139
+4 88 78 304 92
+4 282 287 286 129
+4 303 158 45 46
+4 314 322 267 81
+4 201 223 165 225
+4 230 31 28 98
+4 84 12 258 86
+4 28 29 213 215
+4 59 264 235 18
+4 4 6 89 7
+4 132 170 47 133
+4 51 197 68 102
+4 169 355 47 238
+4 141 68 114 252
+4 356 314 52 51
+4 35 251 8 74
+4 93 18 282 129
+4 218 37 357 299
+4 191 205 204 295
+4 20 23 107 108
+4 149 18 236 320
+4 39 215 178 42
+4 267 309 314 320
+4 47 181 169 144
+4 47 170 132 245
+4 338 337 36 339
+4 216 5 217 259
+4 335 154 212 214
+4 212 157 249 9
+4 173 112 175 72
+4 20 116 22 117
+4 87 358 341 57
+4 2 269 0 104
+4 358 87 351 57
+4 107 173 69 72
+4 217 113 158 172
+4 25 226 273 27
+4 359 329 324 280
+4 41 39 177 40
+4 360 226 137 280
+4 112 7 5 113
+4 4 220 118 56
+4 60 87 361 88
+4 257 5 165 166
+4 224 50 52 53
+4 326 236 319 320
+4 249 251 248 9
+4 218 299 109 118
+4 305 304 361 88
+4 47 133 170 281
+4 100 174 173 112
+4 121 139 122 123
+4 198 293 292 280
+4 316 362 363 146
+4 32 34 211 35
+4 226 280 273 62
+4 294 359 329 364
+4 85 228 83 86
+4 244 105 324 103
+4 40 178 177 163
+4 214 155 154 157
+4 161 151 184 153
+4 169 181 47 281
+4 302 33 36 171
+4 154 207 335 11
+4 302 171 217 172
+4 20 23 22 116
+4 293 103 324 280
+4 283 277 354 285
+4 255 180 345 278
+4 339 302 313 38
+4 154 249 212 157
+4 27 145 210 166
+4 365 93 148 82
+4 28 31 74 98
+4 282 93 347 252
+4 121 275 276 123
+4 261 178 40 164
+4 53 112 173 116
+4 142 21 143 117
+4 81 82 80 17
+4 289 291 366 285
+4 140 137 211 73
+4 360 137 198 280
+4 348 66 253 347
+4 137 27 105 280
+4 101 68 196 102
+4 138 349 367 273
+4 316 248 250 146
+4 51 81 94 197
+4 53 116 100 102
+4 140 73 211 192
+4 368 242 329 256
+4 63 104 269 64
+4 361 341 305 88
+4 182 164 237 229
+4 337 37 339 263
+4 180 243 293 170
+4 77 331 341 78
+4 140 162 182 164
+4 154 157 156 11
+4 44 46 210 172
+4 232 233 268 64
+4 263 10 262 11
+4 51 68 114 102
+4 363 315 316 146
+4 66 68 347 199
+4 140 162 40 123
+4 338 337 187 37
+4 327 320 326 80
+4 174 224 100 369
+4 100 224 99 369
+4 339 37 36 38
+4 43 111 174 265
+4 250 145 210 146
+4 265 176 43 225
+4 180 281 285 278
+4 127 342 333 343
+4 154 156 207 11
+4 69 71 90 72
+4 196 344 200 50
+4 209 210 176 315
+4 140 40 73 192
+4 165 5 12 259
+4 223 112 12 5
+4 220 118 221 6
+4 188 357 187 37
+4 188 77 75 299
+4 304 60 361 88
+4 220 228 221 57
+4 272 178 231 153
+4 182 161 261 164
+4 327 326 267 80
+4 319 58 370 320
+4 58 309 14 311
+4 211 34 73 35
+4 259 171 217 119
+4 238 163 152 144
+4 173 175 371 72
+4 12 53 51 19
+4 89 6 90 7
+4 83 343 219 86
+4 159 111 43 46
+4 182 180 198 131
+4 340 194 348 199
+4 177 162 181 139
+4 88 13 87 228
+4 303 46 44 172
+4 137 106 105 31
+4 69 90 107 72
+4 74 31 105 98
+4 372 303 373 44
+4 329 293 324 280
+4 258 12 84 7
+4 224 53 100 102
+4 47 238 355 144
+4 47 49 168 245
+4 314 51 356 81
+4 304 78 305 92
+4 136 134 374 135
+4 141 114 287 19
+4 165 27 226 257
+4 2 301 230 104
+4 90 7 23 72
+4 27 145 137 74
+4 263 186 8 11
+4 147 79 340 82
+4 359 273 279 280
+4 375 304 305 92
+4 5 7 4 113
+4 223 53 12 112
+4 137 105 293 280
+4 333 85 334 350
+4 148 93 149 17
+4 137 31 73 229
+4 34 33 32 92
+4 91 118 221 57
+4 126 125 41 123
+4 143 260 141 117
+4 330 208 262 263
+4 370 309 267 320
+4 47 185 317 247
+4 83 228 343 86
+4 304 203 191 92
+4 317 307 132 227
+4 30 29 178 229
+4 291 352 366 285
+4 91 57 221 78
+4 184 247 261 237
+4 374 134 136 376
+4 294 254 368 256
+4 371 70 89 296
+4 261 237 247 161
+4 357 218 36 37
+4 347 68 141 252
+4 59 13 264 18
+4 351 333 346 228
+4 124 120 202 123
+4 231 178 272 215
+4 316 315 96 146
+4 36 339 337 37
+4 152 144 163 151
+4 377 296 378 379
+4 255 198 345 180
+4 251 74 248 9
+4 324 103 105 280
+4 88 57 341 78
+4 354 277 240 278
+4 13 228 88 86
+4 192 123 40 42
+4 105 104 96 98
+4 100 53 173 116
+4 331 205 375 92
+4 352 181 169 281
+4 240 255 345 278
+4 355 238 152 144
+4 370 380 381 326
+4 234 61 233 64
+4 105 62 96 64
+4 30 229 178 42
+4 67 117 68 266
+4 63 103 0 104
+4 93 199 347 252
+4 301 2 317 307
+4 132 227 103 306
+4 110 57 91 78
+4 367 359 324 280
+4 293 243 132 170
+4 258 221 85 228
+4 332 256 297 170
+4 382 335 206 10
+4 250 382 313 38
+4 178 29 30 215
+4 333 334 83 219
+4 65 141 253 68
+4 53 116 114 115
+4 97 234 233 64
+4 237 164 261 229
+4 282 347 141 252
+4 326 320 267 80
+4 341 88 87 57
+4 210 328 302 313
+4 169 238 47 144
+4 34 263 8 35
+4 32 88 190 92
+4 40 162 177 123
+4 290 276 291 139
+4 231 272 213 215
+4 25 201 226 225
+4 196 200 101 50
+4 96 315 27 146
+4 342 286 127 128
+4 4 118 6 259
+4 317 247 184 227
+4 12 13 88 86
+4 188 298 357 299
+4 141 347 253 68
+4 290 274 276 121
+4 379 378 371 296
+4 63 62 105 64
+4 110 118 299 119
+4 166 171 302 172
+4 290 289 353 285
+4 358 308 325 57
+4 103 106 105 306
+4 193 46 158 113
+4 165 201 12 223
+4 383 148 286 128
+4 106 29 261 237
+4 12 5 112 7
+4 67 199 195 197
+4 177 238 291 181
+4 58 13 16 18
+4 138 367 324 280
+4 26 315 27 62
+4 37 263 330 76
+4 136 374 344 135
+4 116 173 101 266
+4 237 131 182 306
+4 35 251 250 38
+4 77 76 331 78
+4 168 297 169 170
+4 149 236 326 320
+4 71 89 90 72
+4 105 103 293 280
+4 209 328 384 372
+4 279 226 360 280
+4 354 285 277 278
+4 87 228 351 57
+4 68 199 94 252
+4 93 18 149 17
+4 368 254 242 256
+4 108 90 107 71
+4 196 50 101 102
+4 337 330 187 37
+4 328 210 302 303
+4 265 111 174 225
+4 341 78 331 92
+4 217 216 158 113
+4 345 285 180 139
+4 149 128 127 18
+4 226 257 27 225
+4 249 10 382 251
+4 105 106 137 306
+4 357 338 187 37
+4 130 144 47 151
+4 249 9 157 11
+4 373 158 160 45
+4 10 9 249 11
+4 114 115 287 19
+4 222 322 136 81
+4 173 116 101 100
+4 132 131 293 170
+4 165 223 12 5
+4 331 78 76 92
+4 345 182 122 139
+4 261 29 106 229
+4 265 25 246 318
+4 88 221 12 86
+4 101 200 99 102
+4 145 146 248 74
+4 32 33 91 92
+4 211 35 73 74
+4 177 162 163 144
+4 157 186 156 11
+4 218 217 36 119
+4 287 19 86 129
+4 145 35 137 74
+4 63 105 244 103
+4 6 118 221 259
+4 21 23 20 108
+4 321 125 124 192
+4 24 176 265 225
+4 283 354 353 285
+4 149 93 148 18
+4 165 166 5 171
+4 73 35 8 74
+4 25 27 24 225
+4 55 346 85 308
+4 36 171 33 119
+4 217 171 36 119
+4 103 307 132 245
+4 219 342 383 286
+4 371 378 70 296
+4 316 315 234 64
+4 152 238 177 163
+4 168 243 332 170
+4 138 280 105 62
+4 34 190 92 32
+4 223 5 257 172
+4 33 35 32 171
+4 18 343 13 129
+4 351 235 127 343
+4 13 12 14 16
+4 371 296 89 72
+4 97 233 232 64
+4 220 4 118 6
+4 16 14 58 309
+4 5 113 216 172
+4 73 229 30 42
+4 168 49 244 245
+4 26 138 61 62
+4 96 145 27 74
+4 210 145 27 146
+4 80 82 149 17
+4 305 331 375 92
+4 346 85 308 228
+4 0 307 103 245
+4 230 64 104 98
+4 266 196 101 68
+4 333 343 83 228
+4 226 27 25 225
+4 286 93 282 129
+4 116 53 84 115
+4 161 144 130 151
+4 6 258 5 259
+4 198 131 293 306
+4 50 224 99 102
+4 162 139 177 123
+4 220 221 56 57
+4 181 285 180 281
+4 168 48 47 170
+4 356 51 135 81
+4 103 227 106 306
+4 147 80 79 82
+4 76 78 91 92
+4 221 57 88 78
+4 156 125 295 186
+4 224 201 25 225
+4 183 301 317 184
+4 173 53 100 112
+4 226 201 165 225
+4 58 319 264 320
+4 250 35 145 251
+4 198 180 255 256
+4 230 104 301 31
+4 184 29 106 237
+4 208 263 337 10
+4 168 244 332 243
+4 236 18 264 320
+4 182 306 140 229
+4 283 353 366 285
+4 182 237 130 131
+4 284 285 352 281
+4 178 163 40 164
+4 181 144 47 133
+4 266 117 68 102
+4 308 351 346 228
+4 213 28 230 29
+4 261 237 182 164
+4 122 182 140 139
+4 22 167 287 115
+4 213 385 214 215
+4 267 380 370 326
+4 379 296 371 72
+4 297 281 170 278
+4 366 285 284 283
+4 210 46 257 172
+4 218 118 216 119
+4 146 74 145 98
+4 312 363 316 362
+4 260 114 141 117
+4 141 114 260 115
+4 329 242 332 256
+4 105 244 138 63
+4 262 186 263 11
+4 177 39 179 178
+4 94 199 68 197
+4 325 110 358 57
+4 167 84 83 86
+4 163 162 40 164
+4 136 344 196 135
+4 132 307 3 245
+4 263 251 382 10
+4 361 87 60 59
+4 60 235 87 13
+4 375 203 304 92
+4 321 124 191 192
+4 267 320 314 80
+4 47 132 3 245
+4 222 199 81 197
+4 176 210 44 46
+4 177 163 238 144
+4 299 357 109 298
+4 110 76 75 78
+4 77 341 358 78
+4 357 37 188 299
+4 224 223 201 225
+4 91 171 259 119
+4 34 263 33 76
+4 157 28 8 9
+4 36 338 357 37
+4 344 135 50 197
+4 271 178 39 215
+4 373 45 44 303
+4 105 31 104 98
+4 58 309 370 320
+4 250 210 362 146
+4 267 327 79 386
+4 211 34 190 189
+4 293 131 132 306
+4 8 35 263 251
+4 207 156 262 11
+4 40 192 140 123
+4 189 124 211 192
+4 105 104 63 64
+4 264 18 58 320
+4 272 385 213 215
+4 142 143 141 117
+4 205 295 191 186
+4 50 53 224 102
+4 148 128 149 18
+4 99 224 100 102
+4 261 184 178 153
+4 180 285 345 278
+4 4 7 89 113
+4 231 178 183 153
+4 55 346 350 85
+4 104 31 230 98
+4 222 194 79 81
+4 269 270 268 64
+4 258 5 12 7
+4 110 299 75 76
+4 5 166 257 172
+4 3 49 47 245
+4 93 94 282 17
+4 16 13 12 19
+4 292 255 294 256
+4 27 210 257 166
+4 337 263 336 10
+4 16 320 18 17
+4 332 277 297 256
+4 105 106 104 31
+4 41 40 177 123
+4 39 156 155 157
+4 161 162 182 144
+4 16 18 13 129
+4 131 227 132 306
+4 73 8 34 186
+4 89 296 4 113
+4 210 315 363 146
+4 132 307 103 227
+4 147 82 365 148
+4 339 263 382 10
+4 32 211 165 35
+4 345 180 182 139
+4 289 285 290 139
+4 106 306 237 229
+4 104 106 301 31
+4 242 277 332 256
+4 85 84 258 86
+4 103 132 293 243
+4 224 223 174 112
+4 143 21 260 117
+4 109 110 56 118
+4 336 339 382 10
+4 187 330 77 37
+4 328 362 313 210
+4 196 68 67 197
+4 58 311 370 309
+4 298 110 109 299
+4 1 0 2 300
+4 336 382 206 10
+4 372 44 209 328
+4 137 74 73 31
+4 272 178 271 215
+4 293 105 137 306
+4 145 35 250 166
+4 26 27 25 62
+4 301 106 184 29
+4 240 277 242 278
+4 263 251 35 38
+4 34 211 190 32
+4 249 382 250 251
+4 176 46 43 225
+4 294 293 329 280
+4 148 18 286 128
+4 39 178 40 42
+4 122 121 290 139
+4 261 161 163 164
+4 230 213 95 28
+4 12 223 201 53
+4 36 337 338 37
+4 75 299 77 76
+4 363 210 209 315
+4 216 118 4 259
+4 379 193 296 111
+4 163 144 161 151
+4 270 232 268 64
+4 335 249 154 11
+4 249 157 154 11
+4 130 237 247 227
+4 230 29 28 31
+4 294 255 292 387
+4 285 281 284 278
+4 263 10 8 251
+4 287 114 141 115
+4 282 252 141 19
+4 246 224 174 369
+4 47 247 130 151
+4 26 349 25 318
+4 26 25 24 318
+4 316 234 97 64
+4 182 162 181 144
+4 243 170 168 245
+4 279 329 359 280
+4 286 18 93 129
+4 84 12 167 115
+4 8 28 30 74
+4 32 15 190 88
+4 5 259 165 171
+4 22 287 260 115
+4 190 34 92 189
+4 221 228 88 57
+4 23 116 107 72
+4 353 354 240 278
+4 138 273 367 280
+4 222 81 136 197
+4 258 228 85 86
+4 107 116 173 72
+4 27 257 165 166
+4 254 255 294 387
+4 332 324 329 243
+4 230 28 95 98
+4 244 243 168 245
+4 47 355 185 151
+4 316 250 362 146
+4 40 163 177 162
+4 155 385 271 215
+4 180 256 243 170
+4 156 39 125 42
+4 248 146 95 98
+4 271 385 272 215
+4 84 23 90 7
+4 221 118 56 57
+4 364 368 329 294
+4 8 157 30 28
+4 175 159 379 111
+4 136 81 135 197
+4 204 321 191 192
+4 243 256 332 170
+4 217 259 5 171
+4 340 93 365 82
+4 180 285 181 139
+4 71 70 89 72
+4 84 53 12 115
+4 222 79 322 81
+4 58 14 16 13
+4 141 287 282 19
+4 303 45 44 46
+4 230 270 2 104
+4 33 263 37 76
+4 100 53 224 112
+4 112 116 53 7
+4 2 0 3 307
+4 105 280 27 62
+4 319 370 311 381
+4 35 171 33 38
+4 216 259 217 119
+4 337 339 336 263
+4 33 76 91 92
+4 188 37 77 299
+4 132 170 243 245
+4 248 212 249 9
+4 140 120 122 123
+4 181 162 182 139
+4 382 263 339 38
+4 22 116 23 115
+4 267 79 322 388
+4 41 123 125 42
+4 308 85 220 228
+4 284 281 297 278
+4 279 360 292 280
+4 175 112 174 111
+4 348 194 66 199
+4 18 320 149 17
+4 165 257 226 225
+4 77 299 37 76
+4 137 293 198 280
+4 51 53 50 102
+4 351 228 308 57
+4 32 165 12 259
+4 101 50 200 102
+4 184 29 178 153
+4 49 307 0 245
+4 97 64 230 98
+4 211 34 191 186
+4 282 129 18 17
+4 29 31 106 229
+4 136 135 196 197
+4 111 43 225 265
+4 205 263 34 76
+4 79 327 267 80
+4 291 181 352 285
+4 40 123 41 42
+4 67 68 66 199
+4 34 8 73 35
+4 352 285 181 281
+4 40 178 261 229
+4 43 111 225 46
+4 329 294 368 256
+4 383 365 286 148
+4 103 307 106 227
+4 193 158 216 113
+4 191 34 189 92
+4 293 132 103 306
+4 358 57 110 78
+4 140 106 137 229
+4 30 31 29 229
+4 301 106 104 307
+4 358 110 75 78
+4 52 201 224 53
+4 136 322 222 389
+4 342 219 383 390
+4 277 285 284 278
+4 16 51 314 94
+4 267 326 327 386
+4 23 7 116 72
+4 94 252 282 19
+4 356 135 134 81
+4 130 144 182 133
+4 126 41 275 123
+4 112 5 111 113
+4 134 322 136 389
+4 261 163 178 164
+4 203 205 191 92
+4 296 113 89 72
+4 53 114 51 19
+4 322 323 222 389
+4 257 166 210 172
+4 297 170 256 278
+4 105 74 137 31
+4 343 129 219 86
+4 0 103 244 245
+4 301 29 230 31
+4 88 228 87 57
+4 277 284 297 278
+4 81 199 194 82
+4 125 192 295 186
+4 142 117 67 266
+4 91 76 33 119
+4 173 371 69 72
+4 75 54 298 110
+4 166 302 210 172
+4 351 343 333 228
+4 183 29 184 153
+4 25 265 246 225
+4 324 105 138 280
+4 331 76 205 92
+4 53 116 84 7
+4 249 335 382 10
+4 380 326 267 386
+4 209 362 328 210
+4 370 381 319 326
+4 91 76 110 78
+4 218 109 216 118
+4 322 388 79 323
+4 13 343 87 228
+4 15 88 12 13
+4 353 285 354 278
+4 219 287 167 86
+4 240 256 255 278
+4 262 10 207 11
+4 308 358 351 57
+4 32 91 88 92
+4 170 281 180 278
+4 158 46 303 172
+4 352 238 169 181
+4 191 34 205 186
+4 27 96 105 62
+4 191 295 204 186
+4 267 386 79 388
+4 322 79 267 81
+4 73 74 30 31
+4 367 273 359 280
+4 324 103 293 243
+4 114 252 94 19
+4 136 376 134 389
+4 315 62 61 64
+4 8 74 251 9
+4 261 161 184 153
+4 198 180 293 131
+4 326 319 370 320
+4 214 157 212 215
+4 333 83 85 228
+4 33 171 91 119
+4 85 221 220 228
+4 159 193 379 111
+4 73 186 192 42
+4 353 289 366 285
+4 286 148 365 93
+4 10 251 249 9
+4 169 170 297 281
+4 95 146 96 98
+4 182 144 181 133
+4 182 131 130 133
+4 129 86 13 19
+4 141 252 114 19
+4 130 132 47 133
+4 148 82 93 17
+4 127 128 286 129
+4 128 18 286 129
+4 12 51 16 19
+4 314 320 16 17
+4 12 86 167 19
+4 140 192 124 123
+4 8 251 10 9
+4 239 242 368 254
+4 333 219 83 343
+4 217 302 36 171
+4 13 18 235 343
+4 208 330 337 263
+4 257 46 176 225
+4 118 259 216 119
+4 12 5 258 259
+4 304 190 60 88
+4 358 325 75 110
+4 116 7 112 72
+4 23 116 84 115
+4 32 35 165 171
+4 135 52 50 51
+4 51 114 53 102
+4 77 358 75 78
+4 27 315 96 62
+4 326 370 267 320
+4 253 66 65 68
+4 255 256 180 278
+4 209 363 362 210
+4 212 28 157 9
+4 16 129 13 19
+4 32 91 171 259
+4 149 320 327 80
+4 263 35 34 38
+4 0 269 63 104
+4 355 144 152 151
+4 361 87 341 88
+4 75 110 298 299
+4 290 291 289 139
+4 204 295 321 192
+4 297 256 277 278
+4 256 170 180 278
+4 223 111 112 5
+4 266 107 20 116
+4 28 74 95 98
+4 141 65 142 117
+4 362 250 313 210
+4 177 40 39 178
+4 211 137 165 35
+4 82 147 365 340
+4 191 211 189 34
+4 190 15 60 88
+4 111 43 174 175
+4 89 70 371 72
+4 163 151 161 153
+4 37 76 299 119
+4 330 37 337 263
+4 13 129 343 86
+4 54 109 298 110
+4 75 325 54 110
+4 356 52 135 51
+4 180 170 131 133
+4 273 27 226 62
+4 127 343 18 129
+4 27 257 24 225
+4 35 166 165 171
+4 52 314 16 51
+4 39 157 155 215
+4 345 285 353 278
+4 12 52 16 51
+4 198 106 140 306
+4 12 221 258 86
+4 251 263 382 38
+4 308 228 220 57
+4 286 128 342 383
+4 117 67 68 65
+4 73 157 8 186
+4 28 74 8 9
+4 3 0 49 307
+4 68 197 196 102
+4 293 292 256 198
+4 357 299 109 218
+4 179 163 178 153
+4 150 179 272 153
+4 257 46 223 172
+4 176 257 24 210
+4 266 67 196 68
+4 175 43 159 111
+4 235 13 59 18
+4 270 97 232 64
+4 124 121 120 123
+4 299 76 91 119
+4 348 93 340 199
+4 84 90 258 7
+4 91 259 118 119
+4 185 355 150 151
+4 313 35 250 38
+4 37 263 33 38
+4 301 184 183 29
+4 339 263 37 38
+4 231 29 178 215
+4 136 322 134 81
+4 312 362 384 363
+4 293 180 198 256
+4 68 117 114 102
+4 24 210 27 315
+4 158 113 46 172
+4 12 115 53 19
+4 53 115 114 19
+4 30 74 28 31
+4 185 184 317 247
+4 351 87 235 343
+4 124 192 125 123
+4 355 152 150 151
+4 301 104 2 307
+4 12 221 88 259
+4 212 28 213 215
+4 345 198 122 182
+4 211 73 137 74
+4 292 360 198 280
+4 138 105 63 62
+4 219 343 286 129
+4 206 208 336 10
+4 4 259 5 113
+4 377 296 379 193
+4 214 385 155 215
+4 12 15 32 88
+4 147 327 79 80
+4 217 171 5 172
+4 208 337 336 10
+4 183 272 231 153
+4 291 41 177 123
+4 73 40 140 229
+4 363 61 234 315
+4 36 302 339 38
+4 89 113 7 72
+4 30 215 157 42
+4 194 199 340 82
+4 193 160 158 159
+4 91 32 171 33
+4 60 59 235 13
+4 273 280 138 62
+4 363 234 312 316
+4 24 257 27 210
+4 235 18 127 343
+4 373 158 45 303
+4 16 309 58 320
+4 105 103 63 104
+4 287 115 167 19
+4 353 345 122 139
+4 247 161 130 151
+4 291 285 289 139
+4 313 210 250 166
+4 116 115 22 117
+4 0 104 103 307
+4 134 322 314 81
+4 248 251 145 74
+4 94 68 51 197
+4 60 15 58 13
+4 371 70 69 72
+4 248 74 146 98
+4 156 186 295 11
+4 330 263 205 76
+4 36 33 302 38
+4 18 148 286 93
+4 130 161 182 144
+4 94 199 81 82
+4 155 157 214 215
+4 129 17 282 19
+4 189 140 124 202
+4 290 274 288 276
+4 159 377 193 160
+4 182 162 140 139
+4 194 81 222 199
+4 22 21 20 117
+4 58 264 59 13
+4 379 111 296 72
+4 116 117 266 102
+4 66 194 195 199
+4 330 205 331 76
+4 16 94 314 17
+4 253 347 66 68
+4 185 247 47 151
+4 30 28 157 215
+4 2 104 0 307
+4 101 116 102 100
+4 88 91 32 259
+4 124 202 140 123
+4 84 116 23 7
+4 317 2 3 307
+4 321 295 125 192
+4 27 280 226 62
+4 313 382 339 38
+4 96 104 105 64
+4 342 334 219 390
+4 176 61 363 315
+4 26 61 176 315
+4 165 259 32 171
+4 47 144 355 151
+4 30 29 28 215
+4 95 97 230 98
+4 6 5 4 259
+4 157 186 73 42
+4 156 157 39 42
+4 149 82 148 17
+4 191 192 211 186
+4 185 151 150 153
+4 185 150 183 153
+4 96 315 316 64
+4 180 133 181 281
+4 230 97 270 64
+4 40 229 73 42
+4 157 28 212 215
+4 112 111 175 72
+4 88 221 91 259
+4 184 237 106 227
+4 353 285 345 139
+4 105 96 27 74
+4 301 307 317 227
+4 85 55 308 220
+4 77 37 330 76
+4 226 27 137 280
+4 247 237 130 161
+4 174 112 223 111
+4 126 275 124 123
+4 111 113 296 72
+4 329 279 294 280
+4 276 139 121 123
+4 12 88 32 259
+4 112 7 111 72
+4 110 299 91 119
+4 145 251 35 74
+4 332 243 329 256
+4 124 275 121 123
+4 375 205 203 92
+4 178 29 261 229
+4 379 175 72 371
+4 33 263 34 38
+4 56 110 54 57
+4 372 303 44 328
+4 302 166 210 313
+4 250 251 382 38
+4 341 331 305 92
+4 5 259 216 113
+4 68 199 67 197
+4 107 90 23 72
+4 7 113 111 72
+4 172 217 303 158
+4 184 247 185 151
+4 291 276 41 123
+4 141 68 65 117
+4 56 118 110 57
+4 24 26 176 315
+4 87 13 235 343
+4 262 295 205 186
+4 134 314 356 81
+4 279 294 359 329
+4 328 209 384 362
+4 165 35 145 166
+4 237 227 131 306
+4 24 27 26 315
+4 209 176 363 315
+4 27 315 210 146
+4 276 275 41 123
+4 266 116 20 117
+4 248 316 95 146
+4 340 199 93 82
+4 25 349 26 62
+4 96 74 105 98
+4 190 88 304 92
+4 58 18 16 320
+4 316 96 95 146
+4 159 43 45 46
+4 336 263 339 10
+4 217 172 303 302
+4 240 345 353 278
+4 213 212 95 28
+4 309 16 314 320
+4 84 167 23 115
+4 335 207 206 10
+4 159 45 158 46
+4 20 21 142 117
+4 314 94 51 81
+4 196 197 50 102
+4 285 366 284 352
+4 343 228 13 86
+4 34 263 205 186
+4 96 64 97 98
+4 260 21 22 117
+4 258 6 220 221
+4 333 342 219 343
+4 114 117 116 102
+4 180 131 182 133
+4 305 88 341 78
+4 56 4 109 118
+4 107 23 20 116
+4 247 237 184 227
+4 370 58 319 311
+4 147 149 327 80
+4 362 210 363 146
+4 260 287 141 115
+4 303 44 328 210
+4 335 212 154 249
+4 81 80 314 17
+4 39 271 179 178
+4 191 124 189 192
+4 209 362 363 384
+4 19 287 86 167
+4 91 221 88 78
+4 347 93 348 199
+4 205 263 262 186
+4 316 363 234 315
+4 237 306 182 229
+4 342 127 286 343
+4 317 132 47 227
+4 193 379 377 159
+4 221 228 258 86
+4 167 115 12 19
+4 140 139 162 123
+4 344 50 196 197
+4 242 256 240 278
+4 25 273 349 62
+4 341 57 358 78
+4 205 76 34 92
+4 216 113 217 172
+4 95 28 212 9
+4 189 140 211 124
+4 264 236 235 18
+4 183 178 231 29
+4 175 379 72 111
+4 132 131 130 227
+4 302 171 166 38
+4 317 184 301 227
+4 111 5 223 46
+4 333 334 219 342
+4 195 136 196 197
+4 109 299 110 118
+4 157 215 39 42
+4 130 247 47 227
+4 201 223 224 53
+4 244 105 138 324
+4 299 118 218 119
+4 224 53 223 112
+4 163 162 161 144
+4 287 129 282 19
+4 87 343 351 228
+4 124 140 211 192
+4 51 94 16 19
+4 324 293 329 243
+4 314 81 267 80
+4 95 74 28 9
+4 223 46 5 172
+4 291 238 352 181
+4 37 299 218 119
+389
+323
+388
+386
+380
+368
+364
+359
+274
+366
+283
+241
+239
+367
+349
+318
+369
+99
+200
+344
+374
+381
+319
+264
+59
+361
+305
+375
+204
+321
+126
+275
+203
+322
+267
+246
+309
+289
+288
+354
+376
+314
+370
+294
+279
+121
+353
+240
+254
+273
+25
+224
+50
+135
+311
+58
+60
+304
+191
+124
+290
+134
+292
+360
+120
+345
+255
+387
+226
+201
+52
+310
+14
+15
+190
+189
+202
+122
+356
+1000.0 0.45 7.5e4
diff --git a/SurgSim/Physics/PerformanceTests/Fem3DPerformanceTest.cpp b/SurgSim/Physics/PerformanceTests/Fem3DPerformanceTest.cpp
new file mode 100644
index 0000000..60cd974
--- /dev/null
+++ b/SurgSim/Physics/PerformanceTests/Fem3DPerformanceTest.cpp
@@ -0,0 +1,309 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <unordered_map>
+#include <memory>
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Timer.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Testing/MockPhysicsManager.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+static const double dt = 0.001;
+static const int frameCount = 100;
+
+static std::unordered_map<SurgSim::Math::IntegrationScheme, std::string, std::hash<int>> getIntegrationSchemeNames()
+{
+ std::unordered_map<SurgSim::Math::IntegrationScheme, std::string, std::hash<int>> result;
+
+#define FEM3DPERFORMANCETEST_MAP_NAME(map, name) (map)[name] = #name
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_STATIC);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4);
+#undef FEM3DPERFORMANCETEST_MAP_NAME
+
+ return result;
+}
+
+static std::unordered_map<SurgSim::Math::IntegrationScheme, std::string, std::hash<int>> IntegrationSchemeNames
+ = getIntegrationSchemeNames();
+}
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class DivisbleCubeRepresentation : public Fem3DRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name of the divisible cube representation.
+ /// \param nodesPerAxis The number of nodes per axis
+ DivisbleCubeRepresentation(const std::string& name, size_t nodesPerAxis)
+ : Fem3DRepresentation(name), m_numNodesPerAxis(nodesPerAxis)
+ {
+ // Compute the center point of the cube
+ SurgSim::Math::Vector3d center = SurgSim::Math::Vector3d::Zero();
+
+ // Compute the cube's corners for the Fem3d simulation
+ double halfLength = static_cast<double>(nodesPerAxis);
+ Vector3d X = Vector3d::UnitX();
+ Vector3d Y = Vector3d::UnitY();
+ Vector3d Z = Vector3d::UnitZ();
+ m_cubeNodes[0] = center - halfLength * X - halfLength * Y - halfLength * Z;
+ m_cubeNodes[1] = center + halfLength * X - halfLength * Y - halfLength * Z;
+ m_cubeNodes[2] = center - halfLength * X + halfLength * Y - halfLength * Z;
+ m_cubeNodes[3] = center + halfLength * X + halfLength * Y - halfLength * Z;
+ m_cubeNodes[4] = center - halfLength * X - halfLength * Y + halfLength * Z;
+ m_cubeNodes[5] = center + halfLength * X - halfLength * Y + halfLength * Z;
+ m_cubeNodes[6] = center - halfLength * X + halfLength * Y + halfLength * Z;
+ m_cubeNodes[7] = center + halfLength * X + halfLength * Y + halfLength * Z;
+
+ auto initialState = std::make_shared<SurgSim::Math::OdeState>();
+ fillUpDeformableState(initialState);
+ setInitialState(initialState);
+ addFemCubes(initialState);
+ }
+
+protected:
+ /// Convert a node index from a 3d indexing to a 1d indexing
+ /// \param i, j, k Indices along the X, Y and Z axis
+ /// \return Unique index of the corresponding point (to access a linear array for example)
+ size_t get1DIndexFrom3D(size_t i, size_t j, size_t k)
+ {
+ return m_numNodesPerAxis * m_numNodesPerAxis * i + m_numNodesPerAxis * j + k;
+ }
+
+ /// Fills up a given state with the cube's nodes, border nodes, and internal nodes
+ /// \param[in,out] state The state to be filled up
+ void fillUpDeformableState(std::shared_ptr<SurgSim::Math::OdeState> state)
+ {
+ state->setNumDof(getNumDofPerNode(), m_numNodesPerAxis * m_numNodesPerAxis * m_numNodesPerAxis);
+ SurgSim::Math::Vector& nodePositions = state->getPositions();
+
+ for (size_t i = 0; i < m_numNodesPerAxis; i++)
+ {
+ // For a given index i, we intersect the cube with a (Y Z) plane, which defines a square on a (Y Z) plane
+ Vector3d extremitiesX0[4] = {m_cubeNodes[0], m_cubeNodes[2], m_cubeNodes[4], m_cubeNodes[6]};
+ Vector3d extremitiesX1[4] = {m_cubeNodes[1], m_cubeNodes[3], m_cubeNodes[5], m_cubeNodes[7]};
+ Vector3d extremitiesXi[4];
+ double coefI = static_cast<double>(i) / (static_cast<double>(m_numNodesPerAxis) - 1.0);
+
+ for (size_t index = 0; index < 4; index++)
+ {
+ extremitiesXi[index] =
+ extremitiesX0[index] * (1.0 - coefI) +
+ extremitiesX1[index] * coefI;
+ }
+
+ for (size_t j = 0; j < m_numNodesPerAxis; j++)
+ {
+ // For a given index j, we intersect the square with a (X Z) plane, which defines a line along (Z)
+ Vector3d extremitiesY0[2] = {extremitiesXi[0], extremitiesXi[2]};
+ Vector3d extremitiesY1[2] = {extremitiesXi[1], extremitiesXi[3]};
+ Vector3d extremitiesYi[2];
+ double coefJ = static_cast<double>(j) / (static_cast<double>(m_numNodesPerAxis) - 1.0);
+
+ for (size_t index = 0; index < 2; index++)
+ {
+ extremitiesYi[index] =
+ extremitiesY0[index] * (1.0 - coefJ) +
+ extremitiesY1[index] * coefJ;
+ }
+
+ for (size_t k = 0; k < m_numNodesPerAxis; k++)
+ {
+ // For a given index k, we intersect the line with a (X Y) plane, which defines a 3d point
+ double coefK = static_cast<double>(k) / (static_cast<double>(m_numNodesPerAxis) - 1.0);
+ Vector3d position3d = extremitiesYi[0] * (1.0 - coefK) + extremitiesYi[1] * coefK;
+ SurgSim::Math::setSubVector(position3d, get1DIndexFrom3D(i, j, k), 3, &nodePositions);
+ }
+ }
+ }
+ }
+
+ /// Adds the Fem3D elements of small cubes
+ /// \param state The state for initialization.
+ void addFemCubes(std::shared_ptr<SurgSim::Math::OdeState> state)
+ {
+ for (size_t i = 0; i < m_numNodesPerAxis - 1; i++)
+ {
+ for (size_t j = 0; j < m_numNodesPerAxis - 1; j++)
+ {
+ for (size_t k = 0; k < m_numNodesPerAxis - 1; k++)
+ {
+ std::array<size_t, 8> cubeNodeIds;
+ cubeNodeIds[0] = get1DIndexFrom3D(i , j , k );
+ cubeNodeIds[1] = get1DIndexFrom3D(i+1, j , k );
+ cubeNodeIds[2] = get1DIndexFrom3D(i , j+1, k );
+ cubeNodeIds[3] = get1DIndexFrom3D(i+1, j+1, k );
+ cubeNodeIds[4] = get1DIndexFrom3D(i , j , k+1);
+ cubeNodeIds[5] = get1DIndexFrom3D(i+1, j , k+1);
+ cubeNodeIds[6] = get1DIndexFrom3D(i , j+1, k+1);
+ cubeNodeIds[7] = get1DIndexFrom3D(i+1, j+1, k+1);
+
+ std::array<size_t, 8> cube = {cubeNodeIds[0], cubeNodeIds[1], cubeNodeIds[3], cubeNodeIds[2],
+ cubeNodeIds[4], cubeNodeIds[5], cubeNodeIds[7], cubeNodeIds[6]};
+
+ // Add Fem3DElementCube for each cube
+ std::shared_ptr<Fem3DElementCube> femElement = std::make_shared<Fem3DElementCube>(cube);
+ femElement->setMassDensity(980.0); // 0.98 g/cm^-3 (2-part silicone rubber a.k.a. RTV6166)
+ femElement->setPoissonRatio(0.499); // From the paper (near 0.5)
+ femElement->setYoungModulus(15.3e3); // 15.3 kPa (From the paper)
+ femElement->initialize(*state);
+ addFemElement(femElement);
+ }
+ }
+ }
+ }
+
+private:
+ // Number of point per dimensions
+ size_t m_numNodesPerAxis;
+
+ // Corner nodes of the original cube
+ std::array<SurgSim::Math::Vector3d, 8> m_cubeNodes;
+};
+
+class Fem3DPerformanceTestBase : public ::testing::Test
+{
+public:
+ virtual void SetUp()
+ {
+ m_physicsManager = std::make_shared<SurgSim::Testing::MockPhysicsManager>();
+
+ m_physicsManager->doInitialize();
+ m_physicsManager->doStartUp();
+ }
+
+ void initializeRepresentation(std::shared_ptr<Fem3DRepresentation> fem)
+ {
+ fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ fem->wakeUp();
+ m_physicsManager->executeAdditions(fem);
+ }
+
+ void performTimingTest()
+ {
+ SurgSim::Framework::Timer totalTime;
+ totalTime.beginFrame();
+
+ SurgSim::Framework::Timer timer;
+ timer.setMaxNumberOfFrames(frameCount);
+ for (int i = 0; i < frameCount; i++)
+ {
+ timer.beginFrame();
+ m_physicsManager->doUpdate(dt);
+ timer.endFrame();
+ }
+
+ totalTime.endFrame();
+ RecordProperty("Duration", boost::to_string(totalTime.getCumulativeTime()));
+ RecordProperty("FrameRate", boost::to_string(timer.getAverageFrameRate()));
+ }
+
+protected:
+ std::shared_ptr<SurgSim::Testing::MockPhysicsManager> m_physicsManager;
+};
+
+class IntegrationSchemeParamTest : public Fem3DPerformanceTestBase,
+ public ::testing::WithParamInterface<SurgSim::Math::IntegrationScheme>
+{
+};
+
+class IntegrationSchemeAndCountParamTest
+ : public Fem3DPerformanceTestBase,
+ public ::testing::WithParamInterface<std::tuple<SurgSim::Math::IntegrationScheme, int> >
+{
+};
+
+TEST_P(IntegrationSchemeParamTest, WoundTest)
+{
+ SurgSim::Math::IntegrationScheme integrationScheme = GetParam();
+ RecordProperty("IntegrationScheme", IntegrationSchemeNames[integrationScheme]);
+
+ auto fem = std::make_shared<SurgSim::Physics::Fem3DRepresentation>("wound");
+ fem->setFilename("Data/Fem3DPerformanceTest/wound_deformable.ply");
+ fem->setIntegrationScheme(integrationScheme);
+
+ initializeRepresentation(fem);
+ performTimingTest();
+}
+
+TEST_P(IntegrationSchemeAndCountParamTest, CubeTest)
+{
+ int numCubes;
+ SurgSim::Math::IntegrationScheme integrationScheme;
+ std::tie(integrationScheme, numCubes) = GetParam();
+ RecordProperty("IntegrationScheme", IntegrationSchemeNames[integrationScheme]);
+ RecordProperty("CubeDivisions", boost::to_string(numCubes));
+
+ auto fem = std::make_shared<DivisbleCubeRepresentation>("cube", numCubes);
+ fem->setIntegrationScheme(integrationScheme);
+
+ initializeRepresentation(fem);
+ performTimingTest();
+}
+
+INSTANTIATE_TEST_CASE_P(Fem3DPerformanceTest,
+ IntegrationSchemeParamTest,
+ ::testing::Values(SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_STATIC,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC,
+ SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4));
+
+INSTANTIATE_TEST_CASE_P(
+ Fem3DPerformanceTest,
+ IntegrationSchemeAndCountParamTest,
+ ::testing::Combine(::testing::Values(SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_STATIC,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC,
+ SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4),
+ ::testing::Values(2, 3, 4, 5, 6, 7, 8)));
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/PerformanceTests/config.txt.in b/SurgSim/Physics/PerformanceTests/config.txt.in
new file mode 100644
index 0000000..9c1cf05
--- /dev/null
+++ b/SurgSim/Physics/PerformanceTests/config.txt.in
@@ -0,0 +1 @@
+${CMAKE_CURRENT_BINARY_DIR}/Data/
\ No newline at end of file
diff --git a/SurgSim/Physics/PhysicsConvert.cpp b/SurgSim/Physics/PhysicsConvert.cpp
new file mode 100644
index 0000000..cd67227
--- /dev/null
+++ b/SurgSim/Physics/PhysicsConvert.cpp
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/PhysicsConvert.h"
+
+#include "SurgSim/Physics/RigidRepresentationState.h"
+
+namespace YAML
+{
+
+Node convert<SurgSim::Physics::RigidRepresentationState>::encode(const SurgSim::Physics::RigidRepresentationState& rhs)
+{
+ YAML::Node data(rhs.encode());
+
+ YAML::Node result;
+ result[rhs.getClassName()] = data;
+
+ return result;
+}
+
+bool convert<SurgSim::Physics::RigidRepresentationState>::decode(
+ const Node& node,
+ SurgSim::Physics::RigidRepresentationState& rhs)
+{
+ bool result = false;
+ if (node[rhs.getClassName()].IsDefined())
+ {
+ YAML::Node data;
+ data = node[rhs.getClassName()];
+ rhs.decode(data);
+ result = true;
+ }
+ return result;
+}
+
+} // namespace YAML
diff --git a/SurgSim/Physics/PhysicsConvert.h b/SurgSim/Physics/PhysicsConvert.h
new file mode 100644
index 0000000..1d185e8
--- /dev/null
+++ b/SurgSim/Physics/PhysicsConvert.h
@@ -0,0 +1,42 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_PHYSICSCONVERT_H
+#define SURGSIM_PHYSICS_PHYSICSCONVERT_H
+
+#include <yaml-cpp/yaml.h>
+
+namespace SurgSim
+{
+namespace Physics
+{
+class RigidRepresentationState;
+}
+}
+
+namespace YAML
+{
+
+template <>
+struct convert<SurgSim::Physics::RigidRepresentationState>
+{
+ static Node encode(const SurgSim::Physics::RigidRepresentationState& rhs);
+ static bool decode(const Node& node, SurgSim::Physics::RigidRepresentationState& rhs);
+};
+
+
+}; // namespace YAML
+
+#endif // SURGSIM_PHYSICS_PHYSICSCONVERT_H
diff --git a/SurgSim/Physics/PhysicsManager.cpp b/SurgSim/Physics/PhysicsManager.cpp
new file mode 100644
index 0000000..0339e49
--- /dev/null
+++ b/SurgSim/Physics/PhysicsManager.cpp
@@ -0,0 +1,176 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/PhysicsManager.h"
+
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Physics/BuildMlcp.h"
+#include "SurgSim/Physics/ConstraintComponent.h"
+#include "SurgSim/Physics/ContactConstraintGeneration.h"
+#include "SurgSim/Physics/DcdCollision.h"
+#include "SurgSim/Physics/FreeMotion.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/PostUpdate.h"
+#include "SurgSim/Physics/PreUpdate.h"
+#include "SurgSim/Physics/PushResults.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/SolveMlcp.h"
+#include "SurgSim/Physics/UpdateCollisionRepresentations.h"
+
+#include <list>
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+PhysicsManager::PhysicsManager() :
+ ComponentManager("Physics Manager")
+{
+ setRate(1000.0);
+}
+
+PhysicsManager::~PhysicsManager()
+{
+
+}
+
+int PhysicsManager::getType() const
+{
+ return SurgSim::Framework::MANAGER_TYPE_PHYSICS;
+}
+
+bool PhysicsManager::doInitialize()
+{
+ initializeComputations(false);
+ return m_logger != nullptr;
+}
+
+
+bool PhysicsManager::doStartUp()
+{
+ return true;
+}
+
+void PhysicsManager::getFinalState(SurgSim::Physics::PhysicsManagerState* s) const
+{
+ m_finalState.get(s);
+}
+
+std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>::iterator PhysicsManager::findExcludedCollisionPair(
+ std::shared_ptr<SurgSim::Collision::Representation> representation1,
+ std::shared_ptr<SurgSim::Collision::Representation> representation2)
+{
+ return std::find_if(m_excludedCollisionPairs.begin(), m_excludedCollisionPairs.end(),
+ [&representation1, &representation2] (const std::shared_ptr<SurgSim::Collision::CollisionPair>&pair)
+ {
+ return (pair->getFirst() == representation1 && pair->getSecond() == representation2)
+ || (pair->getFirst() == representation2 && pair->getSecond() == representation1);
+ });
+}
+
+void PhysicsManager::addExcludedCollisionPair(std::shared_ptr<SurgSim::Collision::Representation> representation1,
+ std::shared_ptr<SurgSim::Collision::Representation> representation2)
+{
+ boost::mutex::scoped_lock lock(m_excludedCollisionPairMutex);
+
+ if (findExcludedCollisionPair(representation1, representation2) == m_excludedCollisionPairs.end())
+ {
+ m_excludedCollisionPairs.push_back(
+ std::make_shared<SurgSim::Collision::CollisionPair>(representation1, representation2));
+ }
+}
+
+void PhysicsManager::removeExcludedCollisionPair(std::shared_ptr<SurgSim::Collision::Representation> representation1,
+ std::shared_ptr<SurgSim::Collision::Representation> representation2)
+{
+ boost::mutex::scoped_lock lock(m_excludedCollisionPairMutex);
+
+ auto candidatePair = findExcludedCollisionPair(representation1, representation2);
+
+ if (candidatePair != m_excludedCollisionPairs.end())
+ {
+ m_excludedCollisionPairs.erase(candidatePair);
+ }
+}
+
+bool PhysicsManager::executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component)
+{
+ std::shared_ptr<Representation> representation = tryAddComponent(component, &m_representations);
+ std::shared_ptr<SurgSim::Collision::Representation> collisionRep =
+ tryAddComponent(component, &m_collisionRepresentations);
+ std::shared_ptr<ConstraintComponent> constraintComponent = tryAddComponent(component, &m_constraintComponents);
+ return representation != nullptr || collisionRep != nullptr || constraintComponent != nullptr;
+}
+
+bool PhysicsManager::executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component)
+{
+ bool removed1 = tryRemoveComponent(component, &m_representations);
+ bool removed2 = tryRemoveComponent(component, &m_collisionRepresentations);
+ bool removed3 = tryRemoveComponent(component, &m_constraintComponents);
+ return removed1 || removed2 || removed3;
+}
+
+bool PhysicsManager::doUpdate(double dt)
+{
+ // Add all components that came in before the last update
+ processComponents();
+
+ processBehaviors(dt);
+
+ std::list<std::shared_ptr<PhysicsManagerState>> stateList;
+ std::shared_ptr<PhysicsManagerState> state = std::make_shared<PhysicsManagerState>();
+ stateList.push_back(state);
+ state->setRepresentations(m_representations);
+ state->setCollisionRepresentations(m_collisionRepresentations);
+ state->setConstraintComponents(m_constraintComponents);
+
+ {
+ boost::mutex::scoped_lock lock(m_excludedCollisionPairMutex);
+ state->setExcludedCollisionPairs(m_excludedCollisionPairs);
+ }
+
+ stateList.push_back(m_preUpdateStep->update(dt, stateList.back()));
+ stateList.push_back(m_freeMotionStep->update(dt, stateList.back()));
+ stateList.push_back(m_updateCollisionRepresentationsStep->update(dt, stateList.back()));
+ stateList.push_back(m_dcdCollisionStep->update(dt, stateList.back()));
+ stateList.push_back(m_constraintGenerationStep->update(dt, stateList.back()));
+ stateList.push_back(m_buildMlcpStep->update(dt, stateList.back()));
+ stateList.push_back(m_solveMlcpStep->update(dt, stateList.back()));
+ stateList.push_back(m_pushResultsStep->update(dt, stateList.back()));
+ stateList.push_back(m_updateCollisionRepresentationsStep->update(dt, stateList.back()));
+ stateList.push_back(m_postUpdateStep->update(dt, stateList.back()));
+
+ m_finalState.set(*(stateList.back()));
+
+ return true;
+}
+
+void PhysicsManager::initializeComputations(bool copyState)
+{
+ m_preUpdateStep.reset(new PreUpdate(copyState));
+ m_freeMotionStep.reset(new FreeMotion(copyState));
+ m_dcdCollisionStep.reset(new DcdCollision(copyState));
+ m_constraintGenerationStep.reset(new ContactConstraintGeneration(copyState));
+ m_buildMlcpStep.reset(new BuildMlcp(copyState));
+ m_solveMlcpStep.reset(new SolveMlcp(copyState));
+ m_pushResultsStep.reset(new PushResults(copyState));
+ m_postUpdateStep.reset(new PostUpdate(copyState));
+ m_updateCollisionRepresentationsStep.reset(new UpdateCollisionRepresentations(copyState));
+}
+
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/PhysicsManager.h b/SurgSim/Physics/PhysicsManager.h
new file mode 100644
index 0000000..0d92970
--- /dev/null
+++ b/SurgSim/Physics/PhysicsManager.h
@@ -0,0 +1,147 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_PHYSICSMANAGER_H
+#define SURGSIM_PHYSICS_PHYSICSMANAGER_H
+
+#include <boost/thread/mutex.hpp>
+#include <memory>
+#include <vector>
+
+#include "SurgSim/Framework/ComponentManager.h"
+#include "SurgSim/Framework/LockedContainer.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+class Component;
+}
+
+namespace Collision
+{
+class CollisionPair;
+class Representation;
+}
+namespace Physics
+{
+
+class BuildMlcp;
+class ConstraintComponent;
+class ContactConstraintGeneration;
+class DcdCollision;
+class FreeMotion;
+class PostUpdate;
+class PreUpdate;
+class PushResults;
+class Representation;
+class SolveMlcp;
+class UpdateCollisionRepresentations;
+
+/// PhyicsManager handles the physics and motion calculation, it uses Computations to
+/// separate the algorithmic steps into smaller pieces.
+class PhysicsManager : public SurgSim::Framework::ComponentManager
+{
+public:
+
+ /// Constructor
+ PhysicsManager();
+ virtual ~PhysicsManager();
+
+ /// Overrides ComponentManager::getType()
+ virtual int getType() const override;
+
+ friend class PhysicsManagerTest;
+
+ /// Get the last PhysicsManagerState from the previous PhysicsManager update.
+ /// \param [out] s pointer to an allocated PhysicsManagerState object.
+ /// \warning The state contains many pointers. The objects pointed to are not thread-safe.
+ void getFinalState(SurgSim::Physics::PhysicsManagerState* s) const;
+
+ /// Add an excluded collision pair to the Physics Manager. The pair will not participate in collisions.
+ /// \param representation1 The first Collision::Representation for the pair
+ /// \param representation2 The second Collision::Representation for the pair
+ void addExcludedCollisionPair(std::shared_ptr<SurgSim::Collision::Representation> representation1,
+ std::shared_ptr<SurgSim::Collision::Representation> representation2);
+
+ /// Remove an excluded collision pair to the Physics Manager. The pair will not be excluded from collisions.
+ /// \param representation1 The first Collision::Representation for the pair
+ /// \param representation2 The second Collision::Representation for the pair
+ void removeExcludedCollisionPair(std::shared_ptr<SurgSim::Collision::Representation> representation1,
+ std::shared_ptr<SurgSim::Collision::Representation> representation2);
+
+protected:
+ ///@{
+ /// Overridden from ComponentManager
+ bool executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component) override;
+ bool executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component) override;
+ ///@}
+
+ ///@{
+ /// Overridden from BasicThread
+ virtual bool doInitialize() override;
+ virtual bool doStartUp() override;
+ virtual bool doUpdate(double dt) override;
+ ///@}
+
+ void initializeComputations(bool copyState);
+private:
+ /// Get an iterator to an excluded collision pair.
+ /// \note Lock m_excludedCollisionPairMutex before calling
+ /// \param representation1 The first Collision::Representation for the pair
+ /// \param representation2 The second Collision::Representation for the pair
+ /// \return If the pair is found, an iterator to the excluded collision pair; otherwise an iterator to the
+ /// container's past-the-end element.
+ std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>::iterator findExcludedCollisionPair(
+ std::shared_ptr<SurgSim::Collision::Representation> representation1,
+ std::shared_ptr<SurgSim::Collision::Representation> representation2);
+
+ std::vector<std::shared_ptr<Representation>> m_representations;
+
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> m_collisionRepresentations;
+
+ std::vector<std::shared_ptr<ConstraintComponent>> m_constraintComponents;
+
+ /// List of Collision::Representation pairs to be excluded from contact generation.
+ std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>> m_excludedCollisionPairs;
+
+ /// Mutex to protect m_excludedCollisionPairs from being read/written simultaneously.
+ boost::mutex m_excludedCollisionPairMutex;
+
+ ///@{
+ /// Steps to perform the physics update
+ std::unique_ptr<PreUpdate> m_preUpdateStep;
+ std::unique_ptr<FreeMotion> m_freeMotionStep;
+ std::unique_ptr<DcdCollision> m_dcdCollisionStep;
+ std::unique_ptr<ContactConstraintGeneration> m_constraintGenerationStep;
+ std::unique_ptr<BuildMlcp> m_buildMlcpStep;
+ std::unique_ptr<SolveMlcp> m_solveMlcpStep;
+ std::unique_ptr<PushResults> m_pushResultsStep;
+ std::unique_ptr<PostUpdate> m_postUpdateStep;
+ std::unique_ptr<UpdateCollisionRepresentations> m_updateCollisionRepresentationsStep;
+ ///@}
+
+ /// A thread-safe copy of the last PhysicsManagerState in the previous update.
+ SurgSim::Framework::LockedContainer<SurgSim::Physics::PhysicsManagerState> m_finalState;
+};
+
+}; // namespace Physics
+}; // namespace SurgSim
+
+
+
+#endif
diff --git a/SurgSim/Physics/PhysicsManagerState.cpp b/SurgSim/Physics/PhysicsManagerState.cpp
new file mode 100644
index 0000000..efccea3
--- /dev/null
+++ b/SurgSim/Physics/PhysicsManagerState.cpp
@@ -0,0 +1,208 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintComponent.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/Representation.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+PhysicsManagerState::PhysicsManagerState()
+{
+
+}
+
+PhysicsManagerState::~PhysicsManagerState()
+{
+
+}
+
+void PhysicsManagerState::setRepresentations(const std::vector<std::shared_ptr<Representation>>& val)
+{
+ m_representations = val;
+
+ m_collisionsToPhysicsMap.clear();
+ for (auto it = m_representations.begin(); it != m_representations.end(); it++)
+ {
+ if ((*it)->isActive())
+ {
+ auto collision = (*it)->getCollisionRepresentation();
+ if (collision != nullptr)
+ {
+ m_collisionsToPhysicsMap[collision] = (*it);
+ }
+ }
+ }
+}
+
+const std::vector<std::shared_ptr<Representation>>& PhysicsManagerState::getRepresentations()
+{
+ return m_representations;
+}
+
+void PhysicsManagerState::setActiveRepresentations(
+ const std::vector<std::shared_ptr<Representation>>& activeRepresentations)
+{
+ m_activeRepresentations = activeRepresentations;
+}
+
+const std::vector<std::shared_ptr<Representation>>& PhysicsManagerState::getActiveRepresentations() const
+{
+ return m_activeRepresentations;
+}
+
+const std::unordered_map<
+ std::shared_ptr<SurgSim::Collision::Representation>,
+ std::shared_ptr<SurgSim::Physics::Representation>>&
+ PhysicsManagerState::getCollisionToPhysicsMap() const
+{
+ return m_collisionsToPhysicsMap;
+}
+
+void PhysicsManagerState::setCollisionRepresentations(
+ const std::vector<std::shared_ptr<SurgSim::Collision::Representation>>& val)
+{
+ m_collisionRepresentations = val;
+}
+
+const std::vector<std::shared_ptr<SurgSim::Collision::Representation>>&
+PhysicsManagerState::getCollisionRepresentations()
+{
+ return m_collisionRepresentations;
+}
+
+void PhysicsManagerState::setConstraintComponents(const std::vector<std::shared_ptr<ConstraintComponent>>& val)
+{
+ m_constraintComponents = val;
+
+ std::vector<std::shared_ptr<Constraint>>& constraints = m_constraints[CONSTRAINT_GROUP_TYPE_SCENE];
+
+ constraints.reserve(m_constraintComponents.size());
+ constraints.clear();
+ for (auto it = m_constraintComponents.cbegin(); it != m_constraintComponents.cend(); ++it)
+ {
+ if ((*it)->getConstraint()->isActive())
+ {
+ constraints.push_back((*it)->getConstraint());
+ }
+ }
+
+ setConstraintGroup(CONSTRAINT_GROUP_TYPE_SCENE, constraints);
+}
+
+const std::vector<std::shared_ptr<ConstraintComponent>>& PhysicsManagerState::getConstraintComponents()
+{
+ return m_constraintComponents;
+}
+
+void PhysicsManagerState::setCollisionPairs(std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>> val)
+{
+ m_collisionPairs = val;
+}
+
+const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>& PhysicsManagerState::getCollisionPairs()
+{
+ return m_collisionPairs;
+}
+
+void PhysicsManagerState::setExcludedCollisionPairs(
+ const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>& val)
+{
+ m_excludedCollisionPairs = val;
+}
+
+const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>&
+ PhysicsManagerState::getExcludedCollisionPairs() const
+{
+ return m_excludedCollisionPairs;
+}
+
+void PhysicsManagerState::setConstraintGroup(
+ ConstraintGroupType type,
+ const std::vector<std::shared_ptr<Constraint>>& constraints)
+{
+ m_constraints[type] = constraints;
+}
+
+const std::vector<std::shared_ptr<Constraint>>& PhysicsManagerState::getConstraintGroup(int type) const
+{
+ if (m_constraints.count(type) > 0)
+ {
+ return m_constraints.at(type);
+ }
+ static std::vector<std::shared_ptr<Constraint>> emptyVector;
+ return emptyVector;
+}
+
+void PhysicsManagerState::setActiveConstraints(
+ const std::vector<std::shared_ptr<Constraint>>& activeConstraints)
+{
+ m_activeConstraints = activeConstraints;
+}
+
+const std::vector<std::shared_ptr<Constraint>>& PhysicsManagerState::getActiveConstraints() const
+{
+ return m_activeConstraints;
+}
+
+MlcpPhysicsProblem& PhysicsManagerState::getMlcpProblem()
+{
+ return m_mlcpPhysicsProblem;
+}
+
+const MlcpPhysicsProblem& PhysicsManagerState::getMlcpProblem() const
+{
+ return m_mlcpPhysicsProblem;
+}
+
+MlcpPhysicsSolution& PhysicsManagerState::getMlcpSolution()
+{
+ return m_mlcpPhysicsSolution;
+}
+
+const MlcpPhysicsSolution& PhysicsManagerState::getMlcpSolution() const
+{
+ return m_mlcpPhysicsSolution;
+}
+
+const MlcpMapping<Representation>& PhysicsManagerState::getRepresentationsMapping() const
+{
+ return m_representationsIndexMapping;
+}
+
+void PhysicsManagerState::setRepresentationsMapping(const MlcpMapping<Representation>& representationsMapping)
+{
+ m_representationsIndexMapping = representationsMapping;
+}
+
+const MlcpMapping<Constraint>& PhysicsManagerState::getConstraintsMapping() const
+{
+ return m_constraintsIndexMapping;
+}
+
+void PhysicsManagerState::setConstraintsMapping(const MlcpMapping<Constraint>& constraintsMapping)
+{
+ m_constraintsIndexMapping = constraintsMapping;
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/PhysicsManagerState.h b/SurgSim/Physics/PhysicsManagerState.h
new file mode 100644
index 0000000..f29a8d9
--- /dev/null
+++ b/SurgSim/Physics/PhysicsManagerState.h
@@ -0,0 +1,212 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_PHYSICSMANAGERSTATE_H
+#define SURGSIM_PHYSICS_PHYSICSMANAGERSTATE_H
+
+#include <memory>
+#include <vector>
+#include <unordered_map>
+
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/MlcpMapping.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/MlcpPhysicsSolution.h"
+#include "SurgSim/Physics/Representation.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class ConstraintComponent;
+
+enum ConstraintGroupType
+{
+ CONSTRAINT_GROUP_TYPE_CONTACT = 0,
+ CONSTRAINT_GROUP_TYPE_SCENE,
+ CONSTRAINT_GROUP_TYPE_COUNT
+};
+
+class PhysicsManagerState
+{
+public:
+ /// Constructor
+ PhysicsManagerState();
+
+ /// Destructor
+ ~PhysicsManagerState();
+
+ /// Sets the physics representations for the state, these are the basis for all the computations.
+ /// \param val The list of representations.
+ void setRepresentations(const std::vector<std::shared_ptr<Representation>>& val);
+
+ /// Gets the physics representations.
+ /// \return The physics representations that are known to the state.
+ const std::vector<std::shared_ptr<Representation>>& getRepresentations();
+
+ /// Set the list of representations into the active representations list.
+ /// \param activeRepresentations The active physics representations that are known to the state.
+ void setActiveRepresentations(const std::vector<std::shared_ptr<Representation>>& activeRepresentations);
+
+ /// Gets the active physics representations.
+ /// \return The active physics representations that are known to the state.
+ const std::vector<std::shared_ptr<Representation>>& getActiveRepresentations() const;
+
+ /// Sets the collision representations for the state.
+ /// \param val collection of all collision representations.
+ void setCollisionRepresentations(const std::vector<std::shared_ptr<SurgSim::Collision::Representation>>& val);
+
+ /// Gets the collision representations.
+ /// \return The collision representations that are known to the state.
+ const std::vector<std::shared_ptr<SurgSim::Collision::Representation>>& getCollisionRepresentations();
+
+ /// Sets the list of constraint components
+ /// \param val collection of all constraint components
+ void setConstraintComponents(const std::vector<std::shared_ptr<ConstraintComponent>>& val);
+
+ /// Gets the constraint components
+ /// \return The constraint components known to the state
+ const std::vector<std::shared_ptr<ConstraintComponent>>& getConstraintComponents();
+
+ /// \return A map that associates collision representations with physics representations where
+ /// map[physicsRep->getCollisionRepresentation] = physicsRep
+ const std::unordered_map<std::shared_ptr<SurgSim::Collision::Representation>,
+ std::shared_ptr<SurgSim::Physics::Representation>>& getCollisionToPhysicsMap() const;
+
+ /// Sets collision pairs that should be considered, while this is not being verified the collision pairs
+ /// should only be from the list of representations that are in this state.
+ /// \param val The list of collision pairs.
+ void setCollisionPairs(std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>> val);
+
+ /// Gets collision pairs.
+ /// \return The collision pairs.
+ const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>& getCollisionPairs();
+
+ /// Sets the exclusion pairs
+ /// \param val The list of collision pairs to be excluded from collision.
+ void setExcludedCollisionPairs(const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>& val);
+
+ /// Gets the exclusion pairs
+ /// \return The stored exclusion pairs
+ const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>& getExcludedCollisionPairs() const;
+
+ /// Sets the group of constraints to a given value, the grouping indicates what type of constraint we are dealing
+ /// with.
+ /// \param type The type of constraint grouping e.g. Contact Constraints.
+ /// \param constraints The constraints.
+ void setConstraintGroup(ConstraintGroupType type, const std::vector<std::shared_ptr<Constraint>>& constraints);
+
+ /// Gets constraint group.
+ /// \param type The type.
+ /// \return The constraint group.
+ const std::vector<std::shared_ptr<Constraint>>& getConstraintGroup(int type) const;
+
+ /// Filter the map of constraints into the active constraints list.
+ /// \param activeConstraints The list of active constraints.
+ void setActiveConstraints(const std::vector<std::shared_ptr<Constraint>>& activeConstraints);
+
+ /// \return The list of all active constraints.
+ const std::vector<std::shared_ptr<Constraint>>& getActiveConstraints() const;
+
+ /// Gets the Mlcp problem
+ /// \return The Mlcp problem for this physics manager state (read/write access).
+ MlcpPhysicsProblem& getMlcpProblem();
+
+ /// Gets the Mlcp problem
+ /// \return The Mlcp problem for this physics manager state (const).
+ const MlcpPhysicsProblem& getMlcpProblem() const;
+
+ /// Gets the Mlcp solution
+ /// \return The Mlcp solution for this physics manager state (read/write access).
+ MlcpPhysicsSolution& getMlcpSolution();
+
+ /// Gets the Mlcp solution
+ /// \return The Mlcp solution for this physics manager state (const).
+ const MlcpPhysicsSolution& getMlcpSolution() const;
+
+ /// Gets the representations mapping
+ /// \return The representations mapping (mapping between the representation and the mlcp)
+ /// Each representation has an index in the mlcp. This mapping is about this index.
+ const MlcpMapping<Representation>& getRepresentationsMapping() const;
+
+ /// Set the representations mapping
+ /// \param representationsMapping The representations mapping (mapping between the representation and the mlcp)
+ void setRepresentationsMapping(const MlcpMapping<Representation>& representationsMapping);
+
+ /// Gets the constraints mapping
+ /// \return The constraints mapping (mapping between the constraints and the mlcp)
+ /// Each constraint has an index in the mlcp. This mapping is about this index.
+ const MlcpMapping<Constraint>& getConstraintsMapping() const;
+
+ /// Set the constraints mapping
+ /// \param constraintsMapping The constraints mapping (mapping between the constraints and the mlcp)
+ void setConstraintsMapping(const MlcpMapping<Constraint>& constraintsMapping);
+
+private:
+
+ ///@{
+ /// Local state data structures, please note that the physics state may get copied, these data structures
+ /// should copy their contents on copy. With the caveat that objects contained within those structures might
+ /// not get copied themselves.
+ /// The local list of representations
+ std::vector<std::shared_ptr<Representation>> m_representations;
+
+ /// The list of active representations.
+ std::vector<std::shared_ptr<Representation>> m_activeRepresentations;
+
+ /// List of all the collision representations know to the state
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> m_collisionRepresentations;
+
+ /// List of the constraint components
+ std::vector<std::shared_ptr<ConstraintComponent>> m_constraintComponents;
+
+ /// Mapping of collision representations to their respective physics representation.
+ std::unordered_map<std::shared_ptr<SurgSim::Collision::Representation>,
+ std::shared_ptr<SurgSim::Physics::Representation>> m_collisionsToPhysicsMap;
+
+ /// The local list of collision pairs.
+ std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>> m_collisionPairs;
+
+ /// List of collision pairs to be excluded.
+ std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>> m_excludedCollisionPairs;
+
+ /// The local map of constraints.
+ std::unordered_map<int, std::vector<std::shared_ptr<Constraint>>> m_constraints;
+
+ /// The list of active constraints.
+ std::vector<std::shared_ptr<Constraint>> m_activeConstraints;
+
+ /// Representation mapping
+ MlcpMapping<Representation> m_representationsIndexMapping;
+
+ /// Constraints mapping
+ MlcpMapping<Constraint> m_constraintsIndexMapping;
+
+ ///@}
+ /// Mlcp problem for this Physics Manager State
+ MlcpPhysicsProblem m_mlcpPhysicsProblem;
+
+ /// Mlcp solution for this Physics Manager State
+ MlcpPhysicsSolution m_mlcpPhysicsSolution;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif
diff --git a/SurgSim/Physics/PostUpdate.cpp b/SurgSim/Physics/PostUpdate.cpp
new file mode 100644
index 0000000..992278c
--- /dev/null
+++ b/SurgSim/Physics/PostUpdate.cpp
@@ -0,0 +1,54 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <vector>
+
+#include "SurgSim/Physics/PostUpdate.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+PostUpdate::PostUpdate(bool doCopyState) : Computation(doCopyState)
+{
+
+}
+
+PostUpdate::~PostUpdate()
+{
+
+}
+
+std::shared_ptr<PhysicsManagerState> PostUpdate::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+ std::vector<std::shared_ptr<Representation>> representations = result->getRepresentations();
+ for (auto it = representations.begin(); it != representations.end(); ++it)
+ {
+ (*it)->afterUpdate(dt);
+ }
+
+ return result;
+}
+
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/PostUpdate.h b/SurgSim/Physics/PostUpdate.h
new file mode 100644
index 0000000..12c1a45
--- /dev/null
+++ b/SurgSim/Physics/PostUpdate.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_POSTUPDATE_H
+#define SURGSIM_PHYSICS_POSTUPDATE_H
+
+#include <memory>
+
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+/// Post Update is called prior to anything else at the beginning of each time step
+class PostUpdate : public Computation
+{
+public:
+
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit PostUpdate(bool doCopyState = false);
+
+ /// Destructor
+ virtual ~PostUpdate();
+
+protected:
+
+ /// Override doUpdate from superclass
+ virtual std::shared_ptr<PhysicsManagerState>
+ doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_POSTUPDATE_H
diff --git a/SurgSim/Physics/PreUpdate.cpp b/SurgSim/Physics/PreUpdate.cpp
new file mode 100644
index 0000000..05c0c25
--- /dev/null
+++ b/SurgSim/Physics/PreUpdate.cpp
@@ -0,0 +1,54 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <vector>
+
+#include "SurgSim/Physics/PreUpdate.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+PreUpdate::PreUpdate(bool doCopyState) : Computation(doCopyState)
+{
+
+}
+
+PreUpdate::~PreUpdate()
+{
+
+}
+
+std::shared_ptr<PhysicsManagerState> PreUpdate::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+ std::vector<std::shared_ptr<Representation>> representations = result->getRepresentations();
+ for (auto it = representations.begin(); it != representations.end(); ++it)
+ {
+ (*it)->beforeUpdate(dt);
+ }
+
+ return result;
+}
+
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/PreUpdate.h b/SurgSim/Physics/PreUpdate.h
new file mode 100644
index 0000000..8a35917
--- /dev/null
+++ b/SurgSim/Physics/PreUpdate.h
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_PREUPDATE_H
+#define SURGSIM_PHYSICS_PREUPDATE_H
+
+#include <memory>
+
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+/// Pre Update is called after everything else is done in the physics time step
+class PreUpdate : public Computation
+{
+public:
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit PreUpdate(bool doCopyState = false);
+
+ /// Destructor
+ virtual ~PreUpdate();
+
+protected:
+ /// Override doUpdate from superclass
+ virtual std::shared_ptr<PhysicsManagerState>
+ doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_PREUPDATE_H
diff --git a/SurgSim/Physics/PushResults.cpp b/SurgSim/Physics/PushResults.cpp
new file mode 100644
index 0000000..1930f50
--- /dev/null
+++ b/SurgSim/Physics/PushResults.cpp
@@ -0,0 +1,67 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/MlcpPhysicsSolution.h"
+#include "SurgSim/Physics/PushResults.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+PushResults::PushResults(bool doCopyState) : Computation(doCopyState)
+{}
+
+PushResults::~PushResults()
+{}
+
+std::shared_ptr<PhysicsManagerState>
+ PushResults::doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+ // 1st step
+ // Compute the global dof displacement correction from the constraints forces (result of the MLCP)
+ // correction = CHt . lambda
+ const Eigen::VectorXd& lambda = result->getMlcpSolution().x;
+ if (lambda.size() == 0)
+ {
+ return state;
+ }
+ const SurgSim::Math::MlcpProblem::Matrix& CHt = result->getMlcpProblem().CHt;
+ SurgSim::Math::MlcpSolution::Vector& dofCorrection = result->getMlcpSolution().dofCorrection;
+ dofCorrection = CHt * lambda;
+
+ // 2nd step
+ // Push the dof displacement correction to all representation, using their assigned index
+ std::vector<std::shared_ptr<Representation>> representations = result->getRepresentations();
+ auto const itEnd = representations.end();
+ for (auto it = representations.begin(); it != itEnd; ++it)
+ {
+ if ((*it)->isActive())
+ {
+ ptrdiff_t index = result->getRepresentationsMapping().getValue((*it).get());
+ SURGSIM_ASSERT(index >= 0) << "Bad index found for representation " << (*it)->getName() << std::endl;
+ (*it)->applyCorrection(dt, dofCorrection.segment(index, (*it)->getNumDof()));
+ }
+ }
+
+ return result;
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/PushResults.h b/SurgSim/Physics/PushResults.h
new file mode 100644
index 0000000..1fbb4e6
--- /dev/null
+++ b/SurgSim/Physics/PushResults.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_PUSHRESULTS_H
+#define SURGSIM_PHYSICS_PUSHRESULTS_H
+
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class Representation;
+
+/// Propagates the Mlcp result to the representations
+class PushResults : public Computation
+{
+public:
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit PushResults(bool doCopyState = false);
+
+ /// Destructor
+ virtual ~PushResults();
+
+protected:
+
+ /// Override doUpdate from superclass
+ virtual std::shared_ptr<PhysicsManagerState>
+ doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+};
+
+}; // namespace Physics
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_PUSHRESULTS_H
diff --git a/SurgSim/Physics/RenderTests/CMakeLists.txt b/SurgSim/Physics/RenderTests/CMakeLists.txt
new file mode 100644
index 0000000..7292395
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/CMakeLists.txt
@@ -0,0 +1,54 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+ ${OSG_INCLUDE_DIR}
+ ${OPENTHREADS_INCLUDE_DIR}
+)
+
+set(UNIT_TEST_SOURCES
+ Fem3DMeshRenderTest.cpp
+ Fem3DvsTruthCubeRenderTest.cpp
+ RenderTest.cpp
+ RenderTestFem1D.cpp
+ RenderTestFem2D.cpp
+ RenderTestFem3D.cpp
+ RenderTestFem3DCorotational.cpp
+ RenderTestMassSprings.cpp
+ RenderTestRigidBodies.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ RenderTest.h
+)
+
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+set(LIBS
+ SurgSimBlocks
+ SurgSimGraphics
+ SurgSimPhysics
+)
+
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
+surgsim_add_unit_tests(SurgSimPhysicsRenderTest)
+
+set_target_properties(SurgSimPhysicsRenderTest PROPERTIES FOLDER "Physics")
diff --git a/SurgSim/Physics/RenderTests/Data/box.ply b/SurgSim/Physics/RenderTests/Data/box.ply
new file mode 100644
index 0000000..8cc0fb4
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Data/box.ply
@@ -0,0 +1,30 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 8
+property float x
+property float y
+property float z
+element face 12
+property list uchar uint vertex_indices
+end_header
+-0.050000 -0.050000 0.050000
+-0.050000 0.050000 0.050000
+-0.050000 0.050000 -0.050000
+0.050000 0.050000 0.050000
+0.050000 0.050000 -0.050000
+0.050000 -0.050000 0.050000
+0.050000 -0.050000 -0.050000
+-0.050000 -0.050000 -0.050000
+3 0 1 2
+3 1 3 4
+3 3 5 6
+3 0 7 5
+3 7 2 4
+3 5 3 1
+3 7 0 2
+3 2 1 4
+3 4 3 6
+3 5 7 6
+3 6 7 4
+3 0 5 1
diff --git a/SurgSim/Physics/RenderTests/Data/sphere.ply b/SurgSim/Physics/RenderTests/Data/sphere.ply
new file mode 100644
index 0000000..59b61df
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Data/sphere.ply
@@ -0,0 +1,1932 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 642
+property float x
+property float y
+property float z
+element face 1280
+property list uchar uint vertex_indices
+end_header
+0.000000 0.000000 -0.050000
+0.004824 -0.003504 -0.049643
+-0.001842 -0.005670 -0.049643
+0.036180 -0.026286 -0.022361
+0.033765 -0.024531 -0.027534
+0.038589 -0.021027 -0.023849
+-0.005962 0.000000 -0.049643
+-0.001842 0.005670 -0.049643
+0.004824 0.003504 -0.049643
+0.039727 -0.024531 -0.017887
+-0.013819 -0.042532 -0.022361
+-0.008073 -0.043198 -0.023849
+-0.011054 -0.045364 -0.017887
+-0.044721 0.000000 -0.022361
+-0.043578 -0.005670 -0.023849
+-0.046559 -0.003504 -0.017887
+-0.013819 0.042532 -0.022361
+-0.018859 0.039693 -0.023849
+-0.017720 0.043198 -0.017887
+0.036180 0.026286 -0.022361
+0.031923 0.030202 -0.023849
+0.035608 0.030202 -0.017887
+0.035608 -0.030202 -0.017887
+-0.017720 -0.043198 -0.017887
+-0.046559 0.003504 -0.017887
+-0.011054 0.045364 -0.017887
+0.039727 0.024531 -0.017887
+0.013819 -0.042532 0.022361
+0.018859 -0.039693 0.023849
+0.012897 -0.039693 0.027534
+-0.036180 -0.026286 0.022361
+-0.031923 -0.030202 0.023849
+-0.033765 -0.024531 0.027534
+-0.036180 0.026286 0.022361
+-0.038589 0.021027 0.023849
+-0.033765 0.024531 0.027534
+0.013819 0.042532 0.022361
+0.008073 0.043198 0.023849
+0.012897 0.039693 0.027534
+0.044721 0.000000 0.022361
+0.043578 0.005670 0.023849
+0.041736 0.000000 0.027534
+-0.012897 -0.039693 -0.027534
+-0.011641 -0.035828 -0.032876
+-0.006634 -0.039956 -0.029317
+-0.010034 -0.030883 -0.038020
+-0.004896 -0.035539 -0.034828
+-0.008123 -0.025000 -0.042533
+-0.002912 -0.029955 -0.039928
+-0.006021 -0.018530 -0.046048
+-0.000805 -0.023468 -0.044143
+-0.003880 -0.011943 -0.048397
+0.001270 -0.016561 -0.047160
+0.003174 -0.009769 -0.048934
+-0.001482 -0.043209 -0.025115
+0.000485 -0.039449 -0.030717
+0.005728 -0.042336 -0.025979
+0.002640 -0.034409 -0.036181
+0.008123 -0.037973 -0.031497
+0.013143 -0.040451 -0.026287
+0.004839 -0.028212 -0.040996
+0.010504 -0.032329 -0.036668
+0.015749 -0.035495 -0.031497
+0.020250 -0.037617 -0.025979
+0.006910 -0.021266 -0.044721
+0.012668 -0.025668 -0.040996
+0.018090 -0.029389 -0.036181
+0.022795 -0.032200 -0.030717
+0.026597 -0.034086 -0.025115
+0.008707 -0.014145 -0.047160
+0.014446 -0.018513 -0.044143
+0.019964 -0.022522 -0.039928
+0.024851 -0.025874 -0.034828
+0.028853 -0.028426 -0.029317
+0.031923 -0.030202 -0.023849
+0.010159 -0.007381 -0.048397
+0.015763 -0.011452 -0.046048
+0.021266 -0.015451 -0.042533
+0.026271 -0.019087 -0.038020
+0.030477 -0.022143 -0.032876
+0.038589 0.021027 -0.023849
+0.033765 0.024531 -0.027534
+0.040636 0.014762 -0.025115
+0.035951 0.018657 -0.029317
+0.042034 0.007635 -0.025979
+0.037668 0.011729 -0.030717
+0.042532 -0.000000 -0.026287
+0.038625 0.004009 -0.031497
+0.042034 -0.007635 -0.025979
+0.038625 -0.004009 -0.031497
+0.040636 -0.014762 -0.025115
+0.037668 -0.011729 -0.030717
+0.035951 -0.018657 -0.029317
+0.030477 0.022143 -0.032876
+0.032287 0.015638 -0.034828
+0.026271 0.019087 -0.038020
+0.033541 0.008123 -0.036181
+0.027589 0.012027 -0.039928
+0.021266 0.015451 -0.042533
+0.033992 -0.000000 -0.036668
+0.028327 0.004116 -0.040996
+0.022071 0.008018 -0.044143
+0.015763 0.011452 -0.046048
+0.033541 -0.008123 -0.036180
+0.028327 -0.004116 -0.040996
+0.022361 -0.000000 -0.044721
+0.016143 0.003910 -0.047160
+0.010159 0.007381 -0.048397
+0.032287 -0.015638 -0.034828
+0.027589 -0.012027 -0.039927
+0.022071 -0.008018 -0.044143
+0.016143 -0.003910 -0.047160
+0.010272 -0.000000 -0.048934
+-0.041736 0.000000 -0.027534
+-0.037672 0.000000 -0.032876
+-0.040051 -0.006037 -0.029317
+-0.032473 0.000000 -0.038020
+-0.035313 -0.006326 -0.034828
+-0.026286 0.000000 -0.042533
+-0.029389 -0.006487 -0.039927
+-0.019484 0.000000 -0.046048
+-0.022569 -0.006487 -0.044143
+-0.012557 0.000000 -0.048397
+-0.015358 -0.006326 -0.047160
+-0.008310 -0.006038 -0.048934
+-0.041553 -0.011943 -0.025115
+-0.037368 -0.012652 -0.030717
+-0.038494 -0.018530 -0.025979
+-0.031910 -0.013143 -0.036180
+-0.033604 -0.019460 -0.031497
+-0.034409 -0.025000 -0.026287
+-0.025336 -0.013320 -0.040996
+-0.027500 -0.019980 -0.036668
+-0.028892 -0.025946 -0.031497
+-0.029518 -0.030883 -0.025979
+-0.018090 -0.013143 -0.044721
+-0.020498 -0.019980 -0.040996
+-0.022361 -0.026286 -0.036181
+-0.023580 -0.031630 -0.030717
+-0.024199 -0.035828 -0.025115
+-0.010762 -0.012652 -0.047160
+-0.013143 -0.019460 -0.044143
+-0.015251 -0.025946 -0.039928
+-0.016928 -0.031630 -0.034828
+-0.018118 -0.036225 -0.029317
+-0.018859 -0.039693 -0.023849
+-0.012897 0.039693 -0.027534
+-0.011641 0.035828 -0.032876
+-0.018118 0.036225 -0.029317
+-0.010034 0.030883 -0.038020
+-0.016928 0.031630 -0.034828
+-0.008123 0.025000 -0.042533
+-0.015251 0.025946 -0.039928
+-0.006021 0.018530 -0.046048
+-0.013143 0.019460 -0.044143
+-0.003880 0.011943 -0.048397
+-0.010762 0.012652 -0.047160
+-0.008310 0.006037 -0.048934
+-0.024199 0.035828 -0.025115
+-0.023580 0.031630 -0.030717
+-0.029518 0.030883 -0.025979
+-0.022361 0.026286 -0.036181
+-0.028892 0.025946 -0.031497
+-0.034409 0.025000 -0.026287
+-0.020498 0.019980 -0.040996
+-0.027500 0.019980 -0.036668
+-0.033604 0.019460 -0.031497
+-0.038494 0.018530 -0.025979
+-0.018090 0.013143 -0.044721
+-0.025336 0.013320 -0.040996
+-0.031910 0.013143 -0.036180
+-0.037368 0.012652 -0.030717
+-0.041553 0.011943 -0.025115
+-0.015358 0.006326 -0.047160
+-0.022569 0.006487 -0.044143
+-0.029389 0.006487 -0.039927
+-0.035313 0.006326 -0.034828
+-0.040051 0.006037 -0.029317
+-0.043578 0.005670 -0.023849
+0.028853 0.028426 -0.029317
+0.024850 0.025874 -0.034828
+0.019964 0.022522 -0.039928
+0.014446 0.018513 -0.044143
+0.008707 0.014145 -0.047160
+0.003174 0.009769 -0.048934
+0.026597 0.034086 -0.025115
+0.022795 0.032200 -0.030717
+0.020250 0.037617 -0.025979
+0.018090 0.029389 -0.036181
+0.015749 0.035495 -0.031497
+0.013143 0.040451 -0.026287
+0.012668 0.025669 -0.040996
+0.010504 0.032329 -0.036668
+0.008123 0.037973 -0.031497
+0.005728 0.042336 -0.025979
+0.006910 0.021266 -0.044721
+0.004839 0.028212 -0.040996
+0.002639 0.034409 -0.036181
+0.000485 0.039449 -0.030717
+-0.001482 0.043209 -0.025115
+0.001270 0.016561 -0.047160
+-0.000805 0.023469 -0.044143
+-0.002913 0.029955 -0.039928
+-0.004896 0.035539 -0.034828
+-0.006634 0.039956 -0.029317
+-0.008073 0.043198 -0.023849
+0.046559 -0.003504 0.017887
+0.046559 0.003504 0.017887
+0.047831 -0.007381 0.012557
+0.048361 0.000000 0.012697
+0.048236 -0.011452 0.006495
+0.049401 -0.003910 0.006652
+0.047553 -0.015451 0.000000
+0.049353 -0.008018 0.000000
+0.045755 -0.019087 -0.006495
+0.048050 -0.012027 -0.006820
+0.043035 -0.022143 -0.012558
+0.045590 -0.015638 -0.013303
+0.042299 -0.018657 -0.019045
+0.047831 0.007381 0.012557
+0.049401 0.003910 0.006652
+0.048236 0.011452 0.006495
+0.050000 -0.000000 0.000000
+0.049353 0.008018 -0.000000
+0.047553 0.015451 -0.000000
+0.049336 -0.004116 -0.007003
+0.049336 0.004116 -0.007003
+0.048050 0.012027 -0.006820
+0.045755 0.019087 -0.006495
+0.047361 -0.008123 -0.013820
+0.047998 -0.000000 -0.014006
+0.047361 0.008123 -0.013820
+0.045590 0.015638 -0.013303
+0.043035 0.022143 -0.012558
+0.044320 -0.011729 -0.019955
+0.045445 -0.004009 -0.020461
+0.045445 0.004009 -0.020461
+0.044320 0.011729 -0.019955
+0.042299 0.018657 -0.019045
+0.011054 -0.045364 0.017887
+0.017720 -0.043198 0.017887
+0.007761 -0.047771 0.012558
+0.014944 -0.045994 0.012697
+0.004014 -0.049414 0.006495
+0.011547 -0.048191 0.006652
+0.000000 -0.050000 0.000000
+0.007625 -0.049415 0.000000
+-0.004014 -0.049414 -0.006495
+0.003410 -0.049415 -0.006821
+-0.007761 -0.047771 -0.012558
+-0.000785 -0.048191 -0.013303
+-0.004673 -0.045994 -0.019045
+0.021800 -0.043209 0.012558
+0.018984 -0.045775 0.006652
+0.025797 -0.042336 0.006495
+0.015451 -0.047553 -0.000000
+0.022876 -0.044460 -0.000000
+0.029389 -0.040451 -0.000000
+0.011331 -0.048193 -0.007003
+0.019160 -0.045649 -0.007003
+0.026287 -0.041982 -0.006821
+0.032292 -0.037617 -0.006495
+0.006910 -0.047553 -0.013820
+0.014832 -0.045649 -0.014006
+0.022361 -0.042532 -0.013820
+0.028961 -0.038526 -0.013303
+0.034358 -0.034086 -0.012558
+0.002541 -0.045775 -0.019955
+0.010231 -0.044460 -0.020462
+0.017856 -0.041982 -0.020462
+0.024851 -0.038526 -0.019955
+0.030815 -0.034463 -0.019045
+-0.039727 -0.024531 0.017887
+-0.035608 -0.030202 0.017887
+-0.043035 -0.022143 0.012558
+-0.039125 -0.028426 0.012697
+-0.045755 -0.019087 0.006495
+-0.042264 -0.025874 0.006652
+-0.047553 -0.015451 0.000000
+-0.044640 -0.022522 0.000000
+-0.048236 -0.011452 -0.006495
+-0.045943 -0.018513 -0.006820
+-0.047831 -0.007381 -0.012557
+-0.046075 -0.014145 -0.013303
+-0.045187 -0.009769 -0.019045
+-0.034358 -0.034086 0.012558
+-0.037668 -0.032200 0.006652
+-0.032292 -0.037617 0.006495
+-0.040451 -0.029389 -0.000000
+-0.035215 -0.035495 -0.000000
+-0.029389 -0.040451 -0.000000
+-0.042333 -0.025669 -0.007003
+-0.037494 -0.032329 -0.007003
+-0.031804 -0.037973 -0.006821
+-0.025797 -0.042336 -0.006495
+-0.043090 -0.021266 -0.013820
+-0.038831 -0.028213 -0.014006
+-0.033541 -0.034410 -0.013820
+-0.027691 -0.039449 -0.013303
+-0.021800 -0.043209 -0.012558
+-0.042750 -0.016562 -0.019955
+-0.039122 -0.023469 -0.020461
+-0.034409 -0.029955 -0.020461
+-0.028961 -0.035539 -0.019955
+-0.023254 -0.039957 -0.019045
+-0.035608 0.030202 0.017887
+-0.039727 0.024531 0.017887
+-0.034358 0.034086 0.012558
+-0.039125 0.028426 0.012697
+-0.032292 0.037617 0.006495
+-0.037668 0.032200 0.006652
+-0.029389 0.040451 0.000000
+-0.035215 0.035495 0.000000
+-0.025797 0.042336 -0.006495
+-0.031804 0.037973 -0.006821
+-0.021800 0.043209 -0.012558
+-0.027691 0.039449 -0.013303
+-0.023254 0.039957 -0.019045
+-0.043035 0.022143 0.012558
+-0.042264 0.025874 0.006652
+-0.045755 0.019087 0.006495
+-0.040451 0.029389 0.000000
+-0.044640 0.022522 -0.000000
+-0.047553 0.015451 -0.000000
+-0.037494 0.032329 -0.007003
+-0.042333 0.025669 -0.007003
+-0.045943 0.018513 -0.006820
+-0.048236 0.011452 -0.006495
+-0.033541 0.034410 -0.013820
+-0.038831 0.028213 -0.014006
+-0.043090 0.021266 -0.013820
+-0.046075 0.014145 -0.013303
+-0.047831 0.007381 -0.012557
+-0.028961 0.035539 -0.019955
+-0.034409 0.029955 -0.020461
+-0.039122 0.023469 -0.020461
+-0.042750 0.016562 -0.019955
+-0.045187 0.009769 -0.019045
+0.017720 0.043198 0.017887
+0.011054 0.045364 0.017887
+0.021800 0.043209 0.012558
+0.014944 0.045994 0.012697
+0.025797 0.042336 0.006495
+0.018984 0.045775 0.006652
+0.029389 0.040451 0.000000
+0.022876 0.044460 0.000000
+0.032292 0.037617 -0.006495
+0.026287 0.041982 -0.006821
+0.034358 0.034086 -0.012558
+0.028961 0.038526 -0.013303
+0.030815 0.034463 -0.019045
+0.007761 0.047771 0.012558
+0.011547 0.048191 0.006652
+0.004014 0.049414 0.006495
+0.015451 0.047553 0.000000
+0.007625 0.049415 -0.000000
+-0.000000 0.050000 -0.000000
+0.019160 0.045649 -0.007003
+0.011331 0.048193 -0.007003
+0.003410 0.049415 -0.006821
+-0.004014 0.049414 -0.006495
+0.022361 0.042532 -0.013820
+0.014832 0.045649 -0.014006
+0.006910 0.047553 -0.013820
+-0.000785 0.048191 -0.013303
+-0.007761 0.047771 -0.012558
+0.024851 0.038526 -0.019955
+0.017856 0.041982 -0.020462
+0.010231 0.044460 -0.020462
+0.002541 0.045775 -0.019955
+-0.004673 0.045994 -0.019045
+0.023254 -0.039957 0.019045
+0.027691 -0.039449 0.013303
+0.031804 -0.037973 0.006820
+0.035215 -0.035495 -0.000000
+0.037668 -0.032200 -0.006652
+0.039125 -0.028426 -0.012697
+0.024199 -0.035828 0.025115
+0.028961 -0.035539 0.019955
+0.029518 -0.030883 0.025979
+0.033541 -0.034409 0.013820
+0.034410 -0.029955 0.020461
+0.034409 -0.025000 0.026287
+0.037494 -0.032329 0.007003
+0.038831 -0.028213 0.014006
+0.039122 -0.023469 0.020461
+0.038494 -0.018530 0.025979
+0.040451 -0.029389 -0.000000
+0.042333 -0.025669 0.007003
+0.043090 -0.021266 0.013820
+0.042750 -0.016561 0.019955
+0.041553 -0.011943 0.025115
+0.042264 -0.025874 -0.006652
+0.044640 -0.022522 -0.000000
+0.045943 -0.018513 0.006820
+0.046075 -0.014145 0.013303
+0.045187 -0.009769 0.019045
+0.043578 -0.005670 0.023849
+-0.030815 -0.034463 0.019045
+-0.028961 -0.038526 0.013303
+-0.026287 -0.041982 0.006820
+-0.022876 -0.044460 -0.000000
+-0.018984 -0.045775 -0.006652
+-0.014944 -0.045994 -0.012697
+-0.026597 -0.034086 0.025115
+-0.024851 -0.038526 0.019955
+-0.020250 -0.037617 0.025979
+-0.022361 -0.042532 0.013820
+-0.017856 -0.041982 0.020461
+-0.013143 -0.040451 0.026287
+-0.019160 -0.045649 0.007003
+-0.014832 -0.045649 0.014006
+-0.010231 -0.044460 0.020461
+-0.005728 -0.042336 0.025979
+-0.015451 -0.047553 -0.000000
+-0.011331 -0.048193 0.007003
+-0.006910 -0.047553 0.013820
+-0.002541 -0.045775 0.019955
+0.001482 -0.043209 0.025115
+-0.011547 -0.048191 -0.006652
+-0.007625 -0.049415 -0.000000
+-0.003410 -0.049415 0.006820
+0.000785 -0.048191 0.013303
+0.004673 -0.045994 0.019045
+0.008073 -0.043198 0.023849
+-0.042299 0.018657 0.019045
+-0.045590 0.015638 0.013303
+-0.048050 0.012027 0.006820
+-0.049353 0.008018 0.000000
+-0.049401 0.003910 -0.006652
+-0.048361 -0.000000 -0.012697
+-0.040636 0.014762 0.025115
+-0.044320 0.011729 0.019955
+-0.042034 0.007635 0.025979
+-0.047361 0.008123 0.013820
+-0.045445 0.004009 0.020461
+-0.042532 0.000000 0.026287
+-0.049336 0.004116 0.007003
+-0.047998 -0.000000 0.014006
+-0.045445 -0.004009 0.020461
+-0.042034 -0.007635 0.025979
+-0.050000 -0.000000 0.000000
+-0.049336 -0.004116 0.007003
+-0.047361 -0.008123 0.013820
+-0.044320 -0.011729 0.019955
+-0.040636 -0.014762 0.025115
+-0.049401 -0.003910 -0.006652
+-0.049353 -0.008018 0.000000
+-0.048050 -0.012027 0.006821
+-0.045590 -0.015638 0.013303
+-0.042299 -0.018657 0.019045
+-0.038589 -0.021027 0.023849
+0.004673 0.045994 0.019045
+0.000785 0.048191 0.013303
+-0.003410 0.049415 0.006821
+-0.007625 0.049415 -0.000000
+-0.011547 0.048191 -0.006652
+-0.014944 0.045994 -0.012697
+0.001482 0.043209 0.025115
+-0.002541 0.045775 0.019955
+-0.005728 0.042336 0.025979
+-0.006910 0.047553 0.013820
+-0.010231 0.044460 0.020462
+-0.013143 0.040451 0.026287
+-0.011331 0.048193 0.007003
+-0.014832 0.045649 0.014006
+-0.017856 0.041982 0.020462
+-0.020250 0.037617 0.025979
+-0.015451 0.047553 -0.000000
+-0.019160 0.045649 0.007003
+-0.022361 0.042532 0.013820
+-0.024851 0.038526 0.019955
+-0.026597 0.034086 0.025115
+-0.018984 0.045775 -0.006652
+-0.022876 0.044460 -0.000000
+-0.026287 0.041982 0.006821
+-0.028961 0.038526 0.013303
+-0.030815 0.034463 0.019045
+-0.031923 0.030202 0.023849
+0.045187 0.009769 0.019045
+0.046075 0.014145 0.013303
+0.045943 0.018513 0.006820
+0.044640 0.022522 -0.000000
+0.042265 0.025874 -0.006652
+0.039125 0.028426 -0.012697
+0.041553 0.011943 0.025115
+0.042750 0.016561 0.019955
+0.038494 0.018530 0.025979
+0.043090 0.021266 0.013820
+0.039122 0.023469 0.020461
+0.034409 0.025000 0.026287
+0.042333 0.025669 0.007003
+0.038832 0.028213 0.014006
+0.034410 0.029955 0.020461
+0.029518 0.030883 0.025979
+0.040451 0.029389 -0.000000
+0.037494 0.032329 0.007003
+0.033541 0.034409 0.013820
+0.028961 0.035539 0.019955
+0.024199 0.035828 0.025115
+0.037668 0.032200 -0.006652
+0.035215 0.035495 0.000000
+0.031804 0.037973 0.006821
+0.027691 0.039449 0.013303
+0.023254 0.039957 0.019045
+0.018859 0.039693 0.023849
+0.001842 -0.005670 0.049643
+0.005962 0.000000 0.049643
+0.000000 0.000000 0.050000
+0.003880 -0.011943 0.048397
+0.008310 -0.006037 0.048934
+0.006021 -0.018530 0.046048
+0.010762 -0.012652 0.047160
+0.008123 -0.025000 0.042533
+0.013143 -0.019460 0.044143
+0.010034 -0.030883 0.038020
+0.015251 -0.025946 0.039928
+0.011641 -0.035828 0.032876
+0.016928 -0.031630 0.034828
+0.018118 -0.036225 0.029317
+0.012557 0.000000 0.048397
+0.015358 -0.006326 0.047160
+0.019484 0.000000 0.046048
+0.018090 -0.013143 0.044721
+0.022569 -0.006487 0.044143
+0.026286 0.000000 0.042533
+0.020498 -0.019980 0.040996
+0.025336 -0.013320 0.040996
+0.029389 -0.006487 0.039927
+0.032473 0.000000 0.038020
+0.022360 -0.026286 0.036181
+0.027500 -0.019980 0.036668
+0.031910 -0.013143 0.036181
+0.035313 -0.006326 0.034828
+0.037672 0.000000 0.032876
+0.023580 -0.031630 0.030717
+0.028892 -0.025946 0.031497
+0.033604 -0.019460 0.031497
+0.037368 -0.012652 0.030717
+0.040051 -0.006038 0.029317
+-0.004824 -0.003504 0.049643
+-0.010159 -0.007381 0.048397
+-0.003174 -0.009769 0.048934
+-0.015763 -0.011452 0.046048
+-0.008707 -0.014145 0.047160
+-0.021266 -0.015451 0.042533
+-0.014446 -0.018513 0.044143
+-0.026271 -0.019087 0.038020
+-0.019964 -0.022522 0.039928
+-0.030477 -0.022143 0.032876
+-0.024850 -0.025874 0.034828
+-0.028853 -0.028426 0.029317
+-0.001270 -0.016561 0.047160
+-0.006910 -0.021266 0.044721
+0.000805 -0.023468 0.044143
+-0.012668 -0.025668 0.040996
+-0.004839 -0.028212 0.040996
+0.002912 -0.029955 0.039928
+-0.018090 -0.029389 0.036181
+-0.010504 -0.032329 0.036668
+-0.002639 -0.034409 0.036181
+0.004896 -0.035539 0.034828
+-0.022795 -0.032200 0.030717
+-0.015749 -0.035495 0.031497
+-0.008123 -0.037973 0.031497
+-0.000485 -0.039449 0.030717
+0.006634 -0.039956 0.029317
+-0.004824 0.003504 0.049643
+-0.010159 0.007381 0.048397
+-0.010272 0.000000 0.048934
+-0.015763 0.011452 0.046048
+-0.016143 0.003910 0.047160
+-0.021266 0.015451 0.042533
+-0.022071 0.008018 0.044143
+-0.026271 0.019087 0.038020
+-0.027589 0.012027 0.039928
+-0.030477 0.022143 0.032876
+-0.032287 0.015638 0.034828
+-0.035951 0.018657 0.029317
+-0.016143 -0.003910 0.047160
+-0.022360 0.000000 0.044721
+-0.022071 -0.008018 0.044143
+-0.028327 0.004116 0.040996
+-0.028327 -0.004116 0.040996
+-0.027589 -0.012027 0.039928
+-0.033541 0.008123 0.036181
+-0.033992 0.000000 0.036668
+-0.033541 -0.008123 0.036181
+-0.032287 -0.015638 0.034828
+-0.037668 0.011729 0.030717
+-0.038625 0.004009 0.031497
+-0.038625 -0.004009 0.031497
+-0.037668 -0.011729 0.030717
+-0.035951 -0.018657 0.029317
+0.001842 0.005670 0.049643
+0.003880 0.011943 0.048397
+-0.003174 0.009769 0.048934
+0.006021 0.018530 0.046048
+-0.001270 0.016561 0.047160
+0.008123 0.025000 0.042533
+0.000805 0.023468 0.044143
+0.010034 0.030883 0.038020
+0.002912 0.029955 0.039928
+0.011641 0.035828 0.032876
+0.004896 0.035539 0.034828
+0.006634 0.039956 0.029317
+-0.008707 0.014145 0.047160
+-0.006910 0.021266 0.044721
+-0.014446 0.018513 0.044143
+-0.004839 0.028212 0.040996
+-0.012668 0.025668 0.040996
+-0.019964 0.022522 0.039928
+-0.002639 0.034409 0.036181
+-0.010504 0.032329 0.036668
+-0.018090 0.029389 0.036181
+-0.024850 0.025874 0.034828
+-0.000485 0.039449 0.030717
+-0.008123 0.037973 0.031497
+-0.015749 0.035495 0.031497
+-0.022795 0.032200 0.030717
+-0.028853 0.028426 0.029317
+0.008310 0.006037 0.048934
+0.015358 0.006326 0.047160
+0.022569 0.006487 0.044143
+0.029389 0.006487 0.039927
+0.035313 0.006326 0.034828
+0.040051 0.006038 0.029317
+0.010762 0.012652 0.047160
+0.018090 0.013143 0.044721
+0.013143 0.019460 0.044143
+0.025336 0.013320 0.040996
+0.020498 0.019980 0.040996
+0.015251 0.025946 0.039928
+0.031910 0.013143 0.036181
+0.027500 0.019980 0.036668
+0.022360 0.026286 0.036181
+0.016928 0.031630 0.034828
+0.037368 0.012652 0.030717
+0.033604 0.019460 0.031497
+0.028892 0.025946 0.031497
+0.023580 0.031630 0.030717
+0.018118 0.036225 0.029317
+3 0 1 2
+3 3 4 5
+3 0 2 6
+3 0 6 7
+3 0 7 8
+3 3 5 9
+3 10 11 12
+3 13 14 15
+3 16 17 18
+3 19 20 21
+3 3 9 22
+3 10 12 23
+3 13 15 24
+3 16 18 25
+3 19 21 26
+3 27 28 29
+3 30 31 32
+3 33 34 35
+3 36 37 38
+3 39 40 41
+3 42 11 10
+3 43 44 42
+3 45 46 43
+3 47 48 45
+3 49 50 47
+3 51 52 49
+3 2 53 51
+3 42 44 11
+3 44 54 11
+3 43 46 44
+3 46 55 44
+3 44 55 54
+3 55 56 54
+3 45 48 46
+3 48 57 46
+3 46 57 55
+3 57 58 55
+3 55 58 56
+3 58 59 56
+3 47 50 48
+3 50 60 48
+3 48 60 57
+3 60 61 57
+3 57 61 58
+3 61 62 58
+3 58 62 59
+3 62 63 59
+3 49 52 50
+3 52 64 50
+3 50 64 60
+3 64 65 60
+3 60 65 61
+3 65 66 61
+3 61 66 62
+3 66 67 62
+3 62 67 63
+3 67 68 63
+3 51 53 52
+3 53 69 52
+3 52 69 64
+3 69 70 64
+3 64 70 65
+3 70 71 65
+3 65 71 66
+3 71 72 66
+3 66 72 67
+3 72 73 67
+3 67 73 68
+3 73 74 68
+3 2 1 53
+3 1 75 53
+3 53 75 69
+3 75 76 69
+3 69 76 70
+3 76 77 70
+3 70 77 71
+3 77 78 71
+3 71 78 72
+3 78 79 72
+3 72 79 73
+3 79 4 73
+3 73 4 74
+3 4 3 74
+3 80 81 19
+3 82 83 80
+3 84 85 82
+3 86 87 84
+3 88 89 86
+3 90 91 88
+3 5 92 90
+3 80 83 81
+3 83 93 81
+3 82 85 83
+3 85 94 83
+3 83 94 93
+3 94 95 93
+3 84 87 85
+3 87 96 85
+3 85 96 94
+3 96 97 94
+3 94 97 95
+3 97 98 95
+3 86 89 87
+3 89 99 87
+3 87 99 96
+3 99 100 96
+3 96 100 97
+3 100 101 97
+3 97 101 98
+3 101 102 98
+3 88 91 89
+3 91 103 89
+3 89 103 99
+3 103 104 99
+3 99 104 100
+3 104 105 100
+3 100 105 101
+3 105 106 101
+3 101 106 102
+3 106 107 102
+3 90 92 91
+3 92 108 91
+3 91 108 103
+3 108 109 103
+3 103 109 104
+3 109 110 104
+3 104 110 105
+3 110 111 105
+3 105 111 106
+3 111 112 106
+3 106 112 107
+3 112 8 107
+3 5 4 92
+3 4 79 92
+3 92 79 108
+3 79 78 108
+3 108 78 109
+3 78 77 109
+3 109 77 110
+3 77 76 110
+3 110 76 111
+3 76 75 111
+3 111 75 112
+3 75 1 112
+3 112 1 8
+3 1 0 8
+3 113 14 13
+3 114 115 113
+3 116 117 114
+3 118 119 116
+3 120 121 118
+3 122 123 120
+3 6 124 122
+3 113 115 14
+3 115 125 14
+3 114 117 115
+3 117 126 115
+3 115 126 125
+3 126 127 125
+3 116 119 117
+3 119 128 117
+3 117 128 126
+3 128 129 126
+3 126 129 127
+3 129 130 127
+3 118 121 119
+3 121 131 119
+3 119 131 128
+3 131 132 128
+3 128 132 129
+3 132 133 129
+3 129 133 130
+3 133 134 130
+3 120 123 121
+3 123 135 121
+3 121 135 131
+3 135 136 131
+3 131 136 132
+3 136 137 132
+3 132 137 133
+3 137 138 133
+3 133 138 134
+3 138 139 134
+3 122 124 123
+3 124 140 123
+3 123 140 135
+3 140 141 135
+3 135 141 136
+3 141 142 136
+3 136 142 137
+3 142 143 137
+3 137 143 138
+3 143 144 138
+3 138 144 139
+3 144 145 139
+3 6 2 124
+3 2 51 124
+3 124 51 140
+3 51 49 140
+3 140 49 141
+3 49 47 141
+3 141 47 142
+3 47 45 142
+3 142 45 143
+3 45 43 143
+3 143 43 144
+3 43 42 144
+3 144 42 145
+3 42 10 145
+3 146 17 16
+3 147 148 146
+3 149 150 147
+3 151 152 149
+3 153 154 151
+3 155 156 153
+3 7 157 155
+3 146 148 17
+3 148 158 17
+3 147 150 148
+3 150 159 148
+3 148 159 158
+3 159 160 158
+3 149 152 150
+3 152 161 150
+3 150 161 159
+3 161 162 159
+3 159 162 160
+3 162 163 160
+3 151 154 152
+3 154 164 152
+3 152 164 161
+3 164 165 161
+3 161 165 162
+3 165 166 162
+3 162 166 163
+3 166 167 163
+3 153 156 154
+3 156 168 154
+3 154 168 164
+3 168 169 164
+3 164 169 165
+3 169 170 165
+3 165 170 166
+3 170 171 166
+3 166 171 167
+3 171 172 167
+3 155 157 156
+3 157 173 156
+3 156 173 168
+3 173 174 168
+3 168 174 169
+3 174 175 169
+3 169 175 170
+3 175 176 170
+3 170 176 171
+3 176 177 171
+3 171 177 172
+3 177 178 172
+3 7 6 157
+3 6 122 157
+3 157 122 173
+3 122 120 173
+3 173 120 174
+3 120 118 174
+3 174 118 175
+3 118 116 175
+3 175 116 176
+3 116 114 176
+3 176 114 177
+3 114 113 177
+3 177 113 178
+3 113 13 178
+3 81 20 19
+3 93 179 81
+3 95 180 93
+3 98 181 95
+3 102 182 98
+3 107 183 102
+3 8 184 107
+3 81 179 20
+3 179 185 20
+3 93 180 179
+3 180 186 179
+3 179 186 185
+3 186 187 185
+3 95 181 180
+3 181 188 180
+3 180 188 186
+3 188 189 186
+3 186 189 187
+3 189 190 187
+3 98 182 181
+3 182 191 181
+3 181 191 188
+3 191 192 188
+3 188 192 189
+3 192 193 189
+3 189 193 190
+3 193 194 190
+3 102 183 182
+3 183 195 182
+3 182 195 191
+3 195 196 191
+3 191 196 192
+3 196 197 192
+3 192 197 193
+3 197 198 193
+3 193 198 194
+3 198 199 194
+3 107 184 183
+3 184 200 183
+3 183 200 195
+3 200 201 195
+3 195 201 196
+3 201 202 196
+3 196 202 197
+3 202 203 197
+3 197 203 198
+3 203 204 198
+3 198 204 199
+3 204 205 199
+3 8 7 184
+3 7 155 184
+3 184 155 200
+3 155 153 200
+3 200 153 201
+3 153 151 201
+3 201 151 202
+3 151 149 202
+3 202 149 203
+3 149 147 203
+3 203 147 204
+3 147 146 204
+3 204 146 205
+3 146 16 205
+3 206 207 39
+3 208 209 206
+3 210 211 208
+3 212 213 210
+3 214 215 212
+3 216 217 214
+3 9 218 216
+3 206 209 207
+3 209 219 207
+3 208 211 209
+3 211 220 209
+3 209 220 219
+3 220 221 219
+3 210 213 211
+3 213 222 211
+3 211 222 220
+3 222 223 220
+3 220 223 221
+3 223 224 221
+3 212 215 213
+3 215 225 213
+3 213 225 222
+3 225 226 222
+3 222 226 223
+3 226 227 223
+3 223 227 224
+3 227 228 224
+3 214 217 215
+3 217 229 215
+3 215 229 225
+3 229 230 225
+3 225 230 226
+3 230 231 226
+3 226 231 227
+3 231 232 227
+3 227 232 228
+3 232 233 228
+3 216 218 217
+3 218 234 217
+3 217 234 229
+3 234 235 229
+3 229 235 230
+3 235 236 230
+3 230 236 231
+3 236 237 231
+3 231 237 232
+3 237 238 232
+3 232 238 233
+3 238 26 233
+3 9 5 218
+3 5 90 218
+3 218 90 234
+3 90 88 234
+3 234 88 235
+3 88 86 235
+3 235 86 236
+3 86 84 236
+3 236 84 237
+3 84 82 237
+3 237 82 238
+3 82 80 238
+3 238 80 26
+3 80 19 26
+3 239 240 27
+3 241 242 239
+3 243 244 241
+3 245 246 243
+3 247 248 245
+3 249 250 247
+3 12 251 249
+3 239 242 240
+3 242 252 240
+3 241 244 242
+3 244 253 242
+3 242 253 252
+3 253 254 252
+3 243 246 244
+3 246 255 244
+3 244 255 253
+3 255 256 253
+3 253 256 254
+3 256 257 254
+3 245 248 246
+3 248 258 246
+3 246 258 255
+3 258 259 255
+3 255 259 256
+3 259 260 256
+3 256 260 257
+3 260 261 257
+3 247 250 248
+3 250 262 248
+3 248 262 258
+3 262 263 258
+3 258 263 259
+3 263 264 259
+3 259 264 260
+3 264 265 260
+3 260 265 261
+3 265 266 261
+3 249 251 250
+3 251 267 250
+3 250 267 262
+3 267 268 262
+3 262 268 263
+3 268 269 263
+3 263 269 264
+3 269 270 264
+3 264 270 265
+3 270 271 265
+3 265 271 266
+3 271 22 266
+3 12 11 251
+3 11 54 251
+3 251 54 267
+3 54 56 267
+3 267 56 268
+3 56 59 268
+3 268 59 269
+3 59 63 269
+3 269 63 270
+3 63 68 270
+3 270 68 271
+3 68 74 271
+3 271 74 22
+3 74 3 22
+3 272 273 30
+3 274 275 272
+3 276 277 274
+3 278 279 276
+3 280 281 278
+3 282 283 280
+3 15 284 282
+3 272 275 273
+3 275 285 273
+3 274 277 275
+3 277 286 275
+3 275 286 285
+3 286 287 285
+3 276 279 277
+3 279 288 277
+3 277 288 286
+3 288 289 286
+3 286 289 287
+3 289 290 287
+3 278 281 279
+3 281 291 279
+3 279 291 288
+3 291 292 288
+3 288 292 289
+3 292 293 289
+3 289 293 290
+3 293 294 290
+3 280 283 281
+3 283 295 281
+3 281 295 291
+3 295 296 291
+3 291 296 292
+3 296 297 292
+3 292 297 293
+3 297 298 293
+3 293 298 294
+3 298 299 294
+3 282 284 283
+3 284 300 283
+3 283 300 295
+3 300 301 295
+3 295 301 296
+3 301 302 296
+3 296 302 297
+3 302 303 297
+3 297 303 298
+3 303 304 298
+3 298 304 299
+3 304 23 299
+3 15 14 284
+3 14 125 284
+3 284 125 300
+3 125 127 300
+3 300 127 301
+3 127 130 301
+3 301 130 302
+3 130 134 302
+3 302 134 303
+3 134 139 303
+3 303 139 304
+3 139 145 304
+3 304 145 23
+3 145 10 23
+3 305 306 33
+3 307 308 305
+3 309 310 307
+3 311 312 309
+3 313 314 311
+3 315 316 313
+3 18 317 315
+3 305 308 306
+3 308 318 306
+3 307 310 308
+3 310 319 308
+3 308 319 318
+3 319 320 318
+3 309 312 310
+3 312 321 310
+3 310 321 319
+3 321 322 319
+3 319 322 320
+3 322 323 320
+3 311 314 312
+3 314 324 312
+3 312 324 321
+3 324 325 321
+3 321 325 322
+3 325 326 322
+3 322 326 323
+3 326 327 323
+3 313 316 314
+3 316 328 314
+3 314 328 324
+3 328 329 324
+3 324 329 325
+3 329 330 325
+3 325 330 326
+3 330 331 326
+3 326 331 327
+3 331 332 327
+3 315 317 316
+3 317 333 316
+3 316 333 328
+3 333 334 328
+3 328 334 329
+3 334 335 329
+3 329 335 330
+3 335 336 330
+3 330 336 331
+3 336 337 331
+3 331 337 332
+3 337 24 332
+3 18 17 317
+3 17 158 317
+3 317 158 333
+3 158 160 333
+3 333 160 334
+3 160 163 334
+3 334 163 335
+3 163 167 335
+3 335 167 336
+3 167 172 336
+3 336 172 337
+3 172 178 337
+3 337 178 24
+3 178 13 24
+3 338 339 36
+3 340 341 338
+3 342 343 340
+3 344 345 342
+3 346 347 344
+3 348 349 346
+3 21 350 348
+3 338 341 339
+3 341 351 339
+3 340 343 341
+3 343 352 341
+3 341 352 351
+3 352 353 351
+3 342 345 343
+3 345 354 343
+3 343 354 352
+3 354 355 352
+3 352 355 353
+3 355 356 353
+3 344 347 345
+3 347 357 345
+3 345 357 354
+3 357 358 354
+3 354 358 355
+3 358 359 355
+3 355 359 356
+3 359 360 356
+3 346 349 347
+3 349 361 347
+3 347 361 357
+3 361 362 357
+3 357 362 358
+3 362 363 358
+3 358 363 359
+3 363 364 359
+3 359 364 360
+3 364 365 360
+3 348 350 349
+3 350 366 349
+3 349 366 361
+3 366 367 361
+3 361 367 362
+3 367 368 362
+3 362 368 363
+3 368 369 363
+3 363 369 364
+3 369 370 364
+3 364 370 365
+3 370 25 365
+3 21 20 350
+3 20 185 350
+3 350 185 366
+3 185 187 366
+3 366 187 367
+3 187 190 367
+3 367 190 368
+3 190 194 368
+3 368 194 369
+3 194 199 369
+3 369 199 370
+3 199 205 370
+3 370 205 25
+3 205 16 25
+3 240 28 27
+3 252 371 240
+3 254 372 252
+3 257 373 254
+3 261 374 257
+3 266 375 261
+3 22 376 266
+3 240 371 28
+3 371 377 28
+3 252 372 371
+3 372 378 371
+3 371 378 377
+3 378 379 377
+3 254 373 372
+3 373 380 372
+3 372 380 378
+3 380 381 378
+3 378 381 379
+3 381 382 379
+3 257 374 373
+3 374 383 373
+3 373 383 380
+3 383 384 380
+3 380 384 381
+3 384 385 381
+3 381 385 382
+3 385 386 382
+3 261 375 374
+3 375 387 374
+3 374 387 383
+3 387 388 383
+3 383 388 384
+3 388 389 384
+3 384 389 385
+3 389 390 385
+3 385 390 386
+3 390 391 386
+3 266 376 375
+3 376 392 375
+3 375 392 387
+3 392 393 387
+3 387 393 388
+3 393 394 388
+3 388 394 389
+3 394 395 389
+3 389 395 390
+3 395 396 390
+3 390 396 391
+3 396 397 391
+3 22 9 376
+3 9 216 376
+3 376 216 392
+3 216 214 392
+3 392 214 393
+3 214 212 393
+3 393 212 394
+3 212 210 394
+3 394 210 395
+3 210 208 395
+3 395 208 396
+3 208 206 396
+3 396 206 397
+3 206 39 397
+3 273 31 30
+3 285 398 273
+3 287 399 285
+3 290 400 287
+3 294 401 290
+3 299 402 294
+3 23 403 299
+3 273 398 31
+3 398 404 31
+3 285 399 398
+3 399 405 398
+3 398 405 404
+3 405 406 404
+3 287 400 399
+3 400 407 399
+3 399 407 405
+3 407 408 405
+3 405 408 406
+3 408 409 406
+3 290 401 400
+3 401 410 400
+3 400 410 407
+3 410 411 407
+3 407 411 408
+3 411 412 408
+3 408 412 409
+3 412 413 409
+3 294 402 401
+3 402 414 401
+3 401 414 410
+3 414 415 410
+3 410 415 411
+3 415 416 411
+3 411 416 412
+3 416 417 412
+3 412 417 413
+3 417 418 413
+3 299 403 402
+3 403 419 402
+3 402 419 414
+3 419 420 414
+3 414 420 415
+3 420 421 415
+3 415 421 416
+3 421 422 416
+3 416 422 417
+3 422 423 417
+3 417 423 418
+3 423 424 418
+3 23 12 403
+3 12 249 403
+3 403 249 419
+3 249 247 419
+3 419 247 420
+3 247 245 420
+3 420 245 421
+3 245 243 421
+3 421 243 422
+3 243 241 422
+3 422 241 423
+3 241 239 423
+3 423 239 424
+3 239 27 424
+3 306 34 33
+3 318 425 306
+3 320 426 318
+3 323 427 320
+3 327 428 323
+3 332 429 327
+3 24 430 332
+3 306 425 34
+3 425 431 34
+3 318 426 425
+3 426 432 425
+3 425 432 431
+3 432 433 431
+3 320 427 426
+3 427 434 426
+3 426 434 432
+3 434 435 432
+3 432 435 433
+3 435 436 433
+3 323 428 427
+3 428 437 427
+3 427 437 434
+3 437 438 434
+3 434 438 435
+3 438 439 435
+3 435 439 436
+3 439 440 436
+3 327 429 428
+3 429 441 428
+3 428 441 437
+3 441 442 437
+3 437 442 438
+3 442 443 438
+3 438 443 439
+3 443 444 439
+3 439 444 440
+3 444 445 440
+3 332 430 429
+3 430 446 429
+3 429 446 441
+3 446 447 441
+3 441 447 442
+3 447 448 442
+3 442 448 443
+3 448 449 443
+3 443 449 444
+3 449 450 444
+3 444 450 445
+3 450 451 445
+3 24 15 430
+3 15 282 430
+3 430 282 446
+3 282 280 446
+3 446 280 447
+3 280 278 447
+3 447 278 448
+3 278 276 448
+3 448 276 449
+3 276 274 449
+3 449 274 450
+3 274 272 450
+3 450 272 451
+3 272 30 451
+3 339 37 36
+3 351 452 339
+3 353 453 351
+3 356 454 353
+3 360 455 356
+3 365 456 360
+3 25 457 365
+3 339 452 37
+3 452 458 37
+3 351 453 452
+3 453 459 452
+3 452 459 458
+3 459 460 458
+3 353 454 453
+3 454 461 453
+3 453 461 459
+3 461 462 459
+3 459 462 460
+3 462 463 460
+3 356 455 454
+3 455 464 454
+3 454 464 461
+3 464 465 461
+3 461 465 462
+3 465 466 462
+3 462 466 463
+3 466 467 463
+3 360 456 455
+3 456 468 455
+3 455 468 464
+3 468 469 464
+3 464 469 465
+3 469 470 465
+3 465 470 466
+3 470 471 466
+3 466 471 467
+3 471 472 467
+3 365 457 456
+3 457 473 456
+3 456 473 468
+3 473 474 468
+3 468 474 469
+3 474 475 469
+3 469 475 470
+3 475 476 470
+3 470 476 471
+3 476 477 471
+3 471 477 472
+3 477 478 472
+3 25 18 457
+3 18 315 457
+3 457 315 473
+3 315 313 473
+3 473 313 474
+3 313 311 474
+3 474 311 475
+3 311 309 475
+3 475 309 476
+3 309 307 476
+3 476 307 477
+3 307 305 477
+3 477 305 478
+3 305 33 478
+3 207 40 39
+3 219 479 207
+3 221 480 219
+3 224 481 221
+3 228 482 224
+3 233 483 228
+3 26 484 233
+3 207 479 40
+3 479 485 40
+3 219 480 479
+3 480 486 479
+3 479 486 485
+3 486 487 485
+3 221 481 480
+3 481 488 480
+3 480 488 486
+3 488 489 486
+3 486 489 487
+3 489 490 487
+3 224 482 481
+3 482 491 481
+3 481 491 488
+3 491 492 488
+3 488 492 489
+3 492 493 489
+3 489 493 490
+3 493 494 490
+3 228 483 482
+3 483 495 482
+3 482 495 491
+3 495 496 491
+3 491 496 492
+3 496 497 492
+3 492 497 493
+3 497 498 493
+3 493 498 494
+3 498 499 494
+3 233 484 483
+3 484 500 483
+3 483 500 495
+3 500 501 495
+3 495 501 496
+3 501 502 496
+3 496 502 497
+3 502 503 497
+3 497 503 498
+3 503 504 498
+3 498 504 499
+3 504 505 499
+3 26 21 484
+3 21 348 484
+3 484 348 500
+3 348 346 500
+3 500 346 501
+3 346 344 501
+3 501 344 502
+3 344 342 502
+3 502 342 503
+3 342 340 503
+3 503 340 504
+3 340 338 504
+3 504 338 505
+3 338 36 505
+3 506 507 508
+3 509 510 506
+3 511 512 509
+3 513 514 511
+3 515 516 513
+3 517 518 515
+3 29 519 517
+3 506 510 507
+3 510 520 507
+3 509 512 510
+3 512 521 510
+3 510 521 520
+3 521 522 520
+3 511 514 512
+3 514 523 512
+3 512 523 521
+3 523 524 521
+3 521 524 522
+3 524 525 522
+3 513 516 514
+3 516 526 514
+3 514 526 523
+3 526 527 523
+3 523 527 524
+3 527 528 524
+3 524 528 525
+3 528 529 525
+3 515 518 516
+3 518 530 516
+3 516 530 526
+3 530 531 526
+3 526 531 527
+3 531 532 527
+3 527 532 528
+3 532 533 528
+3 528 533 529
+3 533 534 529
+3 517 519 518
+3 519 535 518
+3 518 535 530
+3 535 536 530
+3 530 536 531
+3 536 537 531
+3 531 537 532
+3 537 538 532
+3 532 538 533
+3 538 539 533
+3 533 539 534
+3 539 41 534
+3 29 28 519
+3 28 377 519
+3 519 377 535
+3 377 379 535
+3 535 379 536
+3 379 382 536
+3 536 382 537
+3 382 386 537
+3 537 386 538
+3 386 391 538
+3 538 391 539
+3 391 397 539
+3 539 397 41
+3 397 39 41
+3 540 506 508
+3 541 542 540
+3 543 544 541
+3 545 546 543
+3 547 548 545
+3 549 550 547
+3 32 551 549
+3 540 542 506
+3 542 509 506
+3 541 544 542
+3 544 552 542
+3 542 552 509
+3 552 511 509
+3 543 546 544
+3 546 553 544
+3 544 553 552
+3 553 554 552
+3 552 554 511
+3 554 513 511
+3 545 548 546
+3 548 555 546
+3 546 555 553
+3 555 556 553
+3 553 556 554
+3 556 557 554
+3 554 557 513
+3 557 515 513
+3 547 550 548
+3 550 558 548
+3 548 558 555
+3 558 559 555
+3 555 559 556
+3 559 560 556
+3 556 560 557
+3 560 561 557
+3 557 561 515
+3 561 517 515
+3 549 551 550
+3 551 562 550
+3 550 562 558
+3 562 563 558
+3 558 563 559
+3 563 564 559
+3 559 564 560
+3 564 565 560
+3 560 565 561
+3 565 566 561
+3 561 566 517
+3 566 29 517
+3 32 31 551
+3 31 404 551
+3 551 404 562
+3 404 406 562
+3 562 406 563
+3 406 409 563
+3 563 409 564
+3 409 413 564
+3 564 413 565
+3 413 418 565
+3 565 418 566
+3 418 424 566
+3 566 424 29
+3 424 27 29
+3 567 540 508
+3 568 569 567
+3 570 571 568
+3 572 573 570
+3 574 575 572
+3 576 577 574
+3 35 578 576
+3 567 569 540
+3 569 541 540
+3 568 571 569
+3 571 579 569
+3 569 579 541
+3 579 543 541
+3 570 573 571
+3 573 580 571
+3 571 580 579
+3 580 581 579
+3 579 581 543
+3 581 545 543
+3 572 575 573
+3 575 582 573
+3 573 582 580
+3 582 583 580
+3 580 583 581
+3 583 584 581
+3 581 584 545
+3 584 547 545
+3 574 577 575
+3 577 585 575
+3 575 585 582
+3 585 586 582
+3 582 586 583
+3 586 587 583
+3 583 587 584
+3 587 588 584
+3 584 588 547
+3 588 549 547
+3 576 578 577
+3 578 589 577
+3 577 589 585
+3 589 590 585
+3 585 590 586
+3 590 591 586
+3 586 591 587
+3 591 592 587
+3 587 592 588
+3 592 593 588
+3 588 593 549
+3 593 32 549
+3 35 34 578
+3 34 431 578
+3 578 431 589
+3 431 433 589
+3 589 433 590
+3 433 436 590
+3 590 436 591
+3 436 440 591
+3 591 440 592
+3 440 445 592
+3 592 445 593
+3 445 451 593
+3 593 451 32
+3 451 30 32
+3 594 567 508
+3 595 596 594
+3 597 598 595
+3 599 600 597
+3 601 602 599
+3 603 604 601
+3 38 605 603
+3 594 596 567
+3 596 568 567
+3 595 598 596
+3 598 606 596
+3 596 606 568
+3 606 570 568
+3 597 600 598
+3 600 607 598
+3 598 607 606
+3 607 608 606
+3 606 608 570
+3 608 572 570
+3 599 602 600
+3 602 609 600
+3 600 609 607
+3 609 610 607
+3 607 610 608
+3 610 611 608
+3 608 611 572
+3 611 574 572
+3 601 604 602
+3 604 612 602
+3 602 612 609
+3 612 613 609
+3 609 613 610
+3 613 614 610
+3 610 614 611
+3 614 615 611
+3 611 615 574
+3 615 576 574
+3 603 605 604
+3 605 616 604
+3 604 616 612
+3 616 617 612
+3 612 617 613
+3 617 618 613
+3 613 618 614
+3 618 619 614
+3 614 619 615
+3 619 620 615
+3 615 620 576
+3 620 35 576
+3 38 37 605
+3 37 458 605
+3 605 458 616
+3 458 460 616
+3 616 460 617
+3 460 463 617
+3 617 463 618
+3 463 467 618
+3 618 467 619
+3 467 472 619
+3 619 472 620
+3 472 478 620
+3 620 478 35
+3 478 33 35
+3 507 594 508
+3 520 621 507
+3 522 622 520
+3 525 623 522
+3 529 624 525
+3 534 625 529
+3 41 626 534
+3 507 621 594
+3 621 595 594
+3 520 622 621
+3 622 627 621
+3 621 627 595
+3 627 597 595
+3 522 623 622
+3 623 628 622
+3 622 628 627
+3 628 629 627
+3 627 629 597
+3 629 599 597
+3 525 624 623
+3 624 630 623
+3 623 630 628
+3 630 631 628
+3 628 631 629
+3 631 632 629
+3 629 632 599
+3 632 601 599
+3 529 625 624
+3 625 633 624
+3 624 633 630
+3 633 634 630
+3 630 634 631
+3 634 635 631
+3 631 635 632
+3 635 636 632
+3 632 636 601
+3 636 603 601
+3 534 626 625
+3 626 637 625
+3 625 637 633
+3 637 638 633
+3 633 638 634
+3 638 639 634
+3 634 639 635
+3 639 640 635
+3 635 640 636
+3 640 641 636
+3 636 641 603
+3 641 38 603
+3 41 40 626
+3 40 485 626
+3 626 485 637
+3 485 487 637
+3 637 487 638
+3 487 490 638
+3 638 490 639
+3 490 494 639
+3 639 494 640
+3 494 499 640
+3 640 499 641
+3 499 505 641
+3 641 505 38
+3 505 36 38
diff --git a/SurgSim/Physics/RenderTests/Data/uniaxial_positions.csv b/SurgSim/Physics/RenderTests/Data/uniaxial_positions.csv
new file mode 100644
index 0000000..a529c9f
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Data/uniaxial_positions.csv
@@ -0,0 +1,350 @@
+Truth Cube,,,,,,,,,,,,,,
+Uniaxial Compression Test displacment field data for all 4 strains,,,,,,,,,,,,,,
+All positions in mm,,,,,,,,,,,,,,
+12-Oct-01,,,,,,,,,,,,,,
+,,,,,,,,,,,,,,
+bead nominal location,,,no strain,,,5% strain,,,12.5% strain,,,18.25% strain,,
+i,j,k,x0,y0,z0,x1,y1,z1,x2,y2,z2,x3,y3,z3
+1,1,1,-29.69,-28.86,12.62,-30.33,-29.38,12.96,-31.04,-30.09,13.65,-31.85,-31.08,14.38
+1,1,2,-29.07,-27.43,3.33,-29.87,-28.12,4.27,-31.03,-29.37,5.9,-32.29,-30.62,7.05
+1,1,3,-28.76,-26.88,-6.49,-29.66,-27.61,-5,-31.17,-29.37,-2.79,-32.62,-30.62,-0.6
+1,1,4,-28.91,-27.44,-16.83,-29.87,-28.12,-14.66,-31.36,-29.37,-11.69,-32.84,-30.62,-8.72
+1,1,5,-28.95,-26.88,-26.91,-30.06,-26.88,-23.94,-31.41,-28.8,-20.07,-32.89,-30.16,-16.55
+1,1,6,-28.76,-26.88,-37.29,-29.87,-26.88,-33.58,-30.99,-28.12,-28.76,-32.47,-29.83,-24.68
+1,1,7,-28.76,-26.88,-47.31,-29.91,-26.88,-42.71,-30.62,-28.12,-37.29,-31.7,-29.37,-32.44
+1,2,1,-30.16,-18.75,12.43,-30.8,-18.95,12.75,-31.64,-19.37,13.36,-32.56,-19.94,14.04
+1,2,2,-29.18,-17.45,3.31,-30.1,-17.76,4.21,-31.4,-18.53,5.44,-32.8,-19.37,6.71
+1,2,3,-28.95,-17.5,-6.68,-29.91,-18.12,-5.04,-31.54,-18.75,-2.97,-33.15,-19.83,-0.96
+1,2,4,-29.01,-17.4,-16.88,-30.09,-17.49,-14.69,-31.73,-18.12,-11.69,-33.4,-19.37,-8.91
+1,2,5,-29.14,-16.88,-26.77,-30.24,-16.88,-23.94,-31.7,-18.12,-20.19,-33.4,-19.37,-16.52
+1,2,6,-28.72,-17.45,-37.14,-29.87,-16.88,-33.58,-30.99,-18.12,-28.76,-32.53,-18.61,-24.6
+1,2,7,-28.72,-17.33,-47.28,-29.87,-16.88,-42.86,-30.44,-17.37,-37.3,-31.73,-18.12,-32.47
+1,3,1,-30.76,-8.13,12.29,-31.53,-8.44,12.73,-32.33,-8.46,13.32,-33.35,-8.75,13.79
+1,3,2,-29.22,-6.88,3.07,-30.24,-6.88,3.9,-31.72,-7.39,5.22,-33.17,-7.68,6.53
+1,3,3,-29.13,-6.88,-6.87,-30.24,-6.88,-5.38,-31.83,-7.43,-3.2,-33.58,-8.13,-1.3
+1,3,4,-29.79,-7.58,-17.02,-31.02,-6.88,-14.69,-32.51,-7.67,-11.73,-34.48,-8.13,-9.05
+1,3,5,-28.95,-6.88,-26.91,-29.91,-6.88,-23.97,-31.54,-6.88,-20.22,-33.22,-7.5,-16.7
+1,3,6,-28.76,-6.88,-37.29,-29.97,-6.32,-33.7,-30.99,-6.88,-28.95,-32.69,-6.88,-24.72
+1,3,7,-28.93,-7.49,-47.15,-29.87,-6.88,-42.86,-30.62,-6.88,-37.29,-31.73,-6.88,-32.47
+1,4,1,-30.05,1.33,12.27,-30.75,1.54,12.57,-31.66,1.32,13.16,-32.61,1.31,13.73
+1,4,2,-29.5,3.13,3.15,-30.62,3.13,3.9,-31.91,3.13,5.2,-33.39,3.13,6.4
+1,4,3,-29.5,3.13,-6.87,-30.62,3.13,-5.38,-32.28,3.13,-3.35,-33.97,3.55,-1.3
+1,4,4,-29.1,3.13,-17.22,-30.25,4.38,-14.84,-31.73,4.38,-12.06,-33.54,4.38,-9.24
+1,4,5,-29.1,3.13,-26.94,-30.06,3.75,-24.12,-31.53,3.8,-20.36,-33.21,4.38,-16.88
+1,4,6,-29.13,3.13,-37.29,-30.22,4.38,-33.92,-31.36,4.38,-29.13,-32.84,4.38,-25.05
+1,4,7,-28.87,3.13,-47.5,-29.85,4.38,-43.2,-30.62,4.38,-37.67,-31.73,4.38,-32.84
+1,5,1,-30.27,12.52,12.08,-30.95,12.88,12.61,-32.02,13.13,13.1,-32.83,13.44,13.69
+1,5,2,-29.74,13.13,3,-30.8,13.75,3.89,-32.14,14.37,5.23,-33.58,14.37,6.49
+1,5,3,-29.35,13.13,-6.91,-30.56,13.93,-5.35,-32.13,14.37,-3.19,-33.91,15.05,-1.46
+1,5,4,-30.62,13.13,-17.26,-31.73,14.37,-15.03,-33.4,15,-11.87,-35.18,15.62,-9.28
+1,5,5,-29.14,13.13,-27.09,-30.24,14.37,-24.31,-31.73,14.87,-20.4,-33.58,15.62,-16.88
+1,5,6,-29.08,12.43,-37.47,-30.06,13.82,-33.89,-31.33,14.37,-29.1,-32.84,15.62,-25.05
+1,5,7,-28.82,12.57,-47.49,-29.87,14.37,-43.23,-30.56,14.9,-37.71,-31.58,15.62,-32.88
+1,6,1,-30.69,23.83,12.29,-31.36,24.37,12.5,-32.3,24.98,13.15,-33.14,25.44,13.74
+1,6,2,-30.11,23.12,3,-31.03,23.86,3.95,-32.49,24.81,5.34,-33.96,25.63,6.49
+1,6,3,-30.08,22.49,-7.03,-31.17,23.62,-5.39,-32.84,24.37,-3.15,-34.51,25.63,-1.18
+1,6,4,-30.24,23.12,-17.26,-31.36,24.37,-15.03,-33.03,25.63,-11.88,-34.74,26.88,-9.24
+1,6,5,-29.32,22.63,-27.28,-30.24,24.37,-24.31,-31.84,25.63,-20.41,-33.36,26.88,-16.93
+1,6,6,-29.13,23.12,-37.67,-30.24,24.37,-33.96,-31.39,25.63,-29.16,-32.84,26.88,-25.05
+1,6,7,-28.72,22.67,-47.65,-29.53,24.37,-43.26,-30.57,25.63,-37.71,-31.73,26.88,-33.21
+1,7,1,-30.71,34.08,12.22,-31.29,34.91,12.76,-32.33,35.9,13.3,-33.1,36.51,14.11
+1,7,2,-29.89,33.63,3,-30.9,34.79,4.06,-32.25,36.27,5.6,-33.56,37.45,7.15
+1,7,3,-29.99,33.13,-7.05,-31.01,34.38,-5.35,-32.66,36.12,-2.79,-34.05,37.56,-0.73
+1,7,4,-29.53,33.13,-17.22,-30.68,34.93,-14.85,-32.28,36.87,-11.69,-33.92,38.12,-8.75
+1,7,5,-29.5,33.13,-27.28,-30.43,35,-24.12,-31.73,36.87,-20.22,-33.24,38.12,-16.85
+1,7,6,-29.5,33.13,-37.67,-30.48,35,-33.81,-31.73,36.87,-29.13,-33.17,38.12,-25.2
+1,7,7,-29.32,33.13,-47.69,-30.11,35,-43.38,-31.36,36.87,-37.67,-32.47,38.12,-33.21
+2,1,1,-19.25,-29.37,12.58,-19.48,-30.14,12.8,-19.96,-31.14,13.53,-20.28,-32.11,14.23
+2,1,2,-19.26,-27.48,3.37,-19.85,-28.12,4.27,-20.56,-29.37,5.6,-21.34,-30.62,6.87
+2,1,3,-17.81,-26.88,-6.49,-18.41,-27.46,-5.16,-19.44,-28.91,-3.07,-20.22,-30.62,-0.93
+2,1,4,-18.93,-26.88,-16.89,-19.7,-27.56,-14.61,-20.6,-29.37,-11.69,-21.71,-30.62,-8.72
+2,1,5,-18.55,-26.88,-26.91,-19.28,-27.49,-23.77,-20.04,-28.88,-19.86,-20.99,-30.62,-16.48
+2,1,6,-18.77,-26.88,-36.96,-19.46,-26.88,-33.55,-20.04,-28.12,-28.57,-20.97,-29.37,-24.31
+2,1,7,-18.55,-26.88,-47.13,-19.17,-26.32,-42.68,-19.4,-27.38,-37.02,-20.22,-28.12,-32.1
+2,2,1,-20.04,-19.37,12.43,-20.4,-19.37,12.71,-20.94,-20,13.21,-21.4,-20.63,13.67
+2,2,2,-19.2,-17.62,3.24,-19.73,-18.12,4.05,-20.67,-18.66,5.31,-21.37,-19.37,6.16
+2,2,3,-18.62,-16.88,-6.69,-19.14,-16.88,-5.35,-20.22,-18.12,-3.15,-21.34,-19.37,-1.3
+2,2,4,-18.77,-18.12,-16.85,-19.48,-18.12,-14.66,-20.45,-18.78,-11.67,-21.62,-19.88,-9
+2,2,5,-18.74,-16.88,-26.9,-19.48,-16.88,-23.94,-20.34,-18.12,-20.04,-21.52,-19.37,-16.52
+2,2,6,-18.74,-16.88,-37.29,-19.48,-16.88,-33.58,-20.2,-17.33,-28.7,-21.15,-18.12,-24.31
+2,2,7,-18.74,-16.88,-46.94,-19.48,-16.88,-42.86,-19.82,-16.88,-36.95,-20.6,-18.12,-32.1
+2,3,1,-20.07,-10.05,12.38,-20.49,-10.15,12.61,-21.02,-10.63,13.39,-21.48,-10.63,13.58
+2,3,2,-19.2,-6.88,3.24,-19.85,-6.88,3.9,-20.63,-6.88,4.98,-21.67,-7.51,6.14
+2,3,3,-19.11,-6.88,-6.87,-19.85,-6.88,-5.38,-20.8,-7.49,-3.36,-21.89,-8.13,-1.55
+2,3,4,-19.14,-6.88,-16.92,-19.89,-6.88,-14.69,-21,-6.88,-11.72,-22.26,-7.57,-9.16
+2,3,5,-18.74,-6.88,-26.9,-19.49,-6.88,-24.12,-20.25,-6.88,-20.19,-21.67,-7.33,-16.55
+2,3,6,-18.93,-6.88,-37.3,-19.6,-6.34,-33.67,-20.22,-6.88,-28.76,-21.34,-6.88,-24.49
+2,3,7,-18.73,-7.38,-47.16,-19.31,-6.38,-42.87,-19.83,-6.88,-37.26,-20.41,-6.88,-32.29
+2,4,1,-20.78,0.98,12.18,-21.12,1.08,12.52,-21.72,1.11,13.02,-22.27,0.99,13.54
+2,4,2,-19.52,3.13,3,-20.2,3.57,3.85,-21.12,3.73,4.86,-22.02,3.75,6.05
+2,4,3,-18.77,3.13,-6.83,-19.51,3.13,-5.41,-20.6,3.13,-3.53,-21.68,3.57,-1.71
+2,4,4,-19.23,3.13,-17.07,-19.97,3.8,-14.9,-20.99,4.38,-12.03,-22.11,4.38,-9.43
+2,4,5,-18.77,3.13,-26.94,-19.67,4.38,-24.31,-20.6,4.38,-20.22,-21.71,4.38,-16.88
+2,4,6,-19.11,3.13,-37.29,-19.67,4.38,-33.84,-20.41,4.38,-28.76,-21.52,4.93,-24.75
+2,4,7,-19.06,3.59,-47.27,-19.65,4.99,-43.06,-19.85,5.63,-37.29,-20.6,5.63,-32.47
+2,5,1,-20.18,11.88,12.38,-20.6,11.88,12.43,-21.23,12.53,13,-21.77,12.65,13.56
+2,5,2,-19.85,13.13,3.15,-20.56,14.37,3.87,-21.34,14.37,5.01,-22.27,15,5.94
+2,5,3,-19.11,13.13,-6.87,-20.04,14.37,-5.57,-21.14,14.87,-3.53,-22.08,15.62,-1.67
+2,5,4,-19.64,13.13,-17.1,-20.41,13.82,-14.96,-21.52,14.37,-12.07,-22.8,15.62,-9.43
+2,5,5,-19.11,13.13,-26.9,-19.85,14.37,-24.31,-20.78,15.62,-20.23,-21.89,15.62,-16.89
+2,5,6,-19.11,13.13,-37.29,-19.83,14.37,-33.92,-20.41,15.62,-29.14,-21.45,15.62,-24.87
+2,5,7,-18.74,13.13,-47.31,-19.46,14.37,-43.2,-19.83,15.62,-37.63,-20.48,15.62,-32.66
+2,6,1,-21.15,23.42,12.14,-21.62,23.68,12.66,-22.15,24.37,13.01,-22.75,24.84,13.52
+2,6,2,-19.67,23.12,2.97,-20.22,24.37,3.9,-21.28,25.17,4.98,-22.05,25.63,6.09
+2,6,3,-19.49,23.12,-7.05,-20.06,24.89,-5.4,-21.16,26.12,-3.53,-22.27,26.88,-1.68
+2,6,4,-18.37,23.12,-17.26,-19.11,24.37,-15.03,-20.22,25.63,-12.06,-21.31,26.88,-9.43
+2,6,5,-19.09,23.12,-27.24,-19.69,24.89,-24.3,-20.6,26.38,-20.4,-21.7,27.66,-16.92
+2,6,6,-19.09,23.12,-37.63,-19.67,24.37,-33.96,-20.43,26.14,-29.15,-21.34,26.88,-25.05
+2,6,7,-18.93,22.5,-47.5,-19.51,24.37,-43.26,-20.22,25.63,-37.67,-20.97,26.88,-32.84
+2,7,1,-20.73,33.58,12.06,-21.15,34.38,12.52,-21.62,35.31,13.33,-22.14,36.33,14.01
+2,7,2,-19.82,33.13,3.12,-20.56,34.38,3.87,-21.34,35.63,5.38,-22.08,37.5,6.68
+2,7,3,-19.69,33.74,-7.03,-20.6,35.63,-5.38,-21.68,36.87,-3.12,-22.6,38.77,-1.08
+2,7,4,-19.11,33.13,-17.26,-19.86,35.63,-14.84,-20.97,36.87,-11.69,-22.08,38.88,-8.92
+2,7,5,-19.46,33.13,-27.24,-19.88,35.63,-24.27,-20.97,36.87,-20.22,-21.92,38.76,-16.72
+2,7,6,-19.46,33.13,-37.63,-20.04,34.87,-33.96,-20.94,36.87,-29.1,-21.74,38.12,-25.01
+2,7,7,-19.11,33.13,-47.69,-19.72,35,-43.38,-20.38,36.87,-37.63,-20.97,38.12,-32.84
+3,1,1,-8.61,-28.92,12.85,-8.73,-29.37,12.99,-8.91,-30.62,13.54,-8.97,-31.39,14.26
+3,1,2,-8.78,-26.88,3.58,-9.1,-28.12,4.08,-9.28,-29.37,5.38,-9.6,-30.63,6.52
+3,1,3,-8.54,-26.88,-6.61,-8.91,-27.5,-5.2,-9.12,-29.37,-2.82,-9.8,-30.62,-0.96
+3,1,4,-8.73,-26.88,-16.7,-8.92,-27.63,-14.66,-9.44,-29.37,-11.65,-9.84,-30.62,-8.91
+3,1,5,-8.35,-26.88,-26.9,-8.72,-26.88,-23.94,-8.91,-28.68,-19.92,-9.46,-30.62,-16.51
+3,1,6,-8.57,-26.88,-37.09,-8.75,-26.88,-33.24,-9.03,-28.58,-28.42,-9.48,-30.17,-24.27
+3,1,7,-8.72,-26.88,-46.94,-9.09,-26.88,-42.49,-8.91,-27.69,-36.79,-9.28,-28.75,-31.91
+3,2,1,-8.91,-19.96,12.66,-9.04,-20.13,12.87,-9.24,-20.95,13.29,-9.32,-21.48,13.9
+3,2,2,-9.12,-16.88,3.19,-9.43,-17.4,4.04,-9.72,-18.12,5.19,-10.02,-19.37,6.12
+3,2,3,-8.69,-16.88,-6.53,-9.07,-16.88,-5.35,-9.46,-18.12,-3.15,-9.83,-19.37,-1.3
+3,2,4,-8.93,-17.49,-16.72,-9.32,-17.46,-14.8,-9.81,-18.12,-11.65,-10.21,-19.37,-8.91
+3,2,5,-8.69,-16.88,-26.87,-8.91,-16.88,-23.94,-9.24,-17.48,-20.01,-9.82,-18.64,-16.35
+3,2,6,-8.73,-16.88,-37.11,-8.81,-16.88,-33.5,-8.94,-17.62,-28.4,-9.47,-18.64,-24.14
+3,2,7,-8.72,-16.88,-46.94,-9.01,-16.37,-42.76,-8.91,-16.88,-36.92,-9.24,-17.52,-31.88
+3,3,1,-8.58,-8.98,12.6,-8.84,-9.02,12.62,-8.91,-9.38,13.17,-9.24,-9.38,13.59
+3,3,2,-9.09,-6.88,3.15,-9.46,-6.88,3.9,-9.81,-7.32,5,-10.21,-8.13,6.12
+3,3,3,-8.98,-6.88,-6.69,-9.28,-6.88,-5.39,-9.83,-6.88,-3.53,-10.39,-7.5,-1.67
+3,3,4,-9.07,-6.88,-16.85,-9.21,-6.88,-14.85,-9.8,-6.88,-11.72,-10.4,-7.63,-9.1
+3,3,5,-8.91,-6.88,-26.91,-9.28,-5.63,-24.12,-9.63,-6.26,-20.06,-10.21,-6.88,-16.51
+3,3,6,-9.07,-6.88,-37.26,-9.3,-6.14,-33.59,-9.47,-6.38,-28.58,-10.02,-6.88,-24.31
+3,3,7,-8.68,-7.45,-47.1,-9.09,-6.88,-42.86,-9.09,-6.88,-36.92,-9.46,-6.88,-32.1
+3,4,1,-10.39,1.88,12.43,-10.43,1.88,12.66,-10.75,1.88,13.08,-11.1,1.88,13.5
+3,4,2,-9.31,3.13,3.11,-9.58,3.13,3.71,-10.02,3.13,4.75,-10.54,3.57,5.78
+3,4,3,-10.17,4.38,-6.9,-10.58,4.38,-5.57,-10.95,4.38,-3.71,-11.66,4.88,-1.96
+3,4,4,-9.09,3.13,-16.88,-9.3,3.69,-14.9,-9.84,4.38,-11.87,-10.55,4.38,-9.43
+3,4,5,-8.69,3.13,-26.94,-9.07,4.38,-24.27,-9.28,4.38,-20.23,-9.86,4.38,-16.85
+3,4,6,-9.09,3.13,-37.29,-9.41,4.38,-33.77,-9.46,4.38,-28.76,-10.02,4.38,-24.49
+3,4,7,-8.87,3.13,-47.17,-9.09,4.38,-43.05,-9.07,4.38,-37.26,-9.44,4.83,-32.16
+3,5,1,-10.03,12.65,12.27,-10.21,13.13,12.43,-10.44,13.13,12.95,-10.64,13.47,13.41
+3,5,2,-9.46,13.13,3.15,-9.84,13.62,3.72,-10.21,14.37,4.82,-10.58,14.37,5.75
+3,5,3,-9.09,13.13,-6.87,-9.46,14.37,-5.57,-10.02,15.62,-3.53,-10.39,15.62,-1.86
+3,5,4,-9.44,13.13,-17.22,-9.81,14.37,-14.99,-10.33,14.9,-12.06,-10.95,15.62,-9.46
+3,5,5,-9.46,13.88,-27.08,-9.81,15.62,-24.27,-10.04,16.26,-20.39,-10.95,16.88,-16.88
+3,5,6,-9.09,13.13,-37.29,-9.44,14.37,-33.92,-9.47,15.62,-28.95,-10.21,15.62,-24.68
+3,5,7,-8.72,13.13,-47.31,-8.98,14.37,-43.05,-8.91,15.62,-37.29,-9.31,15.62,-32.51
+3,6,1,-10.44,22.78,12.12,-10.76,23.12,12.43,-11.01,23.75,12.98,-11.25,24.37,13.54
+3,6,2,-9.44,23.12,2.82,-9.65,24.37,3.9,-10.17,25.63,4.98,-10.39,26.18,6.05
+3,6,3,-9.12,23.12,-6.9,-9.47,24.37,-5.57,-10.02,25.63,-3.53,-10.58,26.88,-1.67
+3,6,4,-9.46,23.12,-17.26,-9.65,24.37,-15.03,-10.14,26.32,-12.01,-10.76,27.5,-9.28
+3,6,5,-9.12,23.12,-27.24,-9.64,24.87,-24.31,-10.01,26.38,-20.22,-10.6,28.12,-16.85
+3,6,6,-9.07,23.12,-37.63,-9.28,25,-33.77,-9.49,26.88,-29.1,-10.04,27.51,-24.88
+3,6,7,-9.09,23.12,-47.31,-9.48,25.16,-43.18,-9.47,26.37,-37.45,-9.84,26.88,-32.66
+3,7,1,-11.04,33.8,12.08,-11.32,34.38,12.62,-11.54,35.63,13.13,-11.77,36.57,13.72
+3,7,2,-9.47,33.13,2.97,-9.78,34.38,4.08,-10.21,36.87,5.38,-10.43,38.12,6.53
+3,7,3,-9.28,34.38,-6.87,-9.65,35.63,-5.38,-10.02,37.63,-3.15,-10.58,39.38,-0.93
+3,7,4,-9.46,33.13,-17.26,-10.02,35,-14.84,-10.54,36.87,-11.72,-11.09,38.85,-9.06
+3,7,5,-9.09,33.13,-27.28,-9.47,35.63,-24.12,-9.96,37.42,-20.13,-10.55,39.38,-16.85
+3,7,6,-8.72,33.13,-37.67,-8.91,35,-33.77,-9.21,36.87,-28.95,-9.65,38.68,-24.75
+3,7,7,-9.42,33.13,-47.53,-9.82,35.2,-43.23,-9.79,36.87,-37.45,-10.21,38.12,-32.66
+4,1,1,2.3,-28.71,12.63,2.37,-29.37,12.95,2.37,-30.11,13.5,2.48,-31.18,14.28
+4,1,2,0.78,-26.88,3.48,0.93,-28.12,4.27,0.81,-29.37,5.56,0.93,-30.62,6.87
+4,1,3,2.04,-26.88,-6.49,2.22,-27.63,-5.01,2.41,-29.37,-2.78,2.44,-30.62,-0.96
+4,1,4,1.31,-27.37,-16.65,1.48,-28.12,-14.66,1.67,-29.37,-11.32,1.8,-31.34,-8.76
+4,1,5,1.64,-26.88,-26.57,1.67,-27.37,-23.75,2.01,-28.92,-19.79,2.04,-30.62,-16.14
+4,1,6,1.48,-26.88,-36.93,1.67,-26.88,-33.21,2.04,-28.12,-28.2,2,-30.18,-23.97
+4,1,7,1.48,-26.88,-46.94,1.67,-26.88,-42.49,2.23,-27.5,-36.56,2.23,-28.12,-31.61
+4,2,1,2.04,-20,12.62,2.04,-20.63,12.8,2.08,-20.63,13.32,2.23,-21.63,13.91
+4,2,2,0.96,-16.88,3.49,1.09,-17.51,4.07,1.27,-18.12,5.35,1.27,-19.37,6.16
+4,2,3,2.41,-16.88,-6.49,2.6,-16.88,-5.2,2.78,-18.12,-3.15,2.97,-19.37,-1.3
+4,2,4,0.95,-16.88,-16.85,0.95,-16.88,-14.62,1.29,-18.12,-11.5,1.29,-18.88,-8.9
+4,2,5,1.64,-16.88,-26.87,1.54,-16.35,-23.95,2.01,-16.88,-19.82,2.01,-18.12,-16.17
+4,2,6,1.33,-16.88,-36.95,1.55,-16.88,-33.4,1.85,-17.57,-28.32,1.86,-18.68,-23.87
+4,2,7,0.93,-16.88,-46.94,1.11,-16.18,-42.56,1.67,-16.88,-36.55,1.64,-16.88,-31.69
+4,3,1,1.3,-9.38,12.43,1.45,-9.38,12.76,1.45,-9.38,13.13,1.66,-10.02,13.67
+4,3,2,1.04,-6.88,3.33,1.3,-6.88,3.9,1.27,-6.88,5.04,1.29,-8.13,6.05
+4,3,3,2.01,-6.88,-6.83,2.04,-6.88,-5.38,2.23,-6.88,-3.53,2.41,-8.13,-1.67
+4,3,4,0.58,-6.88,-16.85,0.59,-6.88,-14.69,0.9,-6.88,-11.72,0.93,-8.13,-9.09
+4,3,5,1.3,-6.88,-26.9,1.48,-5.63,-23.94,1.68,-6.14,-20.02,1.67,-6.88,-16.51
+4,3,6,1.64,-6.88,-36.96,1.66,-6.13,-33.43,1.97,-6.42,-28.39,2.04,-6.88,-23.94
+4,3,7,1.3,-6.88,-46.94,1.29,-5.63,-42.68,1.67,-6.12,-36.75,1.71,-6.3,-31.88
+4,4,1,1.39,1.2,12.46,1.45,1.31,12.49,1.48,1.25,12.95,1.58,1.32,13.52
+4,4,2,1.15,3.13,3.19,1.26,3.57,3.87,1.12,3.68,4.94,1.35,3.68,5.94
+4,4,3,1.33,3.13,-6.83,1.53,3.79,-5.55,1.64,4.38,-3.56,1.7,4.38,-2.01
+4,4,4,0.56,3.13,-16.88,0.66,3.8,-14.89,0.74,4.38,-11.95,0.74,4.38,-9.46
+4,4,5,1.3,3.13,-26.9,1.27,4.38,-24.27,1.48,4.38,-20.23,1.42,4.92,-16.61
+4,4,6,1.27,3.13,-37.26,1.3,4.38,-33.58,1.67,4.38,-28.76,1.67,4.38,-24.31
+4,4,7,1.26,3.13,-47.16,1.3,4.38,-42.86,1.67,4.38,-36.92,1.67,4.38,-32.1
+4,5,1,1.17,11.88,12.28,1.22,12.28,12.48,1.21,12.45,13.15,1.32,12.59,13.57
+4,5,2,0.93,13.13,3.15,0.92,13.66,3.76,0.93,14.37,5.01,0.95,14.37,5.79
+4,5,3,1.48,13.13,-6.87,1.48,14.37,-5.57,1.64,15.62,-3.56,1.67,15.62,-1.67
+4,5,4,1.27,13.13,-16.92,0.95,14.37,-14.99,1.27,15.62,-12.03,1.3,15.62,-9.46
+4,5,5,0.96,13.13,-26.94,1.21,14.38,-24.22,1.3,15.62,-20.22,1.29,16.12,-16.71
+4,5,6,1.11,13.13,-37.3,1.13,14.94,-33.73,1.48,15.62,-28.76,1.51,16.88,-24.52
+4,5,7,1.06,13.13,-47.16,1.27,14.8,-42.89,1.67,15.62,-37.29,1.67,16.25,-32.29
+4,6,1,0.74,23.12,12.25,0.75,23.59,12.47,0.74,24.37,12.8,0.84,24.88,13.45
+4,6,2,0.73,23.12,3.04,0.64,24.38,3.81,0.73,25.63,4.89,0.78,26.27,5.97
+4,6,3,1.3,23.12,-6.87,1.33,24.37,-5.41,1.48,26.12,-3.53,1.66,27.3,-1.67
+4,6,4,0.9,23.12,-16.92,0.93,24.37,-15.03,0.93,26.14,-11.9,0.92,27.37,-9.28
+4,6,5,0.92,23.12,-27.09,0.95,25.63,-24.27,1.3,26.88,-20.22,1.27,28.12,-16.85
+4,6,6,0.96,23.12,-37.33,1.09,25.01,-33.79,1.3,26.88,-28.76,1.3,28.12,-24.68
+4,6,7,0.93,23.12,-47.31,1.11,25,-43.05,1.48,26.88,-37.3,1.48,27.37,-32.48
+4,7,1,-0.02,33.86,12.07,-0.15,34.83,12.47,-0.15,36.08,13.11,-0.07,36.88,13.86
+4,7,2,0.55,33.62,2.98,0.53,34.87,3.93,0.56,36.87,5.38,0.56,38.12,6.49
+4,7,3,0.55,33.13,-7.05,0.56,35.63,-5.38,0.5,37.34,-3.11,0.52,39.38,-0.96
+4,7,4,0.93,33.13,-17.26,0.92,34.87,-14.83,0.93,36.87,-11.69,1.11,38.75,-8.91
+4,7,5,0.9,33.13,-27.24,0.92,35.63,-24.12,0.99,37.57,-20.05,0.93,39.38,-16.51
+4,7,6,0.92,33.13,-37.48,1.11,35,-33.77,1.3,36.87,-28.76,1.26,38.56,-24.68
+4,7,7,0.93,33.13,-47.31,0.95,35.63,-43.2,1.3,36.87,-37.29,1.3,38.12,-32.47
+5,1,1,12.29,-28.62,12.83,12.48,-29.38,13.02,12.81,-30.18,13.72,13.17,-31.18,14.35
+5,1,2,11.69,-26.88,3.53,11.86,-27.63,4.26,12.4,-29.37,5.72,12.8,-30.62,6.87
+5,1,3,11.32,-26.88,-6.49,11.88,-28.12,-5.01,12.43,-29.37,-2.78,13.15,-30.62,-0.89
+5,1,4,11.32,-26.88,-16.51,11.72,-26.88,-14.32,12.61,-28.88,-11.32,13.21,-30.62,-8.57
+5,1,5,11.5,-26.88,-26.54,12.03,-26.88,-23.6,12.84,-28.12,-19.51,13.55,-29.89,-15.98
+5,1,6,11.69,-26.88,-36.92,12.25,-26.88,-33.22,13.03,-28.13,-28.05,13.54,-29.37,-23.94
+5,1,7,11.69,-26.88,-46.94,12.27,-26.24,-42.32,13.17,-26.88,-36.55,13.54,-28.12,-31.36
+5,2,1,12.8,-19.37,12.8,12.99,-19.37,12.99,13.54,-20.63,13.54,13.73,-21.02,13.98
+5,2,2,12.03,-16.88,3.49,12.28,-17.48,4.12,12.8,-18.12,5.38,13.51,-19.37,6.46
+5,2,3,12.43,-16.88,-6.49,12.79,-17.3,-5.01,13.54,-18.12,-3.15,14.25,-19.37,-1.15
+5,2,4,12.03,-16.88,-16.55,12.46,-16.88,-14.62,13.17,-18.12,-11.32,13.94,-19.37,-8.69
+5,2,5,11.66,-16.88,-26.57,12.06,-16.88,-23.75,12.99,-17.5,-19.67,13.55,-18.62,-15.97
+5,2,6,11.69,-16.88,-36.92,12.25,-16.88,-33.22,13.17,-16.88,-28.02,13.73,-18.12,-23.75
+5,2,7,11.66,-16.88,-46.91,12.06,-15.62,-42.49,12.84,-16.42,-36.51,13.17,-16.88,-31.36
+5,3,1,12.76,-8.61,12.78,12.94,-8.75,12.95,13.19,-8.95,13.55,13.66,-9.38,13.97
+5,3,2,10.99,-6.88,3.37,11.32,-6.88,3.9,11.88,-6.88,5.12,12.42,-7.35,6.13
+5,3,3,11.69,-6.88,-6.49,12.06,-6.88,-5.2,12.8,-6.88,-3.15,13.54,-7.5,-1.48
+5,3,4,11.29,-6.88,-16.85,11.69,-6.88,-14.66,12.43,-6.88,-11.5,13.18,-7.61,-8.88
+5,3,5,11.29,-6.88,-26.87,11.82,-6.3,-23.82,12.78,-6.88,-19.82,13.17,-6.88,-16.14
+5,3,6,10.98,-6.88,-36.95,11.68,-5.63,-33.4,12.43,-6.12,-28.2,12.83,-6.88,-23.9
+5,3,7,11.5,-6.88,-46.94,12.06,-5.63,-42.49,12.8,-5.63,-36.55,13.17,-5.63,-31.73
+5,4,1,11.91,1.88,12.39,12.09,2.41,12.75,12.4,2.41,13.23,12.87,2.43,13.55
+5,4,2,10.95,3.13,3.15,11.32,3.13,3.9,11.72,3.59,5.01,12.43,3.62,5.95
+5,4,3,11.69,3.13,-6.87,12.25,3.13,-5.39,12.92,3.85,-3.46,13.54,3.88,-1.53
+5,4,4,11.32,3.13,-16.88,11.69,3.88,-14.84,12.43,4.38,-11.69,13.17,4.38,-9.09
+5,4,5,11.32,3.13,-26.9,11.72,4.38,-23.97,12.62,4.38,-19.86,13.33,4.89,-16.51
+5,4,6,11.34,3.13,-37.26,12.03,4.38,-33.55,12.8,4.38,-28.39,13.36,4.38,-24.31
+5,4,7,11.08,3.13,-47.16,11.69,4.38,-42.86,12.45,4.91,-36.86,12.8,5,-31.91
+5,5,1,11.91,11.88,12.39,12.21,11.88,12.76,12.45,12.45,13.09,12.8,12.65,13.52
+5,5,2,10.8,13.13,3.19,11.13,14.37,3.89,11.69,14.37,5.01,12.1,15.05,5.91
+5,5,3,11.69,13.13,-6.87,12.06,14.37,-5.38,12.64,15.63,-3.37,13.36,15.62,-1.68
+5,5,4,11.17,13.85,-16.92,11.69,15.13,-14.84,12.46,15.62,-11.72,13.17,16.88,-9.09
+5,5,5,10.95,13.13,-26.9,11.34,14.89,-24.08,12.09,15.62,-20.19,12.8,16.88,-16.51
+5,5,6,10.6,13.13,-37.26,11.13,14.87,-33.59,12.03,15.62,-28.73,12.43,16.88,-24.31
+5,5,7,11.5,13.13,-47.13,12.03,14.82,-42.87,12.84,15.62,-36.95,13.17,16.25,-32.29
+5,6,1,11.32,23.12,12.06,11.61,23.65,12.5,11.87,24.37,12.99,12.24,24.81,13.41
+5,6,2,11.23,23.13,2.87,11.35,24.37,3.87,12.03,25.63,4.98,12.4,26.88,5.98
+5,6,3,10.98,23.12,-6.9,11.35,24.37,-5.41,11.95,25.63,-3.34,12.78,26.88,-1.64
+5,6,4,10.57,23.12,-17.07,11.13,25,-14.84,11.69,26.88,-11.69,12.43,28.12,-9.09
+5,6,5,10.98,23.12,-26.94,11.5,25,-24.12,12.09,26.88,-20.19,12.8,28.12,-16.51
+5,6,6,10.38,23.12,-37.41,10.82,24.88,-33.62,11.51,26.32,-28.69,12,27.43,-24.5
+5,6,7,10.58,23.12,-47.31,11.13,25,-43.05,12.06,26.88,-37.29,12.34,27.34,-32.43
+5,7,1,10.61,33.56,12.06,10.8,34.38,12.39,10.95,35.63,13.17,11.41,36.5,13.71
+5,7,2,10.83,33.13,2.97,10.97,34.38,3.93,11.49,36.38,5.38,11.88,38.12,6.61
+5,7,3,10.95,33.13,-6.87,11.26,35.07,-5.19,11.87,36.87,-2.97,12.39,38.92,-0.96
+5,7,4,10.6,33.13,-17.22,10.95,35.63,-14.66,11.5,37.5,-11.5,12.06,39.38,-8.72
+5,7,5,10.94,33.13,-27.09,11.29,35.63,-23.97,11.95,38.12,-19.96,12.43,39.38,-16.51
+5,7,6,10.61,33.13,-37.33,11.13,35,-33.77,11.88,36.87,-28.76,12.27,38.74,-24.51
+5,7,7,11.32,33.13,-47.31,11.57,35.63,-43.05,12.49,37.57,-37.12,12.78,38.7,-32.39
+6,1,1,23.14,-28.59,12.76,23.56,-29.37,13.17,24.11,-30.18,13.76,24.68,-31.25,14.47
+6,1,2,21.52,-26.88,3.64,22.14,-27.43,4.45,23.01,-28.75,5.94,24.12,-30.07,7.16
+6,1,3,22.27,-26.88,-6.31,23.01,-27.5,-4.82,24.34,-29.37,-2.44,25.45,-30.62,-0.52
+6,1,4,21.71,-26.88,-16.51,22.45,-27.33,-14.29,23.76,-28.88,-10.95,25.05,-30.62,-8.35
+6,1,5,21.74,-26.88,-26.56,22.82,-26.88,-23.56,24.31,-28.12,-19.48,25.42,-29.87,-15.95
+6,1,6,21.59,-26.88,-36.74,22.49,-26.88,-33.06,23.94,-28.12,-28.02,25.01,-29.37,-23.72
+6,1,7,21.52,-26.88,-46.76,22.51,-26.18,-42.31,23.75,-26.88,-36.37,24.31,-28.12,-31.36
+6,2,1,23.41,-19.96,12.76,23.75,-20.07,13.1,24.46,-20.63,13.59,25.03,-21.41,14.24
+6,2,2,21.12,-17.45,3.48,21.71,-18.12,4.27,22.81,-18.64,5.59,23.65,-19.37,6.78
+6,2,3,22.43,-17.31,-6.46,23.18,-17.39,-4.85,24.65,-18.12,-2.75,25.76,-19.37,-0.89
+6,2,4,21.37,-16.88,-16.54,22.42,-16.88,-14.32,23.75,-18.12,-11.13,25.05,-19.37,-8.35
+6,2,5,21.68,-16.88,-26.57,22.45,-16.88,-23.56,23.94,-17.69,-19.46,25.23,-18.68,-15.84
+6,2,6,21.71,-16.88,-36.92,22.46,-16.45,-33.21,23.93,-16.88,-27.83,25.05,-18.12,-23.56
+6,2,7,21.68,-16.88,-46.91,22.61,-16.24,-42.32,23.68,-16.88,-36.37,24.31,-16.88,-31.36
+6,3,1,23.16,-8.58,12.74,23.61,-8.7,13.01,24.28,-8.93,13.56,24.9,-9.38,13.96
+6,3,2,21.56,-6.88,3.48,22.11,-6.88,4.24,23.19,-6.88,5.38,24.34,-7.33,6.53
+6,3,3,21.87,-6.23,-6.5,22.64,-5.63,-5.01,23.88,-6.32,-2.95,25.3,-6.88,-1.12
+6,3,4,21.08,-6.88,-16.71,21.92,-6.26,-14.49,23.54,-6.88,-11.28,24.7,-6.88,-8.69
+6,3,5,21.37,-6.88,-26.56,22.23,-5.63,-23.77,23.75,-6.25,-19.67,25.16,-6.88,-15.96
+6,3,6,21.37,-6.88,-36.95,22.45,-5.63,-33.21,23.75,-5.63,-28.02,24.86,-6.25,-23.75
+6,3,7,21.34,-6.88,-46.94,22.08,-5.63,-42.49,23.34,-5.63,-36.51,23.98,-5.63,-31.51
+6,4,1,22.45,1.88,12.43,22.84,2.33,12.81,23.62,2.43,13.37,24.27,2.39,13.87
+6,4,2,21.31,3.13,3.19,21.95,3.81,4.02,22.88,3.82,5.19,23.96,3.92,6.18
+6,4,3,21.79,3.13,-6.58,22.63,3.68,-5.32,24.12,3.82,-3.09,25.36,3.92,-1.32
+6,4,4,21.34,3.13,-16.88,22.08,4.38,-14.66,23.6,4.38,-11.35,25.05,4.38,-8.72
+6,4,5,21.52,3.13,-26.91,22.45,4.38,-23.94,23.85,4.96,-19.74,25.21,5.01,-16.31
+6,4,6,21.15,3.13,-37.11,22.05,4.38,-33.55,23.38,4.93,-28.32,24.46,5.05,-23.98
+6,4,7,21.34,3.13,-46.94,22.07,4.38,-42.68,23.25,5.07,-36.75,23.94,5.63,-31.73
+6,5,1,22.09,12.48,12.27,22.45,12.68,12.77,23.19,13.13,13.17,23.79,13.13,13.69
+6,5,2,21.31,13.13,3.19,21.71,14.37,3.9,22.82,14.87,5.2,23.94,15.62,6.12
+6,5,3,22.27,13.13,-6.87,23.19,14.37,-5.38,24.48,15.13,-3.15,25.79,15.62,-1.3
+6,5,4,20.97,13.13,-16.88,21.71,14.37,-14.66,23.17,15.62,-11.65,24.33,15.62,-9.06
+6,5,5,21.35,13.59,-26.91,22.12,15.18,-23.96,23.74,16.12,-19.86,24.86,16.88,-16.4
+6,5,6,21.36,13.13,-37.26,22.39,14.83,-33.56,23.75,15.62,-28.39,24.69,16.36,-24.15
+6,5,7,21.52,13.13,-47.13,22.41,14.83,-42.82,23.56,15.62,-36.92,24.33,16.1,-32.13
+6,6,1,22.21,23.12,12.28,22.64,23.82,12.5,23.34,24.37,13.13,23.93,25,13.73
+6,6,2,20.94,23.93,3.11,21.55,24.89,3.9,22.45,25.63,5.2,23.56,26.88,6.49
+6,6,3,21.71,23.12,-6.87,22.45,24.37,-5.38,23.72,26.19,-3.11,24.89,27.49,-1.13
+6,6,4,20.63,23.12,-16.92,21.37,24.81,-14.68,22.81,26.36,-11.53,23.93,27.63,-8.9
+6,6,5,21.15,23.75,-27.09,21.89,25.63,-23.94,23.38,26.88,-19.86,24.49,28.12,-16.52
+6,6,6,21.34,23.12,-37.29,22.29,25.11,-33.59,23.68,26.88,-28.58,24.68,28.12,-24.31
+6,6,7,21.12,23.12,-47.27,22.08,25.63,-42.86,23.37,26.88,-37.04,23.99,27.43,-32.28
+6,7,1,21.71,33.95,12.05,22.1,34.92,12.56,22.64,36.03,13.33,23.19,36.87,13.92
+6,7,2,20.78,33.53,2.99,21.39,34.93,4.09,22.11,36.87,5.72,23.19,38.12,6.87
+6,7,3,21.71,33.13,-6.87,22.45,35.63,-5.01,23.59,36.87,-2.75,24.68,39.38,-0.56
+6,7,4,20.56,33.13,-17.04,21.17,35.01,-14.49,22.45,36.87,-11.32,23.56,39.38,-8.53
+6,7,5,21.33,33.62,-27.1,22.08,35.63,-23.94,23.29,38.12,-19.77,24.34,39.38,-16.17
+6,7,6,20.97,33.13,-37.29,21.71,35.63,-33.58,23.04,37.52,-28.54,23.94,39.38,-24.31
+6,7,7,20.97,33.13,-47.31,21.74,35.63,-42.89,23.01,37.5,-37.11,23.76,38.69,-32.24
+7,1,1,32.47,-28.12,12.8,33.06,-28.51,13.3,33.96,-29.37,14.1,35.01,-30.63,14.88
+7,1,2,31.77,-26.88,3.75,32.81,-26.88,4.67,34.21,-28.12,6.31,35.63,-29.37,7.78
+7,1,3,32.22,-26.88,-6.32,33.42,-27.55,-4.6,35.24,-28.88,-2.04,36.92,-30.62,0.19
+7,1,4,31.73,-26.88,-16.51,32.84,-26.88,-14.1,34.88,-28.75,-10.76,36.55,-30.62,-7.98
+7,1,5,32.29,-26.88,-26.54,33.58,-26.88,-23.56,35.48,-28.12,-19.26,37.29,-29.37,-15.77
+7,1,6,31.7,-26.88,-36.89,33.17,-26.88,-32.88,34.95,-28.12,-27.83,36.44,-29.37,-23.75
+7,1,7,31.61,-26.88,-46.76,33.03,-26.25,-42.3,34.7,-26.88,-36.18,35.81,-28.12,-31.36
+7,2,1,32.69,-19,12.77,33.43,-19.37,13.21,34.48,-19.76,13.89,35.54,-20.33,14.5
+7,2,2,31.36,-16.88,3.71,32.47,-16.88,4.64,33.99,-17.68,6.11,35.47,-18.12,7.27
+7,2,3,32.59,-16.88,-6.32,33.76,-16.88,-4.75,35.62,-17.5,-2.23,37.48,-18.62,-0.19
+7,2,4,31.36,-16.88,-16.51,32.68,-16.88,-14.18,34.67,-18.12,-10.91,36.37,-18.82,-8.05
+7,2,5,32.1,-16.88,-26.53,33.4,-16.32,-23.5,35.47,-16.88,-19.14,37.29,-18.12,-15.77
+7,2,6,31.73,-16.88,-36.92,33.03,-16.25,-33.03,34.88,-16.88,-27.83,36.54,-17.69,-23.57
+7,2,7,31.54,-16.88,-46.83,32.76,-16.13,-42.39,34.33,-16.45,-36.19,35.41,-16.88,-31.39
+7,3,1,32.46,-7.7,12.8,33.17,-8.13,13.02,34.14,-8.13,13.92,35.32,-8.13,14.47
+7,3,2,31.73,-6.88,3.53,32.81,-6.88,4.61,34.37,-6.88,5.97,36.18,-6.88,7.24
+7,3,3,32.28,-6.32,-6.43,33.58,-6.12,-4.84,35.59,-6.37,-2.43,37.41,-6.88,-0.38
+7,3,4,31.73,-6.88,-16.51,33.03,-6.18,-14.22,35.07,-6.88,-10.95,37.04,-6.88,-8.17
+7,3,5,31.67,-6.32,-26.72,32.99,-5.63,-23.52,35.11,-5.63,-19.33,36.92,-6.25,-15.96
+7,3,6,31.54,-6.88,-36.92,32.81,-5.63,-33.18,34.69,-5.63,-27.83,36.47,-6.37,-23.66
+7,3,7,31.36,-6.88,-46.94,32.69,-5.63,-42.45,34.33,-5.63,-36.37,35.41,-5.63,-31.39
+7,4,1,32.88,3.13,12.65,33.62,3.13,13.02,34.7,3.13,13.73,35.96,3.13,14.25
+7,4,2,31.32,3.57,3.49,32.44,3.92,4.31,33.96,4.38,5.75,35.47,4.38,6.9
+7,4,3,32.1,3.13,-6.49,33.21,4.38,-5.01,35.4,4.38,-2.63,37.29,4.38,-0.56
+7,4,4,31.33,3.13,-16.85,32.5,4.38,-14.32,34.58,4.38,-11.13,36.52,4.38,-8.38
+7,4,5,31.34,3.94,-26.87,32.66,5,-23.75,34.7,5.63,-19.48,36.55,5.63,-16.14
+7,4,6,31.69,3.13,-37.07,32.84,4.38,-33.21,34.84,4.93,-28.11,36.5,5.09,-23.95
+7,4,7,30.99,3.13,-46.94,32.44,4.38,-42.52,33.98,5.19,-36.59,35.07,5.19,-31.76
+7,5,1,32.35,12.74,12.59,33.03,13.13,12.92,34.21,13.13,13.6,35.25,13.66,14.28
+7,5,2,31.17,13.13,3.34,32.29,13.82,4.34,33.92,14.37,5.72,35.28,14.89,6.87
+7,5,3,32.13,14.37,-6.53,33.21,15.62,-5.01,35.25,15.62,-2.79,37.11,16.88,-0.74
+7,5,4,30.99,13.13,-16.88,32.36,14.37,-14.47,34.33,15.62,-11.32,36.02,16.24,-8.55
+7,5,5,31.7,13.65,-26.83,32.84,15.13,-23.75,34.88,16.25,-19.67,36.74,16.88,-16.14
+7,5,6,31.36,13.13,-37.11,32.47,14.37,-33.21,34.44,15.62,-28.21,36.03,16.27,-24.09
+7,5,7,31.24,13.13,-47.13,32.47,15.13,-42.68,34.23,16.01,-36.76,35.12,16.42,-32.07
+7,6,1,31.88,23.12,12.47,32.74,23.86,12.79,33.54,24.37,13.39,34.61,24.81,14.06
+7,6,2,30.96,23.12,3.19,31.98,24.37,4.08,33.21,25.63,5.75,34.72,26.88,6.9
+7,6,3,31.91,23.62,-6.87,33.15,24.91,-5,34.88,26.25,-2.6,36.63,27.38,-0.59
+7,6,4,31.36,23.12,-16.88,32.48,25.11,-14.5,34.44,26.88,-11.14,36.18,28.12,-8.35
+7,6,5,31.32,23.57,-26.94,32.44,25.63,-23.9,34.33,26.88,-19.48,35.96,28.12,-16.18
+7,6,6,31.36,23.12,-37.29,32.45,25.17,-33.53,34.33,26.88,-28.39,35.77,27.68,-24.28
+7,6,7,31.34,22.66,-47.26,32.47,24.37,-42.86,34.27,26.08,-36.9,35.44,26.88,-32.1
+7,7,1,32.1,34.38,12.25,32.84,35.2,12.79,33.65,36.35,13.62,34.6,37.29,14.44
+7,7,2,30.8,33.13,3.15,31.73,34.38,4.27,33.03,36.32,6.05,34.33,37.63,7.43
+7,7,3,31.73,34.38,-6.87,32.84,35.63,-5.01,34.36,37.68,-2.1,36,39.38,0
+7,7,4,30.58,33.13,-16.92,31.73,35.63,-14.29,33.36,37.44,-10.9,34.88,39.38,-8.16
+7,7,5,31.24,33.61,-27.17,32.29,35.63,-23.75,33.96,38.12,-19.48,35.47,39.38,-16.11
+7,7,6,31.21,33.13,-37.34,32.29,35.63,-33.4,34.14,37.43,-28.32,35.41,38.83,-24.18
+7,7,7,31.14,33.13,-47.36,32.36,35.63,-42.68,33.99,37.31,-36.95,35.1,38.12,-32.13
diff --git a/SurgSim/Physics/RenderTests/Fem3DMeshRenderTest.cpp b/SurgSim/Physics/RenderTests/Fem3DMeshRenderTest.cpp
new file mode 100644
index 0000000..6f03e57
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Fem3DMeshRenderTest.cpp
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h"
+#include "SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Math/OdeSolver.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+static std::shared_ptr<SurgSim::Framework::SceneElement> createFemSceneElement(
+ const std::string& name,
+ const std::string& filename,
+ SurgSim::Math::IntegrationScheme integrationScheme)
+{
+ // Create a SceneElement that bundles the pieces associated with the finite element model
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>(name);
+
+ // Add the Fem3d component
+ // Note that we only specify the filename that contains the full geometrical and physical description.
+ auto fem = std::make_shared<SurgSim::Physics::Fem3DRepresentation>("fem3d");
+ fem->setFilename(filename);
+ fem->setIntegrationScheme(integrationScheme);
+ sceneElement->addComponent(fem);
+
+ // Add the graphics mesh used to display the Fem3d
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgMeshRepresentation>("fem graphics");
+ graphics->setFilename(filename);
+ graphics->setDrawAsWireFrame(true);
+ sceneElement->addComponent(graphics);
+
+ // Create a behavior which transfers the position of the vertices in the FEM to locations in the triangle mesh
+ auto femToMesh =
+ std::make_shared<SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior>("physics to triangle mesh");
+ femToMesh->setSource(fem);
+ femToMesh->setTarget(graphics);
+ sceneElement->addComponent(femToMesh);
+
+ // The point-cloud for visualizing the nodes of the finite element model
+ auto pointCloud
+ = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("point cloud");
+ pointCloud->setColor(SurgSim::Math::Vector4d(0.2, 0.2, 1.0, 1.0));
+ pointCloud->setPointSize(3.0f);
+ pointCloud->setVisible(true);
+ sceneElement->addComponent(pointCloud);
+
+ // The behavior which transfers the position of the vertices in the FEM to locations in the point cloud
+ auto femToCloud = std::make_shared<SurgSim::Blocks::TransferPhysicsToPointCloudBehavior>("fem to point cloud");
+ femToCloud->setSource(fem);
+ femToCloud->setTarget(pointCloud);
+ sceneElement->addComponent(femToCloud);
+
+ return sceneElement;
+}
+
+TEST_F(RenderTests, SimulatedWoundRenderTest)
+{
+ runtime->getScene()->addSceneElement(createFemSceneElement("Fem",
+ "Geometry/wound_deformable.ply",
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER));
+
+ runTest(Vector3d(0.0, 0.0, 0.2), Vector3d::Zero(), 5000.0);
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/RenderTests/Fem3DvsTruthCubeRenderTest.cpp b/SurgSim/Physics/RenderTests/Fem3DvsTruthCubeRenderTest.cpp
new file mode 100644
index 0000000..5d620e9
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Fem3DvsTruthCubeRenderTest.cpp
@@ -0,0 +1,711 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <math.h>
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/PointCloudRepresentation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+
+#include "SurgSim/Framework/Logger.h"
+
+using SurgSim::DataStructures::Vertices;
+using SurgSim::Framework::SceneElement;
+using SurgSim::Graphics::PointCloud;
+using SurgSim::Graphics::PointCloudRepresentation;
+using SurgSim::Graphics::OsgAxesRepresentation;
+using SurgSim::Graphics::OsgPointCloudRepresentation;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Physics::PhysicsManager;
+
+using SurgSim::Framework::Logger;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+struct TruthCubeData
+{
+ // Beads positions in uncompressed material
+ std::vector<Vector3d> cubeData0;
+
+ // Beads positions under 5% strain
+ std::vector<Vector3d> cubeData1;
+
+ // Beads positions under 12.5% strain
+ std::vector<Vector3d> cubeData2;
+
+ // Beads positions under 18.25% strain
+ std::vector<Vector3d> cubeData3;
+};
+
+/// Parsing Truth Cube data from an external file
+/// \param truthCubeData a container of cube data for all strains
+/// \return True if the Truth Cube Data is successful loaded, otherwise false
+bool parseTruthCubeData(std::shared_ptr<TruthCubeData> truthCubeData)
+{
+ const double mm2m = 1000.0; // Conversion constant from millimeter to meter
+ const int numCommentLine = 7; // Number of comment lines to skip before accessing the actual data
+
+ // Position of uncompressed data, 5% strain, 12.5% strain, 18.25% strain
+ std::array<Vector3d, 4> position;
+ std::string line;
+ char comma;
+ int i, j, k;
+ int numLine = 0;
+
+ const SurgSim::Framework::ApplicationData data("config.txt");
+ std::string filename = data.findFile("uniaxial_positions.csv");
+ std::ifstream datafile(filename);
+
+ if (! datafile.good())
+ {
+ SURGSIM_LOG_WARNING(Logger::getDefaultLogger()) << "Could not get uniaxial_positions.csv data file";
+ return false;
+ }
+
+ while (std::getline(datafile, line))
+ {
+ if (++numLine > numCommentLine)
+ {
+ std::stringstream strstream(line);
+ strstream >> i >> comma >> j >> comma >> k >> comma
+ >> position[0].x() >> comma >> position[0].y() >> comma >> position[0].z() >> comma
+ >> position[1].x() >> comma >> position[1].y() >> comma >> position[1].z() >> comma
+ >> position[2].x() >> comma >> position[2].y() >> comma >> position[2].z() >> comma
+ >> position[3].x() >> comma >> position[3].y() >> comma >> position[3].z();
+
+ // Store strains separately, in meter unit and Y up (rotation 90 alond X)
+ // Apparently, -Z seems to be up in the truth cube data set
+ Quaterniond::AngleAxisType aa(M_PI / 2.0, Vector3d::UnitX());
+ RigidTransform3d pose(aa);
+ truthCubeData->cubeData0.push_back(pose * (position[0] / mm2m));
+ truthCubeData->cubeData1.push_back(pose * (position[1] / mm2m));
+ truthCubeData->cubeData2.push_back(pose * (position[2] / mm2m));
+ truthCubeData->cubeData3.push_back(pose * (position[3] / mm2m));
+ }
+ }
+ return true;
+};
+
+/// Search the node in the state that is the closest to a given 3d point
+/// This is necessary because the structure of the nodes in the state and in the truth cube is not necessarily matching
+/// The state is built with a structure aligned on +X +Y +Z, while the truth cube data are defined along +X +Y -Z and
+/// rotated (PI/2 along X) to match Y up, so 3d indices don't match.
+size_t searchForClosestNodeInState(std::shared_ptr<SurgSim::Math::OdeState> state, Vector3d p)
+{
+ size_t res = -1;
+ double minDistance = std::numeric_limits<double>::max();
+
+ SurgSim::Math::Vector& x = state->getPositions();
+ for (size_t nodeId = 0; nodeId < state->getNumNodes(); ++nodeId)
+ {
+ Vector3d diff(SurgSim::Math::getSubVector(x, nodeId, 3) - p);
+ if (diff.norm() < minDistance)
+ {
+ minDistance = diff.norm();
+ res = nodeId;
+ }
+ }
+ return res;
+}
+
+/// Truth cube representation (extension of a Fem3DRepresentation)
+/// Defines a subdivided initial cube with cube FemElements
+class TruthCubeRepresentation : public Fem3DRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name of the truth cube representation.
+ /// \param corners The 8 corners of the truth cube
+ TruthCubeRepresentation(const std::string& name, std::array<SurgSim::Math::Vector3d, 8> corners) :
+ Fem3DRepresentation(name)
+ {
+ m_numNodesPerAxis = 9;
+ m_cubeNodes = corners;
+ }
+
+ /// Convert an node index from a 3d indexing to a 1d indexing
+ /// \param i, j, k Indices along the X, Y and Z axis
+ /// \return Unique index of the corresponding point (to access a linear array for example)
+ size_t get1DIndexFrom3D(size_t i, size_t j, size_t k)
+ {
+ return m_numNodesPerAxis * m_numNodesPerAxis * i + m_numNodesPerAxis * j + k;
+ }
+
+ /// Convert a node index from a 1d indexing to a 3d indexing
+ /// \param index of the node
+ /// \param[in,out] i, j, k Corresponding indices along the X, Y and Z axis
+ /// \param numNodesPerAxis the number of nodes per axis to be considered in the conversion
+ void get3DIndexFrom1D(size_t index, size_t* i, size_t* j, size_t* k, size_t numNodesPerAxis)
+ {
+ size_t remainingIndex = index;
+
+ (*i) = remainingIndex / (numNodesPerAxis * numNodesPerAxis);
+ remainingIndex -= (*i) * (numNodesPerAxis * numNodesPerAxis);
+
+ (*j) = remainingIndex / numNodesPerAxis;
+ remainingIndex -= (*j) * numNodesPerAxis;
+
+ (*k) = remainingIndex;
+ }
+
+ /// Gets the number of nodes per axis
+ /// \return The number of nodes per axis.
+ size_t getNumNodesPerAxis()
+ {
+ return m_numNodesPerAxis;
+ }
+
+ /// Gets the boundary conditions
+ /// \return vector of dof (degrees of freedom) indices representing all boundary conditions
+ std::vector<size_t> getBoundaryConditions()
+ {
+ return m_boundaryConditions;
+ }
+
+ /// Gets the boundary conditions displacement values
+ /// \return The vector of displacement for all boundary conditions
+ std::vector<double> getBoundaryConditionsDisplacement()
+ {
+ return m_boundaryConditionsDisplacement;
+ }
+
+ /// Fills up a given state with the truth cube nodes
+ /// border nodes and internal nodes (i.e. the beads)
+ /// \param[in,out] state The state to be filled up
+ void fillUpDeformableState(std::shared_ptr<SurgSim::Math::OdeState> state)
+ {
+ state->setNumDof(getNumDofPerNode(), m_numNodesPerAxis * m_numNodesPerAxis * m_numNodesPerAxis);
+ SurgSim::Math::Vector& nodePositions = state->getPositions();
+
+ for (int i = 0; i < m_numNodesPerAxis; i++)
+ {
+ // For a given index i, we intersect the cube with a (Y Z) plane, which defines a square on a (Y Z) plane
+ Vector3d extremitiesX0[4] = {m_cubeNodes[0], m_cubeNodes[2], m_cubeNodes[4], m_cubeNodes[6]};
+ Vector3d extremitiesX1[4] = {m_cubeNodes[1], m_cubeNodes[3], m_cubeNodes[5], m_cubeNodes[7]};
+ Vector3d extremitiesXi[4];
+ double coefI = static_cast<double>(i) / (static_cast<double>(m_numNodesPerAxis) - 1.0);
+
+ for (int index = 0; index < 4; index++)
+ {
+ extremitiesXi[index] =
+ extremitiesX0[index] * (1.0 - coefI) +
+ extremitiesX1[index] * coefI;
+ }
+
+ for (int j = 0; j < m_numNodesPerAxis; j++)
+ {
+ // For a given index j, we intersect the square with a (X Z) plane, which defines a line along (Z)
+ Vector3d extremitiesY0[2] = {extremitiesXi[0], extremitiesXi[2]};
+ Vector3d extremitiesY1[2] = {extremitiesXi[1], extremitiesXi[3]};
+ Vector3d extremitiesYi[2];
+ double coefJ = static_cast<double>(j) / (static_cast<double>(m_numNodesPerAxis) - 1.0);
+
+ for (int index = 0; index < 2; index++)
+ {
+ extremitiesYi[index] =
+ extremitiesY0[index] * (1.0 - coefJ) +
+ extremitiesY1[index] * coefJ;
+ }
+
+ for (int k=0; k < m_numNodesPerAxis; k++)
+ {
+ // For a given index k, we intersect the line with a (X Y) plane, which defines a 3d point
+ double coefK = static_cast<double>(k) / (static_cast<double>(m_numNodesPerAxis) - 1.0);
+ Vector3d position3d = extremitiesYi[0] * (1.0 - coefK) + extremitiesYi[1] * coefK;
+ SurgSim::Math::setSubVector(position3d, get1DIndexFrom3D(i, j, k), 3, &nodePositions);
+ }
+ }
+ }
+ }
+
+ /// Adjust the internal nodes position of the state to match the beads initial position
+ /// \param state The state to adjust
+ /// \param beadsInitialPositions The vector of beads initial position
+ /// \note The ordering between the state and the beads vector might be different
+ /// \note The algorithm will not rely on a matching indexing
+ void adjustInitialBeadsPosition(std::shared_ptr<SurgSim::Math::OdeState> state,
+ std::vector<Vector3d> beadsInitialPositions)
+ {
+ SurgSim::Math::Vector& x = state->getPositions();
+
+ for (size_t index = 0; index < beadsInitialPositions.size(); ++index)
+ {
+ size_t nodeId = searchForClosestNodeInState(state, beadsInitialPositions[index]);
+ SurgSim::Math::setSubVector(beadsInitialPositions[index], nodeId, 3, &x);
+ }
+ }
+
+ /// Defines the boundary conditions for the truth cube
+ /// \param displacementForTopLayer The displacement of the boundary conditions for the top layer
+ /// \note The bottom layer is completely fixed (all nodes, all dof)
+ /// \note The top layer is completely fixed and compressed along Y
+ void defineBoundaryCondition(double displacementForTopLayer)
+ {
+ for (int i = 0; i < m_numNodesPerAxis; i++)
+ {
+ for (int k = 0; k < m_numNodesPerAxis; k++)
+ {
+ // Add boundary condition for bottom layer (j = 0)
+ int nodeId = get1DIndexFrom3D(i, 0, k);
+ m_boundaryConditions.push_back(nodeId * 3 + 0);
+ m_boundaryConditions.push_back(nodeId * 3 + 1);
+ m_boundaryConditions.push_back(nodeId * 3 + 2);
+ m_boundaryConditionsDisplacement.push_back(0.0);
+ m_boundaryConditionsDisplacement.push_back(0.0);
+ m_boundaryConditionsDisplacement.push_back(0.0);
+
+ // Add boundary condition for top layer (j = m_numNodesPerAxis - 1)
+ nodeId = get1DIndexFrom3D(i, m_numNodesPerAxis - 1, k);
+ m_boundaryConditions.push_back(nodeId * 3 + 0);
+ m_boundaryConditions.push_back(nodeId * 3 + 1);
+ m_boundaryConditions.push_back(nodeId * 3 + 2);
+ m_boundaryConditionsDisplacement.push_back(0.0);
+ m_boundaryConditionsDisplacement.push_back(displacementForTopLayer);
+ m_boundaryConditionsDisplacement.push_back(0.0);
+ }
+ }
+ }
+
+ /// Adds the Fem3D elements of small cubes
+ /// \param state The state for initialization.
+ void addFemCubes(std::shared_ptr<SurgSim::Math::OdeState> state)
+ {
+ for (size_t i = 0; i < static_cast<size_t>(m_numNodesPerAxis - 1); i++)
+ {
+ for (size_t j = 0; j < static_cast<size_t>(m_numNodesPerAxis - 1); j++)
+ {
+ for (size_t k = 0; k < static_cast<size_t>(m_numNodesPerAxis - 1); k++)
+ {
+ std::array<size_t, 8> cubeNodeIds;
+ cubeNodeIds[0] = get1DIndexFrom3D(i , j , k );
+ cubeNodeIds[1] = get1DIndexFrom3D(i+1, j , k );
+ cubeNodeIds[2] = get1DIndexFrom3D(i , j+1, k );
+ cubeNodeIds[3] = get1DIndexFrom3D(i+1, j+1, k );
+ cubeNodeIds[4] = get1DIndexFrom3D(i , j , k+1);
+ cubeNodeIds[5] = get1DIndexFrom3D(i+1, j , k+1);
+ cubeNodeIds[6] = get1DIndexFrom3D(i , j+1, k+1);
+ cubeNodeIds[7] = get1DIndexFrom3D(i+1, j+1, k+1);
+
+ std::array<size_t, 8> cube = {cubeNodeIds[0], cubeNodeIds[1], cubeNodeIds[3], cubeNodeIds[2],
+ cubeNodeIds[4], cubeNodeIds[5], cubeNodeIds[7], cubeNodeIds[6]};
+
+ // Add Fem3DElementCube for each cube
+ std::shared_ptr<Fem3DElementCube> femElement = std::make_shared<Fem3DElementCube>(cube);
+ femElement->setMassDensity(980.0); // 0.98 g/cm^-3 (2-part silicone rubber a.k.a. RTV6166)
+ femElement->setPoissonRatio(0.499); // From the paper (near 0.5)
+ femElement->setYoungModulus(15.3e3); // 15.3 kPa (From the paper)
+ femElement->initialize(*state);
+ addFemElement(femElement);
+ }
+ }
+ }
+ }
+
+ /// Update the current state based on some offset resulting from compressing the cube
+ /// \param offset to apply.
+ void applyDofCorrection(const SurgSim::Math::Vector& offset)
+ {
+ m_currentState->getPositions() += offset;
+ }
+
+ /// Get beads of the truth cube
+ /// \return coordinate of beads
+ /// \note The beads are all the internal nodes of the cube
+ std::vector<std::vector<std::vector<Vector3d>>> getBeadsLocation()
+ {
+ std::vector<std::vector<std::vector<Vector3d>>> beadsLocation;
+ auto positions = m_currentState->getPositions();
+
+ beadsLocation.resize(m_numNodesPerAxis - 2);
+ for (int i = 1; i < m_numNodesPerAxis - 1; i++)
+ {
+ beadsLocation[i-1].resize(m_numNodesPerAxis - 2);
+ for (int j = 1; j < m_numNodesPerAxis - 1; j++)
+ {
+ beadsLocation[i-1][j-1].resize(m_numNodesPerAxis - 2);
+ for (int k = 1; k < m_numNodesPerAxis - 1; k++)
+ {
+ beadsLocation[i-1][j-1][k-1] = SurgSim::Math::getSubVector(positions, get1DIndexFrom3D(i, j, k), 3);
+ }
+ }
+ }
+ return beadsLocation;
+ }
+
+private:
+ typedef std::array<SurgSim::Math::Vector3d, 8> CubeNodesType;
+
+ // Number of point per dimensions
+ int m_numNodesPerAxis;
+
+ /// Boundary condition (list of degrees of freedom to be fixed)
+ std::vector<size_t> m_boundaryConditions;
+
+ /// Boundary condition displacement (displacement to apply for each boundary condition)
+ std::vector<double> m_boundaryConditionsDisplacement;
+
+ // Nodes of the original truth cube
+ CubeNodesType m_cubeNodes;
+};
+
+
+/// Build the constrained system for a given truth cube representation
+/// \param truthCubeRepresentation The Fem3D representation of the truth cube
+/// \param[out] A the system matrix of size (numDof + numConstraint x numDof + numConstraint)
+/// \param[out] B the system RHS vector of size (numDof + numConstraint)
+/// \note Each row of the matrix H aims at fixing a node as a boundary condition.
+/// \note For example, the constraint equation to fix the dof id 1 is in place (no displacement) is:
+/// \note H(0 1 0 0...0).U(u0 u1 u2 u3...un) = 0 (or desired displacement)
+void buildConstrainedSystem(std::shared_ptr<TruthCubeRepresentation> truthCubeRepresentation,
+ SurgSim::Math::Matrix& A, SurgSim::Math::Vector& B)
+{
+ // The static system with constraints is defined as follow:
+ // (K H^T).(U ) = (F)
+ // (H 0) (-lambda) (E)
+
+ size_t numConstraints = truthCubeRepresentation->getBoundaryConditions().size();
+ size_t numDof = truthCubeRepresentation->getNumDof();
+ A.resize(numDof + numConstraints, numDof + numConstraints);
+ B.resize(numDof + numConstraints);
+ SurgSim::Math::Matrix H(numConstraints, truthCubeRepresentation->getNumDof());
+ H.setZero();
+ B.setZero();
+
+ // Build the temporary constraint matrix H along with the RHS vector B
+ for (size_t i = 0; i < numConstraints; i++)
+ {
+ H(i, truthCubeRepresentation->getBoundaryConditions()[i]) = 1.0;
+ B[numDof + i] = truthCubeRepresentation->getBoundaryConditionsDisplacement()[i];
+ }
+
+ A.setZero();
+ // Copy K into A
+ A.block(0,0, numDof, numDof) = truthCubeRepresentation->computeK(*truthCubeRepresentation->getInitialState());
+ // Copy H into A
+ A.block(numDof,0, numConstraints, numDof) = H;
+ //Copy H^T into A
+ A.block(0, numDof, numDof, numConstraints) = H.transpose();
+}
+
+/// Using static solver to find the displacement of truth cube
+/// \param truthCubeRepresentation The Fem3D representation of truth cube
+/// \return the vector of displacement for each dof (degree of freedom)
+SurgSim::Math::Vector staticSolver(std::shared_ptr<TruthCubeRepresentation> truthCubeRepresentation)
+{
+ int numDof = truthCubeRepresentation->getNumDof();
+
+ // Build the constrained system A.X = B
+ SurgSim::Math::Matrix A;
+ SurgSim::Math::Vector B;
+ buildConstrainedSystem(truthCubeRepresentation, A, B);
+
+ // Solve the constrained system A.X = B
+ SurgSim::Math::Vector X = A.inverse() * B;
+
+ // Extract the dof displacement vector from the solution X
+ return X.segment(0, numDof);
+}
+
+/// Simulate the truth cube statically with the boundary conditions applied
+/// \param truthCubeData The truth cube data (this is only useful to adjust the initial data)
+/// \param truthCubeRepresentation The truth cube representation
+/// \param displacement The displacement to apply along Y on the top layer (compression in meter)
+void doSimulation(std::shared_ptr<TruthCubeData> truthCubeData,
+ std::shared_ptr<TruthCubeRepresentation> truthCubeRepresentation,
+ double displacement)
+{
+ // Create initial state
+ // Note that the boundary conditions are NOT defined in this test in the state itself
+ // This would simply modify the global stiffness matrix, which is not sufficient for this test
+ // We prefer to keep the boundary conditions in a separate structure
+ // (internal to TruthCubeRepresentation and apply the Lagrange multiplier technique to solve them).
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ truthCubeRepresentation->fillUpDeformableState(initialState);
+ truthCubeRepresentation->adjustInitialBeadsPosition(initialState, truthCubeData->cubeData0);
+ truthCubeRepresentation->setInitialState(initialState);
+
+ // Create Fem3d cubes from the subdivision cubes
+ truthCubeRepresentation->addFemCubes(initialState);
+
+ // Setup boundary conditions and displacement
+ truthCubeRepresentation->defineBoundaryCondition(displacement);
+
+ // Wake Up the Representation
+ truthCubeRepresentation->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ truthCubeRepresentation->wakeUp();
+
+ // Call staticSolver to find the offset values
+ SurgSim::Math::Vector offset = staticSolver(truthCubeRepresentation);
+
+ // Apply the correction to the simulated truth cube current state
+ truthCubeRepresentation->applyDofCorrection(offset);
+}
+
+/// Copy simulation beads data into point cloud
+/// \param truthCubeRepresentation The simulated truth cube
+/// \param representation The representation of point cloud
+void copySimulationBeadsIntoPointCloud(std::shared_ptr<TruthCubeRepresentation> truthCubeRepresentation,
+ std::shared_ptr<SurgSim::Graphics::OsgPointCloudRepresentation> representation)
+{
+ std::vector<std::vector<std::vector<Vector3d>>> beads = truthCubeRepresentation->getBeadsLocation();
+ auto pointCloud = representation->getVertices();
+
+ // Add deform to pointCloud
+ for (size_t i = 0; i < beads.size(); i++)
+ {
+ for (size_t j = 0; j < beads[i].size(); j++)
+ {
+ for (size_t k = 0; k < beads[i][j].size(); k++)
+ {
+ pointCloud->addVertex(PointCloud::VertexType(beads[i][j][k]));
+ }
+ }
+ }
+}
+
+// Copy experimental beads data into point cloud
+/// \param truthCube The experimental data for the truth cube
+/// \param representation The representation of point cloud
+void copyExperimentalBeadsIntoPointCloud(std::vector<SurgSim::Math::Vector3d> truthCube,
+ std::shared_ptr<SurgSim::Graphics::OsgPointCloudRepresentation> representation)
+{
+ auto pointCloudCompressed = representation->getVertices();
+
+ /// Loading the Truth Cube data into point cloud
+ for (size_t i = 0; i < truthCube.size(); ++i)
+ {
+ pointCloudCompressed->addVertex(PointCloud::VertexType(truthCube[i]));
+ }
+}
+
+/// Simple error analysis
+/// \param cubeData The vector of all experimental beads 3d position
+/// \param state The state of the simulated truth cube
+/// \return The maximum error measured on each bead (in meter)
+double analyzeError(std::vector<Vector3d> cubeData, std::shared_ptr<SurgSim::Math::OdeState> state)
+{
+ double maxError = 0.0;
+ for (size_t cubeDataNodeId = 0; cubeDataNodeId < cubeData.size(); ++cubeDataNodeId)
+ {
+ size_t stateNodeId = searchForClosestNodeInState(state, cubeData[cubeDataNodeId]);
+ auto diff = cubeData[cubeDataNodeId] - state->getPosition(stateNodeId);
+ double error = diff.norm();
+ if (error > maxError)
+ {
+ maxError = error;
+ }
+ }
+ return maxError;
+}
+
+struct Fem3DVSTruthCubeRenderTests : public RenderTests
+{
+ void addComponent(std::shared_ptr<SurgSim::Framework::Component> component)
+ {
+ viewElement->addComponent(component);
+ }
+
+ virtual void SetUp() override
+ {
+ RenderTests::SetUp();
+
+ // Load truth cube data
+ truthCubeData = std::make_shared<TruthCubeData>();
+ parseTruthCubeData(truthCubeData);
+
+ // Compute the center point of the cube
+ SurgSim::Math::Vector3d center = SurgSim::Math::Vector3d::Zero();
+ for (size_t nodeId = 0; nodeId < truthCubeData->cubeData0.size(); ++nodeId)
+ {
+ center += truthCubeData->cubeData0[nodeId];
+ }
+ center /= truthCubeData->cubeData0.size();
+
+ // Compute the cube's corners for the Fem3d simulation
+ double halfLength = 0.04;
+ Vector3d X = Vector3d::UnitX();
+ Vector3d Y = Vector3d::UnitY();
+ Vector3d Z = Vector3d::UnitZ();
+ cubeCorners[0] = center - halfLength * X - halfLength * Y - halfLength * Z;
+ cubeCorners[1] = center + halfLength * X - halfLength * Y - halfLength * Z;
+ cubeCorners[2] = center - halfLength * X + halfLength * Y - halfLength * Z;
+ cubeCorners[3] = center + halfLength * X + halfLength * Y - halfLength * Z;
+ cubeCorners[4] = center - halfLength * X - halfLength * Y + halfLength * Z;
+ cubeCorners[5] = center + halfLength * X - halfLength * Y + halfLength * Z;
+ cubeCorners[6] = center - halfLength * X + halfLength * Y + halfLength * Z;
+ cubeCorners[7] = center + halfLength * X + halfLength * Y + halfLength * Z;
+ }
+
+ /// The truth cube data set
+ std::shared_ptr<TruthCubeData> truthCubeData;
+
+ /// The 8 corners of the cube defining the truth cube (useful to setup the Fem3D)
+ std::array<Vector3d, 8> cubeCorners;
+};
+
+TEST_F(Fem3DVSTruthCubeRenderTests, rawDataTest)
+{
+ // Load the truth cube data
+ std::shared_ptr<TruthCubeData> truthCubeData = std::make_shared<TruthCubeData>();
+ parseTruthCubeData(truthCubeData);
+
+ auto points0 = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("Data0");
+ points0->setPointSize(4.0);
+ points0->setColor(Vector4d(1.0, 1.0, 1.0, 1.0));
+ copyExperimentalBeadsIntoPointCloud(truthCubeData->cubeData0, points0);
+ addComponent(points0);
+
+ auto points1 = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("Data1");
+ points1->setPointSize(4.0);
+ points1->setColor(Vector4d(1.0, 0.0, 0.0, 1.0));
+ copyExperimentalBeadsIntoPointCloud(truthCubeData->cubeData1, points1);
+ addComponent(points1);
+
+ auto points2 = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("Data2");
+ points2->setPointSize(4.0);
+ points2->setColor(Vector4d(0.0, 1.0, 0.0, 1.0));
+ copyExperimentalBeadsIntoPointCloud(truthCubeData->cubeData2, points2);
+ addComponent(points2);
+
+ auto points3 = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("Data3");
+ points3->setPointSize(4.0);
+ points3->setColor(Vector4d(0.0, 0.0, 1.0, 1.0));
+ copyExperimentalBeadsIntoPointCloud(truthCubeData->cubeData3, points3);
+ addComponent(points3);
+
+ runTest(Vector3d(0.0, 0.0, 0.2), Vector3d::Zero(), 3000.0);
+}
+
+/// Simulate truth cube with 5% strain (4mm of displacement).
+TEST_F(Fem3DVSTruthCubeRenderTests, Test5percentsStrain)
+{
+ /// Displacement of the top layer for this setup
+ double displacement = -0.004;
+
+ // Run the simulation
+ std::shared_ptr<TruthCubeRepresentation> truthCubeRepresentation;
+ truthCubeRepresentation = std::make_shared<TruthCubeRepresentation> ("TruthCube", cubeCorners);
+ doSimulation(truthCubeData, truthCubeRepresentation, displacement);
+
+ std::shared_ptr<SurgSim::Graphics::OsgPointCloudRepresentation> simulatedPoints;
+ simulatedPoints = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("SimulatedPoints");
+ copySimulationBeadsIntoPointCloud(truthCubeRepresentation, simulatedPoints);
+ simulatedPoints->setPointSize(4.0);
+ simulatedPoints->setColor(Vector4d(1.0, 0.0, 0.0, 1.0));
+ addComponent(simulatedPoints);
+
+ std::shared_ptr<SurgSim::Graphics::OsgPointCloudRepresentation> experimentalpoints;
+ experimentalpoints = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("ExperimentalPoints");
+ copyExperimentalBeadsIntoPointCloud(truthCubeData->cubeData1, experimentalpoints);
+ experimentalpoints->setPointSize(4.0);
+ experimentalpoints->setColor(Vector4d(1.0, 1.0, 1.0, 1.0));
+ addComponent(experimentalpoints);
+
+ double maxError = analyzeError(truthCubeData->cubeData1, truthCubeRepresentation->getCurrentState());
+ std::cout << "The maximum error between simulated and experimental setup is " << maxError << " m" << std::endl;
+
+ /// Run the thread
+ runTest(Vector3d(0.0, 0.0, 0.2), Vector3d::Zero(), 3000.0);
+}
+
+/// Simulate truth cube with 12.5% strain (10mm of displacement).
+TEST_F(Fem3DVSTruthCubeRenderTests, Test12percentsAndHalfStrain)
+{
+ /// Displacement of the top layer for this setup
+ double displacement = -0.010;
+
+ // Run the simulation
+ std::shared_ptr<TruthCubeRepresentation> truthCubeRepresentation;
+ truthCubeRepresentation = std::make_shared<TruthCubeRepresentation> ("TruthCube", cubeCorners);
+ doSimulation(truthCubeData, truthCubeRepresentation, displacement);
+
+ std::shared_ptr<SurgSim::Graphics::OsgPointCloudRepresentation> simulatedPoints;
+ simulatedPoints = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("SimulatedPoints");
+ copySimulationBeadsIntoPointCloud(truthCubeRepresentation, simulatedPoints);
+ simulatedPoints->setPointSize(4.0);
+ simulatedPoints->setColor(Vector4d(1.0, 0.0, 0.0, 1.0));
+ addComponent(simulatedPoints);
+
+ std::shared_ptr<SurgSim::Graphics::OsgPointCloudRepresentation> experimentalpoints;
+ experimentalpoints = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("ExperimentalPoints");
+ copyExperimentalBeadsIntoPointCloud(truthCubeData->cubeData2, experimentalpoints);
+ experimentalpoints->setPointSize(4.0);
+ experimentalpoints->setColor(Vector4d(1.0, 1.0, 1.0, 1.0));
+ addComponent(experimentalpoints);
+
+ double maxError = analyzeError(truthCubeData->cubeData2, truthCubeRepresentation->getCurrentState());
+ std::cout << "The maximum error between simulated and experimental setup is " << maxError << " m" << std::endl;
+
+ /// Run the thread
+ runTest(Vector3d(0.0, 0.0, 0.2), Vector3d::Zero(), 3000.0);
+}
+
+/// Simulate truth cube with 18.25% strain (14.6mm of displacement).
+TEST_F(Fem3DVSTruthCubeRenderTests, Test18percentsAndQuarterStrain)
+{
+ /// Displacement of the top layer for this setup
+ double displacement = -0.0146;
+
+ // Run the simulation
+ std::shared_ptr<TruthCubeRepresentation> truthCubeRepresentation;
+ truthCubeRepresentation = std::make_shared<TruthCubeRepresentation> ("TruthCube", cubeCorners);
+ doSimulation(truthCubeData, truthCubeRepresentation, displacement);
+
+ std::shared_ptr<SurgSim::Graphics::OsgPointCloudRepresentation> simulatedPoints;
+ simulatedPoints = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("SimulatedPoints");
+ copySimulationBeadsIntoPointCloud(truthCubeRepresentation, simulatedPoints);
+ simulatedPoints->setPointSize(4.0);
+ simulatedPoints->setColor(Vector4d(1.0, 0.0, 0.0, 1.0));
+ addComponent(simulatedPoints);
+
+ std::shared_ptr<SurgSim::Graphics::OsgPointCloudRepresentation> experimentalpoints;
+ experimentalpoints = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("ExperimentalPoints");
+ copyExperimentalBeadsIntoPointCloud(truthCubeData->cubeData3, experimentalpoints);
+ experimentalpoints->setPointSize(4.0);
+ experimentalpoints->setColor(Vector4d(1.0, 1.0, 1.0, 1.0));
+ addComponent(experimentalpoints);
+
+ double maxError = analyzeError(truthCubeData->cubeData3, truthCubeRepresentation->getCurrentState());
+ std::cout << "The maximum error between simulated and experimental setup is " << maxError << " m" << std::endl;
+
+ /// Run the thread
+ runTest(Vector3d(0.0, 0.0, 0.2), Vector3d::Zero(), 3000.0);
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RenderTests/RenderTest.cpp b/SurgSim/Physics/RenderTests/RenderTest.cpp
new file mode 100644
index 0000000..c73e523
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/RenderTest.cpp
@@ -0,0 +1,67 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+void RenderTests::SetUp()
+{
+ runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+
+ graphicsManager = std::make_shared<SurgSim::Graphics::OsgManager>();
+ runtime->addManager(graphicsManager);
+
+ physicsManager = std::make_shared<SurgSim::Physics::PhysicsManager>();
+ runtime->addManager(physicsManager);
+
+ behaviorManager = std::make_shared<SurgSim::Framework::BehaviorManager>();
+ runtime->addManager(behaviorManager);
+
+ scene = runtime->getScene();
+
+ viewElement = std::make_shared<SurgSim::Graphics::OsgViewElement>("Physics Render Scene");
+ scene->addSceneElement(viewElement);
+}
+
+void RenderTests::TearDown()
+{
+ runtime->stop();
+}
+
+void RenderTests::runTest(const SurgSim::Math::Vector3d& cameraPosition, const SurgSim::Math::Vector3d& cameraLookAt,
+ double miliseconds)
+{
+ using SurgSim::Graphics::OsgAxesRepresentation;
+
+ viewElement->enableManipulator(true);
+ viewElement->setManipulatorParameters(cameraPosition, cameraLookAt);
+
+ std::shared_ptr<OsgAxesRepresentation> axes = std::make_shared<OsgAxesRepresentation>("axes");
+ axes->setSize(1.0);
+ viewElement->addComponent(axes);
+
+ /// Run the thread
+ runtime->start();
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(miliseconds));
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RenderTests/RenderTest.h b/SurgSim/Physics/RenderTests/RenderTest.h
new file mode 100644
index 0000000..6c1ab44
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/RenderTest.h
@@ -0,0 +1,56 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_RENDERTESTS_RENDERTEST_H
+#define SURGSIM_PHYSICS_RENDERTESTS_RENDERTEST_H
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+struct RenderTests : public ::testing::Test
+{
+public:
+
+ virtual void SetUp();
+
+ virtual void TearDown();
+
+ virtual void runTest(const SurgSim::Math::Vector3d& cameraPosition,
+ const SurgSim::Math::Vector3d& cameraLookAt, double miliseconds);
+
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime;
+ std::shared_ptr<SurgSim::Graphics::OsgManager> graphicsManager;
+ std::shared_ptr<SurgSim::Physics::PhysicsManager> physicsManager;
+ std::shared_ptr<SurgSim::Framework::BehaviorManager> behaviorManager;
+ std::shared_ptr<SurgSim::Framework::Scene> scene;
+ std::shared_ptr<SurgSim::Graphics::OsgViewElement> viewElement;
+};
+
+}; // namespace Physics
+}; // namespace SurgSim
+
+#endif //SURGSIM_PHYSICS_RENDERTESTS_RENDERTEST_H
diff --git a/SurgSim/Physics/RenderTests/RenderTestFem1D.cpp b/SurgSim/Physics/RenderTests/RenderTestFem1D.cpp
new file mode 100644
index 0000000..62c9d37
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/RenderTestFem1D.cpp
@@ -0,0 +1,163 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 RenderTestFem1D.cpp render test for Fem1D
+
+#include <memory>
+
+#include "SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+
+using SurgSim::Blocks::TransferPhysicsToPointCloudBehavior;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Graphics::OsgPointCloudRepresentation;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::Fem1DRepresentation;
+using SurgSim::Physics::Fem1DElementBeam;
+
+namespace
+{
+
+void loadModelFem1D(std::shared_ptr<Fem1DRepresentation> physicsRepresentation, size_t numNodes)
+{
+ std::shared_ptr<SurgSim::Math::OdeState> restState = std::make_shared<SurgSim::Math::OdeState>();
+ restState->setNumDof(physicsRepresentation->getNumDofPerNode(), numNodes);
+
+ // Sets the initial state (node positions and boundary conditions)
+ SurgSim::Math::Vector& x = restState->getPositions();
+ for (size_t nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ SurgSim::Math::getSubVector(x, nodeId, physicsRepresentation->getNumDofPerNode()).segment<3>(0)
+ = Vector3d(static_cast<double>(nodeId) / static_cast<double>(numNodes - 1) - 0.5, 0.0, 0.0);
+ }
+
+ // Fix the start and end nodes
+ restState->addBoundaryCondition(0, 0);
+ restState->addBoundaryCondition(0, 1);
+ restState->addBoundaryCondition(0, 2);
+ restState->addBoundaryCondition(numNodes - 1, 0);
+ restState->addBoundaryCondition(numNodes - 1, 1);
+ restState->addBoundaryCondition(numNodes - 1, 2);
+
+ physicsRepresentation->setInitialState(restState);
+
+ // Adds all the FemElements
+ for (size_t beamId = 0; beamId < numNodes - 1; beamId++)
+ {
+ std::array<size_t, 2> beamNodeIds = {{beamId, beamId + 1}};
+ std::shared_ptr<Fem1DElementBeam> beam = std::make_shared<Fem1DElementBeam>(beamNodeIds);
+ beam->setRadius(0.10);
+ beam->setMassDensity(3000.0);
+ beam->setPoissonRatio(0.45);
+ beam->setYoungModulus(1e6);
+ physicsRepresentation->addFemElement(beam);
+ }
+}
+
+// Generates a 1d fem comprised of adjacent elements along a straight line. The number of fem elements is determined
+// by loadModelFem1D.
+std::shared_ptr<SurgSim::Framework::SceneElement> createFem1D(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& gfxPose,
+ const SurgSim::Math::Vector4d& color,
+ SurgSim::Math::IntegrationScheme integrationScheme)
+{
+ std::shared_ptr<Fem1DRepresentation> physicsRepresentation
+ = std::make_shared<Fem1DRepresentation>("Physics Representation: " + name);
+
+ // In this test, the physics representations are not transformed, only the graphics will be transformed
+ loadModelFem1D(physicsRepresentation, 10);
+
+ physicsRepresentation->setIntegrationScheme(integrationScheme);
+ physicsRepresentation->setRayleighDampingMass(5e-2);
+ physicsRepresentation->setRayleighDampingStiffness(5e-3);
+
+ std::shared_ptr<BasicSceneElement> femSceneElement = std::make_shared<BasicSceneElement>(name);
+ femSceneElement->addComponent(physicsRepresentation);
+
+ std::shared_ptr<SurgSim::Graphics::PointCloudRepresentation> graphicsRepresentation
+ = std::make_shared<OsgPointCloudRepresentation>("Graphics Representation: " + name);
+ graphicsRepresentation->setLocalPose(gfxPose);
+ graphicsRepresentation->setColor(color);
+ graphicsRepresentation->setPointSize(3.0f);
+ graphicsRepresentation->setVisible(true);
+
+ femSceneElement->addComponent(graphicsRepresentation);
+
+ auto physicsToGraphics =
+ std::make_shared<TransferPhysicsToPointCloudBehavior>("Transfer from Physics to Graphics");
+ physicsToGraphics->setSource(physicsRepresentation);
+ physicsToGraphics->setTarget(graphicsRepresentation);
+ femSceneElement->addComponent(physicsToGraphics);
+
+ return femSceneElement;
+}
+
+}; // anonymous namespace
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST_F(RenderTests, VisualTestFem1D)
+{
+ using SurgSim::Math::makeRigidTranslation;
+ using SurgSim::Math::Vector4d;
+
+ scene->addSceneElement(
+ createFem1D("Euler Explicit", // name
+ makeRigidTranslation(Vector3d(0.0, 0.5, 0.0)), // graphics pose
+ Vector4d(1, 0, 0, 1), // color (r, g, b, a)
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER)); // technique to update object
+
+ scene->addSceneElement(
+ createFem1D("Modified Euler Explicit",
+ makeRigidTranslation(Vector3d(0.0, 0.25, 0.0)),
+ Vector4d(0.5, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER));
+
+ scene->addSceneElement(
+ createFem1D("Runge Kutta 4",
+ makeRigidTranslation(Vector3d(0.0, 0.0, 0.0)),
+ Vector4d(0, 1, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4));
+
+ scene->addSceneElement(
+ createFem1D("Euler Implicit",
+ makeRigidTranslation(Vector3d(0.0, -0.25, 0.0)),
+ Vector4d(0, 0, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER));
+
+ scene->addSceneElement(
+ createFem1D("Static",
+ makeRigidTranslation(Vector3d(0.0, -0.5, 0.0)),
+ Vector4d(1, 1, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC));
+
+ runTest(Vector3d(0.0, 0.0, 2.0), Vector3d::Zero(), 5000.0);
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RenderTests/RenderTestFem2D.cpp b/SurgSim/Physics/RenderTests/RenderTestFem2D.cpp
new file mode 100644
index 0000000..c6a4e90
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/RenderTestFem2D.cpp
@@ -0,0 +1,250 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 RenderTestFem2D.cpp render test for Fem2D
+
+#include <memory>
+
+#include "SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h"
+#include "SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+
+using SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior;
+using SurgSim::Blocks::TransferPhysicsToPointCloudBehavior;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Graphics::OsgPointCloudRepresentation;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::Fem2DRepresentation;
+using SurgSim::Physics::Fem2DElementTriangle;
+
+namespace
+{
+
+/// Create a Fem2D with a cylinder shape
+/// \note This is defining a cylinder based on cylindrical coordinates M(length, angle)
+/// \note The cylinder is composed of cross-sections with nodes added radially to each cross-section.
+/// \note The nodes of 2 consecutives cross-sections are connected to form square-patches which in turn
+/// \note are decomposed into 2 Fem2DElementTriangle.
+void createFem2DCylinder(std::shared_ptr<Fem2DRepresentation> physicsRepresentation)
+{
+ // Mechanical properties
+ const double youngModulus = 1e6;
+ const double poissonRatio = 0.35;
+ const double massDensity = 5000.0;
+ // Geometrical properties
+ const double length = 1.0;
+ const double radius = 8e-2;
+ const double thickness = 3e-2;
+ // Number of cross-sections and their discretization in nodes
+ const size_t numSections = 7;
+ const size_t numNodesOnSection = 8;
+ const size_t numNodes = numSections * numNodesOnSection;
+
+ // Distance between 2 consecutive cross-section
+ const double deltaL = length / (numSections - 1);
+ // Angle between 2 consecutive nodes on a cross-section
+ const double deltaAngle = 2.0 * M_PI / numNodesOnSection;
+
+ std::shared_ptr<SurgSim::Math::OdeState> restState = std::make_shared<SurgSim::Math::OdeState>();
+ restState->setNumDof(physicsRepresentation->getNumDofPerNode(), numNodes);
+
+ // Sets the initial state (node positions and boundary conditions)
+ SurgSim::Math::Vector& x = restState->getPositions();
+ size_t numDofPerNode = physicsRepresentation->getNumDofPerNode();
+ for (size_t sectionId = 0; sectionId < numSections; sectionId++)
+ {
+ for (size_t nodeIdOnSection = 0; nodeIdOnSection < numNodesOnSection; nodeIdOnSection++)
+ {
+ double angle = deltaAngle * nodeIdOnSection;
+ x.segment<3>(numDofPerNode * (sectionId * numNodesOnSection + nodeIdOnSection)) =
+ Vector3d(-length / 2.0 + sectionId * deltaL, radius * cos(angle), radius * sin(angle));
+ }
+ }
+ // We fix the nodes on the 1st and last cross-sections
+ const size_t section0 = 0;
+ const size_t section1 = numSections - 1;
+ for (size_t nodeId = 0; nodeId < numNodesOnSection; nodeId++)
+ {
+ restState->addBoundaryCondition(nodeId + numNodesOnSection * section0);
+ restState->addBoundaryCondition(nodeId + numNodesOnSection * section1);
+ }
+ physicsRepresentation->setInitialState(restState);
+
+ // Adds all the FemElements
+ for (size_t sectionId = 0; sectionId < numSections - 1; sectionId++)
+ {
+ // For each cross-section, we connect the nodes of this cross-section to the nodes of the next cross-section
+
+ for (size_t nodeIdOnSection = 0; nodeIdOnSection < numNodesOnSection; nodeIdOnSection++)
+ {
+ // On a given cross-section, each node will be connected to the next node
+ // The last node is connected to the 1st node via a modulo in the node index calculation
+
+ std::array<std::array<size_t, 2>, 2> nodeIds =
+ {{
+ {{
+ sectionId * numNodesOnSection + nodeIdOnSection,
+ sectionId * numNodesOnSection + (nodeIdOnSection + 1) % numNodesOnSection
+ }}
+ ,
+ {{
+ (sectionId + 1) * numNodesOnSection + nodeIdOnSection,
+ (sectionId + 1) * numNodesOnSection + (nodeIdOnSection + 1) % numNodesOnSection
+ }}
+ }};
+ std::array<size_t, 3> triangle1NodeIds = {{nodeIds[0][0], nodeIds[0][1], nodeIds[1][1]}};
+ std::shared_ptr<Fem2DElementTriangle> triangle1 = std::make_shared<Fem2DElementTriangle>(triangle1NodeIds);
+ triangle1->setThickness(thickness);
+ triangle1->setMassDensity(massDensity);
+ triangle1->setPoissonRatio(poissonRatio);
+ triangle1->setYoungModulus(youngModulus);
+ physicsRepresentation->addFemElement(triangle1);
+
+ std::array<size_t, 3> triangle2NodeIds = {{nodeIds[0][0], nodeIds[1][1], nodeIds[1][0]}};
+ std::shared_ptr<Fem2DElementTriangle> triangle2 = std::make_shared<Fem2DElementTriangle>(triangle2NodeIds);
+ triangle2->setThickness(thickness);
+ triangle2->setMassDensity(massDensity);
+ triangle2->setPoissonRatio(poissonRatio);
+ triangle2->setYoungModulus(youngModulus);
+ physicsRepresentation->addFemElement(triangle2);
+ }
+ }
+}
+
+// Generates a 2d fem comprised of a cylinder. The number of fem elements is determined by createFem2DCylinder.
+std::shared_ptr<SurgSim::Framework::SceneElement> createFem2D(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& gfxPose,
+ const SurgSim::Math::Vector4d& color,
+ SurgSim::Math::IntegrationScheme integrationScheme)
+{
+ std::shared_ptr<Fem2DRepresentation> physicsRepresentation
+ = std::make_shared<Fem2DRepresentation>("Physics Representation");
+
+ // In this test, the physics representations are not transformed, only the graphics will be transformed
+ createFem2DCylinder(physicsRepresentation);
+
+ physicsRepresentation->setIntegrationScheme(integrationScheme);
+ physicsRepresentation->setRayleighDampingMass(1e0);
+ physicsRepresentation->setRayleighDampingStiffness(1e-3);
+
+ std::shared_ptr<BasicSceneElement> femSceneElement = std::make_shared<BasicSceneElement>(name);
+ femSceneElement->addComponent(physicsRepresentation);
+
+ // Create a triangle mesh for visualizing the surface of the finite element model
+ std::shared_ptr<SurgSim::Graphics::OsgMeshRepresentation> graphicsTriangleMeshRepresentation
+ = std::make_shared<SurgSim::Graphics::OsgMeshRepresentation>("TriangleMesh Representation");
+ graphicsTriangleMeshRepresentation->setLocalPose(gfxPose);
+ auto mesh = graphicsTriangleMeshRepresentation->getMesh();
+ // Create vertices
+ for (size_t vertexId = 0; vertexId < physicsRepresentation->getInitialState()->getNumNodes(); vertexId++)
+ {
+ SurgSim::Graphics::Mesh::VertexType v(physicsRepresentation->getInitialState()->getPosition(vertexId));
+ mesh->addVertex(v);
+ }
+ // Create triangles
+ for (size_t triangleId = 0; triangleId < physicsRepresentation->getNumFemElements(); triangleId++)
+ {
+ auto nodeIdsVector = physicsRepresentation->getFemElement(triangleId)->getNodeIds();
+ std::array<size_t, 3> nodeIds = {{nodeIdsVector[0], nodeIdsVector[1], nodeIdsVector[2]}};
+ SurgSim::Graphics::Mesh::TriangleType t(nodeIds);
+ mesh->addTriangle(t);
+ }
+ femSceneElement->addComponent(graphicsTriangleMeshRepresentation);
+
+ // Create a behavior which transfers the position of the vertices in the FEM to locations in the triangle mesh
+ auto physicsToMesh =
+ std::make_shared<SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior>("physics to triangle mesh");
+ physicsToMesh->setSource(physicsRepresentation);
+ physicsToMesh->setTarget(graphicsTriangleMeshRepresentation);
+ femSceneElement->addComponent(physicsToMesh);
+
+ std::shared_ptr<SurgSim::Graphics::PointCloudRepresentation> graphicsPointCloudRepresentation
+ = std::make_shared<OsgPointCloudRepresentation>("PointCloud Representation");
+ graphicsPointCloudRepresentation->setLocalPose(gfxPose);
+ graphicsPointCloudRepresentation->setColor(color);
+ graphicsPointCloudRepresentation->setPointSize(3.0f);
+ graphicsPointCloudRepresentation->setVisible(true);
+ femSceneElement->addComponent(graphicsPointCloudRepresentation);
+
+ auto physicsToPointCloud =
+ std::make_shared<TransferPhysicsToPointCloudBehavior>("Transfer from Physics to Graphics point cloud");
+ physicsToPointCloud->setSource(physicsRepresentation);
+ physicsToPointCloud->setTarget(graphicsPointCloudRepresentation);
+ femSceneElement->addComponent(physicsToPointCloud);
+
+ return femSceneElement;
+}
+
+}; // anonymous namespace
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST_F(RenderTests, VisualTestFem2D)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::Vector4d;
+
+ const SurgSim::Math::Quaterniond quaternion(Eigen::AngleAxisd(-M_PI / 6.0, Vector3d(0.0, 1.0, 0.0)));
+
+ scene->addSceneElement(
+ createFem2D("Euler Explicit", // name
+ makeRigidTransform(quaternion, Vector3d(0.0, 0.6, 0.0)), // graphics pose (rot., trans.)
+ Vector4d(1, 0, 0, 1), // color (r, g, b, a)
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER)); // technique to update object
+
+ scene->addSceneElement(
+ createFem2D("Modified Euler Explicit",
+ makeRigidTransform(quaternion, Vector3d(0.0, 0.3, 0.0)),
+ Vector4d(0.5, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER));
+
+ scene->addSceneElement(
+ createFem2D("Runge Kutta 4",
+ makeRigidTransform(quaternion, Vector3d(0.0, 0.0, 0.0)),
+ Vector4d(0, 1, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4));
+
+ scene->addSceneElement(
+ createFem2D("Euler Implicit",
+ makeRigidTransform(quaternion, Vector3d(0.0, -0.3, 0.0)),
+ Vector4d(0, 0, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER));
+
+ scene->addSceneElement(
+ createFem2D("Static",
+ makeRigidTransform(quaternion, Vector3d(0.0, -0.6, 0.0)),
+ Vector4d(1, 1, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC));
+
+ runTest(Vector3d(0.0, 0.0, 2.0), Vector3d::Zero(), 5000.0);
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RenderTests/RenderTestFem3D.cpp b/SurgSim/Physics/RenderTests/RenderTestFem3D.cpp
new file mode 100644
index 0000000..fcac735
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/RenderTestFem3D.cpp
@@ -0,0 +1,262 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 RenderTestFem3D.cpp render test for Fem3D
+
+#include <memory>
+
+#include "SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+
+using SurgSim::Blocks::TransferPhysicsToPointCloudBehavior;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Graphics::OsgPointCloudRepresentation;
+using SurgSim::Physics::Fem3DRepresentation;
+using SurgSim::Physics::FemElement;
+using SurgSim::Physics::Fem3DElementCube;
+using SurgSim::Physics::Fem3DElementTetrahedron;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createTetrahedronFem3D(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& pose, SurgSim::Math::Vector4d color,
+ SurgSim::Math::IntegrationScheme integrationScheme)
+{
+ // Physics Representation
+ std::shared_ptr<Fem3DRepresentation> physicsRepresentation;
+ physicsRepresentation = std::make_shared<Fem3DRepresentation>(name + " Physics");
+ physicsRepresentation->setIntegrationScheme(integrationScheme);
+ physicsRepresentation->setRayleighDampingMass(5e-2);
+ physicsRepresentation->setRayleighDampingStiffness(5e-3);
+
+ std::array<Vector3d, 8> vertices = {{
+ Vector3d(-0.5, -0.5, -0.5),
+ Vector3d(0.5, -0.5, -0.5),
+ Vector3d(-0.5, 0.5, -0.5),
+ Vector3d(0.5, 0.5, -0.5),
+ Vector3d(-0.5, -0.5, 0.5),
+ Vector3d(0.5, -0.5, 0.5),
+ Vector3d(-0.5, 0.5, 0.5),
+ Vector3d(0.5, 0.5, 0.5)
+ }
+ };
+
+ // Cube decomposition into 5 tetrahedrons
+ // https://www.math.ucdavis.edu/~deloera/CURRENT_INTERESTS/cube.html
+ std::array< std::array<size_t, 4>, 5> tetrahedrons = {{
+ {{4, 7, 1, 2}}, // CCW (47)cross(41) . (42) > 0
+ {{4, 1, 7, 5}}, // CCW (41)cross(47) . (45) > 0
+ {{4, 2, 1, 0}}, // CCW (42)cross(41) . (40) > 0
+ {{4, 7, 2, 6}}, // CCW (47)cross(42) . (46) > 0
+ {{1, 2, 7, 3}} // CCW (12)cross(17) . (13) > 0
+ }
+ };
+
+ std::array<size_t, 4> boundaryConditionsNodeIdx = {{0, 1, 2, 3}};
+
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(physicsRepresentation->getNumDofPerNode(), 8);
+
+ for (size_t i = 0; i != vertices.size(); i++)
+ {
+ initialState->getPositions().segment(i * 3, 3) = vertices[i];
+ }
+
+ for (auto index = boundaryConditionsNodeIdx.cbegin(); index != boundaryConditionsNodeIdx.cend(); ++index)
+ {
+ initialState->addBoundaryCondition(*index);
+ }
+ physicsRepresentation->setInitialState(initialState);
+
+ for (auto tetrahedron = tetrahedrons.cbegin(); tetrahedron != tetrahedrons.cend(); ++tetrahedron)
+ {
+ std::shared_ptr<FemElement> element = std::make_shared<Fem3DElementTetrahedron>(*tetrahedron);
+ element->setMassDensity(8000.0);
+ element->setPoissonRatio(0.45);
+ element->setYoungModulus(1.0e6);
+ physicsRepresentation->addFemElement(element);
+ }
+
+ // Graphics Representation
+ std::shared_ptr<OsgPointCloudRepresentation> graphicsRepresentation;
+ graphicsRepresentation = std::make_shared<OsgPointCloudRepresentation>(name + " Graphics object ");
+ graphicsRepresentation->setLocalPose(pose);
+ graphicsRepresentation->setColor(color);
+ graphicsRepresentation->setPointSize(3.0f);
+ graphicsRepresentation->setVisible(true);
+
+ // Scene Element
+ std::shared_ptr<BasicSceneElement> femSceneElement = std::make_shared<BasicSceneElement>(name);
+ femSceneElement->addComponent(physicsRepresentation);
+ femSceneElement->addComponent(graphicsRepresentation);
+
+ auto physicsToGraphics =
+ std::make_shared<TransferPhysicsToPointCloudBehavior>("Physics to Graphics deformable points");
+ physicsToGraphics->setSource(physicsRepresentation);
+ physicsToGraphics->setTarget(graphicsRepresentation);
+ femSceneElement->addComponent(physicsToGraphics);
+
+ return femSceneElement;
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createCubeFem3D(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& pose,
+ SurgSim::Math::Vector4d color, SurgSim::Math::IntegrationScheme integrationScheme)
+{
+ // Physics Representation
+ std::shared_ptr<Fem3DRepresentation> physicsRepresentation;
+ physicsRepresentation = std::make_shared<Fem3DRepresentation>(name + " Physics");
+ physicsRepresentation->setIntegrationScheme(integrationScheme);
+ physicsRepresentation->setRayleighDampingMass(5e-2);
+ physicsRepresentation->setRayleighDampingStiffness(5e-3);
+
+ std::array<Vector3d, 8> vertices = {{
+ Vector3d(-0.5, -0.5, -0.5),
+ Vector3d(0.5, -0.5, -0.5),
+ Vector3d(-0.5, 0.5, -0.5),
+ Vector3d(0.5, 0.5, -0.5),
+ Vector3d(-0.5, -0.5, 0.5),
+ Vector3d(0.5, -0.5, 0.5),
+ Vector3d(-0.5, 0.5, 0.5),
+ Vector3d(0.5, 0.5, 0.5)
+ }
+ };
+ std::array<size_t, 8> cube = {{0, 1, 3, 2, 4, 5, 7, 6}};
+ std::array<size_t, 4> boundaryConditionsNodeIdx = {{0, 1, 2, 3}};
+
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(physicsRepresentation->getNumDofPerNode(), 8);
+
+ for (size_t i = 0; i != vertices.size(); i++)
+ {
+ initialState->getPositions().segment(i * 3, 3) = vertices[i];
+ }
+
+ for (auto index = boundaryConditionsNodeIdx.cbegin(); index != boundaryConditionsNodeIdx.cend(); ++index)
+ {
+ initialState->addBoundaryCondition(*index);
+ }
+ physicsRepresentation->setInitialState(initialState);
+
+ std::shared_ptr<FemElement> element = std::make_shared<Fem3DElementCube>(cube);
+ element->setMassDensity(8000.0);
+ element->setPoissonRatio(0.45);
+ element->setYoungModulus(1.0e6);
+ physicsRepresentation->addFemElement(element);
+
+ // Graphics Representation
+ std::shared_ptr<OsgPointCloudRepresentation> graphicsRepresentation;
+ graphicsRepresentation = std::make_shared<OsgPointCloudRepresentation>(name + " Graphics object ");
+ graphicsRepresentation->setLocalPose(pose);
+ graphicsRepresentation->setColor(color);
+ graphicsRepresentation->setPointSize(3.0f);
+ graphicsRepresentation->setVisible(true);
+
+ // Scene Element
+ std::shared_ptr<BasicSceneElement> femSceneElement = std::make_shared<BasicSceneElement>(name);
+ femSceneElement->addComponent(physicsRepresentation);
+ femSceneElement->addComponent(graphicsRepresentation);
+
+ auto physicsToGraphics =
+ std::make_shared<TransferPhysicsToPointCloudBehavior>("Physics to Graphics deformable points");
+ physicsToGraphics->setSource(physicsRepresentation);
+ physicsToGraphics->setTarget(graphicsRepresentation);
+ femSceneElement->addComponent(physicsToGraphics);
+
+ return femSceneElement;
+}
+
+}; // anonymous namespace
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST_F(RenderTests, VisualTestFem3D)
+{
+ using SurgSim::Math::makeRigidTranslation;
+ using SurgSim::Math::Vector4d;
+
+ // Cube with cube FemElement
+ scene->addSceneElement(createCubeFem3D("CubeElement Euler Explicit",
+ makeRigidTranslation(Vector3d(-4.0, 2.0, -2.0)),
+ Vector4d(1, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER));
+
+ scene->addSceneElement(createCubeFem3D("CubeElement Modified Euler Explicit",
+ makeRigidTranslation(Vector3d(-2.0, 2.0, -2.0)),
+ Vector4d(0.5, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER));
+
+ scene->addSceneElement(createCubeFem3D("CubeElement Runge Kutta 4",
+ makeRigidTranslation(Vector3d(0.0, 2.0, -2.0)),
+ Vector4d(0, 1, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4));
+
+ scene->addSceneElement(createCubeFem3D("CubeElement Fem 3D Euler Implicit",
+ makeRigidTranslation(Vector3d(2.0, 2.0, -2.0)),
+ Vector4d(0, 0, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER));
+
+ scene->addSceneElement(createCubeFem3D("CubeElement Static",
+ makeRigidTranslation(Vector3d(4.0, 2.0, -2.0)),
+ Vector4d(1, 1, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_STATIC));
+
+ // Cube with tetrahedron FemElement
+ scene->addSceneElement(createTetrahedronFem3D("TetrahedronElement Euler Explicit",
+ makeRigidTranslation(Vector3d(-4.0, -2.0, -2.0)),
+ Vector4d(1, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER));
+
+ scene->addSceneElement(createTetrahedronFem3D("TetrahedronElement Modified Euler Explicit",
+ makeRigidTranslation(Vector3d(-2.0, -2.0, -2.0)),
+ Vector4d(0.5, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER));
+
+ scene->addSceneElement(createTetrahedronFem3D("TetrahedronElement Runge Kutta 4",
+ makeRigidTranslation(Vector3d(0.0, -2.0, -2.0)),
+ Vector4d(0, 1, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4));
+
+ scene->addSceneElement(createTetrahedronFem3D("TetrahedronElement Fem 3D Euler Implicit",
+ makeRigidTranslation(Vector3d(2.0, -2.0, -2.0)),
+ Vector4d(0, 0, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER));
+
+ scene->addSceneElement(createTetrahedronFem3D("TetrahedronElement Static",
+ makeRigidTranslation(Vector3d(4.0, -2.0, -2.0)),
+ Vector4d(1, 1, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC));
+
+ runTest(Vector3d(0.0, 0.0, 7.0), Vector3d::Zero(), 5000.0);
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RenderTests/RenderTestFem3DCorotational.cpp b/SurgSim/Physics/RenderTests/RenderTestFem3DCorotational.cpp
new file mode 100644
index 0000000..9c3f9df
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/RenderTestFem3DCorotational.cpp
@@ -0,0 +1,166 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 RenderTestFem3DCorotational.cpp render test for Fem3D with corotational elements
+
+#include <memory>
+
+#include "SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+
+using SurgSim::Blocks::TransferPhysicsToPointCloudBehavior;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Graphics::OsgPointCloudRepresentation;
+using SurgSim::Physics::Fem3DRepresentation;
+using SurgSim::Physics::FemElement;
+using SurgSim::Physics::Fem3DElementCorotationalTetrahedron;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createTetrahedronFem3D(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& pose, SurgSim::Math::Vector4d color,
+ SurgSim::Math::IntegrationScheme integrationScheme)
+{
+ // Physics Representation
+ std::shared_ptr<Fem3DRepresentation> physicsRepresentation;
+ physicsRepresentation = std::make_shared<Fem3DRepresentation>(name + " Physics");
+ physicsRepresentation->setIntegrationScheme(integrationScheme);
+ physicsRepresentation->setRayleighDampingMass(1e-2);
+ physicsRepresentation->setRayleighDampingStiffness(1e-3);
+
+ std::array<Vector3d, 8> vertices = {{
+ Vector3d(-0.5, -0.5, -0.5),
+ Vector3d(0.5, -0.5, -0.5),
+ Vector3d(-0.5, 0.5, -0.5),
+ Vector3d(0.5, 0.5, -0.5),
+ Vector3d(-0.5, -0.5, 0.5),
+ Vector3d(0.5, -0.5, 0.5),
+ Vector3d(-0.5, 0.5, 0.5),
+ Vector3d(0.5, 0.5, 0.5)
+ }
+ };
+
+ // Cube decomposition into 5 tetrahedrons
+ // https://www.math.ucdavis.edu/~deloera/CURRENT_INTERESTS/cube.html
+ std::array< std::array<size_t, 4>, 5> tetrahedrons = {{
+ {{4, 7, 1, 2}}, // CCW (47)cross(41) . (42) > 0
+ {{4, 1, 7, 5}}, // CCW (41)cross(47) . (45) > 0
+ {{4, 2, 1, 0}}, // CCW (42)cross(41) . (40) > 0
+ {{4, 7, 2, 6}}, // CCW (47)cross(42) . (46) > 0
+ {{1, 2, 7, 3}} // CCW (12)cross(17) . (13) > 0
+ }
+ };
+
+ std::array<size_t, 2> boundaryConditionsNodeIdx = {{0, 1}};
+
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(physicsRepresentation->getNumDofPerNode(), 8);
+
+ for (size_t i = 0; i != vertices.size(); i++)
+ {
+ initialState->getPositions().segment(i * 3, 3) = vertices[i];
+ }
+
+ for (auto index = boundaryConditionsNodeIdx.cbegin(); index != boundaryConditionsNodeIdx.cend(); ++index)
+ {
+ initialState->addBoundaryCondition(*index);
+ }
+ physicsRepresentation->setInitialState(initialState);
+
+ for (auto tetrahedron = tetrahedrons.cbegin(); tetrahedron != tetrahedrons.cend(); ++tetrahedron)
+ {
+ std::shared_ptr<FemElement> element = std::make_shared<Fem3DElementCorotationalTetrahedron>(*tetrahedron);
+ element->setMassDensity(8000.0);
+ element->setPoissonRatio(0.45);
+ element->setYoungModulus(1.0e6);
+ physicsRepresentation->addFemElement(element);
+ }
+
+ // Graphics Representation
+ std::shared_ptr<OsgPointCloudRepresentation> graphicsRepresentation;
+ graphicsRepresentation = std::make_shared<OsgPointCloudRepresentation>(name + " Graphics object ");
+ graphicsRepresentation->setLocalPose(pose);
+ graphicsRepresentation->setColor(color);
+ graphicsRepresentation->setPointSize(3.0f);
+ graphicsRepresentation->setVisible(true);
+
+ // Scene Element
+ std::shared_ptr<BasicSceneElement> femSceneElement = std::make_shared<BasicSceneElement>(name);
+ femSceneElement->addComponent(physicsRepresentation);
+ femSceneElement->addComponent(graphicsRepresentation);
+
+ auto physicsToGraphics =
+ std::make_shared<TransferPhysicsToPointCloudBehavior>("Physics to Graphics deformable points");
+ physicsToGraphics->setSource(physicsRepresentation);
+ physicsToGraphics->setTarget(graphicsRepresentation);
+ femSceneElement->addComponent(physicsToGraphics);
+
+ return femSceneElement;
+}
+
+}; // anonymous namespace
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST_F(RenderTests, VisualTestFem3DCorotatioal)
+{
+ using SurgSim::Math::makeRigidTranslation;
+
+ // Cube with corotational tetrahedron FemElement
+ scene->addSceneElement(createTetrahedronFem3D("CorotationalTetrahedronElement Euler Explicit",
+ makeRigidTranslation(Vector3d(-4.0, 1.0, -1.0)),
+ SurgSim::Math::Vector4d(1, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER));
+
+ scene->addSceneElement(createTetrahedronFem3D("CorotatinoalTetrahedronElement Modified Euler Explicit",
+ makeRigidTranslation(Vector3d(-2.0, 1.0, -1.0)),
+ SurgSim::Math::Vector4d(0.5, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER));
+
+ scene->addSceneElement(createTetrahedronFem3D("CorotatinoalTetrahedronElement Runge Kutta 4",
+ makeRigidTranslation(Vector3d(0.0, 1.0, -1.0)),
+ SurgSim::Math::Vector4d(0, 1, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4));
+
+ scene->addSceneElement(createTetrahedronFem3D("CorotatinoalTetrahedronElement Fem 3D Euler Implicit",
+ makeRigidTranslation(Vector3d(2.0, 1.0, -1.0)),
+ SurgSim::Math::Vector4d(0, 0, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER));
+
+ scene->addSceneElement(createTetrahedronFem3D("CorotatinoalTetrahedronElement Fem 3D Static",
+ makeRigidTranslation(Vector3d(4.0, 1.0, -1.0)),
+ SurgSim::Math::Vector4d(1, 1, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_STATIC));
+
+ runTest(Vector3d(0.0, 0.0, 7.0), Vector3d::Zero(), 5000.0);
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RenderTests/RenderTestMassSprings.cpp b/SurgSim/Physics/RenderTests/RenderTestMassSprings.cpp
new file mode 100644
index 0000000..ec3926a
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/RenderTestMassSprings.cpp
@@ -0,0 +1,339 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 RenderTestMassSprings.cpp render test for MassSprings
+
+#include <memory>
+
+#include "SurgSim/Blocks/MassSpring1DRepresentation.h"
+#include "SurgSim/Blocks/MassSpring2DRepresentation.h"
+#include "SurgSim/Blocks/MassSpring3DRepresentation.h"
+#include "SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+
+using SurgSim::Blocks::MassSpring1DRepresentation;
+using SurgSim::Blocks::MassSpring2DRepresentation;
+using SurgSim::Blocks::MassSpring3DRepresentation;
+using SurgSim::Blocks::TransferPhysicsToPointCloudBehavior;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Graphics::OsgPointCloudRepresentation;
+using SurgSim::Math::Vector3d;
+
+
+namespace
+{
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createMassSpring1D(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& gfxPose,
+ SurgSim::Math::Vector4d color, SurgSim::Math::IntegrationScheme integrationScheme)
+{
+ std::shared_ptr<MassSpring1DRepresentation> physicsRepresentation =
+ std::make_shared<MassSpring1DRepresentation>(name + " Physics");
+
+ // In this test, the physics representations are not transformed,
+ // only the graphics one will apply a transform
+
+ std::vector<size_t> nodeBoundaryConditions;
+ nodeBoundaryConditions.push_back(0);
+
+ // MassSpring1D with a straight line would define springs only along 1 direction, which would result in
+ // stiffness matrix of rank n/3. The Z axis can be constrained entirely and the simulation
+ // be done in exclusively in 2D (X,Y), but for the 2nd dimension to be defined properly in the stiffness matrix
+ // we need a shape that does not have any straight lines anywhere. This will ensure that the problem is
+ // well defined in 2D. This problem only arise in static resolution (OdeSolverStatic).
+ std::vector<SurgSim::Math::Vector3d> nodes;
+ nodes.push_back(Vector3d(-0.5, 0.0, 0));
+ nodes.push_back(Vector3d(-0.3, -0.5, 0));
+ nodes.push_back(Vector3d(-0.1, -0.4, 0));
+ nodes.push_back(Vector3d(0.1, -0.5, 0));
+ nodes.push_back(Vector3d(0.3, 0.0, 0));
+ physicsRepresentation->init1D(nodes,
+ nodeBoundaryConditions,
+ 0.1, // total mass (in Kg)
+ 5.0, // Stiffness stretching
+ 0.5, // Damping stretching
+ 3.0, // Stiffness bending
+ 0.5); // Damping bending
+
+ // MassSpring1D defines springs only on the XY plane, so the stiffness matrix will contains
+ // rows and columns of 0 for all Z axis. Therefore we need to constrain all Z dof to entirely
+ // define the problem (stiffness matrix invertible in static resolution (OdeSolverStatic)).
+ for (size_t nodeId = 1; nodeId < nodes.size(); ++nodeId)
+ {
+ SurgSim::Math::OdeState* state =
+ const_cast<SurgSim::Math::OdeState*>(physicsRepresentation->getInitialState().get());
+ state->addBoundaryCondition(nodeId, 2);
+ }
+
+ physicsRepresentation->setIntegrationScheme(integrationScheme);
+ physicsRepresentation->setRayleighDampingMass(1e0);
+ physicsRepresentation->setRayleighDampingStiffness(3e-2);
+
+ std::shared_ptr<BasicSceneElement> massSpringElement = std::make_shared<BasicSceneElement>(name);
+ massSpringElement->addComponent(physicsRepresentation);
+
+ std::shared_ptr<OsgPointCloudRepresentation> graphicsRepresentation =
+ std::make_shared<OsgPointCloudRepresentation>("Graphics object");
+ graphicsRepresentation->setLocalPose(gfxPose);
+ graphicsRepresentation->setColor(color);
+ graphicsRepresentation->setPointSize(3.0f);
+ graphicsRepresentation->setVisible(true);
+ massSpringElement->addComponent(graphicsRepresentation);
+
+ auto physicsToGraphics =
+ std::make_shared<TransferPhysicsToPointCloudBehavior>("Physics to Graphics deformable points");
+ physicsToGraphics->setSource(physicsRepresentation);
+ physicsToGraphics->setTarget(graphicsRepresentation);
+
+ massSpringElement->addComponent(physicsToGraphics);
+ return massSpringElement;
+
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createMassSpring2D(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& gfxPose,
+ SurgSim::Math::Vector4d color, SurgSim::Math::IntegrationScheme integrationScheme)
+{
+ std::shared_ptr<MassSpring2DRepresentation> physicsRepresentation =
+ std::make_shared<MassSpring2DRepresentation>(name + " Physics");
+
+ // In this test, the physics representations are not transformed,
+ // only the graphics one will apply a transform
+
+ // The regular structure of the MassSpring2D (in X,Y plane) makes the stiffness matrix close to singular
+ // (Z axis aside) even with 1 fixed node. We run this test with 2 fixed nodes to ensure that a static resolution
+ // will be stable (OdeSolverStatic).
+ std::vector<size_t> nodeBoundaryConditions;
+ nodeBoundaryConditions.push_back(0);
+ nodeBoundaryConditions.push_back(1);
+ std::array<std::array<SurgSim::Math::Vector3d, 2>, 2> extremities =
+ {
+ {
+ {{ Vector3d(-0.5, 0.5, 0), Vector3d(0.5, 0.5, 0) }},
+ {{ Vector3d(-0.5, -0.5, 0), Vector3d(0.5, -0.5, 0) }}
+ }
+ };
+ size_t numNodesPerDim[2] = {3, 3};
+ physicsRepresentation->init2D(extremities,
+ numNodesPerDim,
+ nodeBoundaryConditions,
+ 0.1, // total mass (in Kg)
+ 5.0, // Stiffness stretching
+ 0.5, // Damping stretching
+ 1.0, // Stiffness bending
+ 0.5, // Damping bending
+ 1.0, // Stiffness face diagonal
+ 0.5); // Damping face diagonal
+
+ // MassSpring2D defines springs only on the XY plane, so the stiffness matrix will contains
+ // rows and columns of 0 for all Z axis. Therefore we need to constrain all Z dof to entirely
+ // define the problem (stiffness matrix invertible in static resolution (OdeSolverStatic)).
+ for (size_t nodeId = 2; nodeId < numNodesPerDim[0] * numNodesPerDim[1]; ++nodeId)
+ {
+ SurgSim::Math::OdeState* state =
+ const_cast<SurgSim::Math::OdeState*>(physicsRepresentation->getInitialState().get());
+ state->addBoundaryCondition(nodeId, 2);
+ }
+
+ physicsRepresentation->setIntegrationScheme(integrationScheme);
+ physicsRepresentation->setRayleighDampingMass(1e0);
+ physicsRepresentation->setRayleighDampingStiffness(3e-2);
+
+ std::shared_ptr<BasicSceneElement> massSpringElement = std::make_shared<BasicSceneElement>(name);
+ massSpringElement->addComponent(physicsRepresentation);
+
+ std::shared_ptr<OsgPointCloudRepresentation> graphicsRepresentation =
+ std::make_shared<OsgPointCloudRepresentation>("Graphics object");
+ graphicsRepresentation->setLocalPose(gfxPose);
+ graphicsRepresentation->setColor(color);
+ graphicsRepresentation->setPointSize(3.0f);
+ graphicsRepresentation->setVisible(true);
+ massSpringElement->addComponent(graphicsRepresentation);
+ auto physicsToGraphics =
+ std::make_shared<TransferPhysicsToPointCloudBehavior>("Physics to Graphics deformable points");
+ physicsToGraphics->setSource(physicsRepresentation);
+ physicsToGraphics->setTarget(graphicsRepresentation);
+ massSpringElement->addComponent(physicsToGraphics);
+
+ return massSpringElement;
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createMassSpring3D(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& gfxPose,
+ SurgSim::Math::Vector4d color, SurgSim::Math::IntegrationScheme integrationScheme)
+{
+ std::shared_ptr<MassSpring3DRepresentation> physicsRepresentation =
+ std::make_shared<MassSpring3DRepresentation>(name + " Physics");
+
+ // In this test, the physics representations are not transformed,
+ // only the graphics one will apply a transform
+
+ std::vector<size_t> nodeBoundaryConditions;
+ nodeBoundaryConditions.push_back(0);
+ nodeBoundaryConditions.push_back(1);
+ std::array<std::array<std::array<SurgSim::Math::Vector3d, 2>, 2>, 2> extremities =
+ {
+ {
+ {{
+ {{ Vector3d(-0.5, 0.5, 0.5), Vector3d(0.5, 0.5, 0.5) }}
+ ,
+ {{ Vector3d(-0.5, -0.5, 0.5), Vector3d(0.5, -0.5, 0.5) }}
+ }
+ }
+ ,
+ {{
+ {{ Vector3d(-0.5, 0.5, -0.5), Vector3d(0.5, 0.5, -0.5) }}
+ ,
+ {{ Vector3d(-0.5, -0.5, -0.5), Vector3d(0.5, -0.5, -0.5) }}
+ }
+ },
+ }
+ };
+ size_t numNodesPerDim[3] = {3, 3, 3};
+ physicsRepresentation->init3D(extremities,
+ numNodesPerDim,
+ nodeBoundaryConditions,
+ 0.1, // total mass (in Kg)
+ 5.0, // Stiffness stretching
+ 0.5, // Damping stretching
+ 1.0, // Stiffness bending
+ 0.5, // Damping bending
+ 1.0, // Stiffness face diagonal
+ 0.5, // Damping face diagonal
+ 1.0, // Stiffness volume diagonal
+ 0.5); // Damping volume diagonal
+
+ // MassSpring 3D defines springs in all directions (X, Y, Z, XY, XZ, YZ, XYZ) so the stiffness matrix
+ // is entirely defined (no rows or columns of 0).
+ // Therefore, setting up one node as boundary condition is sufficient to make the static problem invertible.
+
+ physicsRepresentation->setIntegrationScheme(integrationScheme);
+ physicsRepresentation->setRayleighDampingMass(1e0);
+ physicsRepresentation->setRayleighDampingStiffness(3e-2);
+
+ std::shared_ptr<BasicSceneElement> massSpringElement = std::make_shared<BasicSceneElement>(name);
+ massSpringElement->addComponent(physicsRepresentation);
+
+ std::shared_ptr<OsgPointCloudRepresentation> graphicsRepresentation =
+ std::make_shared<OsgPointCloudRepresentation>("Graphics object");
+ graphicsRepresentation->setLocalPose(gfxPose);
+ graphicsRepresentation->setColor(color);
+ graphicsRepresentation->setPointSize(3.0f);
+ graphicsRepresentation->setVisible(true);
+ massSpringElement->addComponent(graphicsRepresentation);
+
+ auto physicsToGraphics =
+ std::make_shared<TransferPhysicsToPointCloudBehavior>("Physics to Graphics deformable points");
+ physicsToGraphics->setSource(physicsRepresentation);
+ physicsToGraphics->setTarget(graphicsRepresentation);
+ massSpringElement->addComponent(physicsToGraphics);
+
+ return massSpringElement;
+}
+
+}; // anonymous namespace
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST_F(RenderTests, VisualTestMassSprings)
+{
+ using SurgSim::Math::makeRigidTranslation;
+ using SurgSim::Math::Vector4d;
+
+ // MassSpring1D
+ {
+ scene->addSceneElement(createMassSpring1D("MassSpring 1D Euler Explicit",
+ makeRigidTranslation(Vector3d(-3.0, 3.0, 0.0)), Vector4d(1, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER));
+
+ scene->addSceneElement(createMassSpring1D("MassSpring 1D Modified Euler Explicit",
+ makeRigidTranslation(Vector3d(-1.25, 3.0, 0.0)), Vector4d(0.5, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER));
+
+ scene->addSceneElement(createMassSpring1D("MassSpring 1D Runge Kutta 4",
+ makeRigidTranslation(Vector3d(0.5, 3.0, 0.0)), Vector4d(0, 1, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4));
+
+ scene->addSceneElement(createMassSpring1D("MassSpring 1D Euler Implicit",
+ makeRigidTranslation(Vector3d(2.25, 3.0, 0.0)), Vector4d(0, 0, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER));
+
+ scene->addSceneElement(createMassSpring1D("MassSpring 1D Static",
+ makeRigidTranslation(Vector3d(4.0, 3.0, 0.0)), Vector4d(1, 1, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_STATIC));
+ }
+
+ // MassSpring2D
+ {
+ scene->addSceneElement(createMassSpring2D("MassSpring 2D Euler Explicit",
+ makeRigidTranslation(Vector3d(-3.0, 0.5, 0.0)), Vector4d(1, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER));
+
+ scene->addSceneElement(createMassSpring2D("MassSpring 2D Modified Euler Explicit",
+ makeRigidTranslation(Vector3d(-1.25, 0.5, 0.0)), Vector4d(0.5, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER));
+
+ scene->addSceneElement(createMassSpring2D("MassSpring 2D Runge Kutta 4",
+ makeRigidTranslation(Vector3d(0.5, 0.5, 0.0)), Vector4d(0, 1, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4));
+
+ scene->addSceneElement(createMassSpring2D("MassSpring 2D Euler Implicit",
+ makeRigidTranslation(Vector3d(2.25, 0.5, 0.0)), Vector4d(0, 0, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER));
+
+ scene->addSceneElement(createMassSpring2D("MassSpring 2D Static",
+ makeRigidTranslation(Vector3d(4.0, 0.5, 0.0)), Vector4d(1, 1, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_STATIC));
+ }
+
+ // MassSpring3D
+ {
+ scene->addSceneElement(createMassSpring3D("MassSpring 3D Euler Explicit",
+ makeRigidTranslation(Vector3d(-3.0, -1.5, 0.0)), Vector4d(1, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER));
+
+ scene->addSceneElement(createMassSpring3D("MassSpring 3D Modified Euler Explicit",
+ makeRigidTranslation(Vector3d(-1.25, -1.5, 0.0)), Vector4d(0.5, 0, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER));
+
+ scene->addSceneElement(createMassSpring3D("MassSpring 3D Runge Kutta 4",
+ makeRigidTranslation(Vector3d(0.5, -1.5, 0.0)), Vector4d(0, 1, 0, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4));
+
+ scene->addSceneElement(createMassSpring3D("MassSpring 3D Euler Implicit",
+ makeRigidTranslation(Vector3d(2.25, -1.5, 0.0)), Vector4d(0, 0, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER));
+
+ scene->addSceneElement(createMassSpring3D("MassSpring 3D Static",
+ makeRigidTranslation(Vector3d(4.0, -1.5, 0.0)), Vector4d(1, 1, 1, 1),
+ SurgSim::Math::INTEGRATIONSCHEME_STATIC));
+ }
+
+ runTest(Vector3d(0.0, 0.0, 8.5), Vector3d::Zero(), 15000.0);
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RenderTests/RenderTestRigidBodies.cpp b/SurgSim/Physics/RenderTests/RenderTestRigidBodies.cpp
new file mode 100644
index 0000000..21824da
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/RenderTestRigidBodies.cpp
@@ -0,0 +1,405 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 RenderTestRigidBodies.cpp render test for RigidRepresentation
+
+#include <memory>
+
+#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/Mesh.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgPlaneRepresentation.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Shapes.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+
+using SurgSim::DataStructures::loadTriangleMesh;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Graphics::OsgBoxRepresentation;
+using SurgSim::Graphics::OsgMeshRepresentation;
+using SurgSim::Graphics::OsgPlaneRepresentation;
+using SurgSim::Graphics::OsgSphereRepresentation;
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::MeshShape;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::FixedRepresentation;
+using SurgSim::Physics::RigidCollisionRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+
+namespace
+{
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createRigidSphereSceneElement(const std::string& name, double radius)
+{
+ // Physics representation
+ std::shared_ptr<RigidRepresentation> physicsRepresentation =
+ std::make_shared<RigidRepresentation>("Physics");
+ std::shared_ptr<SphereShape> shape = std::make_shared<SphereShape>(radius);
+ physicsRepresentation->setShape(shape);
+ physicsRepresentation->setDensity(750.0); // Average mass density of Oak Wood
+ physicsRepresentation->setLinearDamping(1e-2);
+ physicsRepresentation->setAngularDamping(1e-4);
+
+ // Collision representation
+ std::shared_ptr<RigidCollisionRepresentation> collisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("Collision");
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+
+ // Graphic model
+ std::shared_ptr<OsgSphereRepresentation> osgRepresentation =
+ std::make_shared<OsgSphereRepresentation>("OsgRepresentation");
+ osgRepresentation->setRadius(radius);
+
+ std::shared_ptr<BasicSceneElement> sceneElement = std::make_shared<BasicSceneElement>(name);
+ sceneElement->addComponent(osgRepresentation);
+ sceneElement->addComponent(collisionRepresentation);
+ sceneElement->addComponent(physicsRepresentation);
+
+ return sceneElement;
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createRigidBoxSceneElement(const std::string& name, Vector3d size)
+{
+ // Physics representation
+ std::shared_ptr<RigidRepresentation> physicsRepresentation =
+ std::make_shared<RigidRepresentation>("Physics");
+ std::shared_ptr<BoxShape> shape = std::make_shared<BoxShape>(size[0], size[1], size[2]);
+ physicsRepresentation->setShape(shape);
+ physicsRepresentation->setDensity(750.0); // Average mass density of oak wood
+ physicsRepresentation->setLinearDamping(1e-2);
+ physicsRepresentation->setAngularDamping(1e-4);
+
+ // Collision representation
+ std::shared_ptr<RigidCollisionRepresentation> collisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("Collision");
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+
+ // Graphic model
+ std::shared_ptr<OsgBoxRepresentation> osgRepresentation =
+ std::make_shared<OsgBoxRepresentation>("OsgRepresentation");
+ osgRepresentation->setSize(size);
+
+ std::shared_ptr<BasicSceneElement> sceneElement = std::make_shared<BasicSceneElement>(name);
+ sceneElement->addComponent(osgRepresentation);
+ sceneElement->addComponent(collisionRepresentation);
+ sceneElement->addComponent(physicsRepresentation);
+
+ return sceneElement;
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createFixedPlaneSceneElement(const std::string& name)
+{
+ // Physics representation
+ std::shared_ptr<FixedRepresentation> physicsRepresentation =
+ std::make_shared<FixedRepresentation>("Physics");
+ std::shared_ptr<PlaneShape> shape = std::make_shared<PlaneShape>();
+ physicsRepresentation->setShape(shape);
+
+ // Collision representation
+ std::shared_ptr<RigidCollisionRepresentation> collisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("Collision");
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+
+ // Graphic model
+ std::shared_ptr<OsgPlaneRepresentation> osgRepresentation =
+ std::make_shared<OsgPlaneRepresentation>("OsgRepresentation");
+
+ std::shared_ptr<BasicSceneElement> sceneElement = std::make_shared<BasicSceneElement>(name);
+ sceneElement->addComponent(osgRepresentation);
+ sceneElement->addComponent(collisionRepresentation);
+ sceneElement->addComponent(physicsRepresentation);
+
+ return sceneElement;
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createRigidMeshSceneElement(
+ const std::string& name, std::string plyFilename, double scale = 1.0)
+{
+ const SurgSim::Framework::ApplicationData data("config.txt");
+
+ std::string foundFilename = data.findFile(plyFilename);
+ SURGSIM_ASSERT(!foundFilename.empty()) << "Ply file '" << plyFilename << "' could not be located";
+ auto mesh = loadTriangleMesh(foundFilename);
+ for (size_t vertexId = 0; vertexId < mesh->getNumVertices(); ++vertexId)
+ {
+ mesh->setVertexPosition(vertexId, mesh->getVertexPosition(vertexId) * scale);
+ }
+
+ // Physics representation
+ std::shared_ptr<RigidRepresentation> physicsRepresentation =
+ std::make_shared<RigidRepresentation>("Physics");
+ std::shared_ptr<MeshShape> shape = std::make_shared<MeshShape>(*mesh);
+ physicsRepresentation->setShape(shape);
+ physicsRepresentation->setDensity(750.0); // Average mass density of oak wood
+ physicsRepresentation->setLinearDamping(1e-2);
+ physicsRepresentation->setAngularDamping(1e-4);
+
+ // Collision representation
+ std::shared_ptr<RigidCollisionRepresentation> collisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("Collision");
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+
+ // Graphic representation of the physics model
+ std::shared_ptr<OsgMeshRepresentation> osgRepresentation =
+ std::make_shared<OsgMeshRepresentation>("OsgRepresentation");
+ *osgRepresentation->getMesh() = *(std::make_shared<SurgSim::Graphics::Mesh>(*mesh));
+ osgRepresentation->setDrawAsWireFrame(true);
+
+ std::shared_ptr<BasicSceneElement> sceneElement = std::make_shared<BasicSceneElement>(name);
+ sceneElement->addComponent(osgRepresentation);
+ sceneElement->addComponent(collisionRepresentation);
+ sceneElement->addComponent(physicsRepresentation);
+
+ return sceneElement;
+}
+
+}; // anonymous namespace
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST_F(RenderTests, VisualTestFallingRigidBodies)
+{
+ using SurgSim::Math::makeRigidTranslation;
+
+ // Mesh-base objects
+ std::shared_ptr<SurgSim::Framework::SceneElement> boxMesh =
+ createRigidMeshSceneElement("boxMesh1", "box.ply", 1.0);
+ scene->addSceneElement(boxMesh);
+ boxMesh->setPose(makeRigidTranslation(Vector3d(-0.35, 0.3, 0.0)));
+
+ boxMesh = createRigidMeshSceneElement("boxMesh2", "box.ply", 0.5);
+ scene->addSceneElement(boxMesh);
+ boxMesh->setPose(makeRigidTranslation(Vector3d(-0.25, 0.3, 0.0)));
+
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphereMesh =
+ createRigidMeshSceneElement("sphereMesh1", "sphere.ply", 1.0);
+ scene->addSceneElement(sphereMesh);
+ sphereMesh->setPose(makeRigidTranslation(Vector3d(-0.15, 0.3, 0.0)));
+
+ sphereMesh = createRigidMeshSceneElement("sphereMesh2", "sphere.ply", 0.5);
+ scene->addSceneElement(sphereMesh);
+ sphereMesh->setPose(makeRigidTranslation(Vector3d(-0.05, 0.3, 0.0)));
+
+ // Shape-base objects
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphereShape =
+ createRigidSphereSceneElement("sphereShape1", 0.05 * 0.5);
+ scene->addSceneElement(sphereShape);
+ sphereShape->setPose(makeRigidTranslation(Vector3d(0.05, 0.3, 0.0)));
+
+ sphereShape = createRigidSphereSceneElement("sphereShape2", 0.05 * 1.0);
+ scene->addSceneElement(sphereShape);
+ sphereShape->setPose(makeRigidTranslation(Vector3d(0.15, 0.3, 0.0)));
+
+ std::shared_ptr<SurgSim::Framework::SceneElement> boxShape =
+ createRigidBoxSceneElement("boxShape1", Vector3d(0.1, 0.1, 0.1) * 0.5);
+ scene->addSceneElement(boxShape);
+ boxShape->setPose(makeRigidTranslation(Vector3d(0.25, 0.3, 0.0)));
+
+ boxShape = createRigidBoxSceneElement("boxShape2", Vector3d(0.1, 0.1, 0.1) * 1.0);
+ scene->addSceneElement(boxShape);
+ boxShape->setPose(makeRigidTranslation(Vector3d(0.35, 0.3, 0.0)));
+
+ runTest(Vector3d(0.0, 0.0, 1.0), Vector3d::Zero(), 2500.0);
+}
+
+TEST_F(RenderTests, VisualTestRigidBodiesSlidingOnPlanes)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRigidTranslation;
+
+ Eigen::AngleAxisd aaTiltForward(0.15, Vector3d(1.0, 0.0, 0.0));
+ Eigen::AngleAxisd aaTiltBackward(-0.15, Vector3d(1.0, 0.0, 0.0));
+
+ // Mesh-base objects
+ std::shared_ptr<SurgSim::Framework::SceneElement> boxMesh =
+ createRigidMeshSceneElement("boxMesh", "box.ply");
+ scene->addSceneElement(boxMesh);
+ boxMesh->setPose(makeRigidTranslation(Vector3d(-0.3, 0.3, 0.0)));
+
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphereMesh =
+ createRigidMeshSceneElement("sphereMesh", "sphere.ply");
+ scene->addSceneElement(sphereMesh);
+ sphereMesh->setPose(makeRigidTranslation(Vector3d(-0.15, 0.3, 0.0)));
+
+ // Shape-base objects
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphereShape =
+ createRigidSphereSceneElement("sphereShape", 0.05);
+ scene->addSceneElement(sphereShape);
+ sphereShape->setPose(makeRigidTranslation(Vector3d(0.15, 0.3, 0.0)));
+
+ std::shared_ptr<SurgSim::Framework::SceneElement> boxShape =
+ createRigidBoxSceneElement("boxShape", Vector3d(0.1, 0.1, 0.1));
+ scene->addSceneElement(boxShape);
+ boxShape->setPose(makeRigidTranslation(Vector3d(0.3, 0.3, 0.0)));
+
+ // Floors on which the objects are falling (both tilted)
+ std::shared_ptr<SurgSim::Framework::SceneElement> floor1 = createFixedPlaneSceneElement("floor1");
+ scene->addSceneElement(floor1);
+ floor1->setPose(makeRigidTransform(SurgSim::Math::Quaterniond(aaTiltForward), Vector3d(0.0, -0.2, -0.1)));
+
+ std::shared_ptr<SurgSim::Framework::SceneElement> floor2 = createFixedPlaneSceneElement("floor2");
+ scene->addSceneElement(floor2);
+ floor2->setPose(makeRigidTransform(SurgSim::Math::Quaterniond(aaTiltBackward), Vector3d(0.0, -0.2, -0.1)));
+
+ runTest(Vector3d(0.0, 0.0, 1.0), Vector3d::Zero(), 13000.0);
+}
+
+TEST_F(RenderTests, VisualTestRigidBodiesStacking)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRigidTranslation;
+
+ const size_t numBodiesStacked = 3;
+
+ // Mesh-base objects
+ for (size_t i = 0; i < numBodiesStacked; ++i)
+ {
+ std::stringstream ss;
+ ss << "boxMesh" << i;
+ double scale = 1.0 - static_cast<double>(i) / static_cast<double>(numBodiesStacked);
+ std::shared_ptr<SurgSim::Framework::SceneElement> boxMesh =
+ createRigidMeshSceneElement(ss.str(), "box.ply", scale);
+ scene->addSceneElement(boxMesh);
+ Eigen::AngleAxisd aa(0.15 * i, Vector3d(0.0, 1.0, 0.0));
+ boxMesh->setPose(makeRigidTransform(SurgSim::Math::Quaterniond(aa), Vector3d(-0.3, 0.3 + 0.15 * i, 0.0)));
+ }
+
+ for (size_t i = 0; i < numBodiesStacked; ++i)
+ {
+ std::stringstream ss;
+ ss << "sphereMesh" << i;
+ double scale = 1.0 - static_cast<double>(i) / static_cast<double>(numBodiesStacked);
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphereMesh =
+ createRigidMeshSceneElement(ss.str(), "sphere.ply", scale);
+ scene->addSceneElement(sphereMesh);
+ sphereMesh->setPose(makeRigidTranslation(Vector3d(-0.15, 0.3 + 0.15 * i, 0.0)));
+ }
+
+ // Shape-base objects
+ for (size_t i = 0; i < numBodiesStacked; ++i)
+ {
+ std::stringstream ss;
+ ss << "sphereShape" << i;
+ double scale = 1.0 - static_cast<double>(i) / static_cast<double>(numBodiesStacked);
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphereShape =
+ createRigidSphereSceneElement(ss.str(), 0.05 * scale);
+ scene->addSceneElement(sphereShape);
+ sphereShape->setPose(makeRigidTranslation(Vector3d(0.15, 0.3 + 0.15 * i, 0.0)));
+ }
+
+ for (size_t i = 0; i < numBodiesStacked; ++i)
+ {
+ std::stringstream ss;
+ ss << "boxShape" << i;
+ double scale = 1.0 - static_cast<double>(i) / static_cast<double>(numBodiesStacked);
+ std::shared_ptr<SurgSim::Framework::SceneElement> boxShape =
+ createRigidBoxSceneElement(ss.str(), Vector3d::Ones() * 0.1 * scale);
+ scene->addSceneElement(boxShape);
+ boxShape->setPose(makeRigidTranslation(Vector3d(0.3, 0.3 + 0.15 * i, 0.0)));
+ }
+
+ // Floor on which the objects are falling
+ std::shared_ptr<SurgSim::Framework::SceneElement> floor = createFixedPlaneSceneElement("floor");
+ scene->addSceneElement(floor);
+ floor->setPose(makeRigidTranslation(Vector3d(0.0, -0.2, 0.0)));
+
+ runTest(Vector3d(0.0, 0.0, 1.0), Vector3d::Zero(), 10000.0);
+}
+
+TEST_F(RenderTests, VisualTestRigidBodiesStackingReversed)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRigidTranslation;
+
+ const size_t numBodiesStacked = 3;
+
+ // Mesh-base objects
+ for (size_t i = 0; i < numBodiesStacked; ++i)
+ {
+ std::stringstream ss;
+ ss << "boxMesh" << i;
+ double scale = 1.0 / static_cast<double>(numBodiesStacked) +
+ static_cast<double>(i) / static_cast<double>(numBodiesStacked);
+ std::shared_ptr<SurgSim::Framework::SceneElement> boxMesh =
+ createRigidMeshSceneElement(ss.str(), "box.ply", scale);
+ scene->addSceneElement(boxMesh);
+ Eigen::AngleAxisd aa(0.15 * i, Vector3d(0.0, 1.0, 0.0));
+ boxMesh->setPose(makeRigidTransform(SurgSim::Math::Quaterniond(aa), Vector3d(-0.3, 0.3 + 0.15 * i, 0.0)));
+ }
+
+ for (size_t i = 0; i < numBodiesStacked; ++i)
+ {
+ std::stringstream ss;
+ ss << "sphereMesh" << i;
+ double scale = 1.0 / static_cast<double>(numBodiesStacked) +
+ static_cast<double>(i) / static_cast<double>(numBodiesStacked);
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphereMesh =
+ createRigidMeshSceneElement(ss.str(), "sphere.ply", scale);
+ scene->addSceneElement(sphereMesh);
+ sphereMesh->setPose(makeRigidTranslation(Vector3d(-0.15, 0.3 + 0.15 * i, 0.0)));
+ }
+
+ // Shape-base objects
+ for (size_t i = 0; i < numBodiesStacked; ++i)
+ {
+ std::stringstream ss;
+ ss << "sphereShape" << i;
+ double scale = 1.0 / static_cast<double>(numBodiesStacked) +
+ static_cast<double>(i) / static_cast<double>(numBodiesStacked);
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphereShape =
+ createRigidSphereSceneElement(ss.str(), 0.05 * scale);
+ scene->addSceneElement(sphereShape);
+ sphereShape->setPose(makeRigidTranslation(Vector3d(0.15, 0.3 + 0.15 * i, 0.0)));
+ }
+
+ for (size_t i = 0; i < numBodiesStacked; ++i)
+ {
+ std::stringstream ss;
+ ss << "boxShape" << i;
+ double scale = 1.0 / static_cast<double>(numBodiesStacked) +
+ static_cast<double>(i) / static_cast<double>(numBodiesStacked);
+ std::shared_ptr<SurgSim::Framework::SceneElement> boxShape =
+ createRigidBoxSceneElement(ss.str(), Vector3d::Ones() * 0.1 * scale);
+ scene->addSceneElement(boxShape);
+ boxShape->setPose(makeRigidTranslation(Vector3d(0.3, 0.3 + 0.15 * i, 0.0)));
+ }
+
+ // Floor on which the objects are falling
+ std::shared_ptr<SurgSim::Framework::SceneElement> floor = createFixedPlaneSceneElement("floor");
+ scene->addSceneElement(floor);
+ floor->setPose(makeRigidTranslation(Vector3d(0.0, -0.2, 0.0)));
+
+ runTest(Vector3d(0.0, 0.0, 1.0), Vector3d::Zero(), 10000.0);
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RenderTests/config.txt.in b/SurgSim/Physics/RenderTests/config.txt.in
new file mode 100644
index 0000000..c24d2f9
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/config.txt.in
@@ -0,0 +1,2 @@
+${CMAKE_CURRENT_SOURCE_DIR}/Data/
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data
\ No newline at end of file
diff --git a/SurgSim/Physics/Representation.cpp b/SurgSim/Physics/Representation.cpp
new file mode 100644
index 0000000..9473952
--- /dev/null
+++ b/SurgSim/Physics/Representation.cpp
@@ -0,0 +1,133 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+Representation::Representation(const std::string& name) :
+ SurgSim::Framework::Representation(name),
+ m_collisionRepresentation(nullptr),
+ m_gravity(0.0, -9.81, 0.0),
+ m_numDof(0),
+ m_isGravityEnabled(true),
+ m_isDrivingSceneElementPose(true)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, size_t, NumDof, getNumDof, setNumDof);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, bool, IsGravityEnabled, isGravityEnabled, setIsGravityEnabled);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, bool, IsDrivingSceneElementPose,
+ isDrivingSceneElementPose, setIsDrivingSceneElementPose);
+}
+
+Representation::~Representation()
+{
+}
+
+void Representation::resetState()
+{
+}
+
+void Representation::resetParameters()
+{
+}
+
+size_t Representation::getNumDof() const
+{
+ return m_numDof;
+}
+
+void Representation::setIsGravityEnabled(bool isGravityEnabled)
+{
+ m_isGravityEnabled = isGravityEnabled;
+}
+
+bool Representation::isGravityEnabled() const
+{
+ return m_isGravityEnabled;
+}
+
+void Representation::setIsDrivingSceneElementPose(bool isDrivingSceneElementPose)
+{
+ m_isDrivingSceneElementPose = isDrivingSceneElementPose;
+}
+
+bool Representation::isDrivingSceneElementPose()
+{
+ return m_isDrivingSceneElementPose;
+}
+
+void Representation::beforeUpdate(double dt)
+{
+}
+
+void Representation::update(double dt)
+{
+}
+
+void Representation::afterUpdate(double dt)
+{
+}
+
+std::shared_ptr<Localization> Representation::createLocalization(const SurgSim::DataStructures::Location& location)
+{
+ return nullptr;
+}
+
+void Representation::applyCorrection(double dt, const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity)
+{
+}
+
+void Representation::setNumDof(size_t numDof)
+{
+ m_numDof = numDof;
+}
+
+const SurgSim::Math::Vector3d& Representation::getGravity() const
+{
+ return m_gravity;
+}
+
+std::shared_ptr<SurgSim::Collision::Representation> Representation::getCollisionRepresentation() const
+{
+ return m_collisionRepresentation;
+}
+
+void Representation::setCollisionRepresentation(std::shared_ptr<SurgSim::Collision::Representation> val)
+{
+ m_collisionRepresentation = val;
+}
+
+void Representation::driveSceneElementPose(const SurgSim::Math::RigidTransform3d& pose)
+{
+ if (isDrivingSceneElementPose())
+ {
+ std::shared_ptr<SurgSim::Framework::SceneElement> sceneElement = getSceneElement();
+ if (sceneElement != nullptr)
+ {
+ sceneElement->setPose(pose);
+ }
+ }
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/Representation.h b/SurgSim/Physics/Representation.h
new file mode 100644
index 0000000..6496d48
--- /dev/null
+++ b/SurgSim/Physics/Representation.h
@@ -0,0 +1,178 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_REPRESENTATION_H
+#define SURGSIM_PHYSICS_REPRESENTATION_H
+
+#include <string>
+
+#include "SurgSim/Framework/Representation.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+struct Location;
+}
+
+namespace Collision
+{
+class Representation;
+}
+
+namespace Physics
+{
+
+class Localization;
+
+enum RepresentationType
+{
+ REPRESENTATION_TYPE_INVALID = -1,
+ REPRESENTATION_TYPE_FIXED = 0,
+ REPRESENTATION_TYPE_RIGID,
+ REPRESENTATION_TYPE_VTC_RIGID,
+ REPRESENTATION_TYPE_MASSSPRING,
+ REPRESENTATION_TYPE_FEM1D,
+ REPRESENTATION_TYPE_FEM2D,
+ REPRESENTATION_TYPE_FEM3D,
+ REPRESENTATION_TYPE_COUNT
+};
+
+/// The Representation class defines the base class for all physics objects
+class Representation : public SurgSim::Framework::Representation
+{
+public:
+ /// Constructor
+ /// \param name The representation's name
+ explicit Representation(const std::string& name);
+
+ /// Destructor
+ virtual ~Representation();
+
+ /// Query the representation type
+ /// \return the RepresentationType for this representation
+ virtual RepresentationType getType() const = 0;
+
+ /// Reset the representation to its initial/default state
+ virtual void resetState();
+
+ /// Reset the representation parameters to their initial/default values
+ virtual void resetParameters();
+
+ /// Query the object number of degrees of freedom
+ /// \return The number of degrees of freedom
+ size_t getNumDof() const;
+
+ /// Set the gravity enable flag
+ /// \param isGravityEnabled True if gravity enabled, false if not.
+ void setIsGravityEnabled(bool isGravityEnabled);
+
+ /// Get the gravity enable flag
+ /// \return true if gravity enabled, false if not.
+ bool isGravityEnabled() const;
+
+ /// Set whether this Representation is controlling the pose of the SceneElement
+ /// that it is part of.
+ /// \param isDrivingSceneElementPose true if this Representation is driving the pose of the SceneElement
+ void setIsDrivingSceneElementPose(bool isDrivingSceneElementPose);
+
+ /// Query if this Representation is controlling the pose of the SceneElement
+ /// that it is part of.
+ /// \return true if this Representation is controlling the pose of the SceneElement
+ bool isDrivingSceneElementPose();
+
+ /// Preprocessing done before the update call
+ /// This needs to be called from the outside usually from a Computation
+ /// \param dt The time step (in seconds)
+ virtual void beforeUpdate(double dt);
+
+ /// Update the representation state to the current time step
+ /// \param dt The time step (in seconds)
+ virtual void update(double dt);
+
+ /// Postprocessing done after the update call
+ /// This needs to be called from the outside usually from a Computation
+ /// \param dt The time step (in seconds)
+ virtual void afterUpdate(double dt);
+
+ /// Computes a localized coordinate w.r.t this representation, given a Location object.
+ /// \param location A location in 3d space.
+ /// \return A localization object for the given location.
+ virtual std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location& location);
+
+ /// Update the Representation's current position and velocity using a time interval, dt, and change in velocity,
+ /// deltaVelocity.
+ ///
+ /// This function typically is called in the physics pipeline (PhysicsManager::doUpdate) after solving the equations
+ /// that enforce constraints when collisions occur. Specifically it is called in the PushResults::doUpdate step.
+ /// \param dt The time step
+ /// \param deltaVelocity The block of a vector containing the correction to be applied to the velocity
+ virtual void applyCorrection(double dt, const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity);
+
+ /// \return the collision representation for this physics representation.
+ std::shared_ptr<SurgSim::Collision::Representation> getCollisionRepresentation() const;
+
+ /// Set the collision representation for this physics representation, when the collision object
+ /// is involved in a collision, the collision should be resolved inside the dynamics calculation.
+ /// \param representation The appropriate collision representation for this object.
+ virtual void setCollisionRepresentation(std::shared_ptr<SurgSim::Collision::Representation> representation);
+
+protected:
+ /// Set the number of degrees of freedom
+ /// \param numDof The number of degrees of freedom
+ /// \note protected so that nobody can change the number of DOF
+ /// \note except daughter classes
+ void setNumDof(size_t numDof);
+
+ /// Get the gravity used by this Representation
+ /// \return The gravity vector
+ const SurgSim::Math::Vector3d& getGravity() const;
+
+ /// This entity's collision representation, these are usually very specific to the physics representation
+ std::shared_ptr<SurgSim::Collision::Representation> m_collisionRepresentation;
+
+ /// This conditionally updates that pose for the scenelement to the given pose
+ /// The update gets exectuded if the representation actually has sceneelement and isDrivingScenElement() is true
+ /// \param pose New pose for the SceneElement
+ void driveSceneElementPose(const SurgSim::Math::RigidTransform3d& pose);
+
+private:
+ /// NO copy constructor
+ Representation(const Representation&);
+
+ /// NO assignment operator
+ Representation& operator =(const Representation&);
+
+ /// Gravity vector
+ const SurgSim::Math::Vector3d m_gravity;
+
+ /// Number of degrees of freedom for this representation
+ size_t m_numDof;
+
+ /// Gravity enabled flag
+ bool m_isGravityEnabled;
+
+ /// Is this representation driving the sceneElement pose
+ bool m_isDrivingSceneElementPose;
+
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_REPRESENTATION_H
diff --git a/SurgSim/Physics/RigidCollisionRepresentation.cpp b/SurgSim/Physics/RigidCollisionRepresentation.cpp
new file mode 100644
index 0000000..209dccf
--- /dev/null
+++ b/SurgSim/Physics/RigidCollisionRepresentation.cpp
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Physics/RigidRepresentationBase.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::RigidCollisionRepresentation,
+ RigidCollisionRepresentation);
+
+RigidCollisionRepresentation::RigidCollisionRepresentation(const std::string& name):
+ Representation(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidCollisionRepresentation, std::shared_ptr<SurgSim::Math::Shape>,
+ Shape, getShape, setShape);
+}
+
+RigidCollisionRepresentation::~RigidCollisionRepresentation()
+{
+}
+
+void RigidCollisionRepresentation::update(const double& dt)
+{
+ auto meshShape = std::dynamic_pointer_cast<SurgSim::Math::MeshShape>(getShape());
+ if (nullptr != meshShape && nullptr != meshShape->getMesh() && meshShape->getMesh()->isValid())
+ {
+ meshShape->setPose(getPose());
+ }
+}
+
+void RigidCollisionRepresentation::setRigidRepresentation(
+ std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> representation)
+{
+ m_physicsRepresentation = representation;
+}
+
+std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> RigidCollisionRepresentation::getRigidRepresentation()
+{
+ return m_physicsRepresentation.lock();
+}
+
+int RigidCollisionRepresentation::getShapeType() const
+{
+ return getShape()->getType();
+}
+
+const std::shared_ptr<SurgSim::Math::Shape> RigidCollisionRepresentation::getShape() const
+{
+ if (m_shape != nullptr)
+ {
+ return m_shape;
+ }
+ else
+ {
+ auto physicsRepresentation = m_physicsRepresentation.lock();
+ SURGSIM_ASSERT(physicsRepresentation != nullptr) <<
+ "PhysicsRepresentation went out of scope for Collision Representation " << getName();
+ return physicsRepresentation->getShape();
+ }
+}
+
+void RigidCollisionRepresentation::setShape(std::shared_ptr<SurgSim::Math::Shape> shape)
+{
+ m_shape = shape;
+}
+
+SurgSim::Math::RigidTransform3d RigidCollisionRepresentation::getPose() const
+{
+ auto physicsRepresentation = m_physicsRepresentation.lock();
+ SURGSIM_ASSERT(physicsRepresentation != nullptr) <<
+ "PhysicsRepresentation went out of scope for Collision Representation " << getName();
+ const SurgSim::Math::RigidTransform3d& physicsPose = physicsRepresentation->getCurrentState().getPose();
+ return physicsPose * physicsRepresentation->getLocalPose().inverse() * getLocalPose();
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RigidCollisionRepresentation.h b/SurgSim/Physics/RigidCollisionRepresentation.h
new file mode 100644
index 0000000..c843268
--- /dev/null
+++ b/SurgSim/Physics/RigidCollisionRepresentation.h
@@ -0,0 +1,83 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_RIGIDCOLLISIONREPRESENTATION_H
+#define SURGSIM_PHYSICS_RIGIDCOLLISIONREPRESENTATION_H
+
+#include <memory>
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+class RigidRepresentationBase;
+
+SURGSIM_STATIC_REGISTRATION(RigidCollisionRepresentation);
+
+/// Collision Representation class that wraps a RigidRepresentation, this can be used to strictly tie the
+/// Collision Representation to the Rigid, so even if the shape of the rigid changes, the collision representation
+/// will use the appropriate shape.
+class RigidCollisionRepresentation : public SurgSim::Collision::Representation
+{
+public:
+ /// Constructor
+ /// \param name The name of rigid collision representation
+ explicit RigidCollisionRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~RigidCollisionRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::RigidCollisionRepresentation);
+
+ virtual void update(const double& dt) override;
+
+ /// Get the pose of the representation
+ /// \return The pose of this representation
+ virtual SurgSim::Math::RigidTransform3d getPose() const override;
+
+ /// Get the shape type id
+ /// \return The unique type of the shape, used to determine which calculation to use.
+ virtual int getShapeType() const override;
+
+ /// Get the shape
+ /// \return The actual shape used for collision.
+ virtual const std::shared_ptr<SurgSim::Math::Shape> getShape() const override;
+
+ /// Set the shape
+ /// The default is to use the shape of the Rigid Representation, this
+ /// will override that shape.
+ /// \param shape The actual shape used for collision, if nullptr the Rigid Representation shape will be used.
+ void setShape(std::shared_ptr<SurgSim::Math::Shape> shape);
+
+ /// Set rigid representation
+ /// \param representation The rigid representation
+ void setRigidRepresentation(std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> representation);
+
+ /// Gets physics representation.
+ /// \return The physics representation.
+ std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> getRigidRepresentation();
+
+private:
+ std::weak_ptr<SurgSim::Physics::RigidRepresentationBase> m_physicsRepresentation;
+ std::shared_ptr<SurgSim::Math::Shape> m_shape;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_RIGIDCOLLISIONREPRESENTATION_H
diff --git a/SurgSim/Physics/RigidRepresentation.cpp b/SurgSim/Physics/RigidRepresentation.cpp
new file mode 100644
index 0000000..053ee8a
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentation.cpp
@@ -0,0 +1,465 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Math/Valid.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+#include "SurgSim/Physics/RigidRepresentationState.h"
+
+namespace
+{
+const double rotationVectorEpsilon = 1e-8;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::RigidRepresentation, RigidRepresentation);
+
+RigidRepresentation::RigidRepresentation(const std::string& name) :
+ RigidRepresentationBase(name),
+ m_globalInertia(SurgSim::Math::Matrix33d::Zero()),
+ m_invGlobalInertia(SurgSim::Math::Matrix33d::Zero()),
+ m_force(SurgSim::Math::Vector3d::Zero()),
+ m_torque(SurgSim::Math::Vector3d::Zero()),
+ m_C(SurgSim::Math::Matrix66d::Zero()),
+ m_externalGeneralizedForce(SurgSim::Math::Vector6d::Zero()),
+ m_externalGeneralizedStiffness(SurgSim::Math::Matrix66d::Zero()),
+ m_externalGeneralizedDamping(SurgSim::Math::Matrix66d::Zero())
+{
+ // Initialize the number of degrees of freedom
+ // 6 for a rigid body velocity-based (linear and angular velocities are the Dof)
+ setNumDof(6);
+}
+
+RigidRepresentation::~RigidRepresentation()
+{
+}
+
+SurgSim::Physics::RepresentationType RigidRepresentation::getType() const
+{
+ return REPRESENTATION_TYPE_RIGID;
+}
+
+void RigidRepresentation::addExternalGeneralizedForce(const SurgSim::Math::Vector6d& generalizedForce,
+ const SurgSim::Math::Matrix66d& K,
+ const SurgSim::Math::Matrix66d& D)
+{
+ m_externalGeneralizedForce += generalizedForce;
+ m_externalGeneralizedStiffness += K;
+ m_externalGeneralizedDamping += D;
+}
+
+void RigidRepresentation::addExternalGeneralizedForce(const SurgSim::DataStructures::Location& location,
+ const SurgSim::Math::Vector6d& generalizedForce,
+ const SurgSim::Math::Matrix66d& K,
+ const SurgSim::Math::Matrix66d& D)
+{
+ using SurgSim::Math::Matrix33d;
+ using SurgSim::Math::Matrix66d;
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::Vector6d;
+
+ SURGSIM_ASSERT(location.rigidLocalPosition.hasValue()) << "Invalid location (no rigid local position)";
+
+ RigidRepresentationLocalization localization;
+ localization.setRepresentation(std::static_pointer_cast<Representation>(shared_from_this()));
+ localization.setLocalPosition(location.rigidLocalPosition.getValue());
+ const Vector3d point = localization.calculatePosition();
+ const Vector3d massCenter = getCurrentState().getPose() * getMassCenter();
+ const Vector3d lever = point - massCenter;
+ auto force = generalizedForce.segment<3>(0);
+ const Vector3d torque = lever.cross(force);
+
+ // add the generalized force
+ m_externalGeneralizedForce += generalizedForce;
+ // add the generalized stiffness matrix
+ m_externalGeneralizedStiffness += K;
+ // add the generalized damping matrix
+ m_externalGeneralizedDamping += D;
+
+ // add the extra torque produced by the lever
+ m_externalGeneralizedForce.segment<3>(3) += torque;
+
+ // add the extra terms in the stiffness matrix
+ const Vector3d leverInLocalSpace = getCurrentState().getPose().rotation().inverse() * lever;
+ const Eigen::AngleAxisd angleAxis(getCurrentState().getPose().rotation());
+ double angle = angleAxis.angle();
+ const Vector3d axis = angleAxis.axis();
+ const Vector3d rotationVector = axis * angle;
+ double rotationVectorNorm = rotationVector.norm();
+ double rotationVectorNormCubic = rotationVectorNorm * rotationVectorNorm * rotationVectorNorm;
+ double sinAngle = sin(angle);
+ double cosAngle = cos(angle);
+ double oneMinusCos = 1.0 - cosAngle;
+ const Matrix33d skewAxis = SurgSim::Math::makeSkewSymmetricMatrix(axis);
+ Matrix33d dRdAxisX;
+ Matrix33d dRdAxisY;
+ Matrix33d dRdAxisZ;
+ Matrix33d dRdAngle =
+ -sinAngle * Matrix33d::Identity() + cosAngle * skewAxis + sinAngle * axis * axis.transpose();
+ dRdAxisX << oneMinusCos * 2.0 * axis[0], oneMinusCos * axis[1], oneMinusCos * axis[2],
+ oneMinusCos * axis[1], 0.0, -sinAngle,
+ oneMinusCos * axis[2], sinAngle, 0.0;
+ dRdAxisY << 0.0, oneMinusCos * axis[0], sinAngle,
+ oneMinusCos * axis[0], oneMinusCos * 2.0 * axis[1], oneMinusCos * axis[2],
+ -sinAngle, oneMinusCos * axis[2], 0.0;
+ dRdAxisZ << 0.0, -sinAngle, oneMinusCos * axis[0],
+ sinAngle, 0.0, oneMinusCos * axis[1],
+ oneMinusCos * axis[0], oneMinusCos * axis[1], oneMinusCos * 2.0 * axis[2];
+ Vector3d dAngledRotationVector, dAxisXdRotationVector, dAxisYdRotationVector, dAxisZdRotationVector;
+ if (std::abs(rotationVectorNorm) > rotationVectorEpsilon)
+ {
+ const Vector3d tmp = rotationVector / rotationVectorNormCubic;
+ dAngledRotationVector = rotationVector / rotationVectorNorm;
+ dAxisXdRotationVector = Vector3d::UnitX() / rotationVectorNorm - rotationVector[0] * tmp;
+ dAxisYdRotationVector = Vector3d::UnitY() / rotationVectorNorm - rotationVector[1] * tmp;
+ dAxisZdRotationVector = Vector3d::UnitZ() / rotationVectorNorm - rotationVector[2] * tmp;
+ }
+ else
+ {
+ // Development around theta ~ 0 => sin(theta) ~ theta
+ // We simplify by theta across the products dRdAxis[alpha].dAxis[alpha]dRotationVector[beta]
+ dAngledRotationVector = Vector3d::Zero();
+ dAxisXdRotationVector = Vector3d::UnitX();
+ dAxisYdRotationVector = Vector3d::UnitY();
+ dAxisZdRotationVector = Vector3d::UnitZ();
+
+ dRdAxisX << 0.0, 0.0, 0.0,
+ 0.0, 0.0, -1.0,
+ 0.0, 1.0, 0.0;
+
+ dRdAxisY << 0.0, 0.0, 1.0,
+ 0.0, 0.0, 0.0,
+ -1.0, 0.0, 0.0;
+
+ dRdAxisZ << 0.0, -1.0, 0.0,
+ 1.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0;
+ }
+ // add the extra stiffness terms produced by the lever
+ for (size_t column = 0; column < 6; ++column)
+ {
+ m_externalGeneralizedStiffness.block<3, 1>(3, column) += lever.cross(K.block<3, 1>(0, column));
+ }
+ // add extra term - dCP/dW[alpha] ^ F = - dR.CP(local)/dW[alpha] ^ F = -dR/dW[alpha].CP(local) ^ F
+ // = -[(dR/dangle.dangle/dW[alpha] + dR/daxisX.daxisX/dW[alpha] +
+ // dR/daxisY.daxisY/dW[alpha] + dR/daxisZ.daxisZ/dW[alpha]) . CP(local)] ^ F
+ for (size_t i = 0; i < 3; ++i)
+ {
+ m_externalGeneralizedStiffness.block<3, 1>(3, 3 + i) +=
+ -((dRdAngle * dAngledRotationVector[i] +
+ dRdAxisX * dAxisXdRotationVector[i] +
+ dRdAxisY * dAxisYdRotationVector[i] +
+ dRdAxisZ * dAxisZdRotationVector[i]) * leverInLocalSpace).cross(force);
+ }
+
+ // add the extra damping terms produced by the lever
+ for (size_t column = 0; column < 6; ++column)
+ {
+ m_externalGeneralizedDamping.block<3, 1>(3, column) += lever.cross(D.block<3, 1>(0, column));
+ }
+}
+
+const SurgSim::Math::Vector6d& RigidRepresentation::getExternalGeneralizedForce() const
+{
+ return m_externalGeneralizedForce;
+}
+
+const SurgSim::Math::Matrix66d& RigidRepresentation::getExternalGeneralizedStiffness() const
+{
+ return m_externalGeneralizedStiffness;
+}
+
+const SurgSim::Math::Matrix66d& RigidRepresentation::getExternalGeneralizedDamping() const
+{
+ return m_externalGeneralizedDamping;
+}
+
+void RigidRepresentation::beforeUpdate(double dt)
+{
+ RigidRepresentationBase::beforeUpdate(dt);
+
+ SURGSIM_LOG_IF(!m_parametersValid,
+ SurgSim::Framework::Logger::getDefaultLogger(), WARNING) << getName() <<
+ " deactivated in beforeUpdate because parameters are not valid." << std::endl;
+ if (!m_parametersValid)
+ {
+ setLocalActive(false);
+ return;
+ }
+}
+
+void RigidRepresentation::update(double dt)
+{
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::Matrix33d;
+ using SurgSim::Math::Quaterniond;
+
+ SURGSIM_LOG_IF(!m_parametersValid,
+ SurgSim::Framework::Logger::getDefaultLogger(), WARNING) << getName() <<
+ " deactivated in update because parameters are not valid." << std::endl;
+ if (!m_parametersValid)
+ {
+ setLocalActive(false);
+ return;
+ }
+
+ // For convenience
+ Vector3d G = m_currentState.getPose() * getMassCenter();
+ Vector3d dG = m_currentState.getLinearVelocity();
+ Matrix33d R = m_currentState.getPose().linear();
+ Quaterniond q = Quaterniond(R);
+ Vector3d w = m_currentState.getAngularVelocity();
+ Quaterniond dq;
+ double qNorm = q.norm(); // Norm of q before normalization
+
+ // Rigid body dynamics (using backward euler numerical integration scheme):
+ // { Id33.m.(v(t+dt) - v(t))/dt = f
+ // { I .(w(t+dt) - w(t))/dt = t - w(t)^(I.w(t))
+ //
+ // Integrating the Rayleigh damping on the velocity level:
+ // { Id33.m.(1/dt + alphaLinear ).v(t+dt) = Id33.m.v(t)/dt + f
+ // { I .(1/dt + alphaAngular).w(t+dt) = I.w(t)/dt + t - w(t)^(I.w(t))
+
+ // Compute external forces/torques
+ m_force.setZero();
+ m_torque.setZero();
+ m_force += m_externalGeneralizedForce.segment<3>(0);
+ m_torque += m_externalGeneralizedForce.segment<3>(3);
+ if (isGravityEnabled())
+ {
+ m_force += getGravity() * getMass();
+ }
+ m_torque -= w.cross(m_globalInertia * w);
+
+ // Add Backward Euler RHS terms
+ m_force += getMass() * dG / dt;
+ m_torque += m_globalInertia * w / dt;
+
+ // Solve the 6D system on the velocity level
+ {
+ // { Id33.m.(1/dt + alphaLinear ).v(t+dt) = Id33.m.v(t)/dt + f
+ // { I .(1/dt + alphaAngular).w(t+dt) = I.w(t)/dt + t - w(t)^(I.w(t))
+ dG = (1.0 / getMass()) * m_force / (1.0 / dt + getLinearDamping());
+ w = m_invGlobalInertia * m_torque / (1.0 / dt + getAngularDamping());
+ // Compute the quaternion velocity as well: dq = 1/2.(0 w).q
+ dq = Quaterniond(0.0, w[0], w[1], w[2]) * q;
+ dq.coeffs() *= 0.5;
+ }
+ m_currentState.setLinearVelocity(dG);
+ m_currentState.setAngularVelocity(w);
+
+ // Integrate the velocities to get the rigid representation pose
+ {
+ // G(t+dt) = G(t) + dt. dG(t+dt)
+ G += dG * dt;
+ // q(t+dt) = q(t) + dt. dq(t+dt)
+ Quaterniond dq_dt = dq;
+ dq_dt.coeffs() *= dt;
+ q.coeffs() += dq_dt.coeffs();
+ // Normalize the quaternion to make sure we do have a rotation
+ q.normalize();
+ }
+ R = q.matrix();
+ m_currentState.setPose(SurgSim::Math::makeRigidTransform(R, G-R*getMassCenter()));
+
+ // Compute the global inertia matrix with the current state
+ updateGlobalInertiaMatrices(m_currentState);
+
+ // If something went wrong, we deactivate the representation
+ bool condition = SurgSim::Math::isValid(G);
+ condition &= SurgSim::Math::isValid(dG);
+ condition &= SurgSim::Math::isValid(w);
+ condition &= qNorm != 0.0;
+ condition &= SurgSim::Math::isValid(q);
+ condition &= fabs(1.0 - q.norm()) < 1e-3;
+ SURGSIM_LOG_IF(!condition, SurgSim::Framework::Logger::getDefaultLogger(), WARNING) << getName() <<
+ " deactivated and reset because:" << std::endl <<
+ "G=(" << G[0] << "," << G[1] << "," << G[2] << "), " <<
+ "dG=(" << dG[0] << "," << dG[1] << "," << dG[2] << "), " <<
+ "w=(" << w[0] << "," << w[1] << "," << w[2] << "), " <<
+ "q=(" << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << "), " <<
+ "|q| before normalization=" << qNorm << ", and " <<
+ "|q| after normalization=" << q.norm() << std::endl;
+ if (!condition)
+ {
+ setLocalActive(false);
+ }
+
+ // Prepare the compliance matrix
+ computeComplianceMatrix(dt);
+}
+
+void RigidRepresentation::afterUpdate(double dt)
+{
+ RigidRepresentationBase::afterUpdate(dt);
+
+ SURGSIM_LOG_IF(!m_parametersValid,
+ SurgSim::Framework::Logger::getDefaultLogger(), WARNING) << getName() <<
+ " deactivated in afterUpdate because parameters are not valid." << std::endl;
+ if (!m_parametersValid)
+ {
+ setLocalActive(false);
+ return;
+ }
+
+ m_externalGeneralizedForce.setZero();
+ m_externalGeneralizedStiffness.setZero();
+ m_externalGeneralizedDamping.setZero();
+}
+
+void RigidRepresentation::applyCorrection(double dt,
+ const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity)
+{
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::Matrix33d;
+ using SurgSim::Math::Quaterniond;
+
+ SURGSIM_LOG_IF(!m_parametersValid,
+ SurgSim::Framework::Logger::getDefaultLogger(), WARNING) << getName() <<
+ " deactivated in applyCorrection because parameters are not valid." << std::endl;
+ if (!m_parametersValid)
+ {
+ setLocalActive(false);
+ return;
+ }
+
+ Vector3d G = m_currentState.getPose() * getMassCenter();
+ Vector3d dG = m_currentState.getLinearVelocity();
+ Matrix33d R = m_currentState.getPose().linear();
+ Quaterniond q = Quaterniond(R);
+ Vector3d w = m_currentState.getAngularVelocity();
+
+ const Vector3d& delta_dG = deltaVelocity.segment(0, 3);
+ const Vector3d& delta_w = deltaVelocity.segment(3, 3);
+ Quaterniond delta_dq = Quaterniond(0.0, delta_w[0], delta_w[1], delta_w[2]) * q;
+ delta_dq.coeffs() *= 0.5;
+
+ dG += delta_dG;
+ w += delta_w;
+ m_currentState.setLinearVelocity(dG);
+ m_currentState.setAngularVelocity(w);
+
+ // Integrate the velocities to get the rigid representation pose
+ {
+ // G(t+dt) = G(t) + dt. delta_dG(t+dt)
+ G += delta_dG * dt;
+ // q(t+dt) = q(t) + dt. delta_dq(t+dt)
+ q.coeffs() += delta_dq.coeffs() * dt;
+ // Normalize the quaternion to make sure we do have a rotation
+ q.normalize();
+ }
+ R = q.matrix();
+ m_currentState.setPose(SurgSim::Math::makeRigidTransform(R, G-R*getMassCenter()));
+
+ // Compute the global inertia matrix with the current state
+ updateGlobalInertiaMatrices(m_currentState);
+
+ // If something went wrong, we deactivate the representation
+ bool condition = SurgSim::Math::isValid(G);
+ condition &= SurgSim::Math::isValid(q);
+ condition &= fabs(1.0 - q.norm()) < 1e-3;
+ SURGSIM_LOG_IF(!condition, SurgSim::Framework::Logger::getDefaultLogger(), WARNING) << getName() <<
+ " deactivated and reset in applyCorrection because:" << std::endl <<
+ "G=(" << G[0] << "," << G[1] << "," << G[2] << "), " <<
+ "q=(" << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << "), " <<
+ "and |q| after normalization=" << q.norm() << std::endl;
+ if (!condition)
+ {
+ setLocalActive(false);
+ }
+
+ // Prepare the compliance matrix
+ computeComplianceMatrix(dt);
+}
+
+const Eigen::Matrix < double, 6, 6, Eigen::RowMajor > &
+SurgSim::Physics::RigidRepresentation::getComplianceMatrix() const
+{
+ return m_C;
+}
+
+void RigidRepresentation::computeComplianceMatrix(double dt)
+{
+ SURGSIM_LOG_IF(!m_parametersValid,
+ SurgSim::Framework::Logger::getDefaultLogger(), WARNING) << getName() <<
+ " deactivated in computComplianceMatrix because parameters are not valid." <<
+ std::endl;
+ if (!m_parametersValid)
+ {
+ setLocalActive(false);
+ return;
+ }
+
+ SurgSim::Math::Matrix66d systemMatrix;
+ const SurgSim::Math::Matrix33d identity3x3 = SurgSim::Math::Matrix33d::Identity();
+ systemMatrix.block<3, 3>(0, 0) = identity3x3 * (getMass() / dt + getLinearDamping());
+ systemMatrix.block<3, 3>(3, 3) = m_globalInertia / dt + getAngularDamping() * identity3x3;
+ systemMatrix += m_externalGeneralizedDamping + m_externalGeneralizedStiffness * dt;
+
+ m_C.setZero();
+
+ //Invert systemMatrix
+ //We can use this shortcut because we know the linear and angular terms are independent
+ m_C.block<3, 3>(0, 0) = systemMatrix.block<3, 3>(0, 0).inverse();
+ m_C.block<3, 3>(3, 3) = systemMatrix.block<3, 3>(3, 3).inverse();
+}
+
+void RigidRepresentation::updateGlobalInertiaMatrices(const RigidRepresentationState& state)
+{
+ if (!m_parametersValid)
+ {
+ // do not setIsActive(false) due to invalid parameters because RigidRepresentationBase::setInitialParameters may
+ // not have been called before RigidRepresentationBase::setInitialState (which calls this function), in which
+ // case the parameters are invalid but we should not deactivate the representation.
+ return;
+ }
+
+ const SurgSim::Math::Matrix33d& R = state.getPose().linear();
+ m_globalInertia = R * getLocalInertia() * R.transpose();
+ m_invGlobalInertia = m_globalInertia.inverse();
+}
+
+bool RigidRepresentation::doInitialize()
+{
+ bool result = RigidRepresentationBase::doInitialize();
+
+ if (result)
+ {
+ SURGSIM_ASSERT(getShape()->getVolume() > 0.0) << "Cannot use a shape with zero volume for RigidRepresentations";
+ }
+
+ return result;
+}
+
+void RigidRepresentation::setLinearVelocity(const SurgSim::Math::Vector3d& linearVelocity)
+{
+ m_currentState.setLinearVelocity(linearVelocity);
+}
+
+void RigidRepresentation::setAngularVelocity(const SurgSim::Math::Vector3d& angularVelocity)
+{
+ m_currentState.setAngularVelocity(angularVelocity);
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/RigidRepresentation.h b/SurgSim/Physics/RigidRepresentation.h
new file mode 100644
index 0000000..dccc87b
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentation.h
@@ -0,0 +1,139 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_RIGIDREPRESENTATION_H
+#define SURGSIM_PHYSICS_RIGIDREPRESENTATION_H
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Physics/RigidRepresentationBase.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+class RigidRepresentationState;
+class Localization;
+
+SURGSIM_STATIC_REGISTRATION(RigidRepresentation);
+
+/// \class RigidRepresentation
+/// The RigidRepresentation class defines the dynamic rigid body representation
+/// Note that the rigid representation is velocity-based, therefore its degrees of
+/// freedom are the linear and angular velocities: 6 Dof
+class RigidRepresentation : public RigidRepresentationBase
+{
+public:
+ /// Constructor
+ /// \param name The rigid representation's name
+ explicit RigidRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~RigidRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::RigidRepresentation);
+
+ virtual RepresentationType getType() const override;
+
+ /// Set the current linear velocity of the rigid representation
+ /// \param linearVelocity The linear velocity
+ void setLinearVelocity(const SurgSim::Math::Vector3d& linearVelocity);
+
+ /// Set the current angular velocity of the rigid representation
+ /// \param angularVelocity The angular velocity
+ void setAngularVelocity(const SurgSim::Math::Vector3d& angularVelocity);
+
+ /// Add an external generalized force applied to the rigid representation's mass center.
+ /// \note This force is generalized (i.e. it's a 6D vector, containing both 3D force and 3D torque).
+ /// \note The stiffness and damping are 6x6 matrices with coupling between the translational and rotation dof.
+ /// \note All external generalized forces will be zeroed every afterUpdate call of the rigid representation.
+ /// \param generalizedForce The external generalized force to apply at the mass center
+ /// \param K The stiffness matrix associated with the generalized force (jacobian of the force w.r.t position)
+ /// \param D The damping matrix associated with the generalized force (jacobian of the force w.r.t velocity)
+ void addExternalGeneralizedForce(const SurgSim::Math::Vector6d& generalizedForce,
+ const SurgSim::Math::Matrix66d& K = SurgSim::Math::Matrix66d::Zero(),
+ const SurgSim::Math::Matrix66d& D = SurgSim::Math::Matrix66d::Zero());
+
+ /// Add an external generalized force applied to the rigid representation (anywhere).
+ /// \note This force is generalized (i.e. it's a 6D vector, containing both 3D force and 3D torque).
+ /// \note The stiffness and damping are 6x6 matrices with coupling between the translational and rotation dof.
+ /// \note All external generalized forces will be zeroed every afterUpdate call of the rigid representation.
+ /// \param location The application point (must contain a rigid local position)
+ /// \param generalizedForce The external generalized force
+ /// \param K The stiffness matrix associated with generalizedForce (jacobian w.r.t position)
+ /// \param D The damping matrix associated with generalizedForce (jacobian w.r.t velocity)
+ void addExternalGeneralizedForce(const SurgSim::DataStructures::Location& location,
+ const SurgSim::Math::Vector6d& generalizedForce,
+ const SurgSim::Math::Matrix66d& K = SurgSim::Math::Matrix66d::Zero(),
+ const SurgSim::Math::Matrix66d& D = SurgSim::Math::Matrix66d::Zero());
+
+ /// \return the current external generalized 6D force
+ const SurgSim::Math::Vector6d& getExternalGeneralizedForce() const;
+
+ /// \return the current external generalized stiffness 6x6 matrix
+ const SurgSim::Math::Matrix66d& getExternalGeneralizedStiffness() const;
+
+ /// \return the current external generalized damping 6x6 matrix
+ const SurgSim::Math::Matrix66d& getExternalGeneralizedDamping() const;
+
+ virtual void beforeUpdate(double dt) override;
+
+ virtual void update(double dt) override;
+
+ virtual void afterUpdate(double dt) override;
+
+ void applyCorrection(double dt, const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity) override;
+
+ /// Retrieve the rigid body 6x6 compliance matrix
+ /// \return the 6x6 compliance matrix
+ const SurgSim::Math::Matrix66d& getComplianceMatrix() const;
+
+protected:
+ /// Inertia matrices in global coordinates
+ SurgSim::Math::Matrix33d m_globalInertia;
+ /// Inverse of inertia matrix in global coordinates
+ SurgSim::Math::Matrix33d m_invGlobalInertia;
+
+ /// Current force applied on the rigid representation (in N)
+ SurgSim::Math::Vector3d m_force;
+ /// Current torque applied on the rigid representation (in N.m)
+ SurgSim::Math::Vector3d m_torque;
+
+ /// Compliance matrix (size of the number of Dof = 6)
+ SurgSim::Math::Matrix66d m_C;
+
+ SurgSim::Math::Vector6d m_externalGeneralizedForce;
+ SurgSim::Math::Matrix66d m_externalGeneralizedStiffness;
+ SurgSim::Math::Matrix66d m_externalGeneralizedDamping;
+
+private:
+ virtual bool doInitialize() override;
+
+ /// Compute compliance matrix (internal data structure)
+ /// \param dt The time step in use
+ void computeComplianceMatrix(double dt);
+
+ /// Update global inertia matrices (internal data structure)
+ /// \param state The state of the rigid representation to use for the update
+ virtual void updateGlobalInertiaMatrices(const RigidRepresentationState& state) override;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_RIGIDREPRESENTATION_H
diff --git a/SurgSim/Physics/RigidRepresentationBase-inl.h b/SurgSim/Physics/RigidRepresentationBase-inl.h
new file mode 100644
index 0000000..6089fb5
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationBase-inl.h
@@ -0,0 +1,44 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_RIGIDREPRESENTATIONBASE_INL_H
+#define SURGSIM_PHYSICS_RIGIDREPRESENTATIONBASE_INL_H
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+template <class T>
+std::shared_ptr<T> SurgSim::Physics::RigidRepresentationBase::createTypedLocalization(
+ const SurgSim::DataStructures::Location& location)
+{
+ // Change when we deal with the meshes as shapes
+ std::shared_ptr<T> result = std::make_shared<T>();
+
+ SURGSIM_ASSERT(location.rigidLocalPosition.hasValue()) <<
+ "Tried to create a rigid localization without valid position information";
+
+ result->setLocalPosition(location.rigidLocalPosition.getValue());
+
+ return std::move(result);
+}
+
+}; // Physics
+
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_RIGIDREPRESENTATIONBASE_INL_H
diff --git a/SurgSim/Physics/RigidRepresentationBase.cpp b/SurgSim/Physics/RigidRepresentationBase.cpp
new file mode 100644
index 0000000..c7f11f3
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationBase.cpp
@@ -0,0 +1,240 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/Valid.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationBase.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+#include "SurgSim/Physics/PhysicsConvert.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+RigidRepresentationBase::RigidRepresentationBase(const std::string& name) :
+ Representation(name),
+ m_parametersValid(false),
+ m_rho(0.0),
+ m_mass(std::numeric_limits<double>::quiet_NaN()),
+ m_linearDamping(0.0),
+ m_angularDamping(0.0)
+{
+ m_localInertia.setConstant(std::numeric_limits<double>::quiet_NaN());
+ m_massCenter.setConstant(std::numeric_limits<double>::quiet_NaN());
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidRepresentationBase, RigidRepresentationState,
+ RigidRepresentationState, getInitialState, setInitialState);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidRepresentationBase, std::shared_ptr<SurgSim::Collision::Representation>,
+ CollisionRepresentation, getCollisionRepresentation, setCollisionRepresentation);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidRepresentationBase, double, Density, getDensity, setDensity);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidRepresentationBase, double, LinearDamping,
+ getLinearDamping, setLinearDamping);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidRepresentationBase, double, AngularDamping,
+ getAngularDamping, setAngularDamping);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidRepresentationBase, std::shared_ptr<SurgSim::Math::Shape>, Shape,
+ getShape, setShape);
+
+}
+
+RigidRepresentationBase::~RigidRepresentationBase()
+{
+}
+
+bool RigidRepresentationBase::doInitialize()
+{
+ if (m_shape != nullptr)
+ {
+ SURGSIM_ASSERT(m_shape->isValid()) <<
+ "An invalid shape is used in this RigidRepresentationBase.";
+ }
+
+ return true;
+}
+
+bool RigidRepresentationBase::doWakeUp()
+{
+ m_initialState.setPose(getPose());
+ m_currentState = m_initialState;
+ m_finalState = m_initialState;
+ m_previousState = m_initialState;
+ updateGlobalInertiaMatrices(m_currentState);
+
+ return true;
+}
+
+void RigidRepresentationBase::setInitialState(const RigidRepresentationState& state)
+{
+ m_initialState = state;
+ m_currentState = state;
+ m_previousState = state;
+
+ updateGlobalInertiaMatrices(m_currentState);
+}
+
+void RigidRepresentationBase::resetState()
+{
+ Representation::resetState();
+
+ m_currentState = m_initialState;
+ m_previousState = m_initialState;
+ m_finalState = m_initialState;
+
+ updateGlobalInertiaMatrices(m_currentState);
+}
+
+const RigidRepresentationState& RigidRepresentationBase::getInitialState() const
+{
+ return m_initialState;
+}
+
+const RigidRepresentationState& RigidRepresentationBase::getCurrentState() const
+{
+ return m_currentState;
+}
+
+const RigidRepresentationState& RigidRepresentationBase::getPreviousState() const
+{
+ return m_previousState;
+}
+
+std::shared_ptr<Localization> RigidRepresentationBase::createLocalization(
+ const SurgSim::DataStructures::Location& location)
+{
+ return std::move(createTypedLocalization<RigidRepresentationLocalization>(location));
+}
+
+void RigidRepresentationBase::setDensity(double rho)
+{
+ m_rho = rho;
+ updateProperties();
+}
+
+double RigidRepresentationBase::getDensity() const
+{
+ return m_rho;
+}
+
+double RigidRepresentationBase::getMass() const
+{
+ return m_mass;
+}
+
+const SurgSim::Math::Vector3d& RigidRepresentationBase::getMassCenter() const
+{
+ return m_massCenter;
+}
+
+const SurgSim::Math::Matrix33d& RigidRepresentationBase::getLocalInertia() const
+{
+ return m_localInertia;
+}
+
+void RigidRepresentationBase::setLinearDamping(double linearDamping)
+{
+ m_linearDamping = linearDamping;
+}
+
+double RigidRepresentationBase::getLinearDamping() const
+{
+ return m_linearDamping;
+}
+
+void RigidRepresentationBase::setAngularDamping(double angularDamping)
+{
+ m_angularDamping = angularDamping;
+}
+
+double RigidRepresentationBase::getAngularDamping() const
+{
+ return m_angularDamping;
+}
+
+void RigidRepresentationBase::setShape(const std::shared_ptr<SurgSim::Math::Shape> shape)
+{
+ m_shape = shape;
+ if (shape != nullptr)
+ {
+ updateProperties();
+ }
+}
+
+const std::shared_ptr<SurgSim::Math::Shape> RigidRepresentationBase::getShape() const
+{
+ return m_shape;
+}
+
+void RigidRepresentationBase::updateProperties()
+{
+ if (m_shape != nullptr)
+ {
+ SURGSIM_ASSERT(m_shape->isValid()) << "Invalid shape.";
+ m_massCenter = m_shape->getCenter();
+ if (m_rho > 0.0)
+ {
+ m_mass = m_rho * m_shape->getVolume();
+ m_localInertia = m_rho * m_shape->getSecondMomentOfVolume();
+ m_parametersValid = SurgSim::Math::isValid(m_localInertia) &&
+ !m_localInertia.isZero() &&
+ m_localInertia.diagonal().minCoeff() > 0.0 &&
+ SurgSim::Math::isValid(m_mass) && m_mass > 0.0;
+ }
+ }
+}
+
+void SurgSim::Physics::RigidRepresentationBase::beforeUpdate(double dt)
+{
+ m_previousState = m_currentState;
+}
+
+void SurgSim::Physics::RigidRepresentationBase::afterUpdate(double dt)
+{
+ m_finalState = m_currentState;
+ driveSceneElementPose(m_finalState.getPose() * getLocalPose().inverse());
+}
+
+void RigidRepresentationBase::setCollisionRepresentation(
+ std::shared_ptr<SurgSim::Collision::Representation> representation)
+{
+ if (m_collisionRepresentation != representation)
+ {
+ // If we have an old collision representation clear the dependency
+ auto oldCollisionRep = std::dynamic_pointer_cast<RigidCollisionRepresentation>(m_collisionRepresentation);
+ if (oldCollisionRep != nullptr)
+ {
+ oldCollisionRep->setRigidRepresentation(nullptr);
+ }
+
+ Representation::setCollisionRepresentation(representation);
+
+ // If its a RigidCollisionRepresentation connect with this representation
+ auto newCollisionRep = std::dynamic_pointer_cast<RigidCollisionRepresentation>(representation);
+ if (newCollisionRep != nullptr)
+ {
+ newCollisionRep->setRigidRepresentation(std::static_pointer_cast<RigidRepresentationBase>(getSharedPtr()));
+ }
+ }
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/RigidRepresentationBase.h b/SurgSim/Physics/RigidRepresentationBase.h
new file mode 100644
index 0000000..9fdfcf3
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationBase.h
@@ -0,0 +1,179 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_RIGIDREPRESENTATIONBASE_H
+#define SURGSIM_PHYSICS_RIGIDREPRESENTATIONBASE_H
+
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/RigidRepresentationState.h"
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+class Representation;
+}
+
+namespace Physics
+{
+class Localization;
+
+/// The RigidRepresentationBase class defines the base class for
+/// all rigid motion based representations (fixed, rigid body, rigid body + vtc,...)
+class RigidRepresentationBase : public Representation
+{
+public:
+ /// Constructor
+ /// \param name The rigid representation's name
+ explicit RigidRepresentationBase(const std::string& name);
+
+ /// Destructor
+ virtual ~RigidRepresentationBase();
+
+ /// Set the initial state of the rigid representation
+ /// \param state The initial state (pose + lin/ang velocities)
+ /// This will also set the current/previous states to the initial state
+ void setInitialState(const RigidRepresentationState& state);
+ /// Reset the rigid representation state to its initial state
+ void resetState();
+
+ /// Get the initial state of the rigid representation
+ /// \return The initial state (pose + lin/ang velocities)
+ const RigidRepresentationState& getInitialState() const;
+ /// Get the current state of the rigid representation
+ /// \return The current state (pose + lin/ang velocities)
+ const RigidRepresentationState& getCurrentState() const;
+ /// Get the previous state of the rigid representation
+ /// \return The previous state (pose + lin/ang velocities)
+ const RigidRepresentationState& getPreviousState() const;
+
+ std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location& location);
+
+ /// Set the mass density of the rigid representation
+ /// \param rho The density (in Kg.m-3)
+ void setDensity(double rho);
+
+ /// Get the mass density of the rigid representation
+ /// \return The density if it has been provided, 0 otherwise (in Kg.m-3)
+ double getDensity() const;
+
+ /// Get the mass of the rigid body
+ /// \return The mass (in Kg)
+ double getMass() const;
+
+ /// Get the mass center of the rigid body
+ /// \return The mass center (in local coordinate)
+ const SurgSim::Math::Vector3d& getMassCenter() const;
+
+ /// Get the local inertia 3x3 matrix of the rigid body
+ /// \return The inertia 3x3 matrix of the object
+ const SurgSim::Math::Matrix33d& getLocalInertia() const;
+
+ /// Set the linear damping parameter
+ /// \param linearDamping The linear damping parameter (in N.s.m-1)
+ void setLinearDamping(double linearDamping);
+
+ /// Get the linear damping parameter
+ /// \return The linear damping parameter (in N.s.m-1)
+ double getLinearDamping() const;
+
+ /// Set the angular damping parameter
+ /// \param angularDamping The angular damping parameter (in N.m.s.rad-1)
+ void setAngularDamping(double angularDamping);
+
+ /// Get the angular damping parameter
+ /// \return The angular damping parameter (in N.m.s.rad-1)
+ double getAngularDamping() const;
+
+ /// Set the shape to use internally for physical parameters computation
+ /// \param shape The shape to use for the mass/inertia calculation
+ /// \note Also add the shape to the shape list if it has not been added yet
+ void setShape(const std::shared_ptr<SurgSim::Math::Shape> shape);
+
+ /// Get the shape used internally for physical parameters computation
+ /// \return The shape used for calculation, nullptr if none exist
+ const std::shared_ptr<SurgSim::Math::Shape> getShape() const;
+
+ /// Set the collision representation for this physics representation, when the collision object
+ /// is involved in a collision, the collision should be resolved inside the dynamics calculation.
+ /// Specializes to register this representation in the collision representation if the collision representation
+ /// is a RigidCollisionRepresentation.
+ /// \param representation The collision representation to be used.
+ virtual void setCollisionRepresentation(
+ std::shared_ptr<SurgSim::Collision::Representation> representation) override;
+
+ virtual void beforeUpdate(double dt) override;
+ virtual void afterUpdate(double dt) override;
+
+protected:
+ virtual bool doInitialize() override;
+ virtual bool doWakeUp() override;
+
+ /// Initial rigid representation state (useful for reset)
+ RigidRepresentationState m_initialState;
+ /// Previous rigid representation state
+ RigidRepresentationState m_previousState;
+ /// Current rigid representation state
+ RigidRepresentationState m_currentState;
+ /// Last valid/final rigid representation state
+ RigidRepresentationState m_finalState;
+
+ /// Validity of the parameters
+ bool m_parametersValid;
+
+ /// Density of the object (in Kg.m-3)
+ double m_rho;
+
+ /// Total mass of the object (in Kg)
+ double m_mass;
+
+ /// Linear damping parameter (in N.s.m-1 or Kg.s-1)
+ double m_linearDamping;
+
+ /// Angular damping parameter (in N.m.s.rad-1)
+ double m_angularDamping;
+
+ /// Mass-center of the object
+ SurgSim::Math::Vector3d m_massCenter;
+
+ /// Inertia matrix in local coordinates
+ SurgSim::Math::Matrix33d m_localInertia;
+
+ /// Shape to be used for the mass/inertia calculation
+ std::shared_ptr<SurgSim::Math::Shape> m_shape;
+
+ /// Creates typed localization.
+ /// \tparam T Type of localization to create.
+ /// \param location The location for the localization.
+ /// \return The new Localization;
+ template <class T>
+ std::shared_ptr<T> createTypedLocalization(const SurgSim::DataStructures::Location& location);
+
+private:
+ /// Updates mass, mass center and inertia when density and/or shape used for mass inertia is updated.
+ void updateProperties();
+
+ virtual void updateGlobalInertiaMatrices(const RigidRepresentationState& state) = 0;
+};
+
+}; // Physics
+}; // SurgSim
+
+#include "SurgSim/Physics/RigidRepresentationBase-inl.h"
+
+#endif // SURGSIM_PHYSICS_RIGIDREPRESENTATIONBASE_H
diff --git a/SurgSim/Physics/RigidRepresentationBilateral3D.cpp b/SurgSim/Physics/RigidRepresentationBilateral3D.cpp
new file mode 100644
index 0000000..6bd46d8
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationBilateral3D.cpp
@@ -0,0 +1,136 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationBilateral3D.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+RigidRepresentationBilateral3D::RigidRepresentationBilateral3D()
+{
+}
+
+RigidRepresentationBilateral3D::~RigidRepresentationBilateral3D()
+{
+}
+
+void RigidRepresentationBilateral3D::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+ std::shared_ptr<RigidRepresentation> rigid
+ = std::static_pointer_cast<RigidRepresentation>(localization->getRepresentation());
+
+ if (!rigid->isActive())
+ {
+ return;
+ }
+
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+
+ Vector3d globalPosition = localization->calculatePosition();
+
+ // Fixed point constraint in MCLP
+ // p(t) is defined as the point before motion
+ // s is defined as position to be constrained to [see note 1]
+ // u is defined as the displacement needed to enforce the constraint
+ // The equation is
+ // u + (p(t) - s) = 0
+ //
+ // Using backward-Euler integration,
+ // u = dt.v(t + dt)
+ //
+ // In rigid body dynamics, the screw vectors are represented as (x, y, z, wx, wy, wz), where the G = (x, y, z)
+ // represents the vector from the origin to the center of mass, and w = (wx, wy, wz) represents angular rotation
+ // about the center of mass. The twist vector in Cartesian space is--
+ //
+ // dt.v(t+dt) = dt.dG(t+dt) + dt.GP^w(t+dt), where G is the position of the center of mass,
+ // dG is the velocity of the center of mass,
+ // and GP is the vector from the center of mass to the point of interest
+ // = dt.dG(t+dt) + dt.|i j k |
+ // |GPx GPy GPz|, where i, j, k are unit vectors for the x, y, and z directions
+ // |wx wy wz |
+ // = dt.dG(t+dt) + dt.[ GPy.wz - GPz.wy]
+ // [-GPx.wz + GPz.wx]
+ // [ GPx.wy - GPy.wx]
+ // = dt.dG(t+dt) + dt.[ 0 -GPz GPy].w
+ // [ GPz 0 -GPx]
+ // [-GPy GPx 0 ]
+ //
+ // We construct H to transform v(t + dt) into constrained space. Therefore we multiply the translational velocity
+ // by dt, and we must multiply the angular velocity with the skew-symmetric matrix of GP times dt.
+ //
+ // [note 1]: In the physics pipeline, the constraint consists of two implementations and two localizations. The
+ // implementation is processed once for each localization, with the first being processed with scale = 1 and the
+ // second with scale = -1. When applied together in the Mlcp, this effectively takes the difference of the two
+ // constraints. Therefore we can effectively calculate the bilateral constraint using only the position information
+ // of the localization.
+
+ // Fill up b with the constraint violation
+ mlcp->b.segment<3>(indexOfConstraint) += globalPosition * scale;
+
+ // Fill up H with the transform from rigid body velocity -> constraint space
+ Vector3d GP = globalPosition - rigid->getCurrentState().getPose() * rigid->getMassCenter();
+ m_newH.resize(rigid->getNumDof());
+ m_newH.reserve(3);
+
+ m_newH.insert(0) = dt * scale;
+ m_newH.insert(3 + 1) = -dt * scale * GP.z();
+ m_newH.insert(3 + 2) = dt * scale * GP.y();
+ mlcp->updateConstraint(m_newH, rigid->getComplianceMatrix(), indexOfRepresentation, indexOfConstraint + 0);
+
+ m_newH.setZero();
+ m_newH.insert(1) = dt * scale;
+ m_newH.insert(3 + 0) = dt * scale * GP.z();
+ m_newH.insert(3 + 2) = -dt * scale * GP.x();
+ mlcp->updateConstraint(m_newH, rigid->getComplianceMatrix(), indexOfRepresentation, indexOfConstraint + 1);
+
+ m_newH.setZero();
+ m_newH.insert(2) = dt * scale;
+ m_newH.insert(3 + 0) = -dt * scale * GP.y();
+ m_newH.insert(3 + 1) = dt * scale * GP.x();
+ mlcp->updateConstraint(m_newH, rigid->getComplianceMatrix(), indexOfRepresentation, indexOfConstraint + 2);
+}
+
+SurgSim::Math::MlcpConstraintType RigidRepresentationBilateral3D::getMlcpConstraintType() const
+{
+ return SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT;
+}
+
+SurgSim::Physics::RepresentationType RigidRepresentationBilateral3D::getRepresentationType() const
+{
+ return REPRESENTATION_TYPE_RIGID;
+}
+
+size_t RigidRepresentationBilateral3D::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RigidRepresentationBilateral3D.h b/SurgSim/Physics/RigidRepresentationBilateral3D.h
new file mode 100644
index 0000000..dfad5cf
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationBilateral3D.h
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_RIGIDREPRESENTATIONBILATERAL3D_H
+#define SURGSIM_PHYSICS_RIGIDREPRESENTATIONBILATERAL3D_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// RigidRepresentation bilateral 3d constraint implementation.
+///
+/// The family of bilateral3D constraints enforce equality between two points.
+class RigidRepresentationBilateral3D : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ RigidRepresentationBilateral3D();
+
+ /// Destructor
+ virtual ~RigidRepresentationBilateral3D();
+
+ /// Gets the Mixed Linear Complementarity Problem constraint type for this ConstraintImplementation
+ /// \return The MLCP constraint type corresponding to this constraint implementation
+ SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+
+ /// Gets the Type of representation that this implementation is concerned with
+ /// \return RepresentationType for this implementation
+ virtual RepresentationType getRepresentationType() const override;
+
+private:
+ /// Gets the number of degree of freedom.
+ /// \return 3 A bilateral 3d constraint enforces equality in the x, y, and z dimensions between 2 points.
+ size_t doGetNumDof() const override;
+
+ /// Builds the subset of an Mlcp physics problem associated to this implementation.
+ /// \param dt The time step.
+ /// \param data The data associated to the constraint.
+ /// \param localization The localization for the representation.
+ /// \param [in, out] mlcp The Mixed LCP physics problem to fill up.
+ /// \param indexOfRepresentation The index of the representation (associated to this implementation) in the mlcp.
+ /// \param indexOfConstraint The index of the constraint in the mlcp.
+ /// \param sign The sign of this implementation in the constraint (positive or negative side).
+ /// \note Empty for a Rigid Representation
+ void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) override;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_RIGIDREPRESENTATIONBILATERAL3D_H
diff --git a/SurgSim/Physics/RigidRepresentationContact.cpp b/SurgSim/Physics/RigidRepresentationContact.cpp
new file mode 100644
index 0000000..b06485d
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationContact.cpp
@@ -0,0 +1,115 @@
+//// This file is a part of the OpenSurgSim project.
+//// Copyright 2013, SimQuest Solutions Inc.
+////
+//// Licensed under the Apache License, Version 2.0 (the "License");
+//// you may not use this file except in compliance with the License.
+//// You may obtain a copy of the License at
+////
+//// http://www.apache.org/licenses/LICENSE-2.0
+////
+//// Unless required by applicable law or agreed to in writing, software
+//// distributed under the License is distributed on an "AS IS" BASIS,
+//// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//// See the License for the specific language governing permissions and
+//// limitations under the License.
+
+#include <memory>
+
+#include "SurgSim/Physics/RigidRepresentationContact.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+RigidRepresentationContact::RigidRepresentationContact()
+{
+
+}
+
+RigidRepresentationContact::~RigidRepresentationContact()
+{
+
+}
+
+void RigidRepresentationContact::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+ std::shared_ptr<Representation> representation = localization->getRepresentation();
+ std::shared_ptr<RigidRepresentation> rigid = std::static_pointer_cast<RigidRepresentation>(representation);
+
+ if (! rigid->isActive())
+ {
+ return;
+ }
+
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE ? 1.0 : -1.0);
+ const Eigen::Matrix<double, 6,6, Eigen::RowMajor>& C = rigid->getComplianceMatrix();
+ const ContactConstraintData& contactData = static_cast<const ContactConstraintData&>(data);
+ const Vector3d& n = contactData.getNormal();
+ const double d = contactData.getDistance();
+
+ // FRICTIONLESS CONTACT in a LCP
+ // (n, d) defines the plane of contact
+ // P(t) the point of contact (usually after free motion)
+ // The constraint equation is: n.P(t+dt) + d >= 0
+ // n.[ P(t) + dt.V(t+dt) ] + d >= 0 (using the numerical integration scheme Backward Euler)
+ // n.dt.[dG(t+dt) + w(t+dt)^GP] + n.P(t) + d >= 0
+ // n.dt.[dGx(t+dt) + (wy(t+dt).GPz-wz(t+dt).GPy)] + n.P(t) + d >= 0 ]
+ // [dGy(t+dt) + (wz(t+dt).GPx-wx(t+dt).GPz)]
+ // [dGz(t+dt) + (wx(t+dt).GPy-wy(t+dt).GPx)]
+ // H.v(t+dt) + b >= 0
+ // H = dt.[nx ny nz nz.GPy-ny.GPz nx.GPz-nz.GPx ny.GPx-nx.GPy]
+ // b = n.P(t) + d -> P(t) evaluated after free motion
+
+ Vector3d globalPosition = localization->calculatePosition();
+ Vector3d GP = globalPosition - rigid->getCurrentState().getPose() * rigid->getMassCenter();
+
+ // Fill up b with the constraint equation...
+ double violation = n.dot(globalPosition) + d;
+ mlcp->b[indexOfConstraint] += violation * scale;
+
+ m_newH.resize(rigid->getNumDof());
+ m_newH.reserve(6);
+ m_newH.insert(0) = dt * scale * n[0];
+ m_newH.insert(1) = dt * scale * n[1];
+ m_newH.insert(2) = dt * scale * n[2];
+ Eigen::Vector3d rotation = GP.cross(n);
+ m_newH.insert(3) = dt * scale * rotation[0];
+ m_newH.insert(4) = dt * scale * rotation[1];
+ m_newH.insert(5) = dt * scale * rotation[2];
+
+ mlcp->updateConstraint(m_newH, C, indexOfRepresentation, indexOfConstraint);
+}
+
+SurgSim::Math::MlcpConstraintType RigidRepresentationContact::getMlcpConstraintType() const
+{
+ return SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
+}
+
+SurgSim::Physics::RepresentationType RigidRepresentationContact::getRepresentationType() const
+{
+ return REPRESENTATION_TYPE_RIGID;
+}
+
+size_t RigidRepresentationContact::doGetNumDof() const
+{
+ return 1;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RigidRepresentationContact.h b/SurgSim/Physics/RigidRepresentationContact.h
new file mode 100644
index 0000000..ef2cdd4
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationContact.h
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_RIGIDREPRESENTATIONCONTACT_H
+#define SURGSIM_PHYSICS_RIGIDREPRESENTATIONCONTACT_H
+
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/Localization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// RigidRepresentation frictionless contact implementation.
+class RigidRepresentationContact : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ RigidRepresentationContact();
+
+ /// Destructor
+ virtual ~RigidRepresentationContact();
+
+ /// Gets the Mixed Linear Complementarity Problem constraint type for this ConstraintImplementation
+ /// \return The MLCP constraint type corresponding to this constraint implementation
+ virtual SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+
+ /// Gets the Type of representation that this implementation is concerned with
+ /// \return RepresentationType for this implementation
+ virtual RepresentationType getRepresentationType() const override;
+
+private:
+ /// Gets the number of degree of freedom for a frictionless contact.
+ /// \return 1 as a frictionless contact only has 1 equation of constraint (along the normal direction).
+ size_t doGetNumDof() const override;
+
+ /// Builds the subset of an Mlcp physics problem associated to this implementation.
+ /// \param dt The time step.
+ /// \param data The data associated to the constraint.
+ /// \param localization The localization for the representation.
+ /// \param [in, out] mlcp The Mixed LCP physics problem to fill up.
+ /// \param indexOfRepresentation The index of the representation (associated to this implementation) in the mlcp.
+ /// \param indexOfConstraint The index of the constraint in the mlcp.
+ /// \param sign The sign of this implementation in the constraint (positive or negative side).
+ void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) override;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_RIGIDREPRESENTATIONCONTACT_H
diff --git a/SurgSim/Physics/RigidRepresentationLocalization.cpp b/SurgSim/Physics/RigidRepresentationLocalization.cpp
new file mode 100644
index 0000000..cf97736
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationLocalization.cpp
@@ -0,0 +1,88 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+RigidRepresentationLocalization::RigidRepresentationLocalization()
+{
+
+}
+
+RigidRepresentationLocalization::RigidRepresentationLocalization(std::shared_ptr<Representation> representation) :
+ Localization()
+{
+ setRepresentation(representation);
+}
+
+RigidRepresentationLocalization::~RigidRepresentationLocalization()
+{
+
+}
+
+void RigidRepresentationLocalization::setLocalPosition(const SurgSim::Math::Vector3d& p)
+{
+ m_position = p;
+}
+
+const SurgSim::Math::Vector3d& RigidRepresentationLocalization::getLocalPosition() const
+{
+ return m_position;
+}
+
+SurgSim::Math::Vector3d RigidRepresentationLocalization::doCalculatePosition(double time)
+{
+ std::shared_ptr<RigidRepresentationBase> rigidRepresentation =
+ std::static_pointer_cast<RigidRepresentationBase>(getRepresentation());
+
+ SURGSIM_ASSERT(rigidRepresentation != nullptr) << "RigidRepresentation is null, it was probably not" <<
+ " initialized";
+
+ if (time == 0.0)
+ {
+ return rigidRepresentation->getPreviousState().getPose() * m_position;
+ }
+ else if (time == 1.0)
+ {
+ return rigidRepresentation->getCurrentState().getPose() * m_position;
+ }
+ else if (rigidRepresentation->getCurrentState().getPose().
+ isApprox(rigidRepresentation->getPreviousState().getPose()))
+ {
+ return rigidRepresentation->getCurrentState().getPose() * m_position;
+ }
+
+ const SurgSim::Math::RigidTransform3d& currentPose = rigidRepresentation->getCurrentState().getPose();
+ const SurgSim::Math::RigidTransform3d& previousPose = rigidRepresentation->getPreviousState().getPose();
+ SurgSim::Math::RigidTransform3d pose = SurgSim::Math::interpolate(previousPose, currentPose, time);
+
+ return pose * m_position;
+}
+
+bool RigidRepresentationLocalization::isValidRepresentation(std::shared_ptr<Representation> representation)
+{
+
+ std::shared_ptr<RigidRepresentationBase> rigidRepresentation =
+ std::dynamic_pointer_cast<RigidRepresentationBase>(representation);
+
+ // Allows to reset the representation to nullptr ...
+ return (rigidRepresentation != nullptr || representation == nullptr);
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/RigidRepresentationLocalization.h b/SurgSim/Physics/RigidRepresentationLocalization.h
new file mode 100644
index 0000000..9a2f977
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationLocalization.h
@@ -0,0 +1,78 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_RIGIDREPRESENTATIONLOCALIZATION_H
+#define SURGSIM_PHYSICS_RIGIDREPRESENTATIONLOCALIZATION_H
+
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/RigidRepresentationBase.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// This class implement the localization on a RigidRepresentation, as a local position
+/// \todo HS-2013-jun-21 There is a slight mismatch in the way the class was written and
+/// the current use, the constructor needs the correct shared_ptr and that might not
+/// be available, setRepresentation is currently used, but this does not check on the
+/// type of the representation, this needs to be fixed
+class RigidRepresentationLocalization: public Localization
+{
+public:
+ /// Default constructor
+ RigidRepresentationLocalization();
+
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ explicit RigidRepresentationLocalization(std::shared_ptr<Representation> representation);
+
+ /// Destructor
+ virtual ~RigidRepresentationLocalization();
+
+ /// Sets the local position.
+ /// \param p The local position to set the localization at.
+ void setLocalPosition(const SurgSim::Math::Vector3d& p);
+
+ /// Gets the local position.
+ /// \return The local position set for this localization.
+ const SurgSim::Math::Vector3d& getLocalPosition() const;
+
+ /// Query if 'representation' is valid representation.
+ /// \param representation The representation.
+ /// \return true if valid representation, false if not.
+ virtual bool isValidRepresentation(std::shared_ptr<Representation> representation) override;
+
+private:
+ /// Calculates the global position of this localization.
+ /// \param time The time in [0..1] at which the position should be calculated.
+ /// \return The global position of the localization at the requested time.
+ /// \note time can useful when dealing with CCD.
+ SurgSim::Math::Vector3d doCalculatePosition(double time);
+
+ /// 3D position in local coordinates.
+ SurgSim::Math::Vector3d m_position;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_RIGIDREPRESENTATIONLOCALIZATION_H
diff --git a/SurgSim/Physics/RigidRepresentationState.cpp b/SurgSim/Physics/RigidRepresentationState.cpp
new file mode 100644
index 0000000..b4d2ced
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationState.cpp
@@ -0,0 +1,114 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Physics/RigidRepresentationState.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+RigidRepresentationState::RigidRepresentationState() :
+ m_v(SurgSim::Math::Vector3d::Zero()),
+ m_w(SurgSim::Math::Vector3d::Zero()),
+ m_pose(SurgSim::Math::RigidTransform3d::Identity())
+{
+ addSerializableProperty();
+}
+
+RigidRepresentationState::RigidRepresentationState(const RigidRepresentationState& rhs) :
+ m_v(rhs.m_v),
+ m_w(rhs.m_w),
+ m_pose(rhs.m_pose)
+{
+ addSerializableProperty();
+}
+
+void RigidRepresentationState::addSerializableProperty()
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidRepresentationState, SurgSim::Math::Vector3d, LinearVelocity,
+ getLinearVelocity, setLinearVelocity);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidRepresentationState, SurgSim::Math::Vector3d, AngularVelocity,
+ getAngularVelocity, setAngularVelocity);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidRepresentationState, SurgSim::Math::RigidTransform3d, Pose,
+ getPose, setPose);
+}
+
+
+RigidRepresentationState& RigidRepresentationState::operator=(const RigidRepresentationState& rhs)
+{
+ m_v = rhs.m_v;
+ m_w = rhs.m_w;
+ m_pose = rhs.m_pose;
+
+ return *this;
+}
+
+
+RigidRepresentationState::~RigidRepresentationState()
+{
+}
+
+bool RigidRepresentationState::operator==(const RigidRepresentationState& rhs) const
+{
+ return (m_pose.isApprox(rhs.m_pose) && m_v == rhs.m_v && m_w == rhs.m_w);
+}
+
+bool RigidRepresentationState::operator!=(const RigidRepresentationState& rhs) const
+{
+ return !((*this) == rhs);
+}
+
+void RigidRepresentationState::reset()
+{
+ m_v.setZero();
+ m_w.setZero();
+ m_pose.setIdentity();
+}
+
+const SurgSim::Math::Vector3d& RigidRepresentationState::getLinearVelocity() const
+{
+ return m_v;
+}
+
+const SurgSim::Math::Vector3d& RigidRepresentationState::getAngularVelocity() const
+{
+ return m_w;
+}
+
+void RigidRepresentationState::setLinearVelocity(const SurgSim::Math::Vector3d &v)
+{
+ m_v = v;
+}
+
+void RigidRepresentationState::setAngularVelocity(const SurgSim::Math::Vector3d &w)
+{
+ m_w = w;
+}
+
+void RigidRepresentationState::setPose(const SurgSim::Math::RigidTransform3d& pose)
+{
+ m_pose = pose;
+}
+
+const SurgSim::Math::RigidTransform3d& RigidRepresentationState::getPose() const
+{
+ return m_pose;
+}
+
+}; // Physics
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Physics/RigidRepresentationState.h b/SurgSim/Physics/RigidRepresentationState.h
new file mode 100644
index 0000000..8cb5c84
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentationState.h
@@ -0,0 +1,108 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_RIGIDREPRESENTATIONSTATE_H
+#define SURGSIM_PHYSICS_RIGIDREPRESENTATIONSTATE_H
+
+#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// The RigidRepresentationState class describes a state (position and velocity information).
+class RigidRepresentationState : public SurgSim::Framework::Accessible
+{
+public:
+ /// Default constructor
+ RigidRepresentationState();
+
+ /// Default constructor
+ RigidRepresentationState(const RigidRepresentationState& rhs);
+
+ /// Copy assignment
+ /// \param rhs Right hand side RigidRepresentationState from which data are copied.
+ /// \note 'm_functors' in base class Accessible is NOT copied.
+ RigidRepresentationState& operator=(const RigidRepresentationState& rhs);
+
+ /// Destructor
+ virtual ~RigidRepresentationState();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::RigidRepresentationState);
+
+ /// Comparison operator
+ /// \param rhs A RigidRepresentationState to compare it to
+ /// \return True if the 2 states are equals, False otherwise
+ bool operator==(const RigidRepresentationState& rhs) const;
+
+ /// Comparison operator
+ /// \param rhs A RigidRepresentationState to compare it to
+ /// \return False if the 2 states are equals, True otherwise
+ bool operator!=(const RigidRepresentationState& rhs) const;
+
+ /// Reset the state to default values
+ /// Vectors will be filled with 0
+ /// Rotations will be set to identity (quaternion or matrix type)
+ /// If you want to reset to initial values, you need to save them separately
+ /// in another RigidRepresentationState and assign it to this instance.
+ void reset();
+
+ /// Get the linear velocity
+ /// \return the linear velocity
+ const SurgSim::Math::Vector3d& getLinearVelocity() const;
+
+ /// Get the angular velocity
+ /// \return the angular velocity
+ const SurgSim::Math::Vector3d& getAngularVelocity() const;
+
+ /// Set the linear velocity
+ /// \param v The linear velocity
+ void setLinearVelocity(const SurgSim::Math::Vector3d &v);
+
+ /// Set the angular velocity
+ /// \param w The angular velocity
+ void setAngularVelocity(const SurgSim::Math::Vector3d &w);
+
+ /// Set the rigid representation pose
+ /// \param pose The pose to set the rigid representation to
+ void setPose(const SurgSim::Math::RigidTransform3d& pose);
+
+ /// Get the rigid representation pose
+ /// \return A constant reference to the pose (read only)
+ const SurgSim::Math::RigidTransform3d& getPose() const;
+
+private:
+ /// Register accessors of serializable properties
+ void addSerializableProperty();
+
+ /// Linear velocity
+ SurgSim::Math::Vector3d m_v;
+
+ /// Angular velocity
+ SurgSim::Math::Vector3d m_w;
+
+ /// Rigid representation pose (translation + rotation)
+ SurgSim::Math::RigidTransform3d m_pose;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_RIGIDREPRESENTATIONSTATE_H
\ No newline at end of file
diff --git a/SurgSim/Physics/RigidRepresentation_addExternalGeneralizedForce.dox b/SurgSim/Physics/RigidRepresentation_addExternalGeneralizedForce.dox
new file mode 100644
index 0000000..cbbae3e
--- /dev/null
+++ b/SurgSim/Physics/RigidRepresentation_addExternalGeneralizedForce.dox
@@ -0,0 +1,101 @@
+/*!
+ \fn void SurgSim::Physics::RigidRepresentation::addExternalGeneralizedForce(const SurgSim::DataStructures::Location &location, const SurgSim::Math::Vector6d &generalizedForce, const SurgSim::Math::Matrix66d &K=SurgSim::Math::Matrix66d::Zero(), const SurgSim::Math::Matrix66d &D=SurgSim::Math::Matrix66d::Zero())
+ <br>
+
+ The location produces an extra torque and extra terms in the derivatives (stiffness/damping matrices) coming from this extra torque. <br>
+ Let's note the position dof (\f$\mathbf{C}\f$,\f$\mathbf{W}\f$) and the velocity dof (\f$\mathbf{v}\f$, \f$\mathbf{w}\f$), and \f$R\f$ the 3x3 rotation matrix dependent on \f$\mathbf{W}\f$.
+ The force (\f$\mathbf{F}\f$) applied on a point (\f$\mathbf{P}\f$) that is not the mass center (\f$\mathbf{C}\f$), produces a torque (\f$\mathbf{T}\f$) and also affect matrix derivatives (\f$K\f$ and \f$D\f$) as follow: <br>
+ \f[
+ \mathbf{T} = \mathbf{CP} \wedge \mathbf{F}
+ \f]
+ Note that \f$\mathbf{CP}\f$ does not depend on the velocity (neither \f$\mathbf{v}\f$ nor \f$\mathbf{w}\f$, therefore the damping matrix is affected by the terms:
+ \f[ \displaystyle
+ -\frac{\partial \mathbf{T}}{\partial \mathbf{v}} =
+ -\mathbf{CP} \wedge \frac{\partial \mathbf{F}}{\partial \mathbf{v}} = \mathbf{CP} \wedge D_{3 \times 3}^{(0, 0)}
+ \f]
+ \f[ \displaystyle
+ -\frac{\partial \mathbf{T}}{\partial \mathbf{w}} =
+ -\mathbf{CP} \wedge \frac{\partial \mathbf{F}}{\partial \mathbf{w}} = \mathbf{CP} \wedge D_{3 \times 3}^{(0, 3)}
+ \f]
+ Also note that \f$\mathbf{CP}\f$ does not change if the rigid translate (\f$\mathbf{C}\f$ changes), but it changes if the rotation of the rigid changes (\f$\mathbf{W}\f$ changes => \f$R\f$ changes). <br>
+ Therefore \f$\mathbf{CP}\f$ is a constant vector in the rigid local space \f$ \mathbf{CP} = R.\mathbf{CP_{local}} \f$ and its derivative w.r.t. \f$\mathbf{C}\f$ is null and we get: <br>
+ \f[ \displaystyle
+ -\frac{\partial \mathbf{T}}{\partial \mathbf{C}} =
+ -\mathbf{CP} \wedge \frac{\partial \mathbf{F}}{\partial \mathbf{C}} = \mathbf{CP} \wedge K_{3 \times 3}^{(0,0)}
+ \f]
+ \f[ \displaystyle
+ -\frac{\partial \mathbf{T}}{\partial \mathbf{W}} =
+ -\frac{\partial \mathbf{CP}}{\partial \mathbf{W}} \wedge \mathbf{F} - \mathbf{CP} \wedge \frac{\partial \mathbf{F}}{\partial \mathbf{W}} =
+ -\frac{\partial \mathbf{CP}}{\partial \mathbf{W}} \wedge \mathbf{F} + \mathbf{CP} \wedge K_{3 \times 3}^{(0, 3)}
+ \f]
+
+ To compute \f$\frac{\partial \mathbf{CP}}{\partial \mathbf{W}} = \frac{\partial R}{\partial \mathbf{W}}.\mathbf{CP}_{local}\f$, we operate a variable change, from the rotation vector \f$\mathbf{W}\f$ to angle/axis (\f$\theta\f$, \f$\mathbf{u}\f$) representation (4 dof):
+ \f[
+ \left\{
+ \begin{eqnarray*}
+ \theta = |\mathbf{W}| \\
+ \mathbf{u} = \frac{\mathbf{W}}{|\mathbf{W}|}
+ \end{eqnarray*}
+ \right.
+ \f]
+ In this new coordinate system, we have (see reference http://en.wikipedia.org/wiki/Rotation_matrix):
+ \f[
+ R(\theta, \mathbf{u}) = cos(\theta).I + sin(\theta).\left[\mathbf{u}\right] + (1-cos(\theta)).\mathbf{u} \otimes \mathbf{u}
+ \f]
+ where \f$\left[\mathbf{u}\right]\f$ denotes the skew symmetric matrix of the vector \f$\mathbf{u}\f$,
+ \f$\left[\mathbf{u}\right] = \left( \begin{array}{ccc} 0 & -u^z & u^y \\ u^z & 0 & -u^x \\ -u^y & u^x & 0 \end{array} \right)\f$
+ and \f$\mathbf{u} \otimes \mathbf{u}\f$ the tensor product: \f$\left(\mathbf{u} \otimes \mathbf{u}^T\right)_{ij} = u^i * u^j\f$.
+
+ Applying the chain derivation rule, we get:
+ \f[
+ \frac{\partial R}{\partial W^{\alpha}} = \frac{\partial R}{\partial \theta}.\frac{\partial \theta}{\partial W^{\alpha}} +
+ \frac{\partial R}{\partial u^x}.\frac{\partial u^x}{\partial W^{\alpha}} +
+ \frac{\partial R}{\partial u^y}.\frac{\partial u^y}{\partial W^{\alpha}} +
+ \frac{\partial R}{\partial u^z}.\frac{\partial u^z}{\partial W^{\alpha}}
+ \f]
+ with
+ \f[
+ \frac{\partial R}{\partial \theta} = -sin(\theta).I + cos(\theta).\left[\mathbf{u}\right] + sin(\theta).\mathbf{u} \otimes \mathbf{u}
+ \f]
+
+ \f[
+ \frac{\partial R}{\partial u^x} =
+ sin(\theta) \left( \begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{array} \right) +
+ (1 - cos(\theta)) \left( \begin{array}{ccc} 2u^x & u^y & u^z \\ u^y & 0 & 0 \\ u^z & 0 & 0 \end{array} \right)
+ \f]
+
+ \f[
+ \frac{\partial R}{\partial u^y} =
+ sin(\theta) \left( \begin{array}{ccc} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \end{array} \right) +
+ (1 - cos(\theta)) \left( \begin{array}{ccc} 0 & u^x & 0 \\ u^x & 2u^y & u^z \\ 0 & u^z & 0 \end{array} \right)
+ \f]
+
+ \f[
+ \frac{\partial R}{\partial u^z} =
+ sin(\theta) \left( \begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array} \right) +
+ (1 - cos(\theta)) \left( \begin{array}{ccc} 0 & 0 & u^x \\ 0 & 0 & u^y \\ u^x & u^y & 2u^z \end{array} \right)
+ \f]
+
+ and
+
+ \f[
+ \frac{\partial \theta}{\partial W^{\alpha}} = \frac{W^{\alpha}}{|\mathbf{W}|}
+ \f]
+
+ \f[
+ \frac{\partial u}{\partial W^{\alpha}} =
+ \frac{1}{|\mathbf{W}|} .
+ \left(
+ \left(
+ \begin{array}{c}
+ \delta^{x,\alpha} \\
+ \delta^{y,\alpha} \\
+ \delta^{z,\alpha}
+ \end{array}
+ \right)
+ -
+ \frac{W^{\alpha}}{|\mathbf{W}|}
+ \frac{\mathbf{W}}{|\mathbf{W}|}
+ \right)
+ \f]
+*/
\ No newline at end of file
diff --git a/SurgSim/Physics/SolveMlcp.cpp b/SurgSim/Physics/SolveMlcp.cpp
new file mode 100644
index 0000000..5cba61f
--- /dev/null
+++ b/SurgSim/Physics/SolveMlcp.cpp
@@ -0,0 +1,65 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <vector>
+
+#include <Eigen/Core>
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/SolveMlcp.h"
+#include "SurgSim/Math/MlcpGaussSeidelSolver.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+SolveMlcp::SolveMlcp(bool doCopyState) : Computation(doCopyState)
+{}
+
+SolveMlcp::~SolveMlcp()
+{}
+
+std::shared_ptr<PhysicsManagerState> SolveMlcp::doUpdate(const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+
+ // Solve the Mlcp using a Gauss-Seidel solver
+ m_gaussSeidelSolver.solve(result->getMlcpProblem(), &(result->getMlcpSolution()));
+
+ return result;
+}
+
+void SolveMlcp::setMaxIterations(int maxIterations)
+{
+ m_gaussSeidelSolver.setMaxIterations(maxIterations);
+}
+
+void SolveMlcp::setSolverPrecision(double epsilon)
+{
+ m_gaussSeidelSolver.setEpsilonConvergence(epsilon);
+}
+
+void SolveMlcp::setContactTolerance(double epsilon)
+{
+ m_gaussSeidelSolver.setContactTolerance(epsilon);
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/SolveMlcp.h b/SurgSim/Physics/SolveMlcp.h
new file mode 100644
index 0000000..edcde3d
--- /dev/null
+++ b/SurgSim/Physics/SolveMlcp.h
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_SOLVEMLCP_H
+#define SURGSIM_PHYSICS_SOLVEMLCP_H
+
+#include <memory>
+
+#include "SurgSim/Physics/Computation.h"
+#include "SurgSim/Math/MlcpGaussSeidelSolver.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+/// Solve the system Mixed Linear Complementarity Problem (Mlcp)
+class SolveMlcp : public Computation
+{
+public:
+ /// Constructor
+ /// \param doCopyState Specify if the ouput state is a copy or not of the input state in Computation::Update()
+ explicit SolveMlcp(bool doCopyState = false);
+
+ /// Destructor
+ virtual ~SolveMlcp();
+
+ void setMaxIterations(int maxIterations);
+
+ void setSolverPrecision(double epsilon);
+
+ void setContactTolerance(double epsilon);
+
+protected:
+
+ /// Override doUpdate from superclass
+ /// \param dt The time step
+ /// \param state The Physics manager state
+ /// \return The updated physics manager state (input updated or copied updated)
+ virtual std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state) override;
+
+private:
+
+ /// The Gauss-Seidel Mlcp solver
+ SurgSim::Math::MlcpGaussSeidelSolver m_gaussSeidelSolver;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_SOLVEMLCP_H
diff --git a/SurgSim/Physics/Spring.h b/SurgSim/Physics/Spring.h
new file mode 100644
index 0000000..345bd40
--- /dev/null
+++ b/SurgSim/Physics/Spring.h
@@ -0,0 +1,120 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_SPRING_H
+#define SURGSIM_PHYSICS_SPRING_H
+
+#include <vector>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class OdeState;
+};
+
+namespace Physics
+{
+
+/// Base class for all springs
+/// It handles the node ids to which it is connected and requires all derived classes
+/// to compute the force and its derivatives (the stiffness and damping matrices)
+/// A extra method also exist to compute all of them at once for performance purposes.
+/// It holds on to the actual computed values (m_f, m_K, m_D) as its size is not predefined from outside
+/// and would requires intensive (de)allocation or a temporary variable anyway
+class Spring
+{
+public:
+ /// Virtual destructor
+ virtual ~Spring()
+ {}
+
+ /// Gets the number of nodes the spring is connecting
+ /// \return The number of nodes
+ size_t getNumNodes() const
+ {
+ return m_nodeIds.size();
+ }
+
+ /// Gets the springNodeId-th node id
+ /// \return The requested node id
+ size_t getNodeId(size_t springNodeId) const
+ {
+ return m_nodeIds[springNodeId];
+ }
+
+ /// Gets the node ids for this spring
+ /// \return A vector containing the node ids on which the spring is attached
+ const std::vector<size_t>& getNodeIds() const
+ {
+ return m_nodeIds;
+ }
+
+ /// Adds the spring force (computed for a given state) to a complete system force vector F (assembly)
+ /// \param state The state to compute the force with
+ /// \param[in,out] F The complete system force vector to add the spring force into
+ /// \param scale A factor to scale the added force with
+ virtual void addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ double scale = 1.0) = 0;
+
+ /// Adds the spring damping matrix D (= -df/dv) (computed for a given state) to a complete system damping matrix
+ /// D (assembly)
+ /// \param state The state to compute the damping matrix with
+ /// \param[in,out] D The complete system damping matrix to add the spring damping matrix into
+ /// \param scale A factor to scale the added damping matrix with
+ virtual void addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D,
+ double scale = 1.0) = 0;
+
+ /// Adds the spring stiffness matrix K (= -df/dx) (computed for a given state) to a complete system stiffness
+ /// matrix K (assembly)
+ /// \param state The state to compute the stiffness matrix with
+ /// \param[in,out] K The complete system stiffness matrix to add the spring stiffness matrix into
+ /// \param scale A factor to scale the added stiffness matrix with
+ virtual void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K,
+ double scale = 1.0) = 0;
+
+ /// Adds the spring force vector, mass, stiffness and damping matrices (computed for a given state) into a
+ /// complete system data structure F, D, K (assembly)
+ /// \param state The state to compute everything with
+ /// \param[in,out] F The complete system force vector to add the spring force into
+ /// \param[in,out] D The complete system damping matrix to add the spring damping matrix into
+ /// \param[in,out] K The complete system stiffness matrix to add the spring stiffness matrix into
+ virtual void addFDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ SurgSim::Math::Matrix* D, SurgSim::Math::Matrix* K) = 0;
+
+ /// Adds the spring matrix-vector contribution F += (alphaD.D + alphaK.K).x (computed for a given
+ /// state) into a complete system data structure F (assembly)
+ /// \param state The state to compute everything with
+ /// \param alphaD The scaling factor for the damping contribution
+ /// \param alphaK The scaling factor for the stiffness contribution
+ /// \param x A complete system vector to use as the vector in the matrix-vector multiplication
+ /// \param[in,out] F The complete system force vector to add the element matrix-vector contribution into
+ virtual void addMatVec(const SurgSim::Math::OdeState& state, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F) = 0;
+
+protected:
+ /// Node ids connected by this spring
+ std::vector<size_t> m_nodeIds;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_SPRING_H
diff --git a/SurgSim/Physics/UnitTests/BuildMlcpTests.cpp b/SurgSim/Physics/UnitTests/BuildMlcpTests.cpp
new file mode 100644
index 0000000..6201024
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/BuildMlcpTests.cpp
@@ -0,0 +1,523 @@
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 BuildMlcpTests.cpp
+/// Simple Test for BuildMlcp calculation
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "SurgSim/Physics/BuildMlcp.h"
+#include "SurgSim/Physics/UnitTests/CommonTests.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+/// Redefine class
+class BuildMlcpTests : public CommonTests
+{
+public:
+ void SetUp()
+ {
+ CommonTests::SetUp();
+
+ // Create the BuildMlcp computation
+ m_buildMlcpComputation = std::make_shared<BuildMlcp>();
+ }
+
+protected:
+
+ /// The Build Mlcp computation
+ std::shared_ptr<BuildMlcp> m_buildMlcpComputation;
+};
+
+TEST_F(BuildMlcpTests, NoRepresentationNoConstraintTest)
+{
+ // Run the BuildMlcp computation...
+ ASSERT_NO_THROW(m_buildMlcpComputation->update(dt, m_physicsManagerState));
+ MlcpPhysicsProblem& mlcpProblem = m_physicsManagerState->getMlcpProblem();
+ MlcpPhysicsSolution& mlcpSolution = m_physicsManagerState->getMlcpSolution();
+
+ // Test that the Mlcp is as expected
+ EXPECT_EQ(0, mlcpProblem.getSize());
+ EXPECT_EQ(0, mlcpProblem.A.rows());
+ EXPECT_EQ(0, mlcpProblem.A.cols());
+ EXPECT_EQ(0, mlcpProblem.b.rows());
+ EXPECT_EQ(0, mlcpProblem.CHt.rows());
+ EXPECT_EQ(0, mlcpProblem.CHt.cols());
+ EXPECT_EQ(0, mlcpProblem.H.rows());
+ EXPECT_EQ(0, mlcpProblem.H.cols());
+ EXPECT_EQ(0, mlcpProblem.mu.rows());
+ EXPECT_EQ(0u, mlcpProblem.constraintTypes.size());
+ EXPECT_TRUE(mlcpProblem.isConsistent());
+
+ EXPECT_EQ(0, mlcpSolution.x.rows());
+ EXPECT_EQ(0, mlcpSolution.dofCorrection.rows());
+}
+
+TEST_F(BuildMlcpTests, OneRepresentationNoConstraintTest)
+{
+ // Prep the list of representations: use only 1 representation
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Run the BuildMlcp computation...
+ m_buildMlcpComputation->update(dt, m_physicsManagerState);
+ MlcpPhysicsProblem& mlcpProblem = m_physicsManagerState->getMlcpProblem();
+ MlcpPhysicsSolution& mlcpSolution = m_physicsManagerState->getMlcpSolution();
+
+ // Test that the Mlcp is as expected
+ EXPECT_EQ(0, mlcpProblem.getSize());
+ EXPECT_EQ(0, mlcpProblem.A.rows());
+ EXPECT_EQ(0, mlcpProblem.A.cols());
+ EXPECT_EQ(0, mlcpProblem.b.rows());
+ EXPECT_EQ(6, mlcpProblem.CHt.rows());
+ EXPECT_EQ(0, mlcpProblem.CHt.cols());
+ EXPECT_EQ(0, mlcpProblem.H.rows());
+ EXPECT_EQ(6, mlcpProblem.H.cols());
+ EXPECT_EQ(0, mlcpProblem.mu.rows());
+ EXPECT_EQ(0u, mlcpProblem.constraintTypes.size());
+ EXPECT_TRUE(mlcpProblem.isConsistent());
+
+ EXPECT_EQ(0, mlcpSolution.x.rows());
+ EXPECT_EQ(6, mlcpSolution.dofCorrection.rows());
+
+ EXPECT_EQ(0, m_physicsManagerState->getRepresentationsMapping().getValue(m_allRepresentations[0].get()));
+}
+
+TEST_F(BuildMlcpTests, TwoRepresentationsNoConstraintTest)
+{
+ // Prep the list of representations: use 2 representations
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ m_usedRepresentations.push_back(m_allRepresentations[1]);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Run the BuildMlcp computation...
+ m_buildMlcpComputation->update(dt, m_physicsManagerState);
+ MlcpPhysicsProblem& mlcpProblem = m_physicsManagerState->getMlcpProblem();
+ MlcpPhysicsSolution& mlcpSolution = m_physicsManagerState->getMlcpSolution();
+
+ // Test that the Mlcp is as expected
+ EXPECT_EQ(0, mlcpProblem.getSize());
+ EXPECT_EQ(0, mlcpProblem.A.rows());
+ EXPECT_EQ(0, mlcpProblem.A.cols());
+ EXPECT_EQ(0, mlcpProblem.b.rows());
+ EXPECT_EQ(12, mlcpProblem.CHt.rows());
+ EXPECT_EQ(0, mlcpProblem.CHt.cols());
+ EXPECT_EQ(0, mlcpProblem.H.rows());
+ EXPECT_EQ(12, mlcpProblem.H.cols());
+ EXPECT_EQ(0, mlcpProblem.mu.rows());
+ EXPECT_EQ(0u, mlcpProblem.constraintTypes.size());
+ EXPECT_TRUE(mlcpProblem.isConsistent());
+
+ EXPECT_EQ(0, mlcpSolution.x.rows());
+ EXPECT_EQ(12, mlcpSolution.dofCorrection.rows());
+
+ EXPECT_EQ(0, m_physicsManagerState->getRepresentationsMapping().getValue(m_allRepresentations[0].get()));
+ EXPECT_EQ(m_allRepresentations[0]->getNumDof(),
+ m_physicsManagerState->getRepresentationsMapping().getValue(m_allRepresentations[1].get()));
+}
+
+TEST_F(BuildMlcpTests, OneRepresentationOneConstraintTest)
+{
+ // Prep the list of representations: use only 1 rigid representation + 1 fixed
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ m_usedRepresentations.push_back(m_fixedWorldRepresentation);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Prep the list of constraints: use only 1 constraint
+ {
+ std::shared_ptr<Localization> rigidLocalization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Zero());
+ rigidLocalization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSideContact;
+ rigidSideContact = std::make_shared<RigidRepresentationContact>();
+
+ std::shared_ptr<Localization> fixedLocalization;
+ {
+ std::shared_ptr<FixedRepresentationLocalization> fixedLocalizationTyped;
+ fixedLocalizationTyped = std::make_shared<FixedRepresentationLocalization>();
+ fixedLocalizationTyped->setRepresentation(m_fixedWorldRepresentation);
+ fixedLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Zero());
+ fixedLocalization = fixedLocalizationTyped;
+ }
+ std::shared_ptr<FixedRepresentationContact> fixedSideContact;
+ fixedSideContact = std::make_shared<FixedRepresentationContact>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(SurgSim::Math::Vector3d(0.0, 1.0, 0.0), 0.0);
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSideContact, rigidLocalization, fixedSideContact, fixedLocalization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+
+ // Set the constraint list in the Physics Manager State
+ m_physicsManagerState->setConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT, m_usedConstraints);
+
+ // Run the BuildMlcp computation...
+ m_buildMlcpComputation->update(dt, m_physicsManagerState);
+ MlcpPhysicsProblem& mlcpProblem = m_physicsManagerState->getMlcpProblem();
+ MlcpPhysicsSolution& mlcpSolution = m_physicsManagerState->getMlcpSolution();
+
+ // Test that the Mlcp is as expected
+ EXPECT_EQ(1, mlcpProblem.getSize());
+ EXPECT_EQ(1, mlcpProblem.A.rows());
+ EXPECT_EQ(1, mlcpProblem.A.cols());
+ EXPECT_EQ(1, mlcpProblem.b.rows());
+ EXPECT_EQ(6, mlcpProblem.CHt.rows());
+ EXPECT_EQ(1, mlcpProblem.CHt.cols());
+ EXPECT_EQ(1, mlcpProblem.H.rows());
+ EXPECT_EQ(6, mlcpProblem.H.cols());
+ EXPECT_EQ(1, mlcpProblem.mu.rows());
+ EXPECT_EQ(1u, mlcpProblem.constraintTypes.size());
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, mlcpProblem.constraintTypes[0]);
+ EXPECT_TRUE(mlcpProblem.isConsistent());
+
+ EXPECT_EQ(1, mlcpSolution.x.rows());
+ EXPECT_EQ(6, mlcpSolution.dofCorrection.rows());
+
+ EXPECT_EQ(0, m_physicsManagerState->getRepresentationsMapping().getValue(m_allRepresentations[0].get()));
+ EXPECT_EQ(m_allRepresentations[0]->getNumDof(),
+ m_physicsManagerState->getRepresentationsMapping().getValue(m_fixedWorldRepresentation.get()));
+ EXPECT_EQ(0, m_physicsManagerState->getConstraintsMapping().getValue(m_usedConstraints[0].get()));
+}
+
+TEST_F(BuildMlcpTests, TwoRepresentationsOneConstraintSize3Test)
+{
+ // Prep the list of representations: use only 1 rigid representation + 1 fixed
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ m_usedRepresentations.push_back(m_fixedWorldRepresentation);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Prep the list of constraints: use only 1 constraint
+ {
+ std::shared_ptr<Localization> rigidLocalization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Zero());
+ rigidLocalization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<MockRigidConstraintBilateral3D> rigidSideContact;
+ rigidSideContact = std::make_shared<MockRigidConstraintBilateral3D>();
+
+ std::shared_ptr<Localization> fixedLocalization;
+ {
+ std::shared_ptr<FixedRepresentationLocalization> fixedLocalizationTyped;
+ fixedLocalizationTyped = std::make_shared<FixedRepresentationLocalization>();
+ fixedLocalizationTyped->setRepresentation(m_fixedWorldRepresentation);
+ fixedLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Zero());
+ fixedLocalization = fixedLocalizationTyped;
+ }
+ std::shared_ptr<MockFixedConstraintBilateral3D> fixedSideContact;
+ fixedSideContact = std::make_shared<MockFixedConstraintBilateral3D>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ConstraintData> data = std::make_shared<ConstraintData>();
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSideContact, rigidLocalization, fixedSideContact, fixedLocalization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+
+ // Set the constraint list in the Physics Manager State
+ m_physicsManagerState->setConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT, m_usedConstraints);
+
+ // Run the BuildMlcp computation...
+ m_buildMlcpComputation->update(dt, m_physicsManagerState);
+ MlcpPhysicsProblem& mlcpProblem = m_physicsManagerState->getMlcpProblem();
+ MlcpPhysicsSolution& mlcpSolution = m_physicsManagerState->getMlcpSolution();
+
+ // Test that the Mlcp is as expected
+ EXPECT_EQ(3, mlcpProblem.getSize());
+ EXPECT_EQ(3, mlcpProblem.A.rows());
+ EXPECT_EQ(3, mlcpProblem.A.cols());
+ EXPECT_EQ(3, mlcpProblem.b.rows());
+ EXPECT_EQ(6, mlcpProblem.CHt.rows());
+ EXPECT_EQ(3, mlcpProblem.CHt.cols());
+ EXPECT_EQ(3, mlcpProblem.H.rows());
+ EXPECT_EQ(6, mlcpProblem.H.cols());
+ EXPECT_EQ(1, mlcpProblem.mu.rows());
+ EXPECT_EQ(1u, mlcpProblem.constraintTypes.size());
+ EXPECT_EQ(SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT, mlcpProblem.constraintTypes[0]);
+ EXPECT_TRUE(mlcpProblem.isConsistent());
+
+ EXPECT_EQ(3, mlcpSolution.x.rows());
+ EXPECT_EQ(6, mlcpSolution.dofCorrection.rows());
+
+ EXPECT_EQ(0, m_physicsManagerState->getRepresentationsMapping().getValue(m_allRepresentations[0].get()));
+ EXPECT_EQ(m_allRepresentations[0]->getNumDof(),
+ m_physicsManagerState->getRepresentationsMapping().getValue(m_fixedWorldRepresentation.get()));
+ EXPECT_EQ(0, m_physicsManagerState->getConstraintsMapping().getValue(m_usedConstraints[0].get()));
+}
+
+TEST_F(BuildMlcpTests, OneRepresentationTwoConstraintsTest)
+{
+ // Prep the list of representations: use only 1 rigid representation + 1 fixed
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ m_usedRepresentations.push_back(m_fixedWorldRepresentation);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Prep the list of constraints: use 2 constraints
+ {
+ std::shared_ptr<Localization> rigidLocalization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Zero());
+ rigidLocalization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSideContact;
+ rigidSideContact = std::make_shared<RigidRepresentationContact>();
+
+ std::shared_ptr<Localization> fixedLocalization;
+ {
+ std::shared_ptr<FixedRepresentationLocalization> fixedLocalizationTyped;
+ fixedLocalizationTyped = std::make_shared<FixedRepresentationLocalization>();
+ fixedLocalizationTyped->setRepresentation(m_fixedWorldRepresentation);
+ fixedLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Zero());
+ fixedLocalization = fixedLocalizationTyped;
+ }
+ std::shared_ptr<FixedRepresentationContact> fixedSideContact;
+ fixedSideContact = std::make_shared<FixedRepresentationContact>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(SurgSim::Math::Vector3d(0.0, 1.0, 0.0), 0.0);
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSideContact, rigidLocalization, fixedSideContact, fixedLocalization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+ {
+ std::shared_ptr<Localization> rigidLocalization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Ones());
+ rigidLocalization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSideContact;
+ rigidSideContact = std::make_shared<RigidRepresentationContact>();
+
+ std::shared_ptr<Localization> fixedLocalization;
+ {
+ std::shared_ptr<FixedRepresentationLocalization> fixedLocalizationTyped;
+ fixedLocalizationTyped = std::make_shared<FixedRepresentationLocalization>();
+ fixedLocalizationTyped->setRepresentation(m_fixedWorldRepresentation);
+ fixedLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Ones());
+ fixedLocalization = fixedLocalizationTyped;
+ }
+ std::shared_ptr<FixedRepresentationContact> fixedSideContact;
+ fixedSideContact = std::make_shared<FixedRepresentationContact>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(SurgSim::Math::Vector3d(0.0, 1.0, 0.0), 0.0);
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSideContact, rigidLocalization, fixedSideContact, fixedLocalization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+
+ // Set the constraint list in the Physics Manager State
+ m_physicsManagerState->setConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT, m_usedConstraints);
+
+ // Run the BuildMlcp computation...
+ m_buildMlcpComputation->update(dt, m_physicsManagerState);
+ MlcpPhysicsProblem& mlcpProblem = m_physicsManagerState->getMlcpProblem();
+ MlcpPhysicsSolution& mlcpSolution = m_physicsManagerState->getMlcpSolution();
+
+ // Test that the Mlcp is as expected
+ EXPECT_EQ(2, mlcpProblem.getSize());
+ EXPECT_EQ(2, mlcpProblem.A.rows());
+ EXPECT_EQ(2, mlcpProblem.A.cols());
+ EXPECT_EQ(2, mlcpProblem.b.rows());
+ EXPECT_EQ(6, mlcpProblem.CHt.rows());
+ EXPECT_EQ(2, mlcpProblem.CHt.cols());
+ EXPECT_EQ(2, mlcpProblem.H.rows());
+ EXPECT_EQ(6, mlcpProblem.H.cols());
+ EXPECT_EQ(2, mlcpProblem.mu.rows());
+ EXPECT_EQ(2u, mlcpProblem.constraintTypes.size());
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, mlcpProblem.constraintTypes[0]);
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, mlcpProblem.constraintTypes[1]);
+ EXPECT_TRUE(mlcpProblem.isConsistent());
+
+ EXPECT_EQ(2, mlcpSolution.x.rows());
+ EXPECT_EQ(6, mlcpSolution.dofCorrection.rows());
+
+ EXPECT_EQ(0, m_physicsManagerState->getRepresentationsMapping().getValue(m_allRepresentations[0].get()));
+ EXPECT_EQ(m_allRepresentations[0]->getNumDof(),
+ m_physicsManagerState->getRepresentationsMapping().getValue(m_fixedWorldRepresentation.get()));
+ EXPECT_EQ(0, m_physicsManagerState->getConstraintsMapping().getValue(m_usedConstraints[0].get()));
+ EXPECT_EQ(m_usedConstraints[0]->getNumDof(),
+ m_physicsManagerState->getConstraintsMapping().getValue(m_usedConstraints[1].get()));
+}
+
+TEST_F(BuildMlcpTests, TwoRepresentationsTwoConstraintsTest)
+{
+ SurgSim::Math::Vector3d pointOrigin = SurgSim::Math::Vector3d::Zero();
+ SurgSim::Math::Vector3d planeDirection(0.0, 1.0, 0.0);
+ double planeDistance = 0.0;
+ SurgSim::Math::Vector3d pointOne = planeDirection * 1.0;
+
+ // Prep the list of representations: use only 1 rigid representation + 1 fixed
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ m_usedRepresentations.push_back(m_allRepresentations[1]);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Prep the list of constraints: use 2 constraints
+ {
+ std::shared_ptr<Localization> rigid1Localization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(pointOrigin);
+ rigid1Localization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSide1Contact;
+ rigidSide1Contact = std::make_shared<RigidRepresentationContact>();
+
+ std::shared_ptr<Localization> rigid2Localization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[1]);
+ rigidLocalizationTyped->setLocalPosition(pointOrigin);
+ rigid2Localization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSide2Contact;
+ rigidSide2Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(planeDirection, planeDistance);
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSide1Contact, rigid1Localization, rigidSide2Contact, rigid2Localization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+ {
+ std::shared_ptr<Localization> rigid1Localization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(pointOrigin);
+ rigid1Localization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSide1Contact;
+ rigidSide1Contact = std::make_shared<RigidRepresentationContact>();
+
+ std::shared_ptr<Localization> rigid2Localization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[1]);
+ rigidLocalizationTyped->setLocalPosition(pointOne);
+ rigid2Localization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSide2Contact;
+ rigidSide2Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(planeDirection, planeDistance);
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSide1Contact, rigid1Localization, rigidSide2Contact, rigid2Localization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+
+ // Set the constraint list in the Physics Manager State
+ m_physicsManagerState->setConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT, m_usedConstraints);
+
+ // Run the BuildMlcp computation...
+ m_buildMlcpComputation->update(dt, m_physicsManagerState);
+ MlcpPhysicsProblem& mlcpProblem = m_physicsManagerState->getMlcpProblem();
+ MlcpPhysicsSolution& mlcpSolution = m_physicsManagerState->getMlcpSolution();
+
+ // Test that the Mlcp is as expected
+ EXPECT_EQ(2, mlcpProblem.getSize());
+ EXPECT_EQ(2, mlcpProblem.A.rows());
+ EXPECT_EQ(2, mlcpProblem.A.cols());
+ EXPECT_EQ(2, mlcpProblem.b.rows());
+ // One the 1st constraint, both location are the origin (0 0 0), so there is no resulting violation
+ EXPECT_NEAR(0.0, mlcpProblem.b[0], epsilon);
+ // One the 2nd constraint, the constraints points are (0 0 0) and (0 1 0), so the resulting
+ // violation along the normal direction (0 1 0) should be -1.0
+ EXPECT_NEAR(planeDirection.dot(pointOrigin - pointOne), mlcpProblem.b[1], epsilon);
+ EXPECT_EQ(12, mlcpProblem.CHt.rows());
+ EXPECT_EQ(2, mlcpProblem.CHt.cols());
+ EXPECT_EQ(2, mlcpProblem.H.rows());
+ EXPECT_EQ(12, mlcpProblem.H.cols());
+ EXPECT_EQ(2, mlcpProblem.mu.rows());
+ EXPECT_EQ(2u, mlcpProblem.constraintTypes.size());
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, mlcpProblem.constraintTypes[0]);
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, mlcpProblem.constraintTypes[1]);
+ EXPECT_TRUE(mlcpProblem.isConsistent());
+
+ EXPECT_EQ(2, mlcpSolution.x.rows());
+ EXPECT_EQ(12, mlcpSolution.dofCorrection.rows());
+
+ EXPECT_EQ(0, m_physicsManagerState->getRepresentationsMapping().getValue(m_allRepresentations[0].get()));
+ EXPECT_EQ(m_allRepresentations[0]->getNumDof(),
+ m_physicsManagerState->getRepresentationsMapping().getValue(m_allRepresentations[1].get()));
+ EXPECT_EQ(0, m_physicsManagerState->getConstraintsMapping().getValue(m_usedConstraints[0].get()));
+ EXPECT_EQ(m_usedConstraints[0]->getNumDof(),
+ m_physicsManagerState->getConstraintsMapping().getValue(m_usedConstraints[1].get()));
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/CMakeLists.txt b/SurgSim/Physics/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..4c77286
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/CMakeLists.txt
@@ -0,0 +1,108 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ BuildMlcpTests.cpp
+ ComputationTests.cpp
+ ConstraintComponentTests.cpp
+ ConstraintTests.cpp
+ ContactConstraintDataTests.cpp
+ ContactConstraintGenerationTests.cpp
+ DcdCollisionTests.cpp
+ DeformableCollisionRepresentationTest.cpp
+ DeformableRepresentationTest.cpp
+ EigenGtestAsserts.cpp
+ Fem1DElementBeamTests.cpp
+ Fem1DMechanicalValidationTests.cpp
+ Fem1DPlyReaderDelegateTests.cpp
+ Fem1DRepresentationLocalizationTest.cpp
+ Fem1DRepresentationTests.cpp
+ Fem2DElementTriangleTests.cpp
+ Fem2DMechanicalValidationTests.cpp
+ Fem2DPlyReaderDelegateTests.cpp
+ Fem2DRepresentationLocalizationTest.cpp
+ Fem2DRepresentationTests.cpp
+ Fem3DElementCorotationalTetrahedronTests.cpp
+ Fem3DElementCubeTests.cpp
+ Fem3DElementTetrahedronTests.cpp
+ Fem3DPlyReaderDelegateTests.cpp
+ Fem3DRepresentationBilateral3DTests.cpp
+ Fem3DRepresentationContactTests.cpp
+ Fem3DRepresentationLocalizationTest.cpp
+ Fem3DRepresentationTests.cpp
+ FemElementTests.cpp
+ FemRepresentationParametersTest.cpp
+ FemRepresentationTests.cpp
+ FixedRepresentationBilateral3DTests.cpp
+ FixedRepresentationContactTests.cpp
+ FixedRepresentationLocalizationTest.cpp
+ FixedRepresentationTest.cpp
+ FreeMotionTests.cpp
+ LinearSpringTest.cpp
+ MassSpringMechanicalValidationTests.cpp
+ MassSpringRepresentationContactTest.cpp
+ MassSpringRepresentationLocalizationTest.cpp
+ MassSpringRepresentationTests.cpp
+ MassTest.cpp
+ MockObjects.cpp
+ PhysicsManagerStateTests.cpp
+ PhysicsManagerTests.cpp
+ PostUpdateTests.cpp
+ PreUpdateTests.cpp
+ PushResultsTests.cpp
+ RepresentationTest.cpp
+ RigidCollisionRepresentationTest.cpp
+ RigidRepresentationBilateral3DTests.cpp
+ RigidRepresentationContactTests.cpp
+ RigidRepresentationLocalizationTest.cpp
+ RigidRepresentationStateTest.cpp
+ RigidRepresentationTest.cpp
+ SolveMlcpTests.cpp
+ VirtualToolCouplerTest.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ CommonTests.h
+ EigenGtestAsserts.h
+ MockObjects.h
+)
+
+set(LIBS
+ SurgSimBlocks
+ SurgSimGraphics
+ SurgSimPhysics
+ SurgSimMath
+ SurgSimInput
+ MlcpTestIO
+ IdentityPoseDevice
+)
+
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Math/UnitTests/MlcpTestData DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Testing/MeshShapeData DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Data)
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+surgsim_add_unit_tests(SurgSimPhysicsTest)
+
+set_target_properties(SurgSimPhysicsTest PROPERTIES FOLDER "Physics")
diff --git a/SurgSim/Physics/UnitTests/CommonTests.h b/SurgSim/Physics/UnitTests/CommonTests.h
new file mode 100644
index 0000000..91102fe
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/CommonTests.h
@@ -0,0 +1,158 @@
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 CommonTests.h
+/// Common data structure for physics tests
+
+#ifndef SURGSIM_PHYSICS_UNITTESTS_COMMONTESTS_H
+#define SURGSIM_PHYSICS_UNITTESTS_COMMONTESTS_H
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "SurgSim/Math/Shapes.h"
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/FixedRepresentationContact.h"
+#include "SurgSim/Physics/FixedRepresentationLocalization.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/MlcpPhysicsSolution.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationContact.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::Shape;
+using SurgSim::Math::SphereShape;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class CommonTests : public ::testing::Test
+{
+public:
+ /// Setup the test case by creating all object
+ void SetUp()
+ {
+ // Set the time step
+ dt = 1e-3;
+
+ // Create a fixed world to define constraint against it
+ m_fixedWorldRepresentation = std::make_shared<FixedRepresentation>("FixedPlane");
+ m_fixedWorldRepresentation->setIsGravityEnabled(false);
+ {
+ // Simply do 1 time step to make sure things are initialized (compliance matrix...)
+ m_fixedWorldRepresentation->beforeUpdate(dt);
+ m_fixedWorldRepresentation->update(dt);
+ m_fixedWorldRepresentation->afterUpdate(dt);
+ }
+
+ // Create the physics manager state
+ m_physicsManagerState = std::make_shared<PhysicsManagerState>();
+
+ // Create a Rigid Sphere
+ std::shared_ptr<RigidRepresentation> rigidSphereRepresentation;
+ rigidSphereRepresentation = std::make_shared<RigidRepresentation>("RigidSphere");
+ {
+ double radius = 1e-2;
+ std::shared_ptr<Shape> shape = std::make_shared<SphereShape>(radius);
+ rigidSphereRepresentation->setShape(shape);
+ rigidSphereRepresentation->setDensity(1000);
+ rigidSphereRepresentation->setIsGravityEnabled(false);
+ {
+ // Simply do 1 time step to make sure things are initialized (compliance matrix...)
+ rigidSphereRepresentation->beforeUpdate(dt);
+ rigidSphereRepresentation->update(dt);
+ rigidSphereRepresentation->afterUpdate(dt);
+ }
+ m_allRepresentations.push_back(rigidSphereRepresentation);
+ }
+
+ // Create a Rigid Box
+ std::shared_ptr<RigidRepresentation> rigidBoxRepresentation = std::make_shared<RigidRepresentation>("RigidBox");
+ {
+ double size[3]={0.01, 0.02, 0.03};
+ rigidBoxRepresentation->setDensity(1000);
+ std::shared_ptr<Shape> shape = std::make_shared<BoxShape>(size[0], size[1], size[2]);
+ rigidBoxRepresentation->setShape(shape);
+ rigidBoxRepresentation->setIsGravityEnabled(false);
+ {
+ // Simply do 1 time step to make sure things are initialized (compliance matrix...)
+ rigidBoxRepresentation->beforeUpdate(dt);
+ rigidBoxRepresentation->update(dt);
+ rigidBoxRepresentation->afterUpdate(dt);
+ }
+ m_allRepresentations.push_back(rigidBoxRepresentation);
+ }
+ }
+
+ void resetMlcpProblem(int nbDof, int nbConstraint)
+ {
+ if (m_physicsManagerState)
+ {
+ m_physicsManagerState->getMlcpProblem().A.resize(nbConstraint, nbConstraint);
+ m_physicsManagerState->getMlcpProblem().A.setZero();
+ m_physicsManagerState->getMlcpProblem().b.resize(nbConstraint);
+ m_physicsManagerState->getMlcpProblem().b.setZero();
+ m_physicsManagerState->getMlcpProblem().CHt.resize(nbDof, nbConstraint);
+ m_physicsManagerState->getMlcpProblem().CHt.setZero();
+ m_physicsManagerState->getMlcpProblem().H.resize(nbConstraint, nbDof);
+ m_physicsManagerState->getMlcpProblem().H.setZero();
+ m_physicsManagerState->getMlcpProblem().mu.resize(nbConstraint);
+ m_physicsManagerState->getMlcpProblem().mu.setZero();
+ m_physicsManagerState->getMlcpProblem().constraintTypes.clear();
+
+ m_physicsManagerState->getMlcpSolution().x.resize(nbConstraint);
+ m_physicsManagerState->getMlcpSolution().x.setZero();
+ m_physicsManagerState->getMlcpSolution().dofCorrection.resize(nbDof);
+ m_physicsManagerState->getMlcpSolution().dofCorrection.setZero();
+ }
+ }
+
+protected:
+ /// Time step
+ double dt;
+
+ /// Fixed representation to define constraint in fixed space
+ std::shared_ptr<Representation> m_fixedWorldRepresentation;
+
+ /// Vector of all representations
+ std::vector<std::shared_ptr<Representation>> m_allRepresentations;
+
+ /// Vector of representations useful for the current test
+ std::vector<std::shared_ptr<Representation>> m_usedRepresentations;
+
+ /// Vector of constraints useful for the current test
+ std::vector<std::shared_ptr<Constraint>> m_usedConstraints;
+
+ /// The unique physics manager state
+ std::shared_ptr<PhysicsManagerState> m_physicsManagerState;
+};
+
+}; // namespace Physics
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_UNITTESTS_COMMONTESTS_H
diff --git a/SurgSim/Physics/UnitTests/ComputationTests.cpp b/SurgSim/Physics/UnitTests/ComputationTests.cpp
new file mode 100644
index 0000000..023b114
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/ComputationTests.cpp
@@ -0,0 +1,146 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Physics/Computation.h"
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationContact.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class MockComputation : public Computation
+{
+public:
+ explicit MockComputation(bool doCopyState = false) : Computation(doCopyState)
+ {
+
+ }
+
+protected:
+ virtual std::shared_ptr<PhysicsManagerState> doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state) override
+ {
+ return state;
+ }
+};
+
+TEST(ComputationTests, InitTest)
+{
+ EXPECT_NO_THROW({MockComputation c;});
+ MockComputation c;
+ EXPECT_FALSE(c.isCopyingState());
+
+ MockComputation d(true);
+ EXPECT_TRUE(d.isCopyingState());
+}
+
+TEST(ComputationTests, CopyStateTest)
+{
+ std::shared_ptr<PhysicsManagerState> state0 = std::make_shared<PhysicsManagerState>();
+
+ MockComputation c;
+ auto state1 = c.update(1.0, state0);
+
+ EXPECT_EQ(state0.get(), state1.get());
+
+ c.setDoCopyState(true);
+ EXPECT_TRUE(c.isCopyingState());
+
+ auto state2 = c.update(1.0, state0);
+ EXPECT_NE(state0.get(), state2.get());
+}
+
+TEST(ComputationTests, PreparePhysicsState)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+ EXPECT_EQ(0, physicsState->getActiveConstraints().size());
+
+ // Setup the state.
+ std::vector<std::shared_ptr<Representation>> expectedRepresentations;
+
+ // Add a representation.
+ auto rigid1 = std::make_shared<RigidRepresentation>("rigid1");
+ expectedRepresentations.push_back(rigid1);
+
+ // Add a second representation. This one has a collision representation.
+ auto rigid2 = std::make_shared<RigidRepresentation>("rigid2");
+ auto collisionRepresentation = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("rigid2 collision");
+ rigid2->setCollisionRepresentation(collisionRepresentation);
+ expectedRepresentations.push_back(rigid2);
+ physicsState->setRepresentations(expectedRepresentations);
+
+ // Add a constraint.
+ std::vector<std::shared_ptr<Constraint>> expectedConstraints;
+
+ {
+ // Create first side of a constraint.
+ auto rigid1 = std::make_shared<RigidRepresentation>("rigid1");
+ std::shared_ptr<RigidRepresentationLocalization> rigid1LocalizationTyped =
+ std::make_shared<RigidRepresentationLocalization>();
+ rigid1LocalizationTyped->setRepresentation(rigid1);
+ std::shared_ptr<Localization> rigid1Localization = rigid1LocalizationTyped;
+ auto rigid1Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Create second side of a constraint.
+ auto rigid2 = std::make_shared<RigidRepresentation>("rigid2");
+ std::shared_ptr<RigidRepresentationLocalization> rigid2LocalizationTyped =
+ std::make_shared<RigidRepresentationLocalization>();
+ rigid2LocalizationTyped->setRepresentation(rigid2);
+ std::shared_ptr<Localization> rigid2Localization = rigid2LocalizationTyped;
+ auto rigid2Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Create the constraint specific data.
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+
+ // Create the constraint.
+ auto constraint1 = std::make_shared<Constraint>(data, rigid1Contact, rigid1Localization,
+ rigid2Contact, rigid2Localization);
+
+ // Check the active constraints.
+ expectedConstraints.push_back(constraint1);
+ physicsState->setConstraintGroup(SurgSim::Physics::CONSTRAINT_GROUP_TYPE_CONTACT, expectedConstraints);
+ }
+
+ // Call update on Computation.
+ MockComputation c;
+ physicsState = c.update(0.0, physicsState);
+
+ // Check the active representations list.
+ std::vector<std::shared_ptr<Representation>> actualRepresentations;
+ actualRepresentations = physicsState->getActiveRepresentations();
+ ASSERT_EQ(2, actualRepresentations.size());
+ EXPECT_EQ(rigid1, actualRepresentations.front());
+ EXPECT_EQ(rigid2, actualRepresentations.back());
+
+ // Check the active constraints list.
+ std::vector<std::shared_ptr<Constraint>> actualConstraints;
+ actualConstraints = physicsState->getActiveConstraints();
+ ASSERT_EQ(1, actualConstraints.size());
+ EXPECT_EQ(expectedConstraints.front(), actualConstraints.front());
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/ConstraintComponentTests.cpp b/SurgSim/Physics/UnitTests/ConstraintComponentTests.cpp
new file mode 100644
index 0000000..8a53e79
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/ConstraintComponentTests.cpp
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintComponent.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST(ConstraintComponentTest, Constructor)
+{
+ ASSERT_NO_THROW(
+ { ConstraintComponent("component"); });
+}
+
+TEST(ConstraintComponentTest, GetSetConstraint)
+{
+ auto component = std::make_shared<ConstraintComponent>("component");
+
+ EXPECT_EQ(nullptr, component->getConstraint());
+
+ auto constraint
+ = makeMockConstraint(std::make_shared<MockRepresentation>(), std::make_shared<MockRepresentation>());
+
+ ASSERT_NO_THROW(
+ { component->setConstraint(constraint); });
+ EXPECT_EQ(constraint, component->getConstraint());
+
+ ASSERT_NO_THROW(
+ { component->setConstraint(nullptr); });
+ EXPECT_EQ(nullptr, component->getConstraint());
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/ConstraintTests.cpp b/SurgSim/Physics/UnitTests/ConstraintTests.cpp
new file mode 100644
index 0000000..ce84c46
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/ConstraintTests.cpp
@@ -0,0 +1,397 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/FixedRepresentationContact.h"
+#include "SurgSim/Physics/FixedRepresentationLocalization.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationContact.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class ConstraintTests : public ::testing::Test
+{
+protected:
+ /// Fixed plane pose
+ SurgSim::Math::RigidTransform3d m_poseFixed;
+ /// Rigid sphere pose
+ SurgSim::Math::RigidTransform3d m_poseRigid;
+ /// Contact normal direction
+ Vector3d m_n;
+ /// Distance to origin of the contact plane equation
+ double m_d;
+ /// Sphere radius
+ double m_radius;
+ /// Simulation time step
+ double m_dt;
+ /// Mlcp index of the sphere representation
+ size_t m_indexSphereRepresentation;
+ /// Mlcp index of the plane representation
+ size_t m_indexPlaneRepresentation;
+ /// Mlcp index of the constraint (frictionless contact)
+ size_t m_indexConstraint;
+ /// Contact location on the plane (point on the plane with the most penetration)
+ Vector3d m_contactPositionPlane;
+ /// Contact location on the sphere (point on the sphere with the most penetration)
+ Vector3d m_contactPositionSphere;
+
+ /// Rigid sphere
+ std::shared_ptr<RigidRepresentation> m_rigid;
+ /// Fixed plane
+ std::shared_ptr<FixedRepresentation> m_fixed;
+
+ /// Mlcp
+ MlcpPhysicsProblem m_mlcpPhysicsProblem;
+ /// Constraint
+ std::shared_ptr<Constraint> m_constraint;
+ /// Constraint data: frictionless contact
+ std::shared_ptr<ContactConstraintData> m_constraintData;
+ /// Localization on the fixed plane
+ std::shared_ptr<Localization> m_locFixedPlane;
+ /// Localization on the rigid sphere
+ std::shared_ptr<Localization> m_locRigidSphere;
+ /// Constraint implementation for the fixed plane
+ std::shared_ptr<ConstraintImplementation> m_implementationFixedPlane;
+ /// Constraint implementation for the rigid sphere
+ std::shared_ptr<ConstraintImplementation> m_implementationRigidSphere;
+
+ /// Total number of degrees of freedom in the system (plane + sphere)
+ size_t m_numDof;
+ /// Total number of atomic constraint in the system (1 for a frictionless contact)
+ size_t m_numConstraint;
+
+ /// Setup the test case by creating all object
+ void SetUp()
+ {
+ m_d = 0.0;
+ m_radius = 0.01;
+ m_dt = 1e-3;
+ m_numDof = 0;
+ m_indexSphereRepresentation = 0;
+ m_indexConstraint = 0;
+ m_numConstraint = 1;
+ m_contactPositionPlane.setZero();
+ m_contactPositionSphere.setZero();
+ m_contactPositionSphere[1] = -m_radius;
+
+ m_poseFixed.setIdentity();
+ m_poseRigid.setIdentity();
+
+ m_rigid = std::make_shared<RigidRepresentation>("Rigid");
+ m_rigid->setLocalActive(true);
+ m_rigid->setIsGravityEnabled(false);
+ m_rigid->setLocalPose(m_poseRigid);
+ {
+ m_rigid->setDensity(1000.0);
+ std::shared_ptr<SphereShape> shape = std::make_shared<SphereShape>(m_radius);
+ m_rigid->setShape(shape);
+ }
+ m_numDof += m_rigid->getNumDof();
+
+ m_indexPlaneRepresentation = m_indexSphereRepresentation + m_rigid->getNumDof();
+ m_fixed = std::make_shared<FixedRepresentation>("Fixed");
+ m_fixed->setLocalActive(true);
+ m_fixed->setIsGravityEnabled(false);
+ m_fixed->setLocalPose(m_poseFixed);
+ m_numDof += m_fixed->getNumDof();
+
+ std::shared_ptr<FixedRepresentationLocalization> locFixedPlane;
+ locFixedPlane = std::make_shared<FixedRepresentationLocalization>(m_fixed);
+ m_locFixedPlane = locFixedPlane;
+ std::shared_ptr<RigidRepresentationLocalization> locRigidSphere;
+ locRigidSphere = std::make_shared<RigidRepresentationLocalization>(m_rigid);
+ m_locRigidSphere = locRigidSphere;
+
+ locFixedPlane->setLocalPosition(m_contactPositionPlane);
+ locRigidSphere->setLocalPosition(m_contactPositionSphere);
+
+ m_implementationFixedPlane = std::make_shared<FixedRepresentationContact>();
+ m_implementationRigidSphere = std::make_shared<RigidRepresentationContact>();
+
+ m_constraintData = std::make_shared<ContactConstraintData>();
+
+ clearMlcpPhysicsProblem(m_numDof, m_numConstraint);
+ }
+
+ /// Allocate and clear the Mlcp
+ /// \param numDof The number of degrees of freedom in the system
+ /// \param numConstraint The number of atomic constraints in the system
+ void clearMlcpPhysicsProblem(size_t numDof, size_t numConstraint)
+ {
+ // Resize and zero all Eigen types
+ m_mlcpPhysicsProblem.A.resize(numConstraint, numConstraint);
+ m_mlcpPhysicsProblem.A.setZero();
+ m_mlcpPhysicsProblem.b.resize(numConstraint);
+ m_mlcpPhysicsProblem.b.setZero();
+ m_mlcpPhysicsProblem.mu.resize(numConstraint);
+ m_mlcpPhysicsProblem.mu.setZero();
+ m_mlcpPhysicsProblem.CHt.resize(numDof, numConstraint);
+ m_mlcpPhysicsProblem.CHt.setZero();
+ m_mlcpPhysicsProblem.H.resize(numConstraint, numDof);
+ m_mlcpPhysicsProblem.H.setZero();
+
+ // Empty all std::vector types
+ m_mlcpPhysicsProblem.constraintTypes.clear();
+ }
+};
+
+TEST_F (ConstraintTests, TestConstructor)
+{
+ auto fixedRep = std::make_shared<FixedRepresentation>("fixed");
+ auto rigidRep = std::make_shared<RigidRepresentation>("rigid");
+
+ std::shared_ptr<Localization> fixedLoc = std::make_shared<FixedRepresentationLocalization>();
+ std::shared_ptr<Localization> rigidLoc = std::make_shared<RigidRepresentationLocalization>();
+
+ fixedLoc->setRepresentation(fixedRep);
+ rigidLoc->setRepresentation(rigidRep);
+
+ std::shared_ptr<ConstraintImplementation> fixedImp = std::make_shared<FixedRepresentationContact>();
+ std::shared_ptr<ConstraintImplementation> rigidImp = std::make_shared<RigidRepresentationContact>();
+
+ {
+ SCOPED_TRACE("nullptr test");
+ ASSERT_NO_THROW({Constraint c(m_constraintData, fixedImp, fixedLoc, rigidImp, rigidLoc);});
+
+ EXPECT_THROW(
+ { Constraint c(nullptr, nullptr, nullptr, nullptr, nullptr); },
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(
+ { Constraint c(m_constraintData, fixedImp, nullptr, nullptr, nullptr); },
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(
+ { Constraint c(m_constraintData, fixedImp, fixedLoc, nullptr, nullptr); },
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(
+ { Constraint c(m_constraintData, fixedImp, fixedLoc, rigidImp, nullptr); },
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Localization nullptr test");
+
+ fixedLoc = std::make_shared<FixedRepresentationLocalization>();
+ rigidLoc = std::make_shared<RigidRepresentationLocalization>();
+ EXPECT_THROW(
+ { Constraint c(m_constraintData, fixedImp, fixedLoc, rigidImp, rigidLoc); },
+ SurgSim::Framework::AssertionFailure);
+
+ fixedLoc = std::make_shared<FixedRepresentationLocalization>();
+ rigidLoc = std::make_shared<RigidRepresentationLocalization>();
+ fixedLoc->setRepresentation(fixedRep);
+ EXPECT_THROW(
+ { Constraint c(m_constraintData, fixedImp, fixedLoc, rigidImp, rigidLoc); },
+ SurgSim::Framework::AssertionFailure);
+
+ fixedLoc = std::make_shared<FixedRepresentationLocalization>();
+ rigidLoc = std::make_shared<RigidRepresentationLocalization>();
+ rigidLoc->setRepresentation(rigidRep);
+ EXPECT_THROW(
+ { Constraint c(m_constraintData, fixedImp, fixedLoc, rigidImp, rigidLoc); },
+ SurgSim::Framework::AssertionFailure);
+
+ fixedLoc = std::make_shared<FixedRepresentationLocalization>();
+ rigidLoc = std::make_shared<RigidRepresentationLocalization>();
+ fixedLoc->setRepresentation(fixedRep);
+ rigidLoc->setRepresentation(rigidRep);
+ EXPECT_NO_THROW(
+ { Constraint c(m_constraintData, fixedImp, fixedLoc, rigidImp, rigidLoc); });
+ }
+
+ {
+ SCOPED_TRACE("Representation mismatch between Implementation and Localization");
+
+ EXPECT_THROW(
+ { Constraint c(m_constraintData, fixedImp, rigidLoc, rigidImp, fixedLoc); },
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(
+ { Constraint c(m_constraintData, rigidImp, fixedLoc, fixedImp, rigidLoc); },
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ // Need more checks for the other error conditions
+
+ Constraint c(m_constraintData, fixedImp, fixedLoc, rigidImp, rigidLoc);
+
+ EXPECT_EQ(m_constraintData, c.getData());
+ EXPECT_EQ(fixedImp, c.getImplementations().first);
+ EXPECT_EQ(rigidImp, c.getImplementations().second);
+ EXPECT_EQ(fixedLoc, c.getLocalizations().first);
+ EXPECT_EQ(rigidLoc, c.getLocalizations().second);
+}
+
+TEST_F (ConstraintTests, TestGetNumDof)
+{
+ auto fixedRep = std::make_shared<FixedRepresentation>("fixed");
+ auto rigidRep = std::make_shared<RigidRepresentation>("rigid");
+
+ std::shared_ptr<Localization> fixedLoc = std::make_shared<FixedRepresentationLocalization>();
+ std::shared_ptr<Localization> rigidLoc = std::make_shared<RigidRepresentationLocalization>();
+
+ fixedLoc->setRepresentation(fixedRep);
+ rigidLoc->setRepresentation(rigidRep);
+
+ std::shared_ptr<ConstraintImplementation> fixedImp = std::make_shared<FixedRepresentationContact>();
+ std::shared_ptr<ConstraintImplementation> rigidImp = std::make_shared<RigidRepresentationContact>();
+
+ {
+ SCOPED_TRACE("1DOF for a frictionless contact");
+ Constraint c(m_constraintData,
+ m_implementationFixedPlane, m_locFixedPlane,
+ m_implementationRigidSphere, m_locRigidSphere);
+ EXPECT_EQ(1u, c.getNumDof());
+ }
+
+ {
+ SCOPED_TRACE("1DOF for a frictionless contact between 2 fixed representations");
+ Constraint c(m_constraintData,fixedImp, fixedLoc, fixedImp, fixedLoc);
+ EXPECT_EQ(1u, c.getNumDof());
+ }
+
+ {
+ SCOPED_TRACE("1DOF for a frictionless contact between 1 fixed representation and 1 rigid representation");
+ Constraint c(m_constraintData,fixedImp, fixedLoc, rigidImp, rigidLoc);
+ EXPECT_EQ(1u, c.getNumDof());
+ }
+}
+
+// Test case: Rigid sphere at (0 0 0) with radius 0.01 colliding with Fixed plane Y=0
+// Contact location on the rigid sphere is (0 -0.01 0)
+// Contact location on the fixed plane is (0 0 0)
+// Constraint: (Sphere - Plane).n >= 0 with n=(0 1 0) The normal should be the contact normal on the 2nd object
+TEST_F (ConstraintTests, TestBuildMlcpSpherePlane)
+{
+ m_n.setZero();
+ m_n[1] = 1.0;
+ m_constraintData->setPlaneEquation(m_n, m_d);
+ m_constraint = std::make_shared<Constraint>(m_constraintData,
+ m_implementationRigidSphere, m_locRigidSphere,
+ m_implementationFixedPlane, m_locFixedPlane);
+
+ // Simulate 1 time step...to make sure all representation have a valid compliance matrix...
+ {
+ m_fixed->beforeUpdate(m_dt);
+ m_rigid->beforeUpdate(m_dt);
+
+ m_fixed->update(m_dt);
+ m_rigid->update(m_dt);
+
+ m_fixed->afterUpdate(m_dt);
+ m_rigid->afterUpdate(m_dt);
+ }
+
+ // Fill up the Mlcp
+ m_constraint->build(m_dt, &m_mlcpPhysicsProblem, m_indexSphereRepresentation, m_indexPlaneRepresentation,
+ m_indexConstraint);
+
+ // Violation b should be exactly -radius (the sphere center is on the plane)
+ // This should not depend on the ordering of the object...the violation remains the same no matter what
+ EXPECT_NEAR(-m_radius, m_mlcpPhysicsProblem.b[0], epsilon);
+
+ // Constraint H should be
+ // H = dt.[nx ny nz nz.GPy-ny.GPz nx.GPz-nz.GPx ny.GPx-nx.GPy]
+ // The rigid sphere being the 1st representation in the pair, it has the positive sign in the constraint !
+ double sign = 1.0;
+ Vector3d n_GP = m_n.cross(Vector3d(0.0, -m_radius, 0.0));
+
+ EXPECT_NEAR(sign * m_dt * m_n[0] , m_mlcpPhysicsProblem.H(0, 0), epsilon);
+ EXPECT_NEAR(sign * m_dt * m_n[1] , m_mlcpPhysicsProblem.H(0, 1), epsilon);
+ EXPECT_NEAR(sign * m_dt * m_n[2] , m_mlcpPhysicsProblem.H(0, 2), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[0], m_mlcpPhysicsProblem.H(0, 3), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[1], m_mlcpPhysicsProblem.H(0, 4), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[2], m_mlcpPhysicsProblem.H(0, 5), epsilon);
+
+ // ConstraintTypes should contain 1 entry SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
+ ASSERT_EQ(1u, m_mlcpPhysicsProblem.constraintTypes.size());
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, m_mlcpPhysicsProblem.constraintTypes[0]);
+}
+
+// Test case: Rigid sphere at (0 0 0) with radius 0.01 colliding with Fixed plane Y=0
+// Contact location on the rigid sphere is (0 -0.01 0)
+// Contact location on the fixed plane is (0 0 0)
+// Constraint: (Plane - Sphere).n >= 0 with n=(0 -1 0) The normal should be the contact normal on the 2nd object
+TEST_F (ConstraintTests, TestBuildMlcpPlaneSphere)
+{
+ m_n.setZero();
+ m_n[1] = -1.0;
+ m_constraintData->setPlaneEquation(m_n, m_d);
+ m_constraint = std::make_shared<Constraint>(m_constraintData,
+ m_implementationFixedPlane, m_locFixedPlane,
+ m_implementationRigidSphere, m_locRigidSphere);
+
+ // Simulate 1 time step...to make sure all representation have a valid compliance matrix...
+ {
+ m_fixed->beforeUpdate(m_dt);
+ m_rigid->beforeUpdate(m_dt);
+
+ m_fixed->update(m_dt);
+ m_rigid->update(m_dt);
+
+ m_fixed->afterUpdate(m_dt);
+ m_rigid->afterUpdate(m_dt);
+ }
+
+ // Fill up the Mlcp
+ m_constraint->build(m_dt, &m_mlcpPhysicsProblem, m_indexPlaneRepresentation, m_indexSphereRepresentation,
+ m_indexConstraint);
+
+ // Violation b should be exactly -radius (the sphere center is on the plane)
+ // This should not depend on the ordering of the object...the violation remains the same no matter what
+ EXPECT_NEAR(-m_radius, m_mlcpPhysicsProblem.b[0], epsilon);
+
+ // Constraint H should be
+ // H = dt.[nx ny nz nz.GPy-ny.GPz nx.GPz-nz.GPx ny.GPx-nx.GPy]
+ // The rigid sphere being the 2nd representation in the pair, it has the negative sign in the constraint !
+ double sign = -1.0;
+ Vector3d n_GP = m_n.cross(Vector3d(0.0, -m_radius, 0.0));
+
+ EXPECT_NEAR(sign * m_dt * m_n[0] , m_mlcpPhysicsProblem.H(0, 0), epsilon);
+ EXPECT_NEAR(sign * m_dt * m_n[1] , m_mlcpPhysicsProblem.H(0, 1), epsilon);
+ EXPECT_NEAR(sign * m_dt * m_n[2] , m_mlcpPhysicsProblem.H(0, 2), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[0], m_mlcpPhysicsProblem.H(0, 3), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[1], m_mlcpPhysicsProblem.H(0, 4), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[2], m_mlcpPhysicsProblem.H(0, 5), epsilon);
+
+ // ConstraintTypes should contain 1 entry SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
+ ASSERT_EQ(1u, m_mlcpPhysicsProblem.constraintTypes.size());
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, m_mlcpPhysicsProblem.constraintTypes[0]);
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/ContactConstraintDataTests.cpp b/SurgSim/Physics/UnitTests/ContactConstraintDataTests.cpp
new file mode 100644
index 0000000..467ec6a
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/ContactConstraintDataTests.cpp
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+using SurgSim::Physics::Constraint;
+using SurgSim::Physics::ConstraintData;
+using SurgSim::Physics::ContactConstraintData;
+
+#include "SurgSim/Math/Vector.h"
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+TEST (ContactConstraintDataTests, TestSetGet)
+{
+ ContactConstraintData contactConstraintData;
+ Vector3d n(1.2, 4.5, 6.7);
+ double d = 5.566;
+ n.normalize();
+
+ EXPECT_NEAR(0.0 , contactConstraintData.getDistance(), epsilon);
+ EXPECT_TRUE(contactConstraintData.getNormal().isZero());
+
+ contactConstraintData.setPlaneEquation(n, d);
+
+ EXPECT_NEAR(d , contactConstraintData.getDistance(), epsilon);
+ EXPECT_TRUE(contactConstraintData.getNormal().isApprox(n, epsilon));
+}
diff --git a/SurgSim/Physics/UnitTests/ContactConstraintGenerationTests.cpp b/SurgSim/Physics/UnitTests/ContactConstraintGenerationTests.cpp
new file mode 100644
index 0000000..40066a8
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/ContactConstraintGenerationTests.cpp
@@ -0,0 +1,206 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+
+#include <utility>
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/DcdCollision.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ContactConstraintGeneration.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+using SurgSim::Collision::CollisionPair;
+using SurgSim::Collision::ContactCalculation;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+struct ContactConstraintGenerationTests: public ::testing::Test
+{
+ virtual void SetUp()
+ {
+ rigid0 = std::make_shared<RigidRepresentation>("Physics Representation 0");
+ rigid0->setShape(std::make_shared<SphereShape>(2.0));
+ collision0 = std::make_shared<RigidCollisionRepresentation>("Collision Representation 0");
+ rigid0->setCollisionRepresentation(collision0);
+ representations.push_back(rigid0);
+
+ rigid1 = std::make_shared<RigidRepresentation>("Physics Representation 1");
+ rigid1->setShape(std::make_shared<DoubleSidedPlaneShape>());
+ collision1 = std::make_shared<RigidCollisionRepresentation>("Collision Representation 1");
+ rigid1->setCollisionRepresentation(collision1);
+ representations.push_back(rigid1);
+
+ state = std::make_shared<PhysicsManagerState>();
+ state->setRepresentations(representations);
+ }
+
+ virtual void TearDown()
+ {
+ }
+
+ std::shared_ptr<SurgSim::Collision::Representation> collision0;
+ std::shared_ptr<RigidRepresentation> rigid0;
+
+ std::shared_ptr<SurgSim::Collision::Representation> collision1;
+ std::shared_ptr<RigidRepresentation> rigid1;
+
+
+ std::shared_ptr<PhysicsManagerState> state;
+ std::vector<std::shared_ptr<SurgSim::Physics::Representation>> representations;
+
+ std::vector<std::shared_ptr<CollisionPair>> pairs;
+};
+
+TEST_F(ContactConstraintGenerationTests, BasicTest)
+{
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(collision0, collision1);
+ // Test case setup, create a pair with a contact and set up the physics state with it
+ SurgSim::Collision::SphereDoubleSidedPlaneDcdContact contactCalculation;
+
+ contactCalculation.calculateContact(pair);
+ ASSERT_TRUE(pair->hasContacts());
+
+ pairs.push_back(pair);
+
+ state->setCollisionPairs(pairs);
+
+ ContactConstraintGeneration generator;
+ generator.update(0.1, state);
+
+ ASSERT_EQ(1u, state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT).size());
+
+ auto constraints = state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT);
+ auto constraint = constraints[0];
+
+ auto localizations = constraint->getLocalizations();
+ auto implementations = constraint->getImplementations();
+ auto data = constraint->getData();
+
+ EXPECT_NE(nullptr, localizations.first);
+ EXPECT_NE(nullptr, localizations.second);
+ EXPECT_NE(nullptr, implementations.first);
+ EXPECT_NE(nullptr, implementations.second);
+ EXPECT_NE(nullptr, data);
+
+}
+
+TEST_F(ContactConstraintGenerationTests, CountTest)
+{
+ std::shared_ptr<CollisionPair> pair;
+ SurgSim::Collision::SphereDoubleSidedPlaneDcdContact contactCalculation;
+
+ pair = std::make_shared<CollisionPair>(collision0, collision1);
+ contactCalculation.calculateContact(pair);
+ contactCalculation.calculateContact(pair);
+
+ pairs.push_back(pair);
+
+ pair = std::make_shared<CollisionPair>(collision0, collision1);
+ contactCalculation.calculateContact(pair);
+ pairs.push_back(pair);
+
+ pair = std::make_shared<CollisionPair>(collision0, collision1);
+ pairs.push_back(pair);
+
+
+ state->setCollisionPairs(pairs);
+ ContactConstraintGeneration generator;
+ generator.update(0.1, state);
+
+ // 3 Contacts should generate 3 constraints
+ ASSERT_EQ(3u, state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT).size());
+
+}
+
+TEST_F(ContactConstraintGenerationTests, InactivePhysics)
+{
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(collision0, collision1);
+ SurgSim::Collision::SphereDoubleSidedPlaneDcdContact contactCalculation;
+ contactCalculation.calculateContact(pair);
+ pairs.push_back(pair);
+ state->setCollisionPairs(pairs);
+ ContactConstraintGeneration generator;
+
+ rigid0->setLocalActive(false);
+ rigid1->setLocalActive(true);
+ generator.update(0.1, state);
+ ASSERT_EQ(0u, state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT).size());
+
+ rigid0->setLocalActive(true);
+ rigid1->setLocalActive(false);
+ generator.update(0.1, state);
+ ASSERT_EQ(0u, state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT).size());
+
+ rigid0->setLocalActive(false);
+ rigid1->setLocalActive(false);
+ generator.update(0.1, state);
+ ASSERT_EQ(0u, state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT).size());
+
+ rigid0->setLocalActive(true);
+ rigid1->setLocalActive(true);
+ generator.update(0.1, state);
+ ASSERT_EQ(1u, state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT).size());
+}
+
+TEST_F(ContactConstraintGenerationTests, LocalPoses)
+{
+ //Move the collision representation away from the physics representation for the sphere
+ collision0->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(5.0, 0.0, 0.0)));
+
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(collision0, collision1);
+ SurgSim::Collision::SphereDoubleSidedPlaneDcdContact contactCalculation;
+
+ contactCalculation.calculateContact(pair);
+ ASSERT_EQ(1u, pair->getContacts().size());
+
+ pairs.push_back(pair);
+ state->setCollisionPairs(pairs);
+ ContactConstraintGeneration generator;
+ generator.update(0.1, state);
+
+ ASSERT_EQ(1u, state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT).size());
+ auto constraint = state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT)[0];
+
+ auto localization = constraint->getLocalizations().first;
+ auto location = pair->getContacts().front()->penetrationPoints.first;
+
+ Vector3d localizationGlobalPosition = localization->calculatePosition();
+ Vector3d locationGlobalPosition = collision0->getPose() * location.rigidLocalPosition.getValue();
+ EXPECT_TRUE(localizationGlobalPosition.isApprox(locationGlobalPosition)) <<
+ "The contact location is not in the same position as the localization produced by constraint generation";
+}
+
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem1D.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem1D.ply
new file mode 100644
index 0000000..9288efa
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem1D.ply
@@ -0,0 +1,34 @@
+ply
+format ascii 1.0
+comment Created by hand
+element vertex 7
+property double x
+property double y
+property double z
+element 1d_element 4
+property list uint uint vertex_indices
+element boundary_condition 3
+property uint vertex_index
+element radius 1
+property double value
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+1.100000 1.200000 -1.300000
+1.400000 -1.500000 -1.600000
+-1.700000 -1.800000 -1.900000
+2.000000 2.999999 3.000000
+-4.000000 5.000000 6.000000
+7.999999 -8.000001 9.000000
+9.100000 9.200000 9.300000
+2 0 1
+2 2 6
+2 3 5
+2 4 3
+2
+4
+5
+0.11
+0.21 0.31 0.41
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem2D.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem2D.ply
new file mode 100644
index 0000000..81fedbf
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem2D.ply
@@ -0,0 +1,31 @@
+ply
+format ascii 1.0
+comment Created by hand
+element vertex 6
+property double x
+property double y
+property double z
+element 2d_element 3
+property list uint uint vertex_indices
+element boundary_condition 2
+property uint vertex_index
+element thickness 1
+property double value
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+1.000000 1.000000 -1.000000
+1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 -1.000000
+1.000000 0.999999 1.000000
+-1.000000 1.000000 1.000000
+0.999999 -1.000001 1.000000
+3 0 1 2
+3 1 2 4
+3 3 4 5
+3
+2
+0.1
+0.2 0.3 0.4
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem3DCube.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem3DCube.ply
new file mode 100644
index 0000000..42a9ecd
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem3DCube.ply
@@ -0,0 +1,32 @@
+ply
+format ascii 1.0
+comment Created by hand
+element vertex 10
+property double x
+property double y
+property double z
+element 3d_element 3
+property list uint uint vertex_indices
+element boundary_condition 2
+property uint vertex_index
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+1.000000 1.000000 1.000000
+1.000000 1.000000 -1.000000
+1.000000 -1.000000 1.000000
+1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 -1.000000
+2.000000 2.000000 2.000000
+2.000000 -2.000000 2.000000
+2.000000 2.000000 -2.000000
+2.000000 -2.000000 -2.000000
+-2.000000 -2.000000 -2.000000
+8 0 1 2 3 4 5 6 7
+8 1 2 4 5 6 7 8 9
+8 3 4 5 0 2 6 8 7
+9
+5
+0.2 0.3 0.4
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Tetrahedron.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Tetrahedron.ply
new file mode 100644
index 0000000..348ba75
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Tetrahedron.ply
@@ -0,0 +1,63 @@
+ply
+format ascii 1.0
+comment Created by hand
+element vertex 26
+property double x
+property double y
+property double z
+element 3d_element 12
+property list uint uint vertex_indices
+element boundary_condition 8
+property uint vertex_index
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+1.000000 1.000000 -1.000000
+1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 -1.000000
+1.000000 0.999999 1.000000
+-1.000000 1.000000 1.000000
+0.999999 -1.000001 1.000000
+1.000000 1.000000 -1.000000
+1.000000 0.999999 1.000000
+1.000000 -1.000000 -1.000000
+1.000000 -1.000000 -1.000000
+0.999999 -1.000001 1.000000
+-1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 1.000000
+-1.000000 1.000000 1.000000
+1.000000 0.999999 1.000000
+1.000000 1.000000 -1.000000
+-1.000000 1.000000 1.000000
+-1.000000 1.000000 -1.000000
+1.000000 0.999999 1.000000
+0.999999 -1.000001 1.000000
+1.000000 -1.000000 -1.000000
+-1.000000 1.000000 -1.000000
+-1.000000 -1.000000 1.000000
+-1.000000 1.000000 -1.000000
+-1.000000 -1.000000 1.000000
+4 0 1 2 3
+4 3 4 5 6
+4 6 7 8 9
+4 9 10 11 12
+4 12 13 14 15
+4 15 16 17 18
+4 18 0 2 4
+4 19 20 21 5
+4 16 22 17 6
+4 4 23 5 7
+4 24 12 14 8
+4 10 25 11 9
+8
+5
+3
+2
+7
+1
+6
+11
+0.1432 0.224 0.472
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/WrongDataTetrahedron.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/WrongDataTetrahedron.ply
new file mode 100644
index 0000000..09907f9
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/WrongDataTetrahedron.ply
@@ -0,0 +1,64 @@
+ply
+format ascii 1.0
+comment Created by hand
+element vertex 26
+property double x
+property double y
+property double z
+element 3d_element 13
+property list uint uint vertex_indices
+element boundary_condition 8
+property uint vertex_index
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+1.000000 1.000000 -1.000000
+1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 -1.000000
+1.000000 0.999999 1.000000
+-1.000000 1.000000 1.000000
+0.999999 -1.000001 1.000000
+1.000000 1.000000 -1.000000
+1.000000 0.999999 1.000000
+1.000000 -1.000000 -1.000000
+1.000000 -1.000000 -1.000000
+0.999999 -1.000001 1.000000
+-1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 1.000000
+-1.000000 1.000000 1.000000
+1.000000 0.999999 1.000000
+1.000000 1.000000 -1.000000
+-1.000000 1.000000 1.000000
+-1.000000 1.000000 -1.000000
+1.000000 0.999999 1.000000
+0.999999 -1.000001 1.000000
+1.000000 -1.000000 -1.000000
+-1.000000 1.000000 -1.000000
+-1.000000 -1.000000 1.000000
+-1.000000 1.000000 -1.000000
+-1.000000 -1.000000 1.000000
+4 0 1 2 3
+4 3 4 5 6
+4 6 7 8 9
+4 9 10 11 12
+4 12 13 14 15
+4 15 16 17 18
+4 18 0 2 4
+4 19 20 21 5
+3 0 1 2 3
+4 16 22 17 6
+4 4 23 5 7
+4 24 12 14 8
+4 10 25 11 9
+8
+5
+3
+2
+7
+1
+6
+11
+0.1432 0.224 0.472
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/WrongPlyTetrahedron.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/WrongPlyTetrahedron.ply
new file mode 100644
index 0000000..a508e16
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/WrongPlyTetrahedron.ply
@@ -0,0 +1,63 @@
+ply
+format ascii 1.0
+comment Created by hand
+element vertex 26
+property double x
+property double y
+property double zi
+element 3d_element 12
+property list uint uint vertex_indices
+element boundary_condition 8
+property uint vertex_index
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+1.000000 1.000000 -1.000000
+1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 -1.000000
+1.000000 0.999999 1.000000
+-1.000000 1.000000 1.000000
+0.999999 -1.000001 1.000000
+1.000000 1.000000 -1.000000
+1.000000 0.999999 1.000000
+1.000000 -1.000000 -1.000000
+1.000000 -1.000000 -1.000000
+0.999999 -1.000001 1.000000
+-1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 -1.000000
+-1.000000 -1.000000 1.000000
+-1.000000 1.000000 1.000000
+1.000000 0.999999 1.000000
+1.000000 1.000000 -1.000000
+-1.000000 1.000000 1.000000
+-1.000000 1.000000 -1.000000
+1.000000 0.999999 1.000000
+0.999999 -1.000001 1.000000
+1.000000 -1.000000 -1.000000
+-1.000000 1.000000 -1.000000
+-1.000000 -1.000000 1.000000
+-1.000000 1.000000 -1.000000
+-1.000000 -1.000000 1.000000
+4 0 1 2 3
+4 3 4 5 6
+4 6 7 8 9
+4 9 10 11 12
+4 12 13 14 15
+4 15 16 17 18
+4 18 0 2 4
+4 19 20 21 5
+4 16 22 17 6
+4 4 23 5 7
+4 24 12 14 8
+4 10 25 11 9
+8
+5
+3
+2
+7
+1
+6
+11
+0.1432 0.224 0.472
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/DcdCollisionTests.cpp b/SurgSim/Physics/UnitTests/DcdCollisionTests.cpp
new file mode 100644
index 0000000..93cd186
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/DcdCollisionTests.cpp
@@ -0,0 +1,124 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 DcdCollisionTests.cpp
+/// Tests for the DcdCollision Class
+
+#include <memory>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Physics/DcdCollision.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+using SurgSim::Collision::CollisionPair;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::Shape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::FixedRepresentation;
+using SurgSim::Physics::PhysicsManagerState;
+using SurgSim::Physics::Representation;
+using SurgSim::Physics::RigidCollisionRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+
+std::shared_ptr<RigidRepresentation> createSphere(const std::string& name, const SurgSim::Math::Vector3d& position)
+{
+ std::shared_ptr<RigidRepresentation> representation = std::make_shared<RigidRepresentation>(name);
+
+ representation->setDensity(700.0); // Wood
+
+ std::shared_ptr<SphereShape> shape = std::make_shared<SphereShape>(1.0); // 1cm Sphere
+ representation->setShape(shape);
+
+ representation->setLocalPose(SurgSim::Math::makeRigidTransform(SurgSim::Math::Quaterniond::Identity(), position));
+
+ return representation;
+}
+
+
+TEST(DcdCollisionTest, RigidRigidCollisionTest)
+{
+ std::shared_ptr<PhysicsManagerState> state = std::make_shared<PhysicsManagerState>();
+
+ std::shared_ptr<RigidRepresentation> sphere1 = createSphere("Sphere1", Vector3d(0.0,0.0,0.0));
+ std::shared_ptr<RigidRepresentation> sphere2 = createSphere("Sphere2", Vector3d(0.0,0.0,0.5));
+
+ std::shared_ptr<SurgSim::Collision::Representation> sphere1Collision =
+ std::make_shared<RigidCollisionRepresentation>("Sphere1 Collision");
+ sphere1->setCollisionRepresentation(sphere1Collision);
+
+ std::shared_ptr<SurgSim::Collision::Representation> sphere2Collision =
+ std::make_shared<RigidCollisionRepresentation>("Sphere2 Collision");
+ sphere2->setCollisionRepresentation(sphere2Collision);
+
+ std::vector<std::shared_ptr<Representation>> representations;
+ representations.push_back(sphere1);
+ representations.push_back(sphere2);
+ state->setRepresentations(representations);
+
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> collisions;
+ collisions.push_back(sphere1Collision);
+ collisions.push_back(sphere2Collision);
+ state->setCollisionRepresentations(collisions);
+
+ SurgSim::Physics::DcdCollision computation;
+ std::shared_ptr<PhysicsManagerState> newState = computation.update(1.0, state);
+
+ ASSERT_EQ(1u, newState->getCollisionPairs().size());
+ EXPECT_TRUE(newState->getCollisionPairs().at(0)->hasContacts());
+}
+
+TEST(DcdCollisionTest, FixedRigidCollisionTest)
+{
+ std::shared_ptr<PhysicsManagerState> state = std::make_shared<PhysicsManagerState>();
+
+ std::shared_ptr<RigidRepresentation> sphere1 = createSphere("Sphere1", Vector3d(0.0,0.0,0.0));
+
+ std::shared_ptr<SurgSim::Collision::Representation> sphere1Collision =
+ std::make_shared<RigidCollisionRepresentation>("Sphere Collision");
+ sphere1->setCollisionRepresentation(sphere1Collision);
+
+ std::shared_ptr<FixedRepresentation> fixed = std::make_shared<FixedRepresentation>("Fixed");
+ std::shared_ptr<Shape> shape = std::make_shared<DoubleSidedPlaneShape>();
+ fixed->setShape(shape);
+ std::shared_ptr<SurgSim::Collision::Representation> fixedCollision =
+ std::make_shared<RigidCollisionRepresentation>("Plane Collision");
+ fixed->setCollisionRepresentation(fixedCollision);
+
+ std::vector<std::shared_ptr<Representation>> representations;
+ representations.push_back(sphere1);
+ representations.push_back(fixed);
+ state->setRepresentations(representations);
+
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> collisions;
+ collisions.push_back(sphere1Collision);
+ collisions.push_back(fixedCollision);
+ state->setCollisionRepresentations(collisions);
+
+ SurgSim::Physics::DcdCollision computation;
+ std::shared_ptr<PhysicsManagerState> newState = computation.update(1.0, state);
+
+ ASSERT_EQ(1u, newState->getCollisionPairs().size());
+ EXPECT_TRUE(newState->getCollisionPairs().at(0)->hasContacts());
+}
diff --git a/SurgSim/Physics/UnitTests/DeformableCollisionRepresentationTest.cpp b/SurgSim/Physics/UnitTests/DeformableCollisionRepresentationTest.cpp
new file mode 100644
index 0000000..a13f7da
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/DeformableCollisionRepresentationTest.cpp
@@ -0,0 +1,144 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/OdeSolver.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+#include "SurgSim/Physics/DeformableRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace
+{
+const double epsilon = 1e-10;
+}
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+struct DeformableCollisionRepresentationTest : public ::testing::Test
+{
+ void SetUp()
+ {
+ m_runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ m_filename = std::string("Geometry/wound_deformable.ply");
+ m_meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+ m_meshShape->load(m_filename);
+ m_deformableRepresentation = std::make_shared<MockDeformableRepresentation>("DeformableRepresentation");
+ m_deformableCollisionRepresentation =
+ std::make_shared<DeformableCollisionRepresentation>("DeformableCollisionRepresentation");
+ }
+
+ std::shared_ptr<SurgSim::Framework::Runtime> m_runtime;
+ std::string m_filename;
+ std::shared_ptr<SurgSim::Math::MeshShape> m_meshShape;
+ std::shared_ptr<SurgSim::Physics::DeformableRepresentation> m_deformableRepresentation;
+ std::shared_ptr<SurgSim::Physics::DeformableCollisionRepresentation> m_deformableCollisionRepresentation;
+};
+
+TEST_F(DeformableCollisionRepresentationTest, InitTest)
+{
+ EXPECT_NO_THROW(DeformableCollisionRepresentation("TestDeformableCollisionRepresentation"));
+}
+
+TEST_F(DeformableCollisionRepresentationTest, SetGetDeformableRepresentationTest)
+{
+ ASSERT_NO_THROW(m_deformableCollisionRepresentation->setDeformableRepresentation(m_deformableRepresentation));
+ EXPECT_EQ(m_deformableRepresentation, m_deformableCollisionRepresentation->getDeformableRepresentation());
+}
+
+TEST_F(DeformableCollisionRepresentationTest, ShapeTest)
+{
+ EXPECT_ANY_THROW(m_deformableCollisionRepresentation->getShapeType());
+ m_deformableCollisionRepresentation->setShape(m_meshShape);
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_MESH, m_deformableCollisionRepresentation->getShapeType());
+
+ auto meshShape =
+ std::dynamic_pointer_cast<SurgSim::Math::MeshShape>(m_deformableCollisionRepresentation->getShape());
+ EXPECT_NEAR(m_meshShape->getVolume(), meshShape->getVolume(), epsilon);
+ EXPECT_TRUE(m_meshShape->getCenter().isApprox(meshShape->getCenter()));
+ EXPECT_TRUE(m_meshShape->getSecondMomentOfVolume().isApprox(meshShape->getSecondMomentOfVolume()));
+}
+
+TEST_F(DeformableCollisionRepresentationTest, MeshTest)
+{
+ m_deformableCollisionRepresentation->setMesh(m_meshShape->getMesh());
+ EXPECT_EQ(m_meshShape->getMesh()->getNumVertices(),
+ m_deformableCollisionRepresentation->getMesh()->getNumVertices());
+ EXPECT_EQ(m_meshShape->getMesh()->getNumEdges(),
+ m_deformableCollisionRepresentation->getMesh()->getNumEdges());
+ EXPECT_EQ(m_meshShape->getMesh()->getNumTriangles(),
+ m_deformableCollisionRepresentation->getMesh()->getNumTriangles());
+}
+
+TEST_F(DeformableCollisionRepresentationTest, SerializationTest)
+{
+ auto shape = std::dynamic_pointer_cast<SurgSim::Math::Shape>(m_meshShape);
+ m_deformableCollisionRepresentation->setValue("Shape", shape);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*m_deformableCollisionRepresentation));
+
+ std::shared_ptr<SurgSim::Physics::DeformableCollisionRepresentation> newDeformableCollisionRepresentation;
+ ASSERT_NO_THROW(newDeformableCollisionRepresentation =
+ std::dynamic_pointer_cast<SurgSim::Physics::DeformableCollisionRepresentation>
+ (node.as<std::shared_ptr<SurgSim::Framework::Component>>())
+ );
+
+ auto fem3DRepresentation = std::make_shared<SurgSim::Physics::Fem3DRepresentation>("Fem3DRepresentation");
+ fem3DRepresentation->setCollisionRepresentation(newDeformableCollisionRepresentation);
+ newDeformableCollisionRepresentation->initialize(m_runtime);
+
+ auto mesh = std::dynamic_pointer_cast<SurgSim::Math::MeshShape>(newDeformableCollisionRepresentation->getShape());
+ EXPECT_NEAR(m_meshShape->getVolume(), mesh->getVolume(), epsilon);
+ EXPECT_TRUE(m_meshShape->getCenter().isApprox(mesh->getCenter()));
+ EXPECT_TRUE(m_meshShape->getSecondMomentOfVolume().isApprox(mesh->getSecondMomentOfVolume()));
+
+ EXPECT_EQ(m_meshShape->getMesh()->getNumVertices(), mesh->getMesh()->getNumVertices());
+ EXPECT_EQ(m_meshShape->getMesh()->getNumEdges(), mesh->getMesh()->getNumEdges());
+ EXPECT_EQ(m_meshShape->getMesh()->getNumTriangles(), mesh->getMesh()->getNumTriangles());
+}
+
+TEST_F(DeformableCollisionRepresentationTest, UpdateAndInitializationTest)
+{
+ EXPECT_ANY_THROW(m_deformableCollisionRepresentation->update(0.0));
+
+ auto fem3DRepresentation = std::make_shared<SurgSim::Physics::Fem3DRepresentation>("Fem3DRepresentation");
+ fem3DRepresentation->setFilename(m_filename);
+
+ // Member data 'odeState' will be created while loading.
+ ASSERT_TRUE(fem3DRepresentation->initialize(m_runtime));
+
+ // Connect Physics representation with Collision representation.
+ fem3DRepresentation->setCollisionRepresentation(m_deformableCollisionRepresentation);
+
+ // Set the shape used by Collision representation.
+ m_deformableCollisionRepresentation->setShape(m_meshShape);
+ EXPECT_NO_THROW(m_deformableCollisionRepresentation->initialize(m_runtime));
+ EXPECT_NO_THROW(m_deformableCollisionRepresentation->update(0.0));
+}
+
+} // namespace Physics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/DeformableRepresentationTest.cpp b/SurgSim/Physics/UnitTests/DeformableRepresentationTest.cpp
new file mode 100644
index 0000000..2390ff0
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/DeformableRepresentationTest.cpp
@@ -0,0 +1,531 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/OdeSolver.h" // Need access to the enum IntegrationScheme
+#include "SurgSim/Math/OdeSolverEulerExplicit.h"
+#include "SurgSim/Math/OdeSolverEulerExplicitModified.h"
+#include "SurgSim/Math/OdeSolverEulerImplicit.h"
+#include "SurgSim/Math/OdeSolverLinearEulerExplicit.h"
+#include "SurgSim/Math/OdeSolverLinearEulerExplicitModified.h"
+#include "SurgSim/Math/OdeSolverLinearEulerImplicit.h"
+#include "SurgSim/Math/OdeSolverLinearStatic.h"
+#include "SurgSim/Math/OdeSolverStatic.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+#include "SurgSim/Physics/DeformableRepresentation.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::Math::Matrix;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector;
+using SurgSim::Physics::DeformableCollisionRepresentation;
+using SurgSim::Physics::DeformableRepresentation;
+using SurgSim::Physics::MockDeformableRepresentation;
+
+namespace
+{
+const size_t numNodes = 1;
+const double epsilon = 1e-10;
+}; // anonymous namespace
+
+class DeformableRepresentationTest: public MockDeformableRepresentation, public ::testing::Test
+{
+public:
+ DeformableRepresentationTest()
+ : MockDeformableRepresentation()
+ {
+ }
+
+ virtual ~DeformableRepresentationTest()
+ {
+ }
+
+ /// Setup the test case
+ void SetUp() override
+ {
+ m_localInitialState = std::make_shared<SurgSim::Math::OdeState>();
+ m_localInitialState->setNumDof(getNumDofPerNode(), numNodes);
+ m_localInitialState->getPositions().setLinSpaced(0.0, static_cast<double>(getNumDofPerNode() * numNodes- 1));
+ m_localInitialState->getVelocities().setOnes();
+
+ SurgSim::Math::Quaterniond q(0.1, 0.4, 0.5, 0.2);
+ q.normalize();
+ Vector3d t(1.45, 5.4, 2.42);
+ m_nonIdentityTransform = SurgSim::Math::makeRigidTransform(q, t);
+ m_identityTransform = SurgSim::Math::RigidTransform3d::Identity();
+
+ m_localization0 = std::make_shared<SurgSim::Physics::MockDeformableRepresentationLocalization>();
+ m_localization0->setLocalNode(0);
+ }
+
+protected:
+ // Initial state
+ std::shared_ptr<SurgSim::Math::OdeState> m_localInitialState;
+
+ // Identity and nonIdentity (but still valid) transforms
+ SurgSim::Math::RigidTransform3d m_identityTransform;
+ SurgSim::Math::RigidTransform3d m_nonIdentityTransform;
+
+ // Localization of node 0, to apply external force
+ std::shared_ptr<SurgSim::Physics::MockDeformableRepresentationLocalization> m_localization0;
+};
+
+TEST_F(DeformableRepresentationTest, ConstructorTest)
+{
+ // Test the constructor normally
+ ASSERT_NO_THROW({MockDeformableRepresentation deformable;});
+
+ // Test the object creation through the operator new
+ ASSERT_NO_THROW({MockDeformableRepresentation* deformable = new MockDeformableRepresentation; delete deformable;});
+
+ // Test the object creation through the operator new []
+ ASSERT_NO_THROW({MockDeformableRepresentation* deformable = new MockDeformableRepresentation[10];\
+ delete [] deformable;});
+
+ // Test the object creation through a shared_ptr
+ ASSERT_NO_THROW({std::shared_ptr<MockDeformableRepresentation> deformable =\
+ std::make_shared<MockDeformableRepresentation>(); });
+}
+
+TEST_F(DeformableRepresentationTest, SetGetTest)
+{
+ // Test setLocalPose/getLocalPose
+ setLocalPose(m_nonIdentityTransform);
+ EXPECT_TRUE(getLocalPose().isApprox(m_nonIdentityTransform, epsilon));
+ EXPECT_FALSE(getLocalPose().isApprox(m_identityTransform, epsilon));
+ EXPECT_FALSE(getPose().isApprox(m_identityTransform, epsilon));
+
+ setLocalPose(m_identityTransform);
+ EXPECT_FALSE(getLocalPose().isApprox(m_nonIdentityTransform, epsilon));
+ EXPECT_TRUE(getLocalPose().isApprox(m_identityTransform, epsilon));
+ EXPECT_TRUE(getPose().isApprox(m_identityTransform, epsilon));
+
+ // Test set/get states
+ // Note that the initialState is in OdeEquation but is set in DeformableRepresentation
+ // Its getter is actually in OdeEquation (considered tested here)
+ EXPECT_EQ(0, getExternalGeneralizedForce().size());
+ EXPECT_EQ(0, getExternalGeneralizedStiffness().rows());
+ EXPECT_EQ(0, getExternalGeneralizedStiffness().cols());
+ EXPECT_EQ(0, getExternalGeneralizedDamping().rows());
+ EXPECT_EQ(0, getExternalGeneralizedDamping().cols());
+ setInitialState(m_localInitialState);
+ EXPECT_EQ(getNumDof(), getExternalGeneralizedForce().size());
+ EXPECT_EQ(getNumDof(), getExternalGeneralizedStiffness().rows());
+ EXPECT_EQ(getNumDof(), getExternalGeneralizedStiffness().cols());
+ EXPECT_EQ(getNumDof(), getExternalGeneralizedDamping().rows());
+ EXPECT_EQ(getNumDof(), getExternalGeneralizedDamping().cols());
+ EXPECT_TRUE(getExternalGeneralizedForce().isZero());
+ EXPECT_TRUE(getExternalGeneralizedStiffness().isZero());
+ EXPECT_TRUE(getExternalGeneralizedDamping().isZero());
+
+ doWakeUp();
+
+ EXPECT_TRUE(*m_initialState == *m_localInitialState);
+ EXPECT_TRUE(*m_currentState == *m_localInitialState);
+ EXPECT_TRUE(*m_previousState == *m_localInitialState);
+ EXPECT_TRUE(*m_finalState == *m_localInitialState);
+ EXPECT_TRUE(*getInitialState() == *m_localInitialState);
+ EXPECT_TRUE(*getPreviousState() == *m_localInitialState);
+ EXPECT_TRUE(*getCurrentState() == *m_localInitialState);
+ EXPECT_TRUE(*getFinalState() == *m_localInitialState);
+
+ // Test getNumDofPerNode
+ EXPECT_EQ(3, getNumDofPerNode());
+
+ // Test getNumDof (needs to be tested after setInitialState has been called)
+ EXPECT_EQ(getNumDofPerNode() * numNodes, getNumDof());
+
+ /// Set/Get the numerical integration scheme
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER);
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER, getIntegrationScheme());
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER);
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER, getIntegrationScheme());
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER);
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER, getIntegrationScheme());
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_STATIC);
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_STATIC, getIntegrationScheme());
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4);
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4, getIntegrationScheme());
+
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER);
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER, getIntegrationScheme());
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER);
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER, getIntegrationScheme());
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER);
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER, getIntegrationScheme());
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC);
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC, getIntegrationScheme());
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4);
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4, getIntegrationScheme());
+}
+
+TEST_F(DeformableRepresentationTest, GetComplianceMatrix)
+{
+ double dt = 1e-3;
+
+ EXPECT_NO_THROW(setInitialState(m_localInitialState));
+ EXPECT_NO_THROW(EXPECT_TRUE(initialize(std::make_shared<SurgSim::Framework::Runtime>())));
+ EXPECT_NO_THROW(EXPECT_TRUE(wakeUp()));
+
+ // This call solves the Ode equation and computes the compliance matrix using the default ode solver
+ // Explicit euler => M.a(t+dt) = F(t) <=> M/dt.deltaV = F(t)
+ // So the compliance matrix will be (M/dt)^-1
+ // In our case, M = Identity, so the compliance matrix will be Identity*dt
+ EXPECT_NO_THROW(update(dt));
+
+ EXPECT_NO_THROW(EXPECT_TRUE(getComplianceMatrix().isApprox(Matrix::Identity(3,3) * dt)));
+}
+
+TEST_F(DeformableRepresentationTest, ResetStateTest)
+{
+ // setInitialState sets all 4 states (tested in method above !)
+ setInitialState(m_localInitialState);
+
+ // Initialize and wake-up the deformable component
+ EXPECT_NO_THROW(EXPECT_TRUE(initialize(std::make_shared<SurgSim::Framework::Runtime>())));
+ EXPECT_NO_THROW(EXPECT_TRUE(wakeUp()));
+
+ // 1st time step
+ beforeUpdate(1e-3);
+ update(1e-3);
+ afterUpdate(1e-3);
+
+ // 2nd time step
+ beforeUpdate(1e-3);
+ update(1e-3);
+ afterUpdate(1e-3);
+
+ EXPECT_TRUE(*m_localInitialState == *m_initialState);
+ EXPECT_FALSE(*m_localInitialState == *m_previousState);
+ EXPECT_FALSE(*m_localInitialState == *m_currentState);
+ EXPECT_FALSE(*m_localInitialState == *m_finalState);
+ EXPECT_TRUE(*m_localInitialState == *getInitialState());
+ EXPECT_FALSE(*m_localInitialState == *getPreviousState());
+ EXPECT_FALSE(*m_localInitialState == *getCurrentState());
+ EXPECT_FALSE(*m_localInitialState == *getFinalState());
+ resetState();
+ // reset should re-initialized current, previous and final to initial
+ EXPECT_TRUE(*m_localInitialState == *m_initialState);
+ EXPECT_TRUE(*m_localInitialState == *m_previousState);
+ EXPECT_TRUE(*m_localInitialState == *m_currentState);
+ EXPECT_TRUE(*m_localInitialState == *m_finalState);
+ EXPECT_TRUE(*m_localInitialState == *getInitialState());
+ EXPECT_TRUE(*m_localInitialState == *getPreviousState());
+ EXPECT_TRUE(*m_localInitialState == *getCurrentState());
+ EXPECT_TRUE(*m_localInitialState == *getFinalState());
+}
+
+TEST_F(DeformableRepresentationTest, DeactivateAndResetTest)
+{
+ // setInitialState sets all 4 states (tested in method above !)
+ setInitialState(m_localInitialState);
+
+ // Initialize and wake-up the deformable component
+ EXPECT_NO_THROW(EXPECT_TRUE(initialize(std::make_shared<SurgSim::Framework::Runtime>())));
+ EXPECT_NO_THROW(EXPECT_TRUE(wakeUp()));
+
+ // 1st time step
+ ASSERT_TRUE(isActive());
+ beforeUpdate(1e-3);
+ update(1e-3);
+ afterUpdate(1e-3);
+ ASSERT_TRUE(isActive());
+
+ deactivateAndReset();
+ ASSERT_FALSE(isActive());
+ EXPECT_TRUE(*m_localInitialState == *m_initialState);
+ EXPECT_TRUE(*m_localInitialState == *m_previousState);
+ EXPECT_TRUE(*m_localInitialState == *m_currentState);
+ EXPECT_TRUE(*m_localInitialState == *m_finalState);
+ EXPECT_TRUE(*m_localInitialState == *getInitialState());
+ EXPECT_TRUE(*m_localInitialState == *getPreviousState());
+ EXPECT_TRUE(*m_localInitialState == *getCurrentState());
+ EXPECT_TRUE(*m_localInitialState == *getFinalState());
+}
+
+TEST_F(DeformableRepresentationTest, UpdateTest)
+{
+ // update assert on m_odeSolver (wakeUp) and m_initialState (setInitialState)
+ EXPECT_THROW(update(1e-3), SurgSim::Framework::AssertionFailure);
+
+ // setInitialState sets all 4 states (tested in method above !)
+ setInitialState(m_localInitialState);
+
+ // update assert on m_odeSolver (wakeUp) and m_initialState (setInitialState)
+ EXPECT_THROW(update(1e-3), SurgSim::Framework::AssertionFailure);
+
+ // Initialize and wake-up the deformable component
+ EXPECT_NO_THROW(EXPECT_TRUE(initialize(std::make_shared<SurgSim::Framework::Runtime>())));
+ EXPECT_NO_THROW(EXPECT_TRUE(wakeUp()));
+
+ // update should backup current into previous and change current
+ EXPECT_NO_THROW(update(1e-3));
+
+ EXPECT_TRUE(*m_localInitialState == *m_initialState);
+ EXPECT_TRUE(*m_localInitialState == *m_previousState);
+ EXPECT_FALSE(*m_localInitialState == *m_currentState);
+ EXPECT_TRUE(*m_localInitialState == *m_finalState);
+ EXPECT_TRUE(*m_localInitialState == *getInitialState());
+ EXPECT_TRUE(*m_localInitialState == *getPreviousState());
+ EXPECT_FALSE(*m_localInitialState == *getCurrentState());
+ EXPECT_TRUE(*m_localInitialState == *getFinalState());
+ EXPECT_FALSE(*m_previousState == *m_currentState);
+ EXPECT_FALSE(*getCurrentState() == *getPreviousState());
+}
+
+TEST_F(DeformableRepresentationTest, UpdateResetTest)
+{
+ m_localInitialState->getVelocities()[0] = std::numeric_limits<double>::infinity();
+ setInitialState(m_localInitialState);
+
+ // Initialize and wake-up the deformable component
+ EXPECT_NO_THROW(EXPECT_TRUE(initialize(std::make_shared<SurgSim::Framework::Runtime>())));
+ EXPECT_NO_THROW(EXPECT_TRUE(wakeUp()));
+
+ // update should backup current into previous and change current
+ EXPECT_TRUE(isActive());
+ EXPECT_NO_THROW(update(1e-3));
+ EXPECT_FALSE(isActive());
+}
+
+TEST_F(DeformableRepresentationTest, AfterUpdateTest)
+{
+ // setInitialState sets all 4 states (tested in method above !)
+ setInitialState(m_localInitialState);
+
+ // Initialize and wake-up the deformable component
+ EXPECT_NO_THROW(EXPECT_TRUE(initialize(std::make_shared<SurgSim::Framework::Runtime>())));
+ EXPECT_NO_THROW(EXPECT_TRUE(wakeUp()));
+
+ // Set external generalized force/stiffness/damping
+ EXPECT_TRUE(getExternalGeneralizedForce().isZero());
+ EXPECT_TRUE(getExternalGeneralizedStiffness().isZero());
+ EXPECT_TRUE(getExternalGeneralizedDamping().isZero());
+ SurgSim::Math::Vector F = SurgSim::Math::Vector::LinSpaced(getNumDofPerNode(), -2.34, 4.41);
+ SurgSim::Math::Matrix K = SurgSim::Math::Matrix::Ones(getNumDofPerNode(), getNumDofPerNode());
+ SurgSim::Math::Matrix D = 2.3 * K;
+ addExternalGeneralizedForce(m_localization0, F, K, D);
+ EXPECT_FALSE(getExternalGeneralizedForce().isZero());
+ EXPECT_FALSE(getExternalGeneralizedStiffness().isZero());
+ EXPECT_FALSE(getExternalGeneralizedDamping().isZero());
+ EXPECT_TRUE(getExternalGeneralizedForce().isApprox(F));
+ EXPECT_TRUE(getExternalGeneralizedStiffness().isApprox(K));
+ EXPECT_TRUE(getExternalGeneralizedDamping().isApprox(D));
+
+ // update should backup current into previous and change current
+ EXPECT_NO_THROW(update(1e-3));
+ // afterUpdate should backup current into final
+ EXPECT_NO_THROW(afterUpdate(1e-3));
+
+ // External generalized force/stiffness/damping should have been reset
+ EXPECT_TRUE(getExternalGeneralizedForce().isZero());
+ EXPECT_TRUE(getExternalGeneralizedStiffness().isZero());
+ EXPECT_TRUE(getExternalGeneralizedDamping().isZero());
+
+ EXPECT_TRUE(*m_localInitialState == *m_initialState);
+ EXPECT_TRUE(*m_localInitialState == *m_previousState);
+ EXPECT_FALSE(*m_localInitialState == *m_currentState);
+ EXPECT_FALSE(*m_localInitialState == *m_finalState);
+ EXPECT_TRUE(*m_localInitialState == *getInitialState());
+ EXPECT_TRUE(*m_localInitialState == *getPreviousState());
+ EXPECT_FALSE(*m_localInitialState == *getCurrentState());
+ EXPECT_FALSE(*m_localInitialState == *getFinalState());
+ EXPECT_FALSE(*m_currentState == *m_previousState);
+ EXPECT_TRUE(*m_currentState == *m_finalState);
+ EXPECT_FALSE(*getCurrentState() == *getPreviousState());
+ EXPECT_TRUE(*getCurrentState() == *getFinalState());
+
+}
+
+TEST_F(DeformableRepresentationTest, ApplyCorrectionTest)
+{
+ const double dt = 1e-3;
+
+ MockDeformableRepresentation object;
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(object.getNumDofPerNode(), 3);
+ object.setInitialState(initialState);
+
+ SurgSim::Math::Vector dv;
+ dv.resize(object.getNumDof());
+ for (size_t i = 0; i < object.getNumDof(); i++)
+ {
+ dv(i) = static_cast<double>(i);
+ }
+
+ Eigen::VectorXd previousX = object.getCurrentState()->getPositions();
+ Eigen::VectorXd previousV = object.getCurrentState()->getVelocities();
+
+ // Test with a valid state
+ object.applyCorrection(dt, dv.segment(0, object.getNumDof()));
+ Eigen::VectorXd nextX = object.getCurrentState()->getPositions();
+ Eigen::VectorXd nextV = object.getCurrentState()->getVelocities();
+
+ EXPECT_TRUE(nextX.isApprox(previousX + dv * dt, epsilon));
+ EXPECT_TRUE(nextV.isApprox(previousV + dv, epsilon));
+
+ // Test with an invalid state
+ dv(0) = std::numeric_limits<double>::infinity();
+ EXPECT_TRUE(object.isActive());
+ object.applyCorrection(dt, dv.segment(0, object.getNumDof()));
+ EXPECT_FALSE(object.isActive());
+}
+
+TEST_F(DeformableRepresentationTest, SetCollisionRepresentationTest)
+{
+ // setCollisionRepresentation requires the object to be a shared_ptr (using getShared())
+ std::shared_ptr<MockDeformableRepresentation> object = std::make_shared<MockDeformableRepresentation>();
+
+ auto collisionRep = std::make_shared<DeformableCollisionRepresentation>("DeformableCollisionRepresentation");
+ EXPECT_NE(nullptr, collisionRep);
+
+ // Test default collision representation to be nullptr
+ EXPECT_EQ(nullptr, object->getCollisionRepresentation());
+
+ // Test setting a valid non null collision rep
+ EXPECT_NO_THROW(object->setCollisionRepresentation(collisionRep));
+ EXPECT_EQ(collisionRep, object->getCollisionRepresentation());
+
+ // Test setting a null collision rep
+ EXPECT_NO_THROW(object->setCollisionRepresentation(nullptr));
+ EXPECT_EQ(nullptr, object->getCollisionRepresentation());
+}
+
+TEST_F(DeformableRepresentationTest, DoWakeUpTest)
+{
+ using SurgSim::Math::OdeSolverEulerExplicit;
+ using SurgSim::Math::OdeSolverEulerExplicitModified;
+ using SurgSim::Math::OdeSolverEulerImplicit;
+ using SurgSim::Math::OdeSolverStatic;
+ using SurgSim::Math::OdeSolverLinearEulerExplicit;
+ using SurgSim::Math::OdeSolverLinearEulerExplicitModified;
+ using SurgSim::Math::OdeSolverLinearEulerImplicit;
+ using SurgSim::Math::OdeSolverLinearStatic;
+ using SurgSim::Math::LinearSolveAndInverseDenseMatrix;
+
+ // setInitialState sets all 4 states (tested in method above !)
+ setLocalPose(m_nonIdentityTransform);
+ setInitialState(m_localInitialState);
+
+ EXPECT_NO_THROW(EXPECT_TRUE(initialize(std::make_shared<SurgSim::Framework::Runtime>())));
+ EXPECT_NO_THROW(EXPECT_TRUE(wakeUp()));
+
+ // Test the initial transformation applied to all the states
+ for (size_t nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ Vector3d expectedPosition
+ = m_nonIdentityTransform
+ * Vector3d::LinSpaced(static_cast<double>(nodeId) * 3, (static_cast<double>(nodeId) + 1) * 3 - 1);
+ Vector3d expectedVelocity = m_nonIdentityTransform.rotation() * Vector3d::Ones();
+ EXPECT_TRUE(getInitialState()->getPosition(nodeId).isApprox(expectedPosition));
+ EXPECT_TRUE(getInitialState()->getVelocity(nodeId).isApprox(expectedVelocity));
+ }
+ EXPECT_EQ(*getPreviousState(), *getInitialState());
+ EXPECT_EQ(*getCurrentState(), *getInitialState());
+ EXPECT_EQ(*getFinalState(), *getInitialState());
+
+ // Test the Ode Solver
+ ASSERT_NE(nullptr, m_odeSolver);
+ ASSERT_NE(nullptr, m_odeSolver->getLinearSolver());
+ std::shared_ptr<LinearSolveAndInverseDenseMatrix> expectedLinearSolverType;
+ expectedLinearSolverType = std::dynamic_pointer_cast<LinearSolveAndInverseDenseMatrix>
+ (m_odeSolver->getLinearSolver());
+ ASSERT_NE(nullptr, expectedLinearSolverType);
+
+ typedef OdeSolverEulerExplicit EESolver;
+ EESolver* explicitEuler;
+ explicitEuler = dynamic_cast<EESolver*>(m_odeSolver.get());
+ ASSERT_NE(nullptr, explicitEuler);
+
+ typedef OdeSolverEulerExplicitModified MEESolver;
+ MEESolver* modifiedExplicitEuler;
+ modifiedExplicitEuler = dynamic_cast<MEESolver*>(m_odeSolver.get());
+ ASSERT_EQ(nullptr, modifiedExplicitEuler);
+
+ typedef OdeSolverEulerImplicit IESolver;
+ IESolver* implicitEuler;
+ implicitEuler = dynamic_cast<IESolver*>(m_odeSolver.get());
+ ASSERT_EQ(nullptr, implicitEuler);
+
+ typedef OdeSolverStatic StaticSolver;
+ StaticSolver* staticSolver;
+ staticSolver = dynamic_cast<StaticSolver*>(m_odeSolver.get());
+ ASSERT_EQ(nullptr, staticSolver);
+
+ typedef OdeSolverLinearEulerExplicit EELinearSolver;
+ EELinearSolver* explicitEulerLinear;
+ explicitEulerLinear = dynamic_cast<EELinearSolver*>(m_odeSolver.get());
+ ASSERT_EQ(nullptr, explicitEulerLinear);
+
+ typedef OdeSolverLinearEulerExplicitModified MEELinearSolver;
+ MEELinearSolver* modifiedExplicitEulerLinear;
+ modifiedExplicitEulerLinear = dynamic_cast<MEELinearSolver*>(m_odeSolver.get());
+ ASSERT_EQ(nullptr, modifiedExplicitEulerLinear);
+
+ typedef OdeSolverLinearEulerImplicit IELinearSolver;
+ IELinearSolver* implicitEulerLinear;
+ implicitEulerLinear = dynamic_cast<IELinearSolver*>(m_odeSolver.get());
+ ASSERT_EQ(nullptr, implicitEulerLinear);
+
+ typedef OdeSolverLinearStatic StaticLinearSolver;
+ StaticLinearSolver* staticLinearSolver;
+ staticLinearSolver = dynamic_cast<StaticLinearSolver*>(m_odeSolver.get());
+ ASSERT_EQ(nullptr, staticLinearSolver);
+}
+
+TEST_F(DeformableRepresentationTest, SerializationTest)
+{
+ {
+ SCOPED_TRACE("Encode a DeformableRepresentation object with valid DeformableCollisionRepresentation, no throw");
+ auto deformableRepresentation = std::make_shared<MockDeformableRepresentation>("TestRigidRepresentation");
+ deformableRepresentation->setValue("IntegrationScheme", SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC);
+
+ std::shared_ptr<SurgSim::Collision::Representation> deformableCollisionRepresentation =
+ std::make_shared<DeformableCollisionRepresentation>("DeformableCollisionRepresentation");
+ deformableRepresentation->setValue("CollisionRepresentation", deformableCollisionRepresentation);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*deformableRepresentation));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Physics::MockDeformableRepresentation"];
+ EXPECT_EQ(9u, data.size());
+
+ std::shared_ptr<MockDeformableRepresentation> newRepresentation;
+ newRepresentation = std::dynamic_pointer_cast<MockDeformableRepresentation>
+ (node.as<std::shared_ptr<SurgSim::Framework::Component>>());
+ EXPECT_NE(nullptr, newRepresentation->getCollisionRepresentation());
+
+ auto collisionRepresentation =
+ newRepresentation->getValue<std::shared_ptr<SurgSim::Collision::Representation>>("CollisionRepresentation");
+ auto newDeformableCollisionRepresentation =
+ std::dynamic_pointer_cast<DeformableCollisionRepresentation>(collisionRepresentation);
+ EXPECT_NE(nullptr, newDeformableCollisionRepresentation);
+
+ EXPECT_EQ("SurgSim::Physics::MockDeformableRepresentation", newRepresentation->getClassName());
+ EXPECT_EQ(newRepresentation, newDeformableCollisionRepresentation->getDeformableRepresentation());
+ EXPECT_EQ(SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC,
+ newRepresentation->getValue<SurgSim::Math::IntegrationScheme>("IntegrationScheme"));
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/EigenGtestAsserts.cpp b/SurgSim/Physics/UnitTests/EigenGtestAsserts.cpp
new file mode 100644
index 0000000..2910f92
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/EigenGtestAsserts.cpp
@@ -0,0 +1,81 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+#include <string>
+#include <tuple>
+
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+namespace Testing
+{
+::testing::AssertionResult AssertMatricesEqual(const char *expected_expr,
+ const char *actual_expr,
+ const char *abs_err_expr,
+ const Eigen::MatrixXd &expected,
+ const Eigen::MatrixXd &actual,
+ double abs_err)
+{
+ if ((expected.rows() != actual.rows()) || (expected.cols() != actual.cols()))
+ {
+ return ::testing::AssertionFailure()
+ << "Sizes do not match." << std::endl
+ << "Actual: [" << actual.rows() << ", " << actual.cols() << "]" << std::endl
+ << "Expected: [" << expected.rows() << ", " << expected.cols() << "]" << std::endl;
+ }
+
+ if (!expected.isApprox(actual, abs_err))
+ {
+ ::testing::AssertionResult result = ::testing::AssertionFailure()
+ << "Matrices are not within epsilon of each other." << std::endl
+ << "Difference: " << (actual - expected).norm() << std::endl
+ << "Epsilon: " << abs_err << std::endl;
+
+ if (expected.cols() == 1 || expected.rows() == 1)
+ {
+ typedef std::tuple<int, double, double> DifferenceTuple;
+ std::vector<DifferenceTuple> differences;
+ for (int index = 0; index < expected.size(); ++index)
+ {
+ if (std::abs(expected(index) - actual(index)) > abs_err)
+ {
+ differences.emplace_back(index, expected(index), actual(index));
+ }
+ }
+
+ result << "Total differing values: " << differences.size() << std::endl;
+
+ for (std::vector<DifferenceTuple>::iterator it = std::begin(differences); it != std::end(differences); ++it)
+ {
+ result
+ << "[Index, Expected, Actual]: "
+ << std::get<0>(*it) << ", " << std::get<1>(*it) << ", " << std::get<2>(*it) << std::endl;
+ }
+ }
+
+ return result;
+ }
+
+ return ::testing::AssertionSuccess();
+}
+
+} // namespace Testing
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/EigenGtestAsserts.h b/SurgSim/Physics/UnitTests/EigenGtestAsserts.h
new file mode 100644
index 0000000..b7f9834
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/EigenGtestAsserts.h
@@ -0,0 +1,44 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_UNITTESTS_EIGENGTESTASSERTS_H
+#define SURGSIM_PHYSICS_UNITTESTS_EIGENGTESTASSERTS_H
+
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+namespace Physics
+{
+namespace Testing
+{
+::testing::AssertionResult AssertMatricesEqual(const char *expected_expr,
+ const char *actual_expr,
+ const char *abs_err_expr,
+ const Eigen::MatrixXd &expected,
+ const Eigen::MatrixXd &actual,
+ double abs_err);
+
+} // namespace Testing
+} // namespace Physics
+} // namespace SurgSim
+
+#define EXPECT_NEAR_EIGEN(expected, actual, abs_err) \
+ EXPECT_PRED_FORMAT3(SurgSim::Physics::Testing::AssertMatricesEqual, (expected), (actual), (abs_err))
+#define ASSERT_NEAR_EIGEN(expected, actual, abs_err) \
+ ASSERT_PRED_FORMAT3(SurgSim::Physics::Testing::AssertMatricesEqual, (expected), (actual), (abs_err))
+
+#endif // SURGSIM_PHYSICS_UNITTESTS_EIGENGTESTASSERTS_H
diff --git a/SurgSim/Physics/UnitTests/Fem1DElementBeamTests.cpp b/SurgSim/Physics/UnitTests/Fem1DElementBeamTests.cpp
new file mode 100644
index 0000000..5eb46df
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DElementBeamTests.cpp
@@ -0,0 +1,634 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <array>
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/GaussLegendreQuadrature.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+
+using SurgSim::Math::Matrix;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::Fem1DElementBeam;
+
+namespace
+{
+const double epsilon = 1e-9;
+};
+
+class MockFem1DElement : public Fem1DElementBeam
+{
+public:
+ MockFem1DElement(std::array<size_t, 2> nodeIds)
+ : Fem1DElementBeam(nodeIds)
+ {
+ }
+
+ const Eigen::Matrix<double, 12, 12>& getInitialRotation() const
+ {
+ return m_R0;
+ }
+
+ double getRestLength() const
+ {
+ return m_restLength;
+ }
+};
+
+void computeShapeFunction(double xi, double eta, double zeta, double L, Eigen::Ref<Eigen::Matrix<double, 3, 12>> N)
+{
+ // Shape function for beam from "Theory of Matrix Structural Analysis", Przemieniecki. Eqns 11.30.
+ // Note xi = x/l, eta = y/l, zeta = z/l
+
+ double xi2 = xi * xi;
+ double xi3 = xi2 * xi;
+
+ // Node 1
+ N(0, 0) = 1.0 - xi;
+ N(0, 1) = 6.0 * (xi - xi2) * eta;
+ N(0, 2) = 6.0 * (xi - xi2) * zeta;
+ N(0, 3) = 0.0;
+ N(0, 4) = (1.0 - 4.0 * xi + 3.0 * xi2) * L * zeta;
+ N(0, 5) = (-1.0 + 4.0 * xi - 3.0 * xi2) * L * eta;
+
+ N(1, 0) = 0.0;
+ N(1, 1) = 1.0 - 3.0 * xi2 + 2.0 * xi3;
+ N(1, 2) = 0.0;
+ N(1, 3) = -(1.0 - xi) * L * zeta;
+ N(1, 4) = 0.0;
+ N(1, 5) = (xi - 2.0 * xi2 + xi3) * L;
+
+ N(2, 0) = 0.0;
+ N(2, 1) = 0.0;
+ N(2, 2) = 1.0 - 3.0 * xi2 + 2.0 * xi3;
+ N(2, 3) = -(1.0 - xi) * L * eta;
+ N(2, 4) = (-xi + 2.0 * xi2 - xi3) * L;
+ N(2, 5) = 0.0;
+
+ // Node 2
+ N(0, 6 + 0) = xi;
+ N(0, 6 + 1) = 6.0 * (-xi + xi2) * eta;
+ N(0, 6 + 2) = 6.0 * (-xi + xi2) * zeta;
+ N(0, 6 + 3) = 0.0;
+ N(0, 6 + 4) = (-2.0 * xi + 3.0 * xi2) * L * zeta;
+ N(0, 6 + 5) = (2.0 * xi - 3.0 * xi2) * L * eta;
+
+ N(1, 6 + 0) = 0.0;
+ N(1, 6 + 1) = 3.0 * xi2 - 2.0 * xi3;
+ N(1, 6 + 2) = 0.0;
+ N(1, 6 + 3) = -L * xi * zeta;
+ N(1, 6 + 4) = 0.0;
+ N(1, 6 + 5) = (-xi2 + xi3) * L;
+
+ N(2, 6 + 0) = 0.0;
+ N(2, 6 + 1) = 0.0;
+ N(2, 6 + 2) = 3.0 * xi2 - 2.0 * xi3;
+ N(2, 6 + 3) = -L * xi * eta;
+ N(2, 6 + 4) = (xi2 - xi3) * L;
+ N(2, 6 + 5) = 0.0;
+}
+
+class Fem1DElementBeamTests : public ::testing::Test
+{
+public:
+ static const int m_numberNodes = 5;
+
+ std::array<size_t, 2> m_nodeIds;
+ SurgSim::Math::OdeState m_restState;
+ double m_expectedVolume;
+ double m_rho, m_E, m_nu, m_L;
+ double m_radius;
+ Quaterniond m_orientation;
+
+ virtual void SetUp() override
+ {
+ using SurgSim::Math::getSubVector;
+
+ m_rho = 1000.0;
+ m_E = 1e6;
+ m_nu = 0.45;
+ m_radius = 0.01;
+ m_L = 1.0;
+ m_expectedVolume = m_L * (M_PI * m_radius * m_radius);
+
+ // Beam is made of node 3 and 1 in a bigger system containing m_numberNodes nodes (at least 4)
+ m_nodeIds[0] = 3;
+ m_nodeIds[1] = 1;
+
+ m_restState.setNumDof(6, m_numberNodes);
+
+ m_orientation.coeffs().setRandom();
+ m_orientation.normalize();
+
+ Vector3d firstExtremity(0.1, 1.2, 2.3);
+ Vector3d secondExtremity = firstExtremity + m_orientation._transformVector(Vector3d(m_L, 0.0, 0.0));
+
+ Vector& x = m_restState.getPositions();
+ getSubVector(x, m_nodeIds[0], 6).segment<3>(0) = firstExtremity;
+ getSubVector(x, m_nodeIds[1], 6).segment<3>(0) = secondExtremity;
+ }
+
+ void getExpectedMassMatrix(Eigen::Ref<SurgSim::Math::Matrix> mass)
+ {
+ double& L = m_L;
+ double L2 = L * L;
+ double A = M_PI * m_radius * m_radius;
+
+ double Iz = M_PI_4 * m_radius * m_radius * m_radius * m_radius;
+ double Iy = M_PI_4 * m_radius * m_radius * m_radius * m_radius;
+ double I = Iz + Iy;
+
+ Eigen::Matrix<double, 12, 12> untransformedMass;
+
+ untransformedMass.col(0) <<
+ 1.0 / 3.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 1.0 / 6.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+
+ untransformedMass.col(1) <<
+ 0.0, 13.0 / 35.0 + 6.0 * Iz / (5.0 * A * L2), 0.0, 0.0, 0.0, 11.0 * L / 210.0 + Iz / (10.0 * A * L),
+ 0.0, 9.0 / 70.0 - 6.0 * Iz / (5.0 * A * L2), 0.0, 0.0, 0.0, -13.0 * L / 420.0 + Iz / (10.0 * A * L);
+
+ untransformedMass.col(2) <<
+ 0.0, 0.0, 13.0 / 35.0 + 6.0 * Iy / (5.0 * A * L2), 0.0, -11.0 * L / 210.0 - Iy / (10.0 * A * L), 0.0,
+ 0.0, 0.0, 9.0 / 70.0 - 6.0 * Iy / (5.0 * A * L2), 0.0, 13.0 * L / 420.0 - Iy / (10.0 * A * L), 0.0;
+
+ untransformedMass.col(3) <<
+ 0.0, 0.0, 0.0, I / (3.0 * A), 0.0, 0.0,
+ 0.0, 0.0, 0.0, I / (6.0 * A), 0.0, 0.0;
+
+ untransformedMass.col(4) <<
+ 0.0, 0.0, -11.0 * L / 210.0 - Iy / (10.0 * A * L), 0.0, L2 / 105.0 + 2.0 * Iy / (15.0 * A), 0.0,
+ 0.0, 0.0, -13.0 * L / 420.0 + Iy / (10.0 * A * L), 0.0, -L2 / 140.0 - Iy / (30.0 * A), 0.0;
+
+ untransformedMass.col(5) <<
+ 0.0, 11.0 * L / 210.0 + Iz / (10.0 * A * L), 0.0, 0.0, 0.0, L2 / 105.0 + 2.0 * Iz / (15.0 * A),
+ 0.0, 13.0 * L / 420.0 - Iz / (10.0 * A * L), 0.0, 0.0, 0.0, -L2 / 140.0 - Iz / (30.0 * A);
+
+ untransformedMass.col(6) <<
+ 1.0 / 6.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 1.0 / 3.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+
+ untransformedMass.col(7) <<
+ 0.0, 9.0 / 70.0 - 6.0 * Iz / (5.0 * A * L2), 0.0, 0.0, 0.0, 13.0 * L / 420.0 - Iz / (10.0 * A * L),
+ 0.0, 13.0 / 35.0 + 6.0 * Iz / (5.0 * A * L2), 0.0, 0.0, 0.0, -11.0 * L / 210.0 - Iz / (10.0 * A * L);
+
+ untransformedMass.col(8) <<
+ 0.0, 0.0, 9.0 / 70.0 - 6.0 * Iy / (5.0 * A * L2), 0.0, -13.0 * L / 420.0 + Iy / (10.0 * A * L), 0.0,
+ 0.0, 0.0, 13.0 / 35.0 + 6.0 * Iy / (5.0 * A * L2), 0.0, 11.0 * L / 210.0 + Iy / (10.0 * A * L), 0.0;
+
+ untransformedMass.col(9) <<
+ 0.0, 0.0, 0.0, I / (6.0 * A), 0.0, 0.0,
+ 0.0, 0.0, 0.0, I / (3.0 * A), 0.0, 0.0;
+
+ untransformedMass.col(10) <<
+ 0.0, 0.0, 13.0 * L / 420.0 - Iy / (10.0 * A * L), 0.0, -L2 / 140.0 - Iy / (30.0 * A), 0.0,
+ 0.0, 0.0, 11.0 * L / 210.0 + Iy / (10.0 * A * L), 0.0, L2 / 105.0 + 2 * Iy / (15.0 * A), 0.0;
+
+ untransformedMass.col(11) <<
+ 0.0, -13.0 * L / 420.0 + Iz / (10.0 * A * L), 0.0, 0.0, 0.0, -L2 / 140.0 - Iz / (30.0 * A),
+ 0.0, -11.0 * L / 210.0 - Iz / (10.0 * A * L), 0.0, 0.0, 0.0, L2 / 105.0 + 2 * Iz / (15.0 * A);
+
+ untransformedMass *= m_rho * m_expectedVolume;
+
+ mass.setZero();
+ placeIntoAssembly(untransformedMass, mass);
+ }
+
+ void getExpectedMassMatrix2(Eigen::Ref<SurgSim::Math::Matrix> mass)
+ {
+ // Kinetic Energy = K = \int_V 1/2 rho v^2 dV, with rho the density and v the velocity of a volume particle
+ // The shape function N transforms local coordinates into global coordinates, x = N q,
+ // N = (N_1^x 0 0 N_2^x 0 0)
+ // ( 0 N_1^y 0 0 N_2^y 0)
+ // ( 0 0 N_1^z 0 0 N_2^z)
+ //
+ // Using Lagrange's equation, we find
+ // d\dt (dK/d\dot{q}_i) = d\dt( 1/2.rho \int_V d/d\dot(q}_i ( \dot{q}^T.N^T.N.\dot{q} ) dV )
+ // [Note d/dx(ax \cdot ax) = 2 (a \cdot ax)]
+ // = rho d\dt( \int_V N^T.N.\dot{q} dV )
+ // = rho d\dt( \int_V N^T.N dV \dot{q})
+ // = rho \int_V N^T.N dV \ddot{q}
+ //
+ // Therefore the mass 'M' is
+ // M = rho \int_V N^T(x,y,z).N(x,y,z) dV
+ // = rho \int_V f(x,y,z) dV
+ //
+ // Integrate over the cylindrical volume of the beam
+ // M = rho \int_0^L dx \int_{-R}^R dy \int_{-\sqrt{R^2-y^2}}^{\sqrt{R^2-y^2}} dz [f(x,y,z)]
+ //
+ // Let
+ // x = l \xi
+ // y = l \eta
+ // z = l \zeta
+ // Then
+ // \int_0^L dx
+ // => \int_0^1 l d\xi
+ // \int_{-R}^R dy
+ // => \int_{-R/l}^{R/l} l d\eta
+ // \int_{-\sqrt{R^2-y^2}}^{\sqrt{R^2-y^2}} dz
+ // => \int_{-\sqrt{(R/l)^2-\eta^2}}^{\sqrt{(R/l)^2-\eta^2}} l d\zeta
+ //
+ // We know 'f' in transformed coordinates, so we can directly calculate
+ // M = \rho l^3 ...
+ // \int_0^1 d\xi ...
+ // \int_{-R/l}^{R/l} d\eta ...
+ // \int_{-\sqrt{(R/l)^2-\eta^2}}^{\sqrt{(R/l)^2-\eta^2}} d\zeta ...
+ // [f(\xi, \eta, \zeta)]
+
+ using std::pow;
+ using std::sqrt;
+
+ Eigen::Matrix<double, 3, 12> N;
+ Eigen::Matrix<double, 12, 12> untransformedMass = Eigen::Matrix<double, 12, 12>::Zero();
+
+ for (int i = 0; i < 4; i++)
+ {
+ double xi = 0.5 * SurgSim::Math::gaussQuadrature4Points[i].point + 0.5;
+ double hXi = 0.5 * SurgSim::Math::gaussQuadrature4Points[i].weight;
+
+ for (int j = 0; j < 100; j++)
+ {
+ double eta = m_radius / m_L * SurgSim::Math::gaussQuadrature100Points[j].point;
+ double hEta = m_radius / m_L * SurgSim::Math::gaussQuadrature100Points[j].weight;
+
+ for (int k = 0; k < 2; k++)
+ {
+ double zeta = sqrt(pow(m_radius / m_L, 2.0) - pow(eta, 2.0))
+ * SurgSim::Math::gaussQuadrature2Points[k].point;
+ double hZeta = sqrt(pow(m_radius / m_L, 2.0) - pow(eta, 2.0))
+ * SurgSim::Math::gaussQuadrature2Points[k].weight;
+
+ computeShapeFunction(xi, eta, zeta, m_L, N);
+
+ untransformedMass += N.transpose() * N * hZeta * hEta * hXi;
+ }
+ }
+ }
+
+ untransformedMass *= m_rho * pow(m_L, 3.0);
+
+ mass.setZero();
+ placeIntoAssembly(untransformedMass, mass);
+ }
+
+ void getExpectedStiffnessMatrix(Eigen::Ref<SurgSim::Math::Matrix> stiffness)
+ {
+ double& L = m_L;
+ double L2 = L * L;
+ double L3 = L2 * L;
+ double A = (M_PI * m_radius * m_radius);
+
+ double Iz = M_PI / 4.0 * (m_radius * m_radius * m_radius * m_radius);
+ double Iy = M_PI / 4.0 * (m_radius * m_radius * m_radius * m_radius);
+ double I = Iz + Iy;
+
+ double G = m_E / (2.0 * (1.0 + m_nu));
+
+ double asy = A * 5. / 6.;
+ double asz = asy;
+
+ double phi_y = 12.0 * m_E * Iz / (G * asy * L2);
+ double phi_z = 12.0 * m_E * Iy / (G * asz * L2);
+
+ Eigen::Matrix<double, 12, 12> untransformedStiffness;
+
+ untransformedStiffness.col(0) <<
+ m_E * A / L, 0.0, 0.0, 0.0, 0.0, 0.0,
+ -m_E * A / L, 0.0, 0.0, 0.0, 0.0, 0.0;
+
+ untransformedStiffness.col(1) <<
+ 0.0, 12.0 * m_E * Iz / L3 / (1 + phi_y), 0.0, 0.0, 0.0, 6.0 * m_E * Iz / L2 / (1 + phi_y),
+ 0.0, -12.0 * m_E * Iz / L3 / (1 + phi_y), 0.0, 0.0, 0.0, 6.0 * m_E * Iz / L2 / (1 + phi_y);
+
+ untransformedStiffness.col(2) <<
+ 0.0, 0.0, 12.0 * m_E * Iy / L3 / (1 + phi_z), 0.0, -6.0 * m_E * Iy / L2 / (1 + phi_z), 0.0,
+ 0.0, 0.0, -12.0 * m_E * Iy / L3 / (1 + phi_z), 0.0, -6.0 * m_E * Iy / L2 / (1 + phi_z), 0.0;
+
+ untransformedStiffness.col(3) <<
+ 0.0, 0.0, 0.0, G * I / L, 0.0, 0.0,
+ 0.0, 0.0, 0.0, -G * I / L, 0.0, 0.0;
+
+ untransformedStiffness.col(4) <<
+ 0.0, 0.0, -6.0 * m_E * Iy / L2 / (1 + phi_z), 0.0, (4.0 + phi_z) * m_E * Iy / L / (1 + phi_z), 0.0,
+ 0.0, 0.0, 6.0 * m_E * Iy / L2 / (1 + phi_z), 0.0, (2.0 - phi_z) * m_E * Iy / L / (1 + phi_z), 0.0;
+
+ untransformedStiffness.col(5) <<
+ 0.0, 6.0 * m_E * Iz / L2 / (1 + phi_y), 0.0, 0.0, 0.0, (4.0 + phi_y) * m_E * Iz / L / (1 + phi_y),
+ 0.0, -6.0 * m_E * Iz / L2 / (1 + phi_y), 0.0, 0.0, 0.0, (2.0 - phi_y) * m_E * Iz / L / (1 + phi_y);
+
+ untransformedStiffness.col(6) <<
+ -m_E * A / L, 0.0, 0.0, 0.0, 0.0, 0.0,
+ m_E * A / L, 0.0, 0.0, 0.0, 0.0, 0.0;
+
+ untransformedStiffness.col(7) <<
+ 0.0, -12.0 * m_E * Iz / L3 / (1 + phi_y), 0.0, 0.0, 0.0, -6.0 * m_E * Iz / L2 / (1 + phi_y),
+ 0.0, 12.0 * m_E * Iz / L3 / (1 + phi_y), 0.0, 0.0, 0.0, -6.0 * m_E * Iz / L2 / (1 + phi_y);
+
+ untransformedStiffness.col(8) <<
+ 0.0, 0.0, -12.0 * m_E * Iy / L3 / (1 + phi_z), 0.0, 6.0 * m_E * Iy / L2 / (1 + phi_z), 0.0,
+ 0.0, 0.0, 12.0 * m_E * Iy / L3 / (1 + phi_z), 0.0, 6.0 * m_E * Iy / L2 / (1 + phi_z), 0.0;
+
+ untransformedStiffness.col(9) <<
+ 0.0, 0.0, 0.0, -G * I / L, 0.0, 0.0,
+ 0.0, 0.0, 0.0, G * I / L, 0.0, 0.0;
+
+ untransformedStiffness.col(10) <<
+ 0.0, 0.0, -6.0 * m_E * Iy / L2 / (1 + phi_z), 0.0, (2.0 - phi_z) * m_E * Iy / L / (1 + phi_z), 0.0,
+ 0.0, 0.0, 6.0 * m_E * Iy / L2 / (1 + phi_z), 0.0, (4.0 + phi_z) * m_E * Iy / L / (1 + phi_z), 0.0;
+
+ untransformedStiffness.col(11) <<
+ 0.0, 6.0 * m_E * Iz / L2 / (1 + phi_y), 0.0, 0.0, 0.0, (2.0 - phi_y) * m_E * Iz / L / (1 + phi_y),
+ 0.0, -6.0 * m_E * Iz / L2 / (1 + phi_y), 0.0, 0.0, 0.0, (4.0 + phi_y) * m_E * Iz / L / (1 + phi_y);
+
+ stiffness.setZero();
+ placeIntoAssembly(untransformedStiffness, stiffness);
+ }
+
+ void placeIntoAssembly(const Eigen::Ref<SurgSim::Math::Matrix>& in, Eigen::Ref<SurgSim::Math::Matrix> out)
+ {
+ std::vector<size_t> nodeIdsVectorForm(m_nodeIds.begin(), m_nodeIds.end());
+
+ // Transform into correct coordinates and correct place in matrix
+ std::shared_ptr<MockFem1DElement> beam = getBeam();
+ const Eigen::Matrix<double, 12, 12>& r = beam->getInitialRotation();
+
+ SurgSim::Math::addSubMatrix(r * in * r.transpose(), nodeIdsVectorForm, 6, &out);
+ }
+
+ std::shared_ptr<MockFem1DElement> getBeam()
+ {
+ auto beam = std::make_shared<MockFem1DElement>(m_nodeIds);
+ beam->setRadius(m_radius);
+ beam->setMassDensity(m_rho);
+ beam->setPoissonRatio(m_nu);
+ beam->setYoungModulus(m_E);
+ beam->initialize(m_restState);
+ return beam;
+ }
+};
+
+TEST_F(Fem1DElementBeamTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(
+ { MockFem1DElement beam(m_nodeIds); });
+}
+
+TEST_F(Fem1DElementBeamTests, NodeIdsTest)
+{
+ Fem1DElementBeam beam(m_nodeIds);
+ EXPECT_EQ(2u, beam.getNumNodes());
+ EXPECT_EQ(2u, beam.getNodeIds().size());
+ for (int i = 0; i < 2; i++)
+ {
+ EXPECT_EQ(m_nodeIds[i], beam.getNodeId(i));
+ EXPECT_EQ(m_nodeIds[i], beam.getNodeIds()[i]);
+ }
+}
+
+TEST_F(Fem1DElementBeamTests, setGetRadiusTest)
+{
+ Fem1DElementBeam beam(m_nodeIds);
+
+ // Default radius = 0
+ EXPECT_DOUBLE_EQ(0.0, beam.getRadius());
+ // Set to a valid radius
+ beam.setRadius(1.54);
+ EXPECT_DOUBLE_EQ(1.54, beam.getRadius());
+ // Set to an invalid radius
+ EXPECT_ANY_THROW(beam.setRadius(0.0));
+ EXPECT_ANY_THROW(beam.setRadius(-9.4));
+}
+
+TEST_F(Fem1DElementBeamTests, MaterialParameterTest)
+{
+ Fem1DElementBeam beam(m_nodeIds);
+ beam.setRadius(m_radius);
+
+ // Test the various mode of failure related to the physical parameters
+ // This has been already tested in FemElementTests, but this is to make sure this method is called properly
+ // So the same behavior should be expected
+ {
+ // Mass density not set
+ ASSERT_ANY_THROW(beam.initialize(m_restState));
+
+ // Poisson Ratio not set
+ beam.setMassDensity(-1234.56);
+ ASSERT_ANY_THROW(beam.initialize(m_restState));
+
+ // Young modulus not set
+ beam.setPoissonRatio(0.55);
+ ASSERT_ANY_THROW(beam.initialize(m_restState));
+
+ // Invalid mass density
+ beam.setYoungModulus(-4321.33);
+ ASSERT_ANY_THROW(beam.initialize(m_restState));
+
+ // Invalid Poisson ratio
+ beam.setMassDensity(m_rho);
+ ASSERT_ANY_THROW(beam.initialize(m_restState));
+
+ // Invalid Young modulus
+ beam.setPoissonRatio(m_nu);
+ ASSERT_ANY_THROW(beam.initialize(m_restState));
+
+ beam.setYoungModulus(m_E);
+ ASSERT_NO_THROW(beam.initialize(m_restState));
+ }
+}
+
+TEST_F(Fem1DElementBeamTests, VolumeTest)
+{
+ std::shared_ptr<MockFem1DElement> beam = getBeam();
+ EXPECT_NEAR(beam->getVolume(m_restState), m_expectedVolume, 1e-10);
+}
+
+TEST_F(Fem1DElementBeamTests, RestLengthTest)
+{
+ std::shared_ptr<MockFem1DElement> beam = getBeam();
+ EXPECT_NEAR(beam->getRestLength(), m_L, 1e-10);
+}
+
+TEST_F(Fem1DElementBeamTests, InitialRotationTest)
+{
+ std::shared_ptr<MockFem1DElement> beam = getBeam();
+
+ // Use a mask to test the structure of the rotation matrix R0 (4 digonal block 3x3 matrix and 0 elsewhere)
+ Eigen::Matrix<double, 12, 12> mask;
+ mask.setOnes();
+ mask.block<3, 3>(0, 0).setZero();
+ mask.block<3, 3>(3, 3).setZero();
+ mask.block<3, 3>(6, 6).setZero();
+ mask.block<3, 3>(9, 9).setZero();
+ EXPECT_TRUE(beam->getInitialRotation().cwiseProduct(mask).isZero());
+
+ // Only the 1st direction of the frame can be compared as the 2 other ones are randomly
+ // chosen (can be any 2 vectors forming an Orthonormal frame)
+ Vector3d expected_i = m_orientation.matrix().col(0);
+ Vector3d i_0 = beam->getInitialRotation().block<3, 3>(0, 0).col(0);
+ Vector3d i_1 = beam->getInitialRotation().block<3, 3>(3, 3).col(0);
+ Vector3d i_2 = beam->getInitialRotation().block<3, 3>(6, 6).col(0);
+ Vector3d i_3 = beam->getInitialRotation().block<3, 3>(9, 9).col(0);
+ EXPECT_TRUE(i_0.isApprox(expected_i));
+ EXPECT_TRUE(i_1.isApprox(expected_i));
+ EXPECT_TRUE(i_2.isApprox(expected_i));
+ EXPECT_TRUE(i_3.isApprox(expected_i));
+}
+
+TEST_F(Fem1DElementBeamTests, CoordinateTests)
+{
+ Fem1DElementBeam element(m_nodeIds);
+
+ Vector validNaturalCoordinate(2);
+ Vector validNaturalCoordinate2(2);
+ Vector invalidNaturalCoordinateSumNot1(2);
+ Vector invalidNaturalCoordinateNegativeValue(2);
+ Vector invalidNaturalCoordinateSize1(1), invalidNaturalCoordinateSize3(3);
+ Vector3d expectedA(0.1, 1.2, 2.3);
+ Vector3d expectedB = expectedA + m_orientation._transformVector(Vector3d(m_L, 0.0, 0.0));
+
+ validNaturalCoordinate << 0.4, 0.6;
+ validNaturalCoordinate2 << -1e-11, 1 + 1e-11;
+ invalidNaturalCoordinateSumNot1 << 0.5, 0.6;
+ invalidNaturalCoordinateNegativeValue << 1.4, -0.4;
+ invalidNaturalCoordinateSize1 << 1.0;
+ invalidNaturalCoordinateSize3 << 0.2, 0.2, 0.6;
+ EXPECT_TRUE(element.isValidCoordinate(validNaturalCoordinate));
+ EXPECT_TRUE(element.isValidCoordinate(validNaturalCoordinate2));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateSumNot1));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateNegativeValue));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateSize1));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateSize3));
+
+ Vector naturalCoordinateA(2), naturalCoordinateB(2), naturalCoordinateMiddle(2);
+ Vector ptA, ptB, ptMiddle;
+ naturalCoordinateA << 1.0, 0.0;
+ naturalCoordinateB << 0.0, 1.0;
+ naturalCoordinateMiddle << 1.0 / 2.0, 1.0 / 2.0;
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateNegativeValue), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize1), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize3), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSumNot1), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(ptA = element.computeCartesianCoordinate(m_restState, naturalCoordinateA));
+ EXPECT_NO_THROW(ptB = element.computeCartesianCoordinate(m_restState, naturalCoordinateB));
+ EXPECT_NO_THROW(ptMiddle = element.computeCartesianCoordinate(m_restState, naturalCoordinateMiddle));
+ EXPECT_TRUE(ptA.isApprox(expectedA));
+ EXPECT_TRUE(ptB.isApprox(expectedB));
+ EXPECT_TRUE(ptMiddle.isApprox((expectedA + expectedB) * 0.5));
+
+ Vector3d cartesian(0.1, 1.2, 2.3);
+ EXPECT_THROW(element.computeNaturalCoordinate(m_restState, cartesian), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(Fem1DElementBeamTests, ForceAndMatricesTest)
+{
+ using SurgSim::Math::Matrix;
+ using SurgSim::Math::Vector;
+ using SurgSim::Math::getSubVector;
+
+ std::shared_ptr<MockFem1DElement> beam = getBeam();
+
+ Vector vectorOnes = Vector::Ones(6 * m_numberNodes);
+
+ Vector forceVector = Vector::Zero(6 * m_numberNodes);
+ Matrix massMatrix = Matrix::Zero(6 * m_numberNodes, 6 * m_numberNodes);
+ Matrix dampingMatrix = Matrix::Zero(6 * m_numberNodes, 6 * m_numberNodes);
+ Matrix stiffnessMatrix = Matrix::Zero(6 * m_numberNodes, 6 * m_numberNodes);
+
+ Matrix expectedMass(6 * m_numberNodes, 6 * m_numberNodes);
+ Matrix expectedMass2(6 * m_numberNodes, 6 * m_numberNodes);
+ Matrix expectedDamping = Matrix(dampingMatrix);
+ Matrix expectedStiffness(6 * m_numberNodes, 6 * m_numberNodes);
+
+ getExpectedMassMatrix(expectedMass);
+ getExpectedMassMatrix2(expectedMass2);
+ getExpectedStiffnessMatrix(expectedStiffness);
+
+ // No force should be produced when in rest state (x = x0) => F = K.(x-x0) = 0
+ beam->addForce(m_restState, &forceVector);
+ EXPECT_TRUE(forceVector.isZero());
+
+ beam->addMass(m_restState, &massMatrix);
+ EXPECT_TRUE(massMatrix.isApprox(expectedMass));
+ EXPECT_TRUE(massMatrix.isApprox(expectedMass2, 1e-6));
+
+ beam->addDamping(m_restState, &dampingMatrix);
+ EXPECT_TRUE(dampingMatrix.isApprox(expectedDamping));
+
+ beam->addStiffness(m_restState, &stiffnessMatrix);
+ EXPECT_TRUE(stiffnessMatrix.isApprox(expectedStiffness));
+
+ forceVector.setZero();
+ massMatrix.setZero();
+ dampingMatrix.setZero();
+ stiffnessMatrix.setZero();
+
+ beam->addFMDK(m_restState, &forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
+ EXPECT_TRUE(forceVector.isZero());
+ EXPECT_TRUE(massMatrix.isApprox(expectedMass));
+ EXPECT_TRUE(massMatrix.isApprox(expectedMass2, 1e-6));
+ EXPECT_TRUE(dampingMatrix.isApprox(expectedDamping));
+ EXPECT_TRUE(stiffnessMatrix.isApprox(expectedStiffness));
+
+ // Test addMatVec API with Mass component only
+ forceVector.setZero();
+ beam->addMatVec(m_restState, 1.0, 0.0, 0.0, vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 6 * m_numberNodes; rowId++)
+ {
+ EXPECT_NEAR(expectedMass.row(rowId).sum(), forceVector[rowId], epsilon);
+ EXPECT_NEAR(expectedMass2.row(rowId).sum(), forceVector[rowId], 1e-6);
+ }
+ // Test addMatVec API with Damping component only
+ forceVector.setZero();
+ beam->addMatVec(m_restState, 0.0, 1.0, 0.0, vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 6 * m_numberNodes; rowId++)
+ {
+ EXPECT_NEAR(expectedDamping.row(rowId).sum(), forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with Stiffness component only
+ forceVector.setZero();
+ beam->addMatVec(m_restState, 0.0, 0.0, 1.0, vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 6 * m_numberNodes; rowId++)
+ {
+ EXPECT_NEAR(expectedStiffness.row(rowId).sum(), forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with mix Mass/Damping/Stiffness components
+ forceVector.setZero();
+ beam->addMatVec(m_restState, 1.0, 2.0, 3.0, vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 6 * m_numberNodes; rowId++)
+ {
+ double expectedCoef = 1.0 * expectedMass.row(rowId).sum()
+ + 2.0 * expectedDamping.row(rowId).sum()
+ + 3.0 * expectedStiffness.row(rowId).sum();
+ EXPECT_NEAR(expectedCoef, forceVector[rowId], epsilon);
+
+ expectedCoef = 1.0 * expectedMass2.row(rowId).sum()
+ + 2.0 * expectedDamping.row(rowId).sum()
+ + 3.0 * expectedStiffness.row(rowId).sum();
+ EXPECT_NEAR(expectedCoef, forceVector[rowId], 1e-6);
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/Fem1DMechanicalValidationTests.cpp b/SurgSim/Physics/UnitTests/Fem1DMechanicalValidationTests.cpp
new file mode 100644
index 0000000..43dd819
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DMechanicalValidationTests.cpp
@@ -0,0 +1,397 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Fem1DMechanicalValidationTests.cpp
+/// This file tests the mechanical behavior of the class Fem1DRepresentation.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+
+using SurgSim::Math::Matrix;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+static const double dt = 1e-3;
+}
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+class Fem1DBuilder
+{
+public:
+ Fem1DBuilder()
+ : massDensity(0.0),
+ youngModulus(0.0),
+ poissonRatio(0.0),
+ radius(0.0),
+ shearingEnabled(true),
+ initializeElements(false)
+ {
+ }
+
+ std::shared_ptr<Fem1DRepresentation> build(const std::string& name)
+ {
+ size_t& nodes = nodesPerDimension[0];
+ SURGSIM_ASSERT(nodes > 0) << "Number of nodes incorrect: " << nodes;
+
+ auto fem = std::make_shared<Fem1DRepresentation>(name);
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+
+ state->setNumDof(fem->getNumDofPerNode(), nodes);
+
+ Vector3d delta = (extremities[1] - extremities[0]) / static_cast<double>(nodes - 1);
+
+ for (size_t nodeId = 0; nodeId < nodes; nodeId++)
+ {
+ state->getPositions().segment<3>(6 * nodeId) = extremities[0] + static_cast<double>(nodeId) * delta;
+ }
+
+ for (auto boundaryCondition = std::begin(boundaryConditions); boundaryCondition != std::end(boundaryConditions);
+ ++boundaryCondition)
+ {
+ state->addBoundaryCondition(boundaryCondition->first, boundaryCondition->second);
+ }
+
+ std::array<size_t, 2> nodeEnds;
+
+ for (size_t nodeId = 0; nodeId < nodes - 1; nodeId++)
+ {
+ nodeEnds[0] = nodeId;
+ nodeEnds[1] = nodeId + 1;
+ auto element = std::make_shared<Fem1DElementBeam>(nodeEnds);
+ element->setMassDensity(massDensity);
+ element->setYoungModulus(youngModulus);
+ element->setPoissonRatio(poissonRatio);
+ element->setRadius(radius);
+ element->setShearingEnabled(shearingEnabled);
+ if (initializeElements)
+ {
+ element->initialize(*state);
+ }
+ fem->addFemElement(element);
+ }
+
+ fem->setInitialState(state);
+
+ return fem;
+ }
+
+ void addBoundaryCondition(size_t node, size_t dof)
+ {
+ for (size_t i = 0; i < dof; i++)
+ {
+ boundaryConditions.push_back(std::make_pair(node, i));
+ }
+ }
+
+public:
+ std::array<Vector3d, 2> extremities;
+ std::vector<std::pair<size_t, size_t>> boundaryConditions; // <nodeId, dofId>
+ std::array<size_t, 1> nodesPerDimension;
+ double massDensity;
+ double youngModulus;
+ double poissonRatio;
+ double radius;
+ bool shearingEnabled;
+ bool initializeElements;
+};
+
+class Fem1DMechanicalValidationTests : public ::testing::Test
+{
+public:
+ std::shared_ptr<Fem1DRepresentation> m_fem;
+ SurgSim::Math::RigidTransform3d m_initialPose;
+ std::shared_ptr<SurgSim::Math::OdeState> m_initialState;
+
+ // Physical properties
+ double m_rho;
+ double m_nu;
+ double m_E;
+
+ // Geometric properties
+ double m_radius;
+ double m_L;
+ double m_Iz;
+
+ Vector m_expectedTransformedPositions;
+ Vector m_expectedTransformedVelocities;
+
+ Fem1DBuilder m_fem1DBuilder;
+
+protected:
+ virtual void SetUp() override
+ {
+ using SurgSim::Math::getSubVector;
+
+ // Physical properties
+ m_rho = 2000.0;
+ m_nu = 0.45;
+ m_E = 1e6;
+
+ // Geometric properties
+ m_radius = 0.05;
+ m_L = 0.734511;
+ m_Iz = M_PI / 4.0 * (m_radius * m_radius * m_radius * m_radius);
+
+ // Initial Pose
+ SurgSim::Math::Quaterniond q(1.9, 4.2, 9.3, 2.1);
+ q.normalize();
+ Vector3d t(0.1, 0.2, 0.3);
+ m_initialPose = SurgSim::Math::makeRigidTransform(q, t);
+
+ // Define fem
+ m_fem = std::make_shared<Fem1DRepresentation>("name");
+
+ // Initial state
+ m_initialState = std::make_shared<SurgSim::Math::OdeState>();
+ m_initialState->setNumDof(m_fem->getNumDofPerNode(), 2);
+
+ Vector& x = m_initialState->getPositions();
+ x << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ m_L, 0.0, 0.0, 0.0, 0.0, 0.0;
+ Vector& v = m_initialState->getVelocities();
+ v << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 2.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+
+ // Expected transformed values
+ m_expectedTransformedPositions.resize(m_initialState->getNumDof());
+ m_expectedTransformedVelocities.resize(m_initialState->getNumDof());
+
+ getSubVector(m_expectedTransformedPositions, 0, 3) = m_initialPose * Vector3d(getSubVector(x, 0, 3));
+ getSubVector(m_expectedTransformedPositions, 1, 3) = getSubVector(x, 1, 3);
+ getSubVector(m_expectedTransformedPositions, 2, 3) = m_initialPose * Vector3d(getSubVector(x, 2, 3));
+ getSubVector(m_expectedTransformedPositions, 3, 3) = getSubVector(x, 3, 3);
+
+ getSubVector(m_expectedTransformedVelocities, 0, 3) = m_initialPose.linear() * getSubVector(v, 0, 3);
+ getSubVector(m_expectedTransformedVelocities, 1, 3) = getSubVector(v, 1, 3);
+ getSubVector(m_expectedTransformedVelocities, 2, 3) = m_initialPose.linear() * getSubVector(v, 2, 3);
+ getSubVector(m_expectedTransformedVelocities, 3, 3) = getSubVector(v, 3, 3);
+
+ // Create Fem1DElementBeam
+ std::array<size_t, 2> nodeIds = {0, 1};
+ auto element = std::make_shared<Fem1DElementBeam>(nodeIds);
+ element->setMassDensity(m_rho);
+ element->setYoungModulus(m_E);
+ element->setPoissonRatio(m_nu);
+ element->setRadius(m_radius);
+
+ // Add element to fem
+ m_fem->addFemElement(element);
+
+ // Setup m_fem1DBuilder
+ m_fem1DBuilder.extremities[0] = Vector3d(0.0, 0.0, 0.0);
+ m_fem1DBuilder.extremities[1] = Vector3d(m_L, 0.0, 0.0);
+ m_fem1DBuilder.massDensity = m_rho;
+ m_fem1DBuilder.youngModulus = m_E;
+ m_fem1DBuilder.poissonRatio = m_nu;
+ m_fem1DBuilder.radius = m_radius;
+ m_fem1DBuilder.shearingEnabled = false;
+ m_fem1DBuilder.initializeElements = true;
+ }
+};
+
+// Beam tests
+TEST_F(Fem1DMechanicalValidationTests, CantileverEndLoadedTest)
+{
+ // Setup FEM
+ size_t nodesPerDim = 2;
+
+ m_fem1DBuilder.nodesPerDimension[0] = nodesPerDim;
+ m_fem1DBuilder.addBoundaryCondition(0, 6);
+ std::shared_ptr<Fem1DRepresentation> fem = m_fem1DBuilder.build("CantileverEndLoadedTest");
+
+ // For last node, apply load to y-direction and calculate deflection
+ double load = 0.7;
+ size_t applyIndex = (nodesPerDim - 1) * 6 + 1;
+
+ Vector applyForce = Vector::Zero(nodesPerDim * 6);
+ applyForce[applyIndex] = load;
+ Matrix stiffness = fem->computeK(*fem->getInitialState());
+
+ // Apply boundary conditions
+ fem->getInitialState()->applyBoundaryConditionsToVector(&applyForce);
+ fem->getInitialState()->applyBoundaryConditionsToMatrix(&stiffness);
+
+ Vector calculatedDeflection = stiffness.inverse() * applyForce;
+
+ // Compare theoretical deflection with calculated deflection
+ size_t lookIndex = applyIndex;
+
+ double deflection = load * (m_L * m_L * m_L) / (3.0 * m_E * m_Iz);
+
+ EXPECT_NEAR(deflection, calculatedDeflection[lookIndex], 1e-8);
+}
+
+TEST_F(Fem1DMechanicalValidationTests, CantileverPunctualLoadAnywhereTest)
+{
+ // Setup FEM
+ size_t nodesPerDim = 10;
+
+ m_fem1DBuilder.nodesPerDimension[0] = nodesPerDim;
+ m_fem1DBuilder.addBoundaryCondition(0, 6);
+ std::shared_ptr<Fem1DRepresentation> fem = m_fem1DBuilder.build("CantileverPunctualLoadAnywhereTest");
+
+ for (size_t applyNode = 1; applyNode < nodesPerDim; applyNode++)
+ {
+ // For each node, apply load to y-direction and calculate deflection
+ double load = 0.7;
+ size_t applyIndex = applyNode * 6 + 1;
+
+ Vector applyForce = Vector::Zero(nodesPerDim * 6);
+ applyForce[applyIndex] = load;
+ Matrix stiffness = fem->computeK(*fem->getInitialState());
+
+ // Apply boundary conditions
+ fem->getInitialState()->applyBoundaryConditionsToVector(&applyForce);
+ fem->getInitialState()->applyBoundaryConditionsToMatrix(&stiffness);
+
+ Vector calculatedDeflection = stiffness.inverse() * applyForce;
+
+ for (size_t lookNode = 0; lookNode < nodesPerDim; lookNode++)
+ {
+ // For each node, compare theoretical deflection with calculated deflection
+ size_t lookIndex = lookNode * 6 + 1;
+
+ double a = m_L * applyNode / (nodesPerDim - 1);
+ double x = m_L * lookNode / (nodesPerDim - 1);
+ double deflection = (x < a) ? load * x * x * (3 * a - x) / (6 * m_E * m_Iz)
+ : load * a * a * (3 * x - a) / (6 * m_E * m_Iz);
+
+ EXPECT_NEAR(deflection, calculatedDeflection[lookIndex], 1e-8);
+ }
+ }
+}
+
+TEST_F(Fem1DMechanicalValidationTests, CantileverEndBentTest)
+{
+ // Setup FEM
+ size_t nodesPerDim = 5;
+
+ m_fem1DBuilder.nodesPerDimension[0] = nodesPerDim;
+ m_fem1DBuilder.addBoundaryCondition(0, 6);
+ std::shared_ptr<Fem1DRepresentation> fem = m_fem1DBuilder.build("CantileverEndBentTest");
+
+ // For last node, apply moment to z-rotational direction and calculate deflection
+ double moment = 0.7;
+ size_t applyIndex = (nodesPerDim - 1) * 6 + 5;
+
+ Vector applyForce = Vector::Zero(nodesPerDim * 6);
+ applyForce[applyIndex] = moment;
+ Matrix stiffness = fem->computeK(*fem->getInitialState());
+
+ // Apply boundary conditions
+ fem->getInitialState()->applyBoundaryConditionsToVector(&applyForce);
+ fem->getInitialState()->applyBoundaryConditionsToMatrix(&stiffness);
+
+ Vector calculatedDeflection = stiffness.inverse() * applyForce;
+
+ for (size_t lookNode = 0; lookNode < nodesPerDim; lookNode++)
+ {
+ // For each node, compare theoretical deflection with calculated deflection
+ size_t lookIndex = lookNode * 6 + 1;
+
+ double x = m_L * lookNode / (nodesPerDim - 1);
+ double deflection = moment * x * x / (2 * m_E * m_Iz);
+
+ EXPECT_NEAR(deflection, calculatedDeflection[lookIndex], 1e-8);
+ }
+}
+
+TEST_F(Fem1DMechanicalValidationTests, EndSupportedBeamCenterLoadedTest)
+{
+ // Setup FEM
+ size_t nodesPerDim = 5;
+
+ m_fem1DBuilder.nodesPerDimension[0] = nodesPerDim;
+ m_fem1DBuilder.addBoundaryCondition(0, 3);
+ m_fem1DBuilder.addBoundaryCondition(nodesPerDim - 1, 3);
+ std::shared_ptr<Fem1DRepresentation> fem = m_fem1DBuilder.build("EndSupportedBeamCenterLoadedTest");
+
+ // For middle node, apply load to y-direction and calculate deflection
+ double load = 0.7;
+ size_t applyIndex = (nodesPerDim / 2) * 6 + 1;
+
+ Vector applyForce = Vector::Zero(nodesPerDim * 6);
+ applyForce[applyIndex] = load;
+ Matrix stiffness = fem->computeK(*fem->getInitialState());
+
+ // Apply boundary conditions
+ fem->getInitialState()->applyBoundaryConditionsToVector(&applyForce);
+ fem->getInitialState()->applyBoundaryConditionsToMatrix(&stiffness);
+
+ Vector calculatedDeflection = stiffness.inverse() * applyForce;
+
+ // Compare theoretical deflection with calculated deflection
+ size_t lookIndex = applyIndex;
+
+ double deflection = load * (m_L * m_L * m_L) / (48.0 * m_E * m_Iz);
+
+ EXPECT_NEAR(deflection, calculatedDeflection[lookIndex], 1e-8);
+}
+
+TEST_F(Fem1DMechanicalValidationTests, EndSupportedBeamIntermediatelyLoadedTest)
+{
+ // Setup FEM
+ size_t nodesPerDim = 5;
+
+ m_fem1DBuilder.nodesPerDimension[0] = nodesPerDim;
+ m_fem1DBuilder.addBoundaryCondition(0, 3);
+ m_fem1DBuilder.addBoundaryCondition(nodesPerDim - 1, 3);
+ std::shared_ptr<Fem1DRepresentation> fem = m_fem1DBuilder.build("EndSupportedBeamIntermediatelyLoadedTest");
+
+ for (size_t node = 0; node < nodesPerDim; node++)
+ {
+ // For each node, apply load to y-direction and calculate deflection
+ double load = 0.7;
+ size_t applyIndex = node * 6 + 1;
+
+ Vector applyForce = Vector::Zero(nodesPerDim * 6);
+ applyForce[applyIndex] = load;
+ Matrix stiffness = fem->computeK(*fem->getInitialState());
+
+ // Apply boundary conditions
+ fem->getInitialState()->applyBoundaryConditionsToVector(&applyForce);
+ fem->getInitialState()->applyBoundaryConditionsToMatrix(&stiffness);
+
+ Vector calculatedDeflection = stiffness.inverse() * applyForce;
+
+ // Compare theoretical deflection with calculated deflection
+ size_t lookIndex = applyIndex;
+
+ double a = static_cast<double>(node) / static_cast<double>(nodesPerDim - 1) * m_L;
+ double b = m_L - a;
+ double deflection = load * b * a / (6 * m_L * m_E * m_Iz) * (m_L * m_L - a * a - b * b);
+
+ EXPECT_NEAR(deflection, calculatedDeflection[lookIndex], 1e-8);
+ }
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem1DPlyReaderDelegateTests.cpp b/SurgSim/Physics/UnitTests/Fem1DPlyReaderDelegateTests.cpp
new file mode 100644
index 0000000..f4c1128
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DPlyReaderDelegateTests.cpp
@@ -0,0 +1,90 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+using SurgSim::Math::Vector3d;
+using SurgSim::DataStructures::PlyReader;
+
+TEST(Fem1DRepresentationReaderTests, DelegateTest)
+{
+ auto femRepresentation = std::make_shared<Fem1DRepresentation>("Representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+
+ femRepresentation->setFilename("PlyReaderTests/Fem1D.ply");
+ ASSERT_TRUE(femRepresentation->initialize(runtime));
+
+ // Vertices
+ ASSERT_EQ(6u, femRepresentation->getNumDofPerNode());
+ ASSERT_EQ(6u * 7u, femRepresentation->getNumDof());
+
+ Vector3d vertex0(1.1, 1.2, -1.3);
+ Vector3d vertex6(9.100000, 9.200000, 9.300000);
+
+ EXPECT_TRUE(vertex0.isApprox(femRepresentation->getInitialState()->getPosition(0)));
+ EXPECT_TRUE(vertex6.isApprox(femRepresentation->getInitialState()->getPosition(6)));
+
+ // Number of beams
+ ASSERT_EQ(4u, femRepresentation->getNumFemElements());
+
+ std::array<size_t, 2> beam0 = {0, 1};
+ std::array<size_t, 2> beam2 = {3, 5};
+
+ EXPECT_TRUE(std::equal(std::begin(beam0), std::end(beam0),
+ std::begin(femRepresentation->getFemElement(0)->getNodeIds())));
+ EXPECT_TRUE(std::equal(std::begin(beam2), std::end(beam2),
+ std::begin(femRepresentation->getFemElement(2)->getNodeIds())));
+
+ // Boundary conditions
+ ASSERT_EQ(3u * 6u, femRepresentation->getInitialState()->getNumBoundaryConditions());
+
+ // Boundary condition 0 is on node 8
+ size_t boundaryNode0 = 2;
+ size_t boundaryNode2 = 5;
+
+ EXPECT_EQ(6 * boundaryNode0, femRepresentation->getInitialState()->getBoundaryConditions().at(0));
+ EXPECT_EQ(6 * boundaryNode0 + 1, femRepresentation->getInitialState()->getBoundaryConditions().at(1));
+ EXPECT_EQ(6 * boundaryNode0 + 2, femRepresentation->getInitialState()->getBoundaryConditions().at(2));
+ EXPECT_EQ(6 * boundaryNode2, femRepresentation->getInitialState()->getBoundaryConditions().at(12));
+ EXPECT_EQ(6 * boundaryNode2 + 1, femRepresentation->getInitialState()->getBoundaryConditions().at(13));
+ EXPECT_EQ(6 * boundaryNode2 + 2, femRepresentation->getInitialState()->getBoundaryConditions().at(14));
+
+ // Material
+ for (size_t i = 0; i < femRepresentation->getNumFemElements(); ++i)
+ {
+ auto fem = femRepresentation->getFemElement(i);
+ EXPECT_DOUBLE_EQ(0.21, fem->getMassDensity());
+ EXPECT_DOUBLE_EQ(0.31, fem->getPoissonRatio());
+ EXPECT_DOUBLE_EQ(0.41, fem->getYoungModulus());
+
+ auto fem1DBeam = std::dynamic_pointer_cast<SurgSim::Physics::Fem1DElementBeam>(fem);
+ ASSERT_NE(nullptr, fem1DBeam);
+ EXPECT_DOUBLE_EQ(0.11, fem1DBeam->getRadius());
+ }
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem1DRepresentationLocalizationTest.cpp b/SurgSim/Physics/UnitTests/Fem1DRepresentationLocalizationTest.cpp
new file mode 100644
index 0000000..4a1379b
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DRepresentationLocalizationTest.cpp
@@ -0,0 +1,228 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem1DRepresentationLocalization.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+void addBeam(Fem1DRepresentation *fem, std::array<size_t, 2> nodes,
+ const SurgSim::Math::OdeState& state, double radius = 0.01,
+ double massDensity = 1.0, double poissonRatio = 0.1, double youngModulus = 1.0)
+{
+ auto element = std::make_shared<Fem1DElementBeam>(nodes);
+ element->setRadius(radius);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ element->initialize(state);
+ fem->addFemElement(element);
+}
+
+class Fem1DRepresentationLocalizationTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::getSubVector;
+
+ m_fem = std::make_shared<Fem1DRepresentation>("Fem1dRepresentation");
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 3);
+
+ auto& x = state->getPositions();
+ getSubVector(x, 0, 6).segment<3>(0) = Vector3d( 0.0, 0.0, 0.0);
+ getSubVector(x, 1, 6).segment<3>(0) = Vector3d( 0.0, 1.0, -1.0);
+ getSubVector(x, 2, 6).segment<3>(0) = Vector3d(-1.0, 1.0, 0.0);
+
+ // Define Beams
+ {
+ std::array<size_t, 2> nodes = {{0, 1}};
+ addBeam(m_fem.get(), nodes, *state);
+ }
+
+ {
+ std::array<size_t, 2> nodes = {{1, 2}};
+ addBeam(m_fem.get(), nodes, *state);
+ }
+
+ m_fem->setInitialState(state);
+ m_fem->setLocalActive(true);
+
+ m_validLocalPosition.index = 1;
+ m_validLocalPosition.coordinate = SurgSim::Math::Vector::Zero(2);
+ m_validLocalPosition.coordinate[0] = 0.4;
+ m_validLocalPosition.coordinate[1] = 0.6;
+
+ m_invalidIndexLocalPosition.index = 3;
+ m_validLocalPosition.coordinate = SurgSim::Math::Vector::Zero(2);
+ m_validLocalPosition.coordinate[0] = 0.4;
+ m_validLocalPosition.coordinate[1] = 0.6;
+
+ m_invalidCoordinateLocalPosition.index = 1;
+ m_invalidCoordinateLocalPosition.coordinate = SurgSim::Math::Vector::Zero(2);
+ m_invalidCoordinateLocalPosition.coordinate[0] = 0.6;
+ m_invalidCoordinateLocalPosition.coordinate[1] = 0.6;
+ }
+
+ void TearDown()
+ {
+ }
+
+ std::shared_ptr<Fem1DRepresentation> m_fem;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_validLocalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_invalidIndexLocalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_invalidCoordinateLocalPosition;
+};
+
+TEST_F(Fem1DRepresentationLocalizationTest, ConstructorTest)
+{
+ ASSERT_THROW(
+ std::make_shared<Fem1DRepresentationLocalization>(m_fem, m_invalidIndexLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_THROW(
+ std::make_shared<Fem1DRepresentationLocalization>(m_fem, m_invalidCoordinateLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_NO_THROW(std::make_shared<Fem1DRepresentationLocalization>(m_fem, m_validLocalPosition););
+}
+
+TEST_F(Fem1DRepresentationLocalizationTest, SetGetRepresentation)
+{
+ Fem1DRepresentationLocalization localization(m_fem, m_validLocalPosition);
+
+ EXPECT_NE(nullptr, localization.getRepresentation());
+ EXPECT_EQ(m_fem, localization.getRepresentation());
+
+ EXPECT_EQ(1u, localization.getLocalPosition().index);
+ EXPECT_TRUE(localization.getLocalPosition().coordinate.isApprox(m_validLocalPosition.coordinate));
+
+ localization.setRepresentation(nullptr);
+ EXPECT_EQ(nullptr, localization.getRepresentation());
+ localization.setRepresentation(m_fem);
+ EXPECT_EQ(m_fem, localization.getRepresentation());
+
+ SurgSim::DataStructures::IndexedLocalCoordinate m_otherValidLocalPosition;
+ m_otherValidLocalPosition.index = 0;
+ m_otherValidLocalPosition.coordinate = SurgSim::Math::Vector::Zero(2);
+ m_otherValidLocalPosition.coordinate[1] = 1.0;
+
+ localization.setLocalPosition(m_otherValidLocalPosition);
+ EXPECT_EQ(m_otherValidLocalPosition.index, localization.getLocalPosition().index);
+ EXPECT_TRUE(localization.getLocalPosition().coordinate.isApprox(m_otherValidLocalPosition.coordinate));
+}
+
+TEST_F(Fem1DRepresentationLocalizationTest, SetGetLocalization)
+{
+ using SurgSim::Math::Vector2d;
+ using SurgSim::Math::Vector3d;
+
+ {
+ SCOPED_TRACE("Uninitialized Representation");
+
+ // Uninitialized Representation
+ EXPECT_THROW(std::make_shared<Fem1DRepresentationLocalization>(nullptr, m_validLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Incorrectly formed natural coordinate");
+
+ // Incorrectly formed natural coordinate
+ auto localization = std::make_shared<Fem1DRepresentationLocalization>(m_fem, m_validLocalPosition);
+ EXPECT_THROW(localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector2d(0.89, 0.54))),
+ SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector3d(1.0, 0.0, 0.0))),
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Out of bounds element Id");
+
+ // Out of bounds element Id
+ auto localization = std::make_shared<Fem1DRepresentationLocalization>(m_fem, m_validLocalPosition);
+ EXPECT_THROW(localization->setLocalPosition(IndexedLocalCoordinate(6u, Vector2d(1.0, 0.0))),
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("valid");
+
+ auto localization = std::make_shared<Fem1DRepresentationLocalization>(m_fem, m_validLocalPosition);
+ EXPECT_NO_THROW(localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector2d(0.2, 0.8))));
+ EXPECT_EQ(1u, localization->getLocalPosition().index);
+ EXPECT_TRUE(Vector2d(0.2, 0.8).isApprox(localization->getLocalPosition().coordinate));
+ }
+}
+
+TEST_F(Fem1DRepresentationLocalizationTest, CalculatePositionTest)
+{
+ using SurgSim::Math::Vector;
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::Vector2d;
+
+ auto localization = std::make_shared<Fem1DRepresentationLocalization>(m_fem, m_validLocalPosition);
+
+ // Test beam 1: nodes 0, 1
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector2d(1.0, 0.0)));
+ EXPECT_TRUE(Vector3d(0.0, 0.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector2d(0.0, 1.0)));
+ EXPECT_TRUE(Vector3d(0.0, 1.0, -1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test beam 2: nodes 0, 1
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector2d(1.0, 0.0)));
+ EXPECT_TRUE(Vector3d(0.0, 1.0, -1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector2d(0.0, 1.0)));
+ EXPECT_TRUE(Vector3d(-1.0, 1.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Advanced tests
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector2d(0.31, 0.69)));
+ // 0.31 * ( 0.0, 0.0, 0.0) => ( 0.0, 0.0, 0.0 )
+ // + 0.69 * ( 0.0, 1.0,-1.0) => ( 0.0, 0.69,-0.69)
+ // = ( 0.0, 0.69,-0.69)
+ EXPECT_TRUE(Vector3d(0.0, 0.69, -0.69).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector2d(0.95, 0.05)));
+ // 0.95 * ( 0.0, 1.0,-1.0) => ( 0.0, 0.95,-0.95)
+ // + 0.05 * (-1.0, 1.0, 0.0) => (-0.05, 0.05, 0.0 )
+ // = (-0.05, 1.0, -0.95)
+ EXPECT_TRUE(Vector3d(-0.05, 1.0, -0.95).isApprox(localization->calculatePosition(), epsilon));
+}
+
+} // namespace SurgSim
+} // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/Fem1DRepresentationTests.cpp b/SurgSim/Physics/UnitTests/Fem1DRepresentationTests.cpp
new file mode 100644
index 0000000..bb35dec
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DRepresentationTests.cpp
@@ -0,0 +1,239 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Fem1DRepresentationTests.cpp
+/// This file tests the functionalities of the class Fem1DRepresentation.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem1DRepresentationLocalization.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST(Fem1DRepresentationTests, ConstructorTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<Fem1DRepresentation> fem = std::make_shared<Fem1DRepresentation>("Fem1D");});
+}
+
+TEST(Fem1DRepresentationTests, GetTypeTest)
+{
+ std::shared_ptr<Fem1DRepresentation> fem = std::make_shared<Fem1DRepresentation>("Fem1D");
+ EXPECT_EQ(REPRESENTATION_TYPE_FEM1D, fem->getType());
+}
+
+TEST(Fem1DRepresentationTests, GetNumDofPerNodeTest)
+{
+ std::shared_ptr<Fem1DRepresentation> fem = std::make_shared<Fem1DRepresentation>("Fem1D");
+ EXPECT_EQ(6u, fem->getNumDofPerNode());
+}
+
+TEST(Fem1DRepresentationTests, TransformInitialStateTest)
+{
+ using SurgSim::Math::Vector;
+
+ std::shared_ptr<Fem1DRepresentation> fem = std::make_shared<Fem1DRepresentation>("Fem1D");
+
+ const size_t numNodes = 2;
+ const size_t numDofPerNode = fem->getNumDofPerNode();
+ const size_t numDof = numDofPerNode * numNodes;
+
+ SurgSim::Math::RigidTransform3d initialPose;
+ SurgSim::Math::Quaterniond q(1.0, 2.0, 3.0, 4.0);
+ SurgSim::Math::Vector3d t(1.0, 2.0, 3.0);
+ q.normalize();
+ initialPose = SurgSim::Math::makeRigidTransform(q, t);
+ fem->setLocalPose(initialPose);
+
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(numDofPerNode, numNodes);
+ Vector x = Vector::LinSpaced(numDof, 1.0, static_cast<double>(numDof));
+ Vector v = Vector::Ones(numDof);
+ Vector a = Vector::Ones(numDof) * 2.0;
+ initialState->getPositions() = x;
+ initialState->getVelocities() = v;
+ fem->setInitialState(initialState);
+
+ Vector expectedX = x, expectedV = v, expectedA = a;
+ for (size_t nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ expectedX.segment<3>(numDofPerNode * nodeId) = initialPose * x.segment<3>(numDofPerNode * nodeId);
+ expectedV.segment<3>(numDofPerNode * nodeId) = initialPose.linear() * v.segment<3>(numDofPerNode * nodeId);
+ expectedA.segment<3>(numDofPerNode * nodeId) = initialPose.linear() * a.segment<3>(numDofPerNode * nodeId);
+ }
+
+ // Initialize the component
+ ASSERT_TRUE(fem->initialize(std::make_shared<SurgSim::Framework::Runtime>()));
+ // Wake-up the component => apply the pose to the initial state
+ ASSERT_TRUE(fem->wakeUp());
+
+ EXPECT_TRUE(fem->getInitialState()->getPositions().isApprox(expectedX));
+ EXPECT_TRUE(fem->getInitialState()->getVelocities().isApprox(expectedV));
+}
+
+TEST(Fem1DRepresentationTests, DoWakeUpTest)
+{
+ using SurgSim::Math::LinearSolveAndInverse;
+ using SurgSim::Math::LinearSolveAndInverseTriDiagonalBlockMatrix;
+
+ std::shared_ptr<MockFem1DRepresentation> fem = std::make_shared<MockFem1DRepresentation>("Fem1D");
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(fem->getNumDofPerNode(), 2);
+ fem->setInitialState(initialState);
+
+ // Initialize the component
+ ASSERT_TRUE(fem->initialize(std::make_shared<SurgSim::Framework::Runtime>()));
+ // Wake-up the component => create the proper ode solver with the specialized linear solver
+ ASSERT_TRUE(fem->wakeUp());
+
+ // Test that the OdeSolver has the proper linear solver type
+ EXPECT_NE(nullptr, fem->getOdeSolver());
+ std::shared_ptr<LinearSolveAndInverse> linearSolver = fem->getOdeSolver()->getLinearSolver();
+ EXPECT_NE(nullptr, linearSolver);
+ std::shared_ptr<LinearSolveAndInverseTriDiagonalBlockMatrix<6>> expectedLinearSolverType;
+ expectedLinearSolverType = std::dynamic_pointer_cast<LinearSolveAndInverseTriDiagonalBlockMatrix<6>>(linearSolver);
+ EXPECT_NE(nullptr, expectedLinearSolverType);
+}
+
+TEST(Fem1DRepresentationTests, ExternalForceAPITest)
+{
+ std::shared_ptr<Fem1DRepresentation> fem = std::make_shared<Fem1DRepresentation>("Fem");
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(6, 3);
+
+ // External force vector not initialized until the initial state has been set (it contains the #dof...)
+ EXPECT_EQ(0, fem->getExternalGeneralizedForce().size());
+ EXPECT_EQ(0, fem->getExternalGeneralizedStiffness().rows());
+ EXPECT_EQ(0, fem->getExternalGeneralizedStiffness().cols());
+ EXPECT_EQ(0, fem->getExternalGeneralizedDamping().rows());
+ EXPECT_EQ(0, fem->getExternalGeneralizedDamping().cols());
+
+ fem->setInitialState(initialState);
+
+ // Vector initialized (properly sized and zeroed)
+ EXPECT_NE(0, fem->getExternalGeneralizedForce().size());
+ EXPECT_NE(0, fem->getExternalGeneralizedStiffness().rows());
+ EXPECT_NE(0, fem->getExternalGeneralizedStiffness().cols());
+ EXPECT_NE(0, fem->getExternalGeneralizedDamping().rows());
+ EXPECT_NE(0, fem->getExternalGeneralizedDamping().cols());
+ EXPECT_EQ(fem->getNumDof(), fem->getExternalGeneralizedForce().size());
+ EXPECT_EQ(fem->getNumDof(), fem->getExternalGeneralizedStiffness().cols());
+ EXPECT_EQ(fem->getNumDof(), fem->getExternalGeneralizedStiffness().rows());
+ EXPECT_EQ(fem->getNumDof(), fem->getExternalGeneralizedDamping().cols());
+ EXPECT_EQ(fem->getNumDof(), fem->getExternalGeneralizedDamping().rows());
+ EXPECT_TRUE(fem->getExternalGeneralizedForce().isZero());
+ EXPECT_TRUE(fem->getExternalGeneralizedStiffness().isZero());
+ EXPECT_TRUE(fem->getExternalGeneralizedDamping().isZero());
+
+ std::array<size_t, 2> element1NodeIds = {{0, 1}};
+ auto element1 = std::make_shared<Fem1DElementBeam>(element1NodeIds);
+ fem->addFemElement(element1);
+ std::array<size_t, 2> element2NodeIds = {{1, 2}};
+ auto element2 = std::make_shared<Fem1DElementBeam>(element2NodeIds);
+ fem->addFemElement(element2);
+
+ SurgSim::DataStructures::IndexedLocalCoordinate femRepCoordinate;
+ femRepCoordinate.index = 0;
+ femRepCoordinate.coordinate = SurgSim::Math::Vector::Zero(2);
+ femRepCoordinate.coordinate[0] = 1.0;
+ auto localization = std::make_shared<Fem1DRepresentationLocalization>(fem, femRepCoordinate);
+ auto wrongLocalizationType = std::make_shared<MockLocalization>();
+
+ Vector FLocalWrongSize = Vector::Ones(2 * fem->getNumDofPerNode());
+ Matrix KLocalWrongSize = Matrix::Ones(3 * fem->getNumDofPerNode(), 3 * fem->getNumDofPerNode());
+ Matrix DLocalWrongSize = Matrix::Ones(4 * fem->getNumDofPerNode(), 4 * fem->getNumDofPerNode());
+ Vector Flocal = Vector::LinSpaced(fem->getNumDofPerNode(), -3.12, 4.09);
+ Matrix Klocal = Matrix::Ones(fem->getNumDofPerNode(), fem->getNumDofPerNode()) * 0.34;
+ Matrix Dlocal = Klocal + Matrix::Identity(fem->getNumDofPerNode(), fem->getNumDofPerNode());
+ Vector F = Vector::Zero(fem->getNumDof());
+ F.segment(0, fem->getNumDofPerNode()) = Flocal;
+ Matrix K = Matrix::Zero(fem->getNumDof(), fem->getNumDof());
+ K.block(0, 0, fem->getNumDofPerNode(), fem->getNumDofPerNode()) = Klocal;
+ Matrix D = Matrix::Zero(fem->getNumDof(), fem->getNumDof());
+ D.block(0, 0, fem->getNumDofPerNode(), fem->getNumDofPerNode()) = Dlocal;
+
+ // Test invalid localization nullptr
+ ASSERT_THROW(fem->addExternalGeneralizedForce(nullptr, Flocal),
+ SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(fem->addExternalGeneralizedForce(nullptr, Flocal, Klocal, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid localization type
+ ASSERT_THROW(fem->addExternalGeneralizedForce(wrongLocalizationType, Flocal),
+ SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(fem->addExternalGeneralizedForce(wrongLocalizationType, Flocal, Klocal, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid force size
+ ASSERT_THROW(fem->addExternalGeneralizedForce(localization, FLocalWrongSize),
+ SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(fem->addExternalGeneralizedForce(localization, FLocalWrongSize, Klocal, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid stiffness size
+ ASSERT_THROW(fem->addExternalGeneralizedForce(localization, Flocal, KLocalWrongSize, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid damping size
+ ASSERT_THROW(fem->addExternalGeneralizedForce(localization, Flocal, Klocal, DLocalWrongSize),
+ SurgSim::Framework::AssertionFailure);
+
+ // Test valid call to addExternalGeneralizedForce
+ fem->addExternalGeneralizedForce(localization, Flocal, Klocal, Dlocal);
+ EXPECT_FALSE(fem->getExternalGeneralizedForce().isZero());
+ EXPECT_FALSE(fem->getExternalGeneralizedStiffness().isZero());
+ EXPECT_FALSE(fem->getExternalGeneralizedDamping().isZero());
+ EXPECT_TRUE(fem->getExternalGeneralizedForce().isApprox(F));
+ EXPECT_TRUE(fem->getExternalGeneralizedStiffness().isApprox(K));
+ EXPECT_TRUE(fem->getExternalGeneralizedDamping().isApprox(D));
+
+ // Test valid call to addExternalGeneralizedForce to add things up
+ fem->addExternalGeneralizedForce(localization, Flocal, Klocal, Dlocal);
+ EXPECT_TRUE(fem->getExternalGeneralizedForce().isApprox(2.0 * F));
+ EXPECT_TRUE(fem->getExternalGeneralizedStiffness().isApprox(2.0 * K));
+ EXPECT_TRUE(fem->getExternalGeneralizedDamping().isApprox(2.0 * D));
+}
+
+TEST(Fem1DRepresentationTests, SerializationTest)
+{
+ auto fem1DRepresentation = std::make_shared<Fem1DRepresentation>("Test-Fem1D");
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*fem1DRepresentation));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Physics::Fem1DRepresentation"];
+ EXPECT_EQ(10u, data.size());
+
+ std::shared_ptr<Fem1DRepresentation> newRepresentation;
+ ASSERT_NO_THROW(newRepresentation =
+ std::dynamic_pointer_cast<Fem1DRepresentation>(node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ ASSERT_NE(nullptr, newRepresentation);
+
+ EXPECT_EQ("SurgSim::Physics::Fem1DRepresentation", newRepresentation->getClassName());
+ EXPECT_EQ(REPRESENTATION_TYPE_FEM1D, newRepresentation->getType());
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem2DElementTriangleTests.cpp b/SurgSim/Physics/UnitTests/Fem2DElementTriangleTests.cpp
new file mode 100644
index 0000000..1581b8f
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem2DElementTriangleTests.cpp
@@ -0,0 +1,1128 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <array>
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/GaussLegendreQuadrature.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+
+using SurgSim::Math::Matrix;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::Fem2DElementTriangle;
+
+namespace
+{
+const double epsilon = 1e-8;
+};
+
+class MockFem2DElement : public Fem2DElementTriangle
+{
+public:
+ MockFem2DElement(std::array<size_t, 3> nodeIds)
+ : Fem2DElementTriangle(nodeIds)
+ {
+ }
+
+ const Matrix33d& getInitialRotation() const
+ {
+ return m_initialRotation;
+ }
+
+ const Eigen::Matrix<double, 18, 18>& getInitialRotationTimes6()
+ {
+ const Matrix33d& R0 = getInitialRotation();
+
+ m_initialRotationTimes6.setZero();
+
+ SurgSim::Math::setSubMatrix(R0, 0, 0, 3, 3, &m_initialRotationTimes6);
+ SurgSim::Math::setSubMatrix(R0, 1, 1, 3, 3, &m_initialRotationTimes6);
+ SurgSim::Math::setSubMatrix(R0, 2, 2, 3, 3, &m_initialRotationTimes6);
+ SurgSim::Math::setSubMatrix(R0, 3, 3, 3, 3, &m_initialRotationTimes6);
+ SurgSim::Math::setSubMatrix(R0, 4, 4, 3, 3, &m_initialRotationTimes6);
+ SurgSim::Math::setSubMatrix(R0, 5, 5, 3, 3, &m_initialRotationTimes6);
+
+ return m_initialRotationTimes6;
+ }
+
+ const Eigen::Matrix<double, 3, 9> getBatozStrainDisplacement(double xi, double neta) const
+ {
+ return batozStrainDisplacement(xi, neta);
+ }
+
+ const Eigen::Matrix<double, 18, 18>& getLocalStiffnessMatrix() const
+ {
+ return m_KLocal;
+ }
+
+ const Eigen::Matrix<double, 18, 18>& getGlobalStiffnessMatrix() const
+ {
+ return m_K;
+ }
+
+ const Eigen::Matrix<double, 18, 18>& getLocalMassMatrix() const
+ {
+ return m_MLocal;
+ }
+
+ const Eigen::Matrix<double, 18, 18>& getGlobalMassMatrix() const
+ {
+ return m_M;
+ }
+
+ double getRestArea() const
+ {
+ return m_restArea;
+ }
+
+ const Eigen::Matrix<double, 18, 1>& getInitialPosition() const
+ {
+ return m_x0;
+ }
+
+ /// Get the Membrane (in-plane) shape functions parameters
+ /// \param i The ith shape function
+ /// \param ai, bi, ci The ith shape function parameters
+ /// \note fi(x,y) = ai + x.bi + y.ci
+ void getMembraneShapeFunction(int i, double *ai, double *bi, double *ci) const
+ {
+ *ai = m_membraneShapeFunctionsParameters(i, 0);
+ *bi = m_membraneShapeFunctionsParameters(i, 1);
+ *ci = m_membraneShapeFunctionsParameters(i, 2);
+ }
+
+ // The Thin-Plate shape functions (Batoz shape functions)
+ // N1(xi, neta) = 2(1-xi-neta)(0.5-xi-neta)
+ double batozN1(double xi, double neta) const { return 2.0 * (1.0 - xi - neta) * (0.5 - xi - neta); }
+ // N2(xi, neta) = xi(2 xi-1)
+ double batozN2(double xi, double neta) const { return xi * (2.0 * xi - 1.0); }
+ // N3(xi, neta) = neta(2 neta-1)
+ double batozN3(double xi, double neta) const { return neta * (2.0 * neta - 1.0); }
+ // N4(xi, neta) = 4 xi neta
+ double batozN4(double xi, double neta) const { return 4.0 * xi * neta; }
+ // N5(xi, neta) = 4 neta(1-xi-neta)
+ double batozN5(double xi, double neta) const { return 4.0 * neta * (1.0 - xi - neta); }
+ // N6(xi, neta) = 4 xi(1-xi-neta)
+ double batozN6(double xi, double neta) const { return 4.0 * xi * (1.0 - xi - neta); }
+
+ // dN1/dxi(xi, neta) = 2[(-0.5+xi+neta) + (-1+xi+neta)] = 2(-3/2+2(xi+neta)) = 4(xi+neta) - 3
+ double batozDN1Dxi(double xi, double neta) const { return 4.0 * (xi + neta) - 3.0; }
+ // dN2/dxi(xi, neta) = (2xi-1) + 2xi = 4xi-1
+ double batozDN2Dxi(double xi, double neta) const { return 4.0 * xi - 1.0; }
+ // dN3/dxi(xi, neta) = 0
+ double batozDN3Dxi(double xi, double neta) const { return 0.0; }
+ // dN4/dxi(xi, neta) = 4 neta
+ double batozDN4Dxi(double xi, double neta) const { return 4.0 * neta; }
+ // dN5/dxi(xi,neta) = -4 neta
+ double batozDN5Dxi(double xi, double neta) const { return -4.0 * neta; }
+ // dN6/dxi(xi,neta) = 4(1-xi-neta) -4xi = 4(1-2xi-neta)
+ double batozDN6Dxi(double xi, double neta) const { return 4.0 * (1.0 - 2.0 * xi - neta); }
+
+ // dN1/dneta(xi, neta) = 2[(-0.5+xi+neta) + (-1+xi+neta)] = 2(-3/2 + 2xi + 2neta) = 4(xi+neta) - 3
+ double batozDN1Dneta(double xi, double neta) const { return 4.0 * (xi + neta) - 3.0; }
+ // dN2/dneta(xi, neta) = 0
+ double batozDN2Dneta(double xi, double neta) const { return 0.0; }
+ // dN3/dneta(xi, neta) = 2neta-1 + 2neta = 4neta-1
+ double batozDN3Dneta(double xi, double neta) const { return 4.0 * neta - 1.0; }
+ // dN4/dneta(xi, neta) = 4xi
+ double batozDN4Dneta(double xi, double neta) const { return 4.0 * xi; }
+ // dN5/dneta(xi, neta) = 4[(1-xi-neta) - neta] = 4(1-xi-2neta)
+ double batozDN5Dneta(double xi, double neta) const { return 4.0 * (1.0 - xi - 2.0 * neta); }
+ // dN6/dneta(xi, neta) = -4xi
+ double batozDN6Dneta(double xi, double neta) const { return -4.0 * xi; }
+
+ std::array<double, 9> batozHx(double xi, double neta)
+ {
+ std::array<double, 9> res;
+
+ // 1.5 (a6N6-a5N5)
+ res[0] = 1.5 * (m_ak[2] * batozN6(xi, neta) - m_ak[1] * batozN5(xi, neta));
+ // b5N5+b6N6
+ res[1] = m_bk[1] * batozN5(xi, neta) + m_bk[2] * batozN6(xi, neta);
+ // N1 - c5N5 - c6N6
+ res[2] = batozN1(xi, neta) - m_ck[1] * batozN5(xi, neta) - m_ck[2] * batozN6(xi, neta);
+
+ // 1.5 (a4N4-a6N6)
+ res[3] = 1.5 * (m_ak[0] * batozN4(xi, neta) - m_ak[2] * batozN6(xi, neta));
+ // b6N6+b4N4
+ res[4] = m_bk[2] * batozN6(xi, neta) + m_bk[0] * batozN4(xi, neta);
+ // N2 - c6N6 - c4N4
+ res[5] = batozN2(xi, neta) - m_ck[2] * batozN6(xi, neta) - m_ck[0] * batozN4(xi, neta);
+
+ // 1.5 (a5N5-a4N4)
+ res[6] = 1.5 * (m_ak[1] * batozN5(xi, neta) - m_ak[0] * batozN4(xi, neta));
+ // b4N4+b5N5
+ res[7] = m_bk[0] * batozN4(xi, neta) + m_bk[1] * batozN5(xi, neta);
+ // N3 - c4N4 - c5N5
+ res[8] = batozN3(xi, neta) - m_ck[0] * batozN4(xi, neta) - m_ck[1] * batozN5(xi, neta);
+
+ return res;
+ }
+ std::array<double, 9> batozDhxDxiAlternative(double xi, double neta)
+ {
+ std::array<double, 9> res;
+
+ // 1.5 (a6dN6-a5dN5)
+ res[0] = 1.5 * (m_ak[2] * batozDN6Dxi(xi, neta) - m_ak[1] * batozDN5Dxi(xi, neta));
+ // b5dN5+b6dN6
+ res[1] = m_bk[1] * batozDN5Dxi(xi, neta) + m_bk[2] * batozDN6Dxi(xi, neta);
+ // dN1 - c5dN5 - c6dN6
+ res[2] = batozDN1Dxi(xi, neta) - m_ck[1] * batozDN5Dxi(xi, neta) - m_ck[2] * batozDN6Dxi(xi, neta);
+
+ // 1.5 (a4dN4-a6dN6)
+ res[3] = 1.5 * (m_ak[0] * batozDN4Dxi(xi, neta) - m_ak[2] * batozDN6Dxi(xi, neta));
+ // b6dN6+b4dN4
+ res[4] = m_bk[2] * batozDN6Dxi(xi, neta) + m_bk[0] * batozDN4Dxi(xi, neta);
+ // dN2 - c6dN6 - c4dN4
+ res[5] = batozDN2Dxi(xi, neta) - m_ck[2] * batozDN6Dxi(xi, neta) - m_ck[0] * batozDN4Dxi(xi, neta);
+
+ // 1.5 (a5dN5-a4dN4)
+ res[6] = 1.5 * (m_ak[1] * batozDN5Dxi(xi, neta) - m_ak[0] * batozDN4Dxi(xi, neta));
+ // b4dN4+b5dN5
+ res[7] = m_bk[0] * batozDN4Dxi(xi, neta) + m_bk[1] * batozDN5Dxi(xi, neta);
+ // dN3 - c4dN4 - c5dN5
+ res[8] = batozDN3Dxi(xi, neta) - m_ck[0] * batozDN4Dxi(xi, neta) - m_ck[1] * batozDN5Dxi(xi, neta);
+
+ return res;
+ }
+ std::array<double, 9> batozDhxDnetaAlternative(double xi, double neta)
+ {
+ std::array<double, 9> res;
+
+ // 1.5 (a6dN6-a5dN5)
+ res[0] = 1.5 * (m_ak[2] * batozDN6Dneta(xi, neta) - m_ak[1] * batozDN5Dneta(xi, neta));
+ // b5dN5+b6dN6
+ res[1] = m_bk[1] * batozDN5Dneta(xi, neta) + m_bk[2] * batozDN6Dneta(xi, neta);
+ // dN1 - c5dN5 - c6dN6
+ res[2] = batozDN1Dneta(xi, neta) - m_ck[1] * batozDN5Dneta(xi, neta) - m_ck[2] * batozDN6Dneta(xi, neta);
+
+ // 1.5 (a4dN4-a6dN6)
+ res[3] = 1.5 * (m_ak[0] * batozDN4Dneta(xi, neta) - m_ak[2] * batozDN6Dneta(xi, neta));
+ // b6dN6+b4dN4
+ res[4] = m_bk[2] * batozDN6Dneta(xi, neta) + m_bk[0] * batozDN4Dneta(xi, neta);
+ // dN2 - c6dN6 - c4dN4
+ res[5] = batozDN2Dneta(xi, neta) - m_ck[2] * batozDN6Dneta(xi, neta) - m_ck[0] * batozDN4Dneta(xi, neta);
+
+ // 1.5 (a5dN5-a4dN4)
+ res[6] = 1.5 * (m_ak[1] * batozDN5Dneta(xi, neta) - m_ak[0] * batozDN4Dneta(xi, neta));
+ // b4dN4+b5dN5
+ res[7] = m_bk[0] * batozDN4Dneta(xi, neta) + m_bk[1] * batozDN5Dneta(xi, neta);
+ // dN3 - c4dN4 - c5dN5
+ res[8] = batozDN3Dneta(xi, neta) - m_ck[0] * batozDN4Dneta(xi, neta) - m_ck[1] * batozDN5Dneta(xi, neta);
+
+ return res;
+ }
+
+ std::array<double, 9> batozHy(double xi, double neta)
+ {
+ std::array<double, 9> res;
+
+ // 1.5 (d6N6-d5N5)
+ res[0] = 1.5 * (m_dk[2] * batozN6(xi, neta) - m_dk[1] * batozN5(xi, neta));
+ // -N1 + e5N5 + e6N6
+ res[1] = -batozN1(xi, neta) + m_ek[1] * batozN5(xi, neta) + m_ek[2] * batozN6(xi, neta);
+ // -b5N5-b6N6
+ res[2] = -m_bk[1] * batozN5(xi, neta) - m_bk[2] * batozN6(xi, neta);
+
+ // 1.5 (d4N4-d6N6)
+ res[3] = 1.5 * (m_dk[0] * batozN4(xi, neta) - m_dk[2] * batozN6(xi, neta));
+ // -N2 + e6N6 + e4N4
+ res[4] = -batozN2(xi, neta) + m_ek[2] * batozN6(xi, neta) + m_ek[0] * batozN4(xi, neta);
+ // -b6N6-b4N4
+ res[5] = -m_bk[2] * batozN6(xi, neta) - m_bk[0] * batozN4(xi, neta);
+
+ // 1.5 (d5N5-d4N4)
+ res[6] = 1.5 * (m_dk[1] * batozN5(xi, neta) - m_dk[0] * batozN4(xi, neta));
+ // -N3 + e4N4 + e5N5
+ res[7] = -batozN3(xi, neta) + m_ek[0] * batozN4(xi, neta) + m_ek[1] * batozN5(xi, neta);
+ // -b4N4-b5N5
+ res[8] = -m_bk[0] * batozN4(xi, neta) - m_bk[1] * batozN5(xi, neta);
+
+ return res;
+ }
+ std::array<double, 9> batozDhyDxiAlternative(double xi, double neta)
+ {
+ std::array<double, 9> res;
+
+ // 1.5 (d6dN6-d5dN5)
+ res[0] = 1.5 * (m_dk[2] * batozDN6Dxi(xi, neta) - m_dk[1] * batozDN5Dxi(xi, neta));
+ // -dN1 + e5dN5 + e6dN6
+ res[1] = -batozDN1Dxi(xi, neta) + m_ek[1] * batozDN5Dxi(xi, neta) + m_ek[2] * batozDN6Dxi(xi, neta);
+ // -b5dN5-b6dN6
+ res[2] = -m_bk[1] * batozDN5Dxi(xi, neta) - m_bk[2] * batozDN6Dxi(xi, neta);
+
+ // 1.5 (d4dN4-d6dN6)
+ res[3] = 1.5 * (m_dk[0] * batozDN4Dxi(xi, neta) - m_dk[2] * batozDN6Dxi(xi, neta));
+ // -dN2 + e6dN6 + e4dN4
+ res[4] = -batozDN2Dxi(xi, neta) + m_ek[2] * batozDN6Dxi(xi, neta) + m_ek[0] * batozDN4Dxi(xi, neta);
+ // -b6dN6-b4dN4
+ res[5] = -m_bk[2] * batozDN6Dxi(xi, neta) - m_bk[0] * batozDN4Dxi(xi, neta);
+
+ // 1.5 (d5dN5-d4dN4)
+ res[6] = 1.5 * (m_dk[1] * batozDN5Dxi(xi, neta) - m_dk[0] * batozDN4Dxi(xi, neta));
+ // -dN3 + e4dN4 + e5dN5
+ res[7] = -batozDN3Dxi(xi, neta) + m_ek[0] * batozDN4Dxi(xi, neta) + m_ek[1] * batozDN5Dxi(xi, neta);
+ // -b4dN4-b5dN5
+ res[8] = -m_bk[0] * batozDN4Dxi(xi, neta) - m_bk[1] * batozDN5Dxi(xi, neta);
+
+ return res;
+ }
+ std::array<double, 9> batozDhyDnetaAlternative(double xi, double neta)
+ {
+ std::array<double, 9> res;
+
+ // 1.5 (d6dN6-d5dN5)
+ res[0] = 1.5 * (m_dk[2] * batozDN6Dneta(xi, neta) - m_dk[1] * batozDN5Dneta(xi, neta));
+ // -dN1 + e5dN5 + e6dN6
+ res[1] = -batozDN1Dneta(xi, neta) + m_ek[1] * batozDN5Dneta(xi, neta) + m_ek[2] * batozDN6Dneta(xi, neta);
+ // -b5dN5-b6dN6
+ res[2] = -m_bk[1] * batozDN5Dneta(xi, neta) - m_bk[2] * batozDN6Dneta(xi, neta);
+
+ // 1.5 (d4dN4-d6dN6)
+ res[3] = 1.5 * (m_dk[0] * batozDN4Dneta(xi, neta) - m_dk[2] * batozDN6Dneta(xi, neta));
+ // -dN2 + e6dN6 + e4dN4
+ res[4] = -batozDN2Dneta(xi, neta) + m_ek[2] * batozDN6Dneta(xi, neta) + m_ek[0] * batozDN4Dneta(xi, neta);
+ // -b6dN6-b4dN4
+ res[5] = -m_bk[2] * batozDN6Dneta(xi, neta) - m_bk[0] * batozDN4Dneta(xi, neta);
+
+ // -dN3 + e4dN4 + e5dN5
+ res[6] = 1.5 * (m_dk[1] * batozDN5Dneta(xi, neta) - m_dk[0] * batozDN4Dneta(xi, neta));
+ // -dN3 + e4dN4 + e5dN5
+ res[7] = -batozDN3Dneta(xi, neta) + m_ek[0] * batozDN4Dneta(xi, neta) + m_ek[1] * batozDN5Dneta(xi, neta);
+ // -b4dN4-b5dN5
+ res[8] = -m_bk[0] * batozDN4Dneta(xi, neta) - m_bk[1] * batozDN5Dneta(xi, neta);
+
+ return res;
+ }
+
+ Eigen::Matrix<double, 3, 9> batozStrainDisplacementAlternativeDerivative(double xi, double neta)
+ {
+ Eigen::Matrix<double, 3, 9> res;
+ std::array<double, 9> dHx_dxi, dHx_dneta, dHy_dxi, dHy_dneta;
+ double coefficient = 1.0 / (2.0 * m_restArea);
+
+ dHx_dxi = batozDhxDxiAlternative(xi, neta);
+ dHx_dneta = batozDhxDnetaAlternative(xi, neta);
+ dHy_dxi = batozDhyDxiAlternative(xi, neta);
+ dHy_dneta = batozDhyDnetaAlternative(xi, neta);
+
+ for(size_t i = 0; i < 9; ++i)
+ {
+ // 4 -> mid-edge 12
+ // 5 -> mid-edge 20
+ // 6 -> mid-edge 01
+ res(0, i) = coefficient * ( m_yij[1] * dHx_dxi[i] + m_yij[2] * dHx_dneta[i]);
+ res(1, i) = coefficient * (-m_xij[1] * dHy_dxi[i] - m_xij[2] * dHy_dneta[i]);
+ res(2, i) = coefficient *
+ (-m_xij[1] * dHx_dxi[i] - m_xij[2] * dHx_dneta[i] + m_yij[1] * dHy_dxi[i] + m_yij[2] * dHy_dneta[i]);
+ }
+
+ return res;
+ }
+
+ std::array<double, 9> batozFx(double xi, double neta,
+ double (MockFem2DElement::*f1)(double,double) const,
+ double (MockFem2DElement::*f2)(double,double) const,
+ double (MockFem2DElement::*f3)(double,double) const,
+ double (MockFem2DElement::*f4)(double,double) const,
+ double (MockFem2DElement::*f5)(double,double) const,
+ double (MockFem2DElement::*f6)(double,double) const)
+ {
+ std::array<double, 9> res;
+
+ // 1.5 (a6N6-a5N5)
+ res[0] = 1.5 * (m_ak[2] * (this->*f6)(xi, neta) - m_ak[1] * (this->*f5)(xi, neta));
+ // b5N5+b6N6
+ res[1] = m_bk[1] * (this->*f5)(xi, neta) + m_bk[2] * (this->*f6)(xi, neta);
+ // N1 - c5N5 - c6N6
+ res[2] = (this->*f1)(xi, neta) - m_ck[1] * (this->*f5)(xi, neta) - m_ck[2] * (this->*f6)(xi, neta);
+
+ // 1.5 (a4N4-a6N6)
+ res[3] = 1.5 * (m_ak[0] * (this->*f4)(xi, neta) - m_ak[2] * (this->*f6)(xi, neta));
+ // b6N6+b4N4
+ res[4] = m_bk[2] * (this->*f6)(xi, neta) + m_bk[0] * (this->*f4)(xi, neta);
+ // N2 - c6N6 - c4N4
+ res[5] = (this->*f2)(xi, neta) - m_ck[2] * (this->*f6)(xi, neta) - m_ck[0] * (this->*f4)(xi, neta);
+
+ // 1.5 (a5N5-a4N4)
+ res[6] = 1.5 * (m_ak[1] * (this->*f5)(xi, neta) - m_ak[0] * (this->*f4)(xi, neta));
+ // b4N4+b5N5
+ res[7] = m_bk[0] * (this->*f4)(xi, neta) + m_bk[1] * (this->*f5)(xi, neta);
+ // N3 - c4N4 - c5N5
+ res[8] = (this->*f3)(xi, neta) - m_ck[0] * (this->*f4)(xi, neta) - m_ck[1] * (this->*f5)(xi, neta);
+
+ return res;
+ }
+
+ std::array<double, 9> batozFy(double xi, double neta,
+ double (MockFem2DElement::*f1)(double,double) const,
+ double (MockFem2DElement::*f2)(double,double) const,
+ double (MockFem2DElement::*f3)(double,double) const,
+ double (MockFem2DElement::*f4)(double,double) const,
+ double (MockFem2DElement::*f5)(double,double) const,
+ double (MockFem2DElement::*f6)(double,double) const)
+ {
+ std::array<double, 9> res;
+
+ // 1.5 (d6N6-d5N5)
+ res[0] = 1.5 * (m_dk[2] * (this->*f6)(xi, neta) - m_dk[1] * (this->*f5)(xi, neta));
+ // -N1 + e5N5 + e6N6
+ res[1] = -(this->*f1)(xi, neta) + m_ek[1] * (this->*f5)(xi, neta) + m_ek[2] * (this->*f6)(xi, neta);
+ // -b5N5-b6N6
+ res[2] = -m_bk[1] * (this->*f5)(xi, neta) - m_bk[2] * (this->*f6)(xi, neta);
+
+ // 1.5 (d4N4-d6N6)
+ res[3] = 1.5 * (m_dk[0] * (this->*f4)(xi, neta) - m_dk[2] * (this->*f6)(xi, neta));
+ // -N2 + e6N6 + e4N4
+ res[4] = -(this->*f2)(xi, neta) + m_ek[2] * (this->*f6)(xi, neta) + m_ek[0] * (this->*f4)(xi, neta);
+ // -b6N6-b4N4
+ res[5] = -m_bk[2] * (this->*f6)(xi, neta) - m_bk[0] * (this->*f4)(xi, neta);
+
+ // 1.5 (d5N5-d4N4)
+ res[6] = 1.5 * (m_dk[1] * (this->*f5)(xi, neta) - m_dk[0] * (this->*f4)(xi, neta));
+ // -N3 + e4N4 + e5N5
+ res[7] = -(this->*f3)(xi, neta) + m_ek[0] * (this->*f4)(xi, neta) + m_ek[1] * (this->*f5)(xi, neta);
+ // -b4N4-b5N5
+ res[8] = -m_bk[0] * (this->*f4)(xi, neta) - m_bk[1] * (this->*f5)(xi, neta);
+
+ return res;
+ }
+
+ Eigen::Matrix<double, 3, 9> batozStrainDisplacementNumericalDerivation(double xi, double neta)
+ {
+ Eigen::Matrix<double, 3, 9> res;
+ std::array<double, 9> dHx_dxi, dHx_dneta, dHy_dxi, dHy_dneta;
+ double coefficient = 1.0 / (2.0 * m_restArea);
+
+ dHx_dxi = batozFx(xi, neta, &MockFem2DElement::batozDN1Dxi , &MockFem2DElement::batozDN2Dxi ,
+ &MockFem2DElement::batozDN3Dxi , &MockFem2DElement::batozDN4Dxi , &MockFem2DElement::batozDN5Dxi ,
+ &MockFem2DElement::batozDN6Dxi);
+ dHx_dneta = batozFx(xi, neta, &MockFem2DElement::batozDN1Dneta, &MockFem2DElement::batozDN2Dneta,
+ &MockFem2DElement::batozDN3Dneta, &MockFem2DElement::batozDN4Dneta, &MockFem2DElement::batozDN5Dneta,
+ &MockFem2DElement::batozDN6Dneta);
+ dHy_dxi = batozFy(xi, neta, &MockFem2DElement::batozDN1Dxi, &MockFem2DElement::batozDN2Dxi ,
+ &MockFem2DElement::batozDN3Dxi, &MockFem2DElement::batozDN4Dxi, &MockFem2DElement::batozDN5Dxi,
+ &MockFem2DElement::batozDN6Dxi);
+ dHy_dneta = batozFy(xi, neta, &MockFem2DElement::batozDN1Dneta, &MockFem2DElement::batozDN2Dneta,
+ &MockFem2DElement::batozDN3Dneta, &MockFem2DElement::batozDN4Dneta, &MockFem2DElement::batozDN5Dneta,
+ &MockFem2DElement::batozDN6Dneta);
+
+ for(size_t i = 0; i < 9; ++i)
+ {
+ // 4 -> mid-edge 12
+ // 5 -> mid-edge 20
+ // 6 -> mid-edge 01
+ res(0, i) = coefficient * ( m_yij[1] * dHx_dxi[i] + m_yij[2] * dHx_dneta[i]);
+ res(1, i) = coefficient * (-m_xij[1] * dHy_dxi[i] - m_xij[2] * dHy_dneta[i]);
+ res(2, i) = coefficient *
+ (-m_xij[1] * dHx_dxi[i] - m_xij[2] * dHx_dneta[i] + m_yij[1] * dHy_dxi[i] + m_yij[2] * dHy_dneta[i]);
+ }
+
+ return res;
+ }
+
+private:
+ Eigen::Matrix<double, 18, 18> m_initialRotationTimes6;
+};
+
+class Fem2DElementTriangleTests : public ::testing::Test
+{
+public:
+ static const int m_numberNodes = 6;
+
+ std::array<size_t, 3> m_nodeIds;
+ SurgSim::Math::OdeState m_restState;
+ double m_expectedVolume;
+ double m_rho, m_E, m_nu;
+ double m_A; // area
+ double m_thickness; // thickness
+ Quaterniond m_rotation, m_expectedRotation;
+ Eigen::Matrix<double, 18, 1> m_expectedX0;
+
+ virtual void SetUp() override
+ {
+ using SurgSim::Math::getSubVector;
+
+ m_rho = 1000.0;
+ m_E = 1e9;
+ m_nu = 0.45;
+ m_thickness = 1e-2;
+ m_A = 1.0 / 2.0;
+ m_expectedVolume = m_A * m_thickness;
+
+ // Triangle is made of node 3, 1 and 5 in a bigger system containing m_numberNodes nodes (at least 6)
+ m_nodeIds[0] = 3;
+ m_nodeIds[1] = 1;
+ m_nodeIds[2] = 5;
+
+ m_restState.setNumDof(6, m_numberNodes);
+
+ m_rotation.coeffs().setRandom();
+ m_rotation.normalize();
+
+ Vector3d A(0.0, 0.0, 0.0);
+ Vector3d B(1.0, 0.0, 0.0);
+ Vector3d C(0.0, 1.0, 0.0);
+ Vector& x = m_restState.getPositions();
+ getSubVector(x, m_nodeIds[0], 6).segment<3>(0) = m_rotation._transformVector(A);
+ getSubVector(x, m_nodeIds[1], 6).segment<3>(0) = m_rotation._transformVector(B);
+ getSubVector(x, m_nodeIds[2], 6).segment<3>(0) = m_rotation._transformVector(C);
+
+ for (size_t nodeId = 0; nodeId < 3; ++nodeId)
+ {
+ m_expectedX0.segment(6 * nodeId, 6) = getSubVector(x, m_nodeIds[nodeId], 6);
+ }
+
+ // The initial rotation of ABC is defined by (i=AB(1 0 0), j=AC(0 1 0), k=AB^AC(0 0 1)) = Identity
+ // Therefore, by applying m_rotation to the triangle, the initial rotation of the element should be m_rotation
+ m_expectedRotation = m_rotation;
+ }
+
+ void getExpectedLocalMassMatrix(Eigen::Ref<SurgSim::Math::Matrix> mass)
+ {
+ // This is hard-coded for density(rho)=1000 A=0.5 thickness=1e-2
+ mass.setIdentity();
+ for (size_t nodeId = 0; nodeId < 3; ++nodeId)
+ {
+ mass.block(6 * nodeId, 6 * nodeId, 3, 3).setConstant(5.0 / 12.0);
+ mass.block(6 * nodeId, 6 * nodeId, 3, 3).diagonal().setConstant(5.0 / 6.0);
+ mass.block(6 * nodeId + 3, 6 * nodeId + 3, 2, 2).setConstant(-6.25e-6);
+ mass.block(6 * nodeId + 3, 6 * nodeId + 3, 2, 2).diagonal().setConstant(6.0416666666666666e-5);
+ }
+ }
+
+ void getExpectedLocalStiffnessMatrix(Eigen::Ref<SurgSim::Math::Matrix> stiffness)
+ {
+ typedef Eigen::Matrix<double, 9, 9> Matrix99Type;
+ typedef Eigen::Matrix<double, 6, 6> Matrix66Type;
+
+ Matrix66Type membraneStiffness = getMembraneLocalStiffnessMatrix();
+ Matrix99Type plateStiffness = getPlateLocalStiffnessMatrix();
+
+ // Assemble the membrane and plane stiffness
+ stiffness.setIdentity();
+ for(size_t row = 0; row < 3; ++row)
+ {
+ for(size_t column = 0; column < 3; ++column)
+ {
+ // Membrane part
+ stiffness.block(6 * row, 6 * column, 2, 2) = membraneStiffness.block(2 * row, 2 * column, 2, 2);
+
+ // Thin-plate part
+ stiffness.block(6 * row + 2, 6 * column + 2, 3, 3) = plateStiffness.block(3 * row, 3 * column, 3, 3);
+ }
+ }
+ }
+
+ Eigen::Matrix<double, 6, 6> getMembraneLocalStiffnessMatrix()
+ {
+ typedef Eigen::Matrix<double, 3, 6> Matrix36Type;
+
+ std::shared_ptr<MockFem2DElement> element = getElement();
+
+ // Membrane theory (using "Theory of Matrix Structural Analysis" - Przemieniecki)
+ // ux = c1.x + c2.y + c3
+ // uy = c4.x + c5.y + c6
+ // ux(x1, y1) = u1x = c1.x1 + c2.y1 + c3 (u1x) (x1 y1 1)(c1)
+ // ux(x2, y2) = u2x = c1.x2 + c2.y2 + c3 <=> (u2x)=(x2 y2 1)(c2)
+ // ux(x3, y3) = u3x = c1.x3 + c2.y3 + c3 (u3x) (x3 y3 1)(c3)
+ // <=> (c1) = 1/det( y23 -y13 y12 )(u1x)
+ // (c2) (-x23 x13 -x12 )(u2x)
+ // (c3) ( x2y3-x3y2 -(x1y3-x3y1) x1y2-x2y1)(u3x)
+ // det = (x1y2 + x2y3 + x3y1 - x3y2 - x2y1 - x1y3)
+ // = x21(y3) - y21(x3) +x2(-y1) - y2(-x1) - x1(-y1) - (-y1)(-x1) = x21y31 - y21x31 = 2A > 0
+ //
+ // and similarily for uy
+ // <=> (c4) = 1/(2A)( y23 -y13 y12 )(u1y)
+ // (c5) (-x23 x13 -x12 )(u2y)
+ // (c6) ( x2y3-x3y2 -(x1y3-x3y1) x1y2-x2y1)(u3y)
+ //
+ // Therefore ux = 1/(2A) [x.(y23.u1x - y13.u2x + y12.u3x) + y.(-x23.u1x + x13.u2x - x12.u3x) + constant]
+ // Exx = dux/dx = 1/(2A) (y23.u1x - y13.u2x + y12.u3x) = b.u
+ // Therefore uy = 1/(2A) [x.(y23.u1y - y13.u2y + y12.u3y) + y.(-x23.u1y + x13.u2y - x12.u3y) + constant]
+ // Eyy = duy/dy = 1/(2A) (-x23.u1y + x13.u2y - x12.u3y) = b.u
+ // Exy = dux/dy + duy/dx = 1/(2A) (-x23.u1x + x13.u2x - x12.u3x + y23.u1y - y13.u2y + y12.u3y) = b.u
+ Vector3d A2D = m_expectedRotation.inverse()._transformVector(m_expectedX0.segment(0,3));
+ Vector3d B2D = m_expectedRotation.inverse()._transformVector(m_expectedX0.segment(6,3));
+ Vector3d C2D = m_expectedRotation.inverse()._transformVector(m_expectedX0.segment(12,3));
+ double x12 = A2D[0] - B2D[0];
+ double x13 = A2D[0] - C2D[0];
+ double x23 = B2D[0] - C2D[0];
+ double y12 = A2D[1] - B2D[1];
+ double y13 = A2D[1] - C2D[1];
+ double y23 = B2D[1] - C2D[1];
+ Matrix36Type b = Matrix36Type::Zero();
+ b(0, 0) = y23;
+ b(0, 2) = -y13;
+ b(0, 4) = y12;
+ b(1, 1) = -x23;
+ b(1, 3) = x13;
+ b(1, 5) = -x12;
+ b(2, 0) = -x23;
+ b(2, 1) = y23;
+ b(2, 2) = x13;
+ b(2, 3) = -y13;
+ b(2, 4) = -x12;
+ b(2, 5) = y12;
+ b *= 1.0 / (2.0 * m_A);
+ Matrix33d Emembrane;
+ Emembrane << 1.0, m_nu, 0.0, m_nu, 1.0, 0.0, 0.0, 0.0, (1.0 - m_nu) / 2.0;
+ Emembrane *= m_E / (1.0 - m_nu * m_nu);
+ return (m_thickness * m_A) * b.transpose() * Emembrane * b;
+ }
+
+ Eigen::Matrix<double, 9, 9> getPlateLocalStiffnessMatrix()
+ {
+ typedef Eigen::Matrix<double, 3, 9> Matrix39Type;
+
+ Eigen::Matrix<double, 9, 9> stiffness;
+ std::shared_ptr<MockFem2DElement> element = getElement();
+
+ // Thin-plate theory (Batoz)
+ Matrix39Type B0 = element->batozStrainDisplacementNumericalDerivation(0.5, 0.0);
+ Matrix39Type B1 = element->batozStrainDisplacementNumericalDerivation(0.0, 0.5);
+ Matrix39Type B2 = element->batozStrainDisplacementNumericalDerivation(0.5, 0.5);
+ Matrix33d Eplate;
+ Eplate << 1.0, m_nu, 0.0, m_nu, 1.0, 0.0, 0.0, 0.0, (1.0 - m_nu) / 2.0;
+ Eplate *= m_E * m_thickness * m_thickness * m_thickness / (12.0 * (1.0 - m_nu * m_nu));
+ // Integration using 3 Gauss point on the mid-point of each triangle edge
+ // weight = A/3 for all 3 (A is the area of the parametrized triangle = 0.5)
+ stiffness = (1.0 / 6.0) * B0.transpose() * Eplate * B0;
+ stiffness += (1.0 / 6.0) * B1.transpose() * Eplate * B1;
+ stiffness += (1.0 / 6.0) * B2.transpose() * Eplate * B2;
+ stiffness *= 2.0 * m_A;
+
+ return stiffness;
+ }
+
+ std::shared_ptr<MockFem2DElement> getElement()
+ {
+ auto element = std::make_shared<MockFem2DElement>(m_nodeIds);
+ element->setThickness(m_thickness);
+ element->setMassDensity(m_rho);
+ element->setPoissonRatio(m_nu);
+ element->setYoungModulus(m_E);
+ element->initialize(m_restState);
+ return element;
+ }
+};
+
+TEST_F(Fem2DElementTriangleTests, ConstructorTest)
+{
+ ASSERT_NO_THROW({ MockFem2DElement triangle(m_nodeIds); });
+}
+
+TEST_F(Fem2DElementTriangleTests, NodeIdsTest)
+{
+ Fem2DElementTriangle element(m_nodeIds);
+ EXPECT_EQ(3u, element.getNumNodes());
+ EXPECT_EQ(3u, element.getNodeIds().size());
+ for (int i = 0; i < 3; i++)
+ {
+ EXPECT_EQ(m_nodeIds[i], element.getNodeId(i));
+ EXPECT_EQ(m_nodeIds[i], element.getNodeIds()[i]);
+ }
+}
+
+TEST_F(Fem2DElementTriangleTests, setGetThicknessTest)
+{
+ Fem2DElementTriangle element(m_nodeIds);
+
+ // Default thickness = 0.0
+ EXPECT_DOUBLE_EQ(0.0, element.getThickness());
+ // Set to a valid thickness
+ element.setThickness(1.54);
+ EXPECT_DOUBLE_EQ(1.54, element.getThickness());
+ // Set to an invalid thickness
+ EXPECT_ANY_THROW(element.setThickness(0.0));
+ EXPECT_ANY_THROW(element.setThickness(-9.4));
+}
+
+TEST_F(Fem2DElementTriangleTests, MaterialParameterTest)
+{
+ Fem2DElementTriangle element(m_nodeIds);
+ element.setThickness(m_thickness);
+
+ // Test the various mode of failure related to the physical parameters
+ // This has been already tested in FemElementTests, but this is to make sure this method is called properly
+ // So the same behavior should be expected
+ {
+ // Mass density not set
+ ASSERT_ANY_THROW(element.initialize(m_restState));
+
+ // Poisson Ratio not set
+ element.setMassDensity(-1234.56);
+ ASSERT_ANY_THROW(element.initialize(m_restState));
+
+ // Young modulus not set
+ element.setPoissonRatio(0.55);
+ ASSERT_ANY_THROW(element.initialize(m_restState));
+
+ // Invalid mass density
+ element.setYoungModulus(-4321.33);
+ ASSERT_ANY_THROW(element.initialize(m_restState));
+
+ // Invalid Poisson ratio
+ element.setMassDensity(m_rho);
+ ASSERT_ANY_THROW(element.initialize(m_restState));
+
+ // Invalid Young modulus
+ element.setPoissonRatio(m_nu);
+ ASSERT_ANY_THROW(element.initialize(m_restState));
+
+ element.setYoungModulus(m_E);
+ ASSERT_NO_THROW(element.initialize(m_restState));
+ }
+}
+
+TEST_F(Fem2DElementTriangleTests, VolumeTest)
+{
+ std::shared_ptr<MockFem2DElement> element = getElement();
+ EXPECT_NEAR(element->getVolume(m_restState), m_expectedVolume, 1e-10);
+}
+
+TEST_F(Fem2DElementTriangleTests, CoordinateTests)
+{
+ Fem2DElementTriangle element(m_nodeIds);
+
+ Vector validNaturalCoordinate(3);
+ Vector validNaturalCoordinate2(3);
+ Vector invalidNaturalCoordinateSumNot1(3);
+ Vector invalidNaturalCoordinateNegativeValue(3);
+ Vector invalidNaturalCoordinateBiggerThan1Value(3);
+ Vector invalidNaturalCoordinateSize2(2), invalidNaturalCoordinateSize4(4);
+
+ validNaturalCoordinate << 0.4, 0.5, 0.1;
+ validNaturalCoordinate2 << -1e-11, 1.0 + 1e-11, 0.0;
+ invalidNaturalCoordinateSumNot1 << 0.4, 0.5, 0.3;
+ invalidNaturalCoordinateNegativeValue << 0.7, 0.7, -0.4;
+ invalidNaturalCoordinateBiggerThan1Value << 1.4, 0.6, -1.0;
+ invalidNaturalCoordinateSize2 << 0.4, 0.6;
+ invalidNaturalCoordinateSize4 << 0.2, 0.2, 0.2, 0.4;
+ EXPECT_TRUE(element.isValidCoordinate(validNaturalCoordinate));
+ EXPECT_TRUE(element.isValidCoordinate(validNaturalCoordinate2));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateSumNot1));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateNegativeValue));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateBiggerThan1Value));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateSize2));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateSize4));
+
+ Vector naturalCoordinateA(3), naturalCoordinateB(3), naturalCoordinateC(3), naturalCoordinateMiddle(3);
+ Vector ptA, ptB, ptC, ptMiddle;
+ naturalCoordinateA << 1.0, 0.0, 0.0;
+ naturalCoordinateB << 0.0, 1.0, 0.0;
+ naturalCoordinateC << 0.0, 0.0, 1.0;
+ naturalCoordinateMiddle << 1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0;
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateBiggerThan1Value), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateNegativeValue), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize2), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize4), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSumNot1), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(ptA = element.computeCartesianCoordinate(m_restState, naturalCoordinateA));
+ EXPECT_NO_THROW(ptB = element.computeCartesianCoordinate(m_restState, naturalCoordinateB));
+ EXPECT_NO_THROW(ptC = element.computeCartesianCoordinate(m_restState, naturalCoordinateC));
+ EXPECT_NO_THROW(ptMiddle = element.computeCartesianCoordinate(m_restState, naturalCoordinateMiddle));
+ EXPECT_TRUE(ptA.isApprox(m_rotation._transformVector(Vector3d(0.0, 0.0, 0.0))));
+ EXPECT_TRUE(ptB.isApprox(m_rotation._transformVector(Vector3d(1.0, 0.0, 0.0))));
+ EXPECT_TRUE(ptC.isApprox(m_rotation._transformVector(Vector3d(0.0, 1.0, 0.0))));
+ EXPECT_TRUE(ptMiddle.isApprox(m_rotation._transformVector(Vector3d(1.0 / 3.0, 1.0 / 3.0, 0.0))));
+
+ Vector3d cartesian = m_rotation._transformVector(Vector3d(0.0, 0.0, 0.0));
+ EXPECT_THROW(element.computeNaturalCoordinate(m_restState, cartesian), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(Fem2DElementTriangleTests, RestAreaTest)
+{
+ std::shared_ptr<MockFem2DElement> element = getElement();
+ EXPECT_NEAR(element->getRestArea(), m_A, 1e-10);
+}
+
+TEST_F(Fem2DElementTriangleTests, InitialRotationTest)
+{
+ std::shared_ptr<MockFem2DElement> element = getElement();
+
+ // Use a mask to test the structure of the rotation matrix R0 (6 digonal block 3x3 matrix and 0 elsewhere)
+ Eigen::Matrix<double, 18, 18> mask;
+ mask.setOnes();
+ mask.block<3, 3>(0, 0).setZero();
+ mask.block<3, 3>(3, 3).setZero();
+ mask.block<3, 3>(6, 6).setZero();
+ mask.block<3, 3>(9, 9).setZero();
+ mask.block<3, 3>(12, 12).setZero();
+ mask.block<3, 3>(15, 15).setZero();
+ EXPECT_TRUE(element->getInitialRotationTimes6().cwiseProduct(mask).isZero());
+
+ EXPECT_TRUE(element->getInitialRotationTimes6().block(0, 0, 3, 3).isApprox(m_expectedRotation.matrix()));
+ EXPECT_TRUE(element->getInitialRotationTimes6().block(3, 3, 3, 3).isApprox(m_expectedRotation.matrix()));
+ EXPECT_TRUE(element->getInitialRotationTimes6().block(6, 6, 3, 3).isApprox(m_expectedRotation.matrix()));
+ EXPECT_TRUE(element->getInitialRotationTimes6().block(9, 9, 3, 3).isApprox(m_expectedRotation.matrix()));
+ EXPECT_TRUE(element->getInitialRotationTimes6().block(12, 12, 3, 3).isApprox(m_expectedRotation.matrix()));
+ EXPECT_TRUE(element->getInitialRotationTimes6().block(15, 15, 3, 3).isApprox(m_expectedRotation.matrix()));
+}
+
+TEST_F(Fem2DElementTriangleTests, StrainDisplacementPlateAtGaussPointTest)
+{
+ std::shared_ptr<MockFem2DElement> element = getElement();
+
+ Eigen::Matrix<double, 3, 9> strainDisplacement[3];
+ strainDisplacement[0] = element->getBatozStrainDisplacement(0.0, 0.5);
+ strainDisplacement[1] = element->getBatozStrainDisplacement(0.5, 0.0);
+ strainDisplacement[2] = element->getBatozStrainDisplacement(0.5, 0.5);
+
+ Eigen::Matrix<double, 3, 9> strainDisplacementExpected1[3];
+ strainDisplacementExpected1[0] = element->batozStrainDisplacementAlternativeDerivative(0.0, 0.5);
+ strainDisplacementExpected1[1] = element->batozStrainDisplacementAlternativeDerivative(0.5, 0.0);
+ strainDisplacementExpected1[2] = element->batozStrainDisplacementAlternativeDerivative(0.5, 0.5);
+ Eigen::Matrix<double, 3, 9> strainDisplacementExpected2[3];
+ strainDisplacementExpected2[0] = element->batozStrainDisplacementNumericalDerivation(0.0, 0.5);
+ strainDisplacementExpected2[1] = element->batozStrainDisplacementNumericalDerivation(0.5, 0.0);
+ strainDisplacementExpected2[2] = element->batozStrainDisplacementNumericalDerivation(0.5, 0.5);
+
+ // Validate the alternative technique against the numerical evaluation
+ EXPECT_TRUE(strainDisplacementExpected1[0].isApprox(strainDisplacementExpected2[0])) <<
+ strainDisplacementExpected1[0] << std::endl <<
+ strainDisplacementExpected2[0] << std::endl;
+ EXPECT_TRUE(strainDisplacementExpected1[1].isApprox(strainDisplacementExpected2[1])) <<
+ strainDisplacementExpected1[1] << std::endl <<
+ strainDisplacementExpected2[1] << std::endl;
+ EXPECT_TRUE(strainDisplacementExpected1[2].isApprox(strainDisplacementExpected2[2])) <<
+ strainDisplacementExpected1[2] << std::endl <<
+ strainDisplacementExpected2[2] << std::endl;
+
+ // Validate the Fem2DElementTriangle internal calculation against both technique
+ EXPECT_TRUE(strainDisplacement[0].isApprox(strainDisplacementExpected1[0])) <<
+ strainDisplacement[0] << std::endl <<
+ strainDisplacementExpected1[0] << std::endl;
+ EXPECT_TRUE(strainDisplacement[0].isApprox(strainDisplacementExpected2[0])) <<
+ strainDisplacement[0] << std::endl <<
+ strainDisplacementExpected2[0] << std::endl;
+
+ EXPECT_TRUE(strainDisplacement[1].isApprox(strainDisplacementExpected1[1])) <<
+ strainDisplacement[1] << std::endl <<
+ strainDisplacementExpected1[1] << std::endl;
+ EXPECT_TRUE(strainDisplacement[1].isApprox(strainDisplacementExpected2[1])) <<
+ strainDisplacement[1] << std::endl <<
+ strainDisplacementExpected2[1] << std::endl;
+
+ EXPECT_TRUE(strainDisplacement[2].isApprox(strainDisplacementExpected1[2])) <<
+ strainDisplacement[2] << std::endl <<
+ strainDisplacementExpected1[2] << std::endl;
+ EXPECT_TRUE(strainDisplacement[2].isApprox(strainDisplacementExpected2[2])) <<
+ strainDisplacement[2] << std::endl <<
+ strainDisplacementExpected2[2] << std::endl;
+}
+
+namespace
+{
+/// Shape function evaluation Ni(x,y) = ai + bi.x + ci.y
+/// \param i Defines which shape function to evaluate
+/// \param ai, bi, ci The shape functions parameters
+/// \param p The 2D point (x, y) to evaluate the shape function at
+/// \return The shape function evaluation ai + bi.x + ci.y
+double N(size_t i,
+ const std::array<double, 3>& ai, const std::array<double, 3>& bi, const std::array<double, 3>& ci,
+ const SurgSim::Math::Vector2d& p)
+{
+ return ai[i] + bi[i] * p[0] + ci[i] * p[1];
+}
+};
+
+TEST_F(Fem2DElementTriangleTests, MembraneShapeFunctionsTest)
+{
+ using SurgSim::Math::getSubVector;
+
+ std::shared_ptr<MockFem2DElement> tri = getElement();
+
+ EXPECT_TRUE(tri->getInitialPosition().isApprox(m_expectedX0)) <<
+ "x0 = " << tri->getInitialPosition().transpose() << std::endl << "x0 expected = " << m_expectedX0.transpose();
+
+ // Ni(x,y) = (ai + bi.x + ci.y)
+ std::array<double, 3> ai, bi, ci;
+ for (int i = 0; i < 3; ++i)
+ {
+ tri->getMembraneShapeFunction(i, &(ai[i]), &(bi[i]), &(ci[i]));
+ }
+
+ // We should (in local frame) have by construction:
+ // { N0(p0) = 1 N1(p0)=N2(p0)=N3(p0)=0
+ // { N1(p1) = 1 N1(p1)=N2(p1)=N3(p1)=0
+ // { N2(p2) = 1 N1(p2)=N2(p2)=N3(p2)=0
+ // { N3(p3) = 1 N1(p3)=N2(p3)=N3(p3)=0
+ const Vector3d p0 = getSubVector(m_expectedX0, 0, 6).segment(0, 3);
+ const Vector3d p1 = getSubVector(m_expectedX0, 1, 6).segment(0, 3);
+ const Vector3d p2 = getSubVector(m_expectedX0, 2, 6).segment(0, 3);
+ SurgSim::Math::Vector2d p02D = m_expectedRotation.inverse()._transformVector(p0).segment(0, 2);
+ SurgSim::Math::Vector2d p12D = m_expectedRotation.inverse()._transformVector(p1).segment(0, 2);
+ SurgSim::Math::Vector2d p22D = m_expectedRotation.inverse()._transformVector(p2).segment(0, 2);
+ std::array<double, 3> Ni_p0, Ni_p1, Ni_p2;
+ for (int i = 0; i < 3; i++)
+ {
+ Ni_p0[i] = N(i, ai, bi, ci, p02D);
+ Ni_p1[i] = N(i, ai, bi, ci, p12D);
+ Ni_p2[i] = N(i, ai, bi, ci, p22D);
+ }
+ EXPECT_NEAR(Ni_p0[0], 1.0, 1e-12);
+ EXPECT_NEAR(Ni_p0[1], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p0[2], 0.0, 1e-12);
+
+ EXPECT_NEAR(Ni_p1[0], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p1[1], 1.0, 1e-12);
+ EXPECT_NEAR(Ni_p1[2], 0.0, 1e-12);
+
+ EXPECT_NEAR(Ni_p2[0], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p2[1], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p2[2], 1.0, 1e-12);
+
+ // We should have the relation sum(Ni(x,y) = 1) for all points in the triangle
+ // We verify that relation by sampling the tetrahedron volume
+ for (double sp0p1 = 0; sp0p1 <= 1.0; sp0p1+=0.1)
+ {
+ for (double sp0p2 = 0; sp0p1 + sp0p2 <= 1.0; sp0p2+=0.1)
+ {
+ Vector3d p = p0 + sp0p1 * (p1 - p0) + sp0p2 * (p2 - p0);
+ SurgSim::Math::Vector2d p2D = m_expectedRotation.inverse()._transformVector(p).segment(0, 2);
+ std::array<double, 3> Ni_p;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ Ni_p[i] = N(i, ai, bi, ci, p2D);
+ }
+ EXPECT_NEAR(Ni_p[0] + Ni_p[1] + Ni_p[2], 1.0, 1e-10) <<
+ " for sp0p1 = " << sp0p1 << ", sp0p2 = " << sp0p2 << std::endl <<
+ " N0(x,y,z) = " << Ni_p[0] << " N1(x,y,z) = " << Ni_p[1] << " N2(x,y,z) = " << Ni_p[2];
+ }
+ }
+}
+
+TEST_F(Fem2DElementTriangleTests, PlateShapeFunctionsTest)
+{
+ std::shared_ptr<MockFem2DElement> tri = getElement();
+
+ // Shape function N1 weigth point 0 (parametric coordinate 0 0)
+ EXPECT_DOUBLE_EQ(1.0, tri->batozN1(0.0, 0.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN1(1.0, 0.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN1(0.0, 1.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN1(0.5, 0.5));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN1(0.0, 0.5));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN1(0.5, 0.0));
+
+ // Shape function N2 weigth point 1 (parametric coordinate 1 0)
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN2(0.0, 0.0));
+ EXPECT_DOUBLE_EQ(1.0, tri->batozN2(1.0, 0.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN2(0.0, 1.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN2(0.5, 0.5));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN2(0.0, 0.5));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN2(0.5, 0.0));
+
+ // Shape function N3 weigth point 2 (parametric coordinate 0 1)
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN3(0.0, 0.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN3(1.0, 0.0));
+ EXPECT_DOUBLE_EQ(1.0, tri->batozN3(0.0, 1.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN3(0.5, 0.5));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN3(0.0, 0.5));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN3(0.5, 0.0));
+
+ // Shape function N4 weigth point 4 (mid-point 12) (parametric coordinate 0.5 0.5)
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN4(0.0, 0.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN4(1.0, 0.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN4(0.0, 1.0));
+ EXPECT_DOUBLE_EQ(1.0, tri->batozN4(0.5, 0.5));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN4(0.0, 0.5));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN4(0.5, 0.0));
+
+ // Shape function N5 weigth point 5 (mid-point 20) (parametric coordinate 0.0 0.5)
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN5(0.0, 0.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN5(1.0, 0.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN5(0.0, 1.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN5(0.5, 0.5));
+ EXPECT_DOUBLE_EQ(1.0, tri->batozN5(0.0, 0.5));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN5(0.5, 0.0));
+
+ // Shape function N6 weigth point 6 (mid-point 01) (parametric coordinate 0.5 0.0)
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN6(0.0, 0.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN6(1.0, 0.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN6(0.0, 1.0));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN6(0.5, 0.5));
+ EXPECT_DOUBLE_EQ(0.0, tri->batozN6(0.0, 0.5));
+ EXPECT_DOUBLE_EQ(1.0, tri->batozN6(0.5, 0.0));
+
+ // We should have the relation sum(Ni(xi, neta) = 1) for all points in the triangle
+ for (double xi = 0.0; xi <= 1.0; xi += 0.1)
+ {
+ for (double neta = 0.0; xi + neta <= 1.0; neta += 0.1)
+ {
+ EXPECT_DOUBLE_EQ(1.0, tri->batozN1(xi, neta) + tri->batozN2(xi, neta) + tri->batozN3(xi, neta) + \
+ tri->batozN4(xi, neta) + tri->batozN5(xi, neta) + tri->batozN6(xi, neta)) <<
+ "For (xi = " << xi << ", neta = " << neta << "), " << std::endl <<
+ " N1 = " << tri->batozN1(xi, neta) << std::endl <<
+ " N2 = " << tri->batozN2(xi, neta) << std::endl <<
+ " N3 = " << tri->batozN3(xi, neta) << std::endl <<
+ " N4 = " << tri->batozN4(xi, neta) << std::endl <<
+ " N5 = " << tri->batozN5(xi, neta) << std::endl <<
+ " N6 = " << tri->batozN6(xi, neta) << std::endl <<
+ " N1+N2+N3+N4+N5+N6 = " <<
+ tri->batozN1(xi, neta) + tri->batozN2(xi, neta) + tri->batozN3(xi, neta) +
+ tri->batozN4(xi, neta) + tri->batozN5(xi, neta) + tri->batozN6(xi, neta);
+ }
+ }
+}
+
+TEST_F(Fem2DElementTriangleTests, StiffnessMatrixTest)
+{
+ std::shared_ptr<MockFem2DElement> tri = getElement();
+
+ Eigen::Matrix<double, 18 ,18> expectedLocalStiffness;
+ getExpectedLocalStiffnessMatrix(expectedLocalStiffness);
+ EXPECT_TRUE(tri->getLocalStiffnessMatrix().isApprox(expectedLocalStiffness)) <<
+ "KLocal = " << std::endl << tri->getLocalStiffnessMatrix() << std::endl <<
+ "KLocal expected = " << std::endl << expectedLocalStiffness << std::endl;
+
+ Eigen::Matrix<double, 18 ,18> R0 = tri->getInitialRotationTimes6();
+ EXPECT_TRUE(tri->getGlobalStiffnessMatrix().isApprox(R0 * expectedLocalStiffness * R0.transpose())) <<
+ "R0 = " << std::endl << R0 << std::endl <<
+ "KGlobal = " << std::endl << tri->getLocalStiffnessMatrix() << std::endl <<
+ "KGlobal expected = " << std::endl << expectedLocalStiffness << std::endl;
+}
+
+TEST_F(Fem2DElementTriangleTests, MassMatrixTest)
+{
+ std::shared_ptr<MockFem2DElement> tri = getElement();
+
+ // We analytically test the 3x3 (x y z) component
+ // m = rho.A(123).t/12.0.[2 1 1]
+ // [1 2 1]
+ // [1 1 2]
+ double mass = m_rho * m_A * m_thickness;
+ Matrix33d M;
+ M.setConstant(mass / 12.0);
+ M.diagonal().setConstant(mass / 6.0);
+ EXPECT_TRUE(tri->getLocalMassMatrix().block(0,0,3,3).isApprox(M));
+
+ // And use a hard-coded mass matrix for expected matrix
+ Eigen::Matrix<double, 18, 18> expectedMassMatrix;
+ getExpectedLocalMassMatrix(expectedMassMatrix);
+ EXPECT_TRUE(tri->getLocalMassMatrix().isApprox(expectedMassMatrix));
+ Eigen::Matrix<double, 18 ,18> R0 = tri->getInitialRotationTimes6();
+ EXPECT_TRUE(tri->getGlobalMassMatrix().isApprox(R0 * expectedMassMatrix * R0.transpose()));
+}
+
+TEST_F(Fem2DElementTriangleTests, ForceAndMatricesAPITest)
+{
+ using SurgSim::Math::addSubMatrix;
+
+ std::shared_ptr<MockFem2DElement> tri = getElement();
+
+ const size_t numDof = 6 * m_restState.getNumNodes();
+ SurgSim::Math::Vector forceVector(numDof);
+ SurgSim::Math::Vector ones(numDof);
+ SurgSim::Math::Matrix massMatrix(numDof, numDof);
+ SurgSim::Math::Matrix dampingMatrix(numDof, numDof);
+ SurgSim::Math::Matrix stiffnessMatrix(numDof, numDof);
+ SurgSim::Math::Matrix expectedMassMatrix(numDof, numDof);
+ SurgSim::Math::Matrix expectedDampingMatrix(numDof, numDof);
+ SurgSim::Math::Matrix expectedStiffnessMatrix(numDof, numDof);
+
+ // Assemble manually the expectedStiffnessMatrix
+ Eigen::Matrix<double, 18 ,18> R0 = tri->getInitialRotationTimes6();
+ Eigen::Matrix<double, 18, 18> expected18x18StiffnessMatrix;
+ getExpectedLocalStiffnessMatrix(expected18x18StiffnessMatrix);
+ expectedStiffnessMatrix.setZero();
+ addSubMatrix(R0 * expected18x18StiffnessMatrix * R0.transpose(), tri->getNodeIds(), 6, &expectedStiffnessMatrix);
+
+ // Assemble manually the expectedMassMatrix
+ Eigen::Matrix<double, 18, 18> expected18x18MassMatrix;
+ getExpectedLocalMassMatrix(expected18x18MassMatrix);
+ expectedMassMatrix.setZero();
+ addSubMatrix(R0 * expected18x18MassMatrix * R0.transpose(), tri->getNodeIds(), 6, &expectedMassMatrix);
+
+ forceVector.setZero();
+ massMatrix.setZero();
+ dampingMatrix.setZero();
+ stiffnessMatrix.setZero();
+
+ // No force should be produced when in rest state (x = x0) => F = K.(x-x0) = 0
+ tri->addForce(m_restState, &forceVector);
+ EXPECT_TRUE(forceVector.isZero());
+
+ tri->addMass(m_restState, &massMatrix);
+ EXPECT_TRUE(massMatrix.isApprox(expectedMassMatrix));
+
+ tri->addDamping(m_restState, &dampingMatrix);
+ EXPECT_TRUE(dampingMatrix.isZero());
+
+ tri->addStiffness(m_restState, &stiffnessMatrix);
+ EXPECT_TRUE(stiffnessMatrix.isApprox(expectedStiffnessMatrix));
+
+ forceVector.setZero();
+ massMatrix.setZero();
+ dampingMatrix.setZero();
+ stiffnessMatrix.setZero();
+
+ tri->addFMDK(m_restState, &forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
+ EXPECT_TRUE(forceVector.isZero());
+ EXPECT_TRUE(massMatrix.isApprox(expectedMassMatrix));
+ EXPECT_TRUE(dampingMatrix.isZero());
+ EXPECT_TRUE(stiffnessMatrix.isApprox(expectedStiffnessMatrix));
+
+ // Test addMatVec API with Mass component only
+ forceVector.setZero();
+ ones.setOnes();
+ tri->addMatVec(m_restState, 1.0, 0.0, 0.0, ones, &forceVector);
+ for (size_t rowId = 0; rowId < numDof; rowId++)
+ {
+ SCOPED_TRACE("Test addMatVec API with Mass component only");
+ EXPECT_NEAR(expectedMassMatrix.row(rowId).sum(), forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with Damping component only
+ forceVector.setZero();
+ tri->addMatVec(m_restState, 0.0, 1.0, 0.0, ones, &forceVector);
+ for (size_t rowId = 0; rowId < numDof; rowId++)
+ {
+ SCOPED_TRACE("Test addMatVec API with Damping component only");
+ EXPECT_NEAR(0.0, forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with Stiffness component only
+ forceVector.setZero();
+ tri->addMatVec(m_restState, 0.0, 0.0, 1.0, ones, &forceVector);
+ for (size_t rowId = 0; rowId < numDof; rowId++)
+ {
+ SCOPED_TRACE("Test addMatVec API with Stiffness component only");
+ EXPECT_NEAR(expectedStiffnessMatrix.row(rowId).sum(), forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with mix Mass/Damping/Stiffness components
+ forceVector.setZero();
+ tri->addMatVec(m_restState, 1.0, 2.0, 3.0, ones, &forceVector);
+ for (size_t rowId = 0; rowId < numDof; rowId++)
+ {
+ SCOPED_TRACE("Test addMatVec API with mix Mass/Damping/Stiffness components");
+ double expectedCoef = 1.0 * expectedMassMatrix.row(rowId).sum() +
+ 3.0 * expectedStiffnessMatrix.row(rowId).sum();
+ EXPECT_NEAR(expectedCoef, forceVector[rowId], epsilon * 10);
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/Fem2DMechanicalValidationTests.cpp b/SurgSim/Physics/UnitTests/Fem2DMechanicalValidationTests.cpp
new file mode 100644
index 0000000..f3fcfe4
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem2DMechanicalValidationTests.cpp
@@ -0,0 +1,698 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Fem2DMechanicalValidationTests.cpp
+/// This file tests the mechanical behaviors of the Fem2DRepresentation.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+
+using SurgSim::Math::Matrix;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+
+static const double inchToMeter = 0.0254;
+static const double ksiToPascal = 6894757.293; // Kip.in^-2 -> Pa = N.m^-2
+static const double kipToNewton = 4448.221615; // Kip -> N
+
+const double epsilonCantilever1 = 1e-8;
+const double epsilonCantilever2 = 1e-8;
+const double epsilonPlateWithSemiCirculatHole = 5e-5;
+const double epsilonPlateBending = 2e-7;
+
+}
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Mechanical validation tests class
+/// Our validation tests are based on the following Thesis:
+/// "Development of Membrane, Plate and Flat Shell Elements in Java"
+/// Kaushalkumar Kansara, May 2004
+/// In this thesis, the same formulation has been chosen to simulate the plate bending deformation,
+/// based on Batoz et al. work. Also, a large section of the thesis is dedicated to mechanical validation with
+/// all necessary details to reproduce the simulation and comparison have been done against a commercial finite
+/// element analysis program (SAP 2000).
+class Fem2DMechanicalValidationTests : public ::testing::Test
+{
+private:
+ std::shared_ptr<Fem2DRepresentation> m_fem;
+
+ // Physical properties
+ double m_nu;
+ double m_E;
+
+ // Geometric properties
+ double m_thickness;
+
+ // Force and displacement vectors
+ SurgSim::Math::Vector m_F, m_U;
+
+protected:
+ virtual void SetUp() override
+ {
+ m_fem = std::make_shared<Fem2DRepresentation>("Fem2D");
+ }
+
+ void applyBoundaryConditions()
+ {
+ auto boundaryConditions = m_fem->getInitialState()->getBoundaryConditions();
+ for (auto bc = boundaryConditions.begin(); bc != boundaryConditions.end(); bc++)
+ {
+ m_F[*bc] = 0.0;
+ }
+ }
+
+public:
+ void setNodePositions(const std::vector<Vector3d>& nodes, const std::vector<size_t>& fixedNodes)
+ {
+ const size_t numDofPerNode = m_fem->getNumDofPerNode();
+ std::shared_ptr<SurgSim::Math::OdeState> state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(numDofPerNode, nodes.size());
+ for (size_t nodeId = 0; nodeId < nodes.size(); nodeId++)
+ {
+ state->getPositions().segment<3>(numDofPerNode * nodeId) = nodes[nodeId];
+ }
+ for (auto fixedNodeId = fixedNodes.begin(); fixedNodeId != fixedNodes.end(); fixedNodeId++)
+ {
+ state->addBoundaryCondition(*fixedNodeId);
+ }
+ m_fem->setInitialState(state);
+ }
+
+ void addTriangle(const std::array<size_t, 3>& t, double youngModulus, double poissonRatio, double thickness)
+ {
+ std::shared_ptr<Fem2DElementTriangle> triangle = std::make_shared<Fem2DElementTriangle>(t);
+ triangle->setYoungModulus(youngModulus);
+ triangle->setPoissonRatio(poissonRatio);
+ triangle->setMassDensity(1.0); // In static mode, the mass density is not used, but it needs to be non null to
+ // pass the initialize validation
+ triangle->setThickness(thickness);
+ m_fem->addFemElement(triangle);
+ }
+
+ // Make sure you call setNodePositions first to initialize the initialState
+ void addPunctualLoad(size_t nodeId, const Vector3d& f)
+ {
+ // Apply load at extremity
+ if (m_F.size() != static_cast<Vector::Index>(m_fem->getInitialState()->getNumDof()))
+ {
+ m_F.resize(m_fem->getInitialState()->getNumDof());
+ m_F.setZero();
+ }
+ m_F.segment(m_fem->getNumDofPerNode() * nodeId, 3) = f;
+ }
+
+ // Make sure you call setNodePositions first to initialize the initialState
+ void addUniformSurfaceLoad(const Vector3d& forceInNewtonPerSquareMeter)
+ {
+ std::vector<Vector3d> forces;
+ const size_t numNodes = m_fem->getInitialState()->getNumNodes();
+
+ forces.resize(numNodes);
+ for (auto f = std::begin(forces); f != std::end(forces); f++)
+ {
+ (*f).setZero();
+ }
+ for (size_t triangleId = 0; triangleId < m_fem->getNumFemElements(); triangleId++)
+ {
+ auto triangle = std::static_pointer_cast<Fem2DElementTriangle>(m_fem->getFemElement(triangleId));
+ size_t nodeId0 = triangle->getNodeId(0);
+ size_t nodeId1 = triangle->getNodeId(1);
+ size_t nodeId2 = triangle->getNodeId(2);
+ const Vector3d A = m_fem->getInitialState()->getPosition(nodeId0);
+ const Vector3d B = m_fem->getInitialState()->getPosition(nodeId1);
+ const Vector3d C = m_fem->getInitialState()->getPosition(nodeId2);
+ const double Area = ((B - A).cross(C - A)).norm() / 2.0;
+ Vector3d f = Area * forceInNewtonPerSquareMeter;
+ // Uniform distribution, so the resulting force f is to be applied at the triangle center of mass:
+ forces[nodeId0] += f / 3.0;
+ forces[nodeId1] += f / 3.0;
+ forces[nodeId2] += f / 3.0;
+ }
+
+ for (size_t nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ addPunctualLoad(nodeId, forces[nodeId]);
+ }
+
+ applyBoundaryConditions();
+ }
+
+ void solve()
+ {
+ m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ Matrix K = m_fem->computeK(*(m_fem->getCurrentState()));
+ m_fem->getCurrentState()->applyBoundaryConditionsToMatrix(&K);
+ m_fem->getCurrentState()->applyBoundaryConditionsToVector(&m_F);
+ m_U = K.inverse() * m_F;
+ }
+
+ double getUx(size_t nodeId) const
+ {
+ return m_U[m_fem->getNumDofPerNode() * nodeId];
+ }
+
+ double getUy(size_t nodeId) const
+ {
+ return m_U[m_fem->getNumDofPerNode() * nodeId + 1];
+ }
+
+ double getUz(size_t nodeId) const
+ {
+ return m_U[m_fem->getNumDofPerNode() * nodeId + 2];
+ }
+};
+
+TEST_F(Fem2DMechanicalValidationTests, MembraneCantileverTest1)
+{
+ const double youngModulus = 30000 * ksiToPascal;
+ const double poissonRatio = 0.25;
+ const double thickness = 1.0 * inchToMeter;
+ const double L = 48.0 * inchToMeter;
+ const double h = 12.0 * inchToMeter;
+
+ std::vector<Vector3d> nodes;
+ nodes.push_back(Vector3d(-L / 2.0, - h / 2.0, 0.0));
+ nodes.push_back(Vector3d(-L / 4.0, - h / 2.0, 0.0));
+ nodes.push_back(Vector3d( 0.0, - h / 2.0, 0.0));
+ nodes.push_back(Vector3d( L / 4.0, - h / 2.0, 0.0));
+ nodes.push_back(Vector3d( L / 2.0, - h / 2.0, 0.0));
+ nodes.push_back(Vector3d(-L / 2.0, h / 2.0, 0.0));
+ nodes.push_back(Vector3d(-L / 4.0, h / 2.0, 0.0));
+ nodes.push_back(Vector3d( 0.0, h / 2.0, 0.0));
+ nodes.push_back(Vector3d( L / 4.0, h / 2.0, 0.0));
+ nodes.push_back(Vector3d( L / 2.0, h / 2.0, 0.0));
+
+ std::vector<size_t> fixedNodes;
+ fixedNodes.push_back(0);
+ fixedNodes.push_back(5);
+
+ setNodePositions(nodes, fixedNodes);
+
+ const int numTriangles = 8;
+ const std::array<std::array<size_t, 3>, numTriangles> trianglesNodeIds =
+ {{
+ {{0, 6, 5}}, {{0, 1, 6}}, {{1, 7, 6}}, {{1, 2, 7}},
+ {{2, 8, 7}}, {{2, 3, 8}}, {{3, 9, 8}}, {{3, 4, 9}}
+ }};
+
+ for (size_t triangleId = 0; triangleId < numTriangles; triangleId++)
+ {
+ addTriangle(trianglesNodeIds[triangleId], youngModulus, poissonRatio, thickness);
+ }
+
+ addPunctualLoad(4, Vector3d(0, 20.0 * kipToNewton, 0));
+ addPunctualLoad(9, Vector3d(0, 20.0 * kipToNewton, 0));
+
+ solve();
+
+ EXPECT_NEAR(-0.014159 * inchToMeter, getUx(9), epsilonCantilever1);
+ EXPECT_NEAR( 0.090347 * inchToMeter, getUy(9), epsilonCantilever1);
+ EXPECT_NEAR(-0.010825 * inchToMeter, getUx(7), epsilonCantilever1);
+ EXPECT_NEAR( 0.030403 * inchToMeter, getUy(7), epsilonCantilever1);
+}
+
+TEST_F(Fem2DMechanicalValidationTests, MembraneCantileverTest2)
+{
+ const double youngModulus = 30000 * ksiToPascal;
+ const double poissonRatio = 0.25;
+ const double thickness = 1.0 * inchToMeter;
+ const double L = 48.0 * inchToMeter;
+ const double h = 12.0 * inchToMeter;
+
+ std::vector<Vector3d> nodes;
+ nodes.push_back(Vector3d(-L / 2.0, - h / 2.0, 0.0)); // 0
+ nodes.push_back(Vector3d(-L / 4.0, - h / 2.0, 0.0)); // 1
+ nodes.push_back(Vector3d( 0.0, - h / 2.0, 0.0)); // 2
+ nodes.push_back(Vector3d( L / 4.0, - h / 2.0, 0.0)); // 3
+ nodes.push_back(Vector3d( L / 2.0, - h / 2.0, 0.0)); // 4
+ nodes.push_back(Vector3d(-L / 2.0, h / 2.0, 0.0)); // 5
+ nodes.push_back(Vector3d(-L / 4.0, h / 2.0, 0.0)); // 6
+ nodes.push_back(Vector3d( 0.0, h / 2.0, 0.0)); // 7
+ nodes.push_back(Vector3d( L / 4.0, h / 2.0, 0.0)); // 8
+ nodes.push_back(Vector3d( L / 2.0, h / 2.0, 0.0)); // 9
+
+ // Subdivision
+ nodes.push_back((nodes[0] + nodes[1]) * 0.5); // 10
+ nodes.push_back((nodes[0] + nodes[6]) * 0.5); // 11
+ nodes.push_back((nodes[0] + nodes[5]) * 0.5); // 12
+ nodes.push_back((nodes[5] + nodes[6]) * 0.5); // 13
+ nodes.push_back((nodes[1] + nodes[6]) * 0.5); // 14
+ nodes.push_back((nodes[1] + nodes[2]) * 0.5); // 15
+ nodes.push_back((nodes[1] + nodes[7]) * 0.5); // 16
+ nodes.push_back((nodes[6] + nodes[7]) * 0.5); // 17
+ nodes.push_back(Vector3d::Zero()); // 18
+ nodes.push_back((nodes[2] + nodes[3]) * 0.5); // 19
+ nodes.push_back((nodes[2] + nodes[8]) * 0.5); // 20
+ nodes.push_back((nodes[7] + nodes[8]) * 0.5); // 21
+ nodes.push_back((nodes[3] + nodes[8]) * 0.5); // 22
+ nodes.push_back((nodes[3] + nodes[4]) * 0.5); // 23
+ nodes.push_back((nodes[3] + nodes[9]) * 0.5); // 24
+ nodes.push_back((nodes[8] + nodes[9]) * 0.5); // 25
+ nodes.push_back((nodes[4] + nodes[9]) * 0.5); // 26
+
+ std::vector<size_t> fixedNodes;
+ fixedNodes.push_back(0);
+ fixedNodes.push_back(5);
+ fixedNodes.push_back(12);
+
+ setNodePositions(nodes, fixedNodes);
+
+ const int numTriangles = 32;
+ const std::array<std::array<size_t, 3>, numTriangles> trianglesNodeIds =
+ {{
+ {{0, 10, 12}}, {{10, 11, 12}}, {{12, 11, 13}}, {{12, 13, 5}},
+ {{10, 1, 11}}, {{1, 14, 11}}, {{11, 14, 6}}, {{11, 6, 13}},
+ {{1, 15, 14}}, {{15, 16, 14}}, {{14, 16, 17}}, {{14, 17, 6}},
+ {{15, 2, 16}}, {{2, 18, 16}}, {{16, 18, 7}}, {{16, 7, 17}},
+ {{2, 19, 18}}, {{19, 20, 18}}, {{18, 20, 21}}, {{18, 21, 7}},
+ {{19, 3, 20}}, {{3, 22, 20}}, {{20, 22, 8}}, {{20, 8, 21}},
+ {{3, 23, 22}}, {{23, 24, 22}}, {{22, 24, 25}}, {{22, 25, 8}},
+ {{23, 4, 24}}, {{4, 26, 24}}, {{24, 26, 9}}, {{24, 9, 25}}
+ }};
+
+ for (size_t triangleId = 0; triangleId < numTriangles; triangleId++)
+ {
+ addTriangle(trianglesNodeIds[triangleId], youngModulus, poissonRatio, thickness);
+ }
+
+ addPunctualLoad( 4, Vector3d(0, 6.67 * kipToNewton, 0));
+ addPunctualLoad( 9, Vector3d(0, 6.67 * kipToNewton, 0));
+ addPunctualLoad(26, Vector3d(0, 26.67 * kipToNewton, 0));
+
+ solve();
+
+ EXPECT_NEAR(-0.034271 * inchToMeter, getUx(9), epsilonCantilever2);
+ EXPECT_NEAR( 0.194456 * inchToMeter, getUy(9), epsilonCantilever2);
+ EXPECT_NEAR(-0.025605 * inchToMeter, getUx(7), epsilonCantilever2);
+ EXPECT_NEAR( 0.062971 * inchToMeter, getUy(7), epsilonCantilever2);
+}
+
+/// Generic algorithm to define the triangles in between 2 arrays of consecutive node indices:
+/// beginIndex2
+/// *---1---2---3---4 array2
+/// | \ | \ | \ | \ |
+/// *---1---2---3---4 array1
+/// beginIndex1
+template <size_t M>
+void defineTriangleStrips(size_t beginIndex1, size_t beginIndex2, size_t number,
+ std::array<std::array<size_t, 3>, M>* triangleLists, size_t* triangleId)
+{
+ for (size_t i = 0; i < number - 1; i++)
+ {
+ std::array<size_t, 3> triangle1 = {{beginIndex1 + i, beginIndex1 + i + 1, beginIndex2 + i}};
+ std::array<size_t, 3> triangle2 = {{beginIndex1 + i + 1, beginIndex2 + i + 1, beginIndex2 + i}};
+ (*triangleLists)[(*triangleId)++] = triangle1;
+ (*triangleLists)[(*triangleId)++] = triangle2;
+ }
+}
+
+/// Helper function to define the nodes of the membrane plate with semi-circular hole test
+/// In this test, nodes are defined around concentric circles. This function adds the nodes that are along the same
+/// axis (going from the most interior circle to the most exterior circle) for a given angle.
+/// \param angle The angle for which the nodes will be computed and added
+/// \param numNodes The number of nodes to add along this axis (defined by the angle)
+/// \param L The length of the plate
+/// \param radius The radius of the most internal circle
+/// \param [in,out] nodes The list of nodes in which the new nodes will be added
+/// \note The spacing between 2 consecutive nodes is constant equal to (L / 2.0 - radius) / 5.0
+void membranePlateWithSemiCircularHoleAddNodesForAngle(double angle, size_t numNodes, double L, double radius,
+ std::vector<Vector3d>* nodes)
+{
+ for (size_t nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ double distance = radius + nodeId * (L / 2.0 - radius) / 5.0;
+ (*nodes).push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0));
+ }
+}
+
+TEST_F(Fem2DMechanicalValidationTests, MembranePlateWithSemiCircularHoleTest)
+{
+ const double youngModulus = 30000 * ksiToPascal;
+ const double poissonRatio = 0.3;
+ const double thickness = 0.45 * inchToMeter;
+ const double radius = 3.0 * inchToMeter;
+ const double L = 16.0 * inchToMeter;
+ const double h = 6.0 * inchToMeter;
+
+ double startAngle = -M_PI / 2.0;
+ double deltaAngle = M_PI / 12.0;
+
+ std::vector<Vector3d> nodes;
+ double angle, distance;
+
+ // Node 0..5
+ angle = startAngle + 0.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 6, L, radius, &nodes);
+
+ // Node 6..10
+ angle = startAngle + 1.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 5, L, radius, &nodes);
+ // Node 11 (we have distance.sin(angle) = -L/2)
+ distance = - L / (2.0 * sin(angle));
+ nodes.push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0)); // 11
+
+ // Node 12..17
+ angle = startAngle + 2.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 6, L, radius, &nodes);
+ // Node 18 (we have distance.sin(angle) = -L/2)
+ distance = - L / (2.0 * sin(angle));
+ nodes.push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0)); // 18
+
+ // Node 19..23
+ angle = startAngle + 3.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 5, L, radius, &nodes);
+ // Node 24 (we have distance.cos(angle) = h)
+ distance = h / cos(angle);
+ nodes.push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0)); // 24
+ // Node 25 (corner)
+ nodes.push_back(Vector3d(h, -L / 2.0, 0.0)); // 25
+
+ // Node 26..29
+ angle = startAngle + 4.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 4, L, radius, &nodes);
+ // Node 30 (we have distance.cos(angle) = h)
+ distance = h / cos(angle);
+ nodes.push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0)); // 30
+
+ // Node 31..33
+ angle = startAngle + 5.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 3, L, radius, &nodes);
+ // Node 34 (we have distance.cos(angle) = h)
+ distance = h / cos(angle);
+ nodes.push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0)); // 34
+
+ // Node 35..38
+ angle = startAngle + 6.0 * deltaAngle; // (=0)
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 4, L, radius, &nodes);
+
+ // Node 39..41
+ angle = startAngle + 7.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 3, L, radius, &nodes);
+ // Node 42 (we have distance.cos(angle) = h)
+ distance = h / cos(angle);
+ nodes.push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0)); // 42
+
+ // Node 43..46
+ angle = startAngle + 8.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 4, L, radius, &nodes);
+ // Node 47 (we have distance.cos(angle) = h)
+ distance = h / cos(angle);
+ nodes.push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0)); // 47
+
+ // Node 48..52
+ angle = startAngle + 9.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 5, L, radius, &nodes);
+ // Node 53 (we have distance.cos(angle) = h)
+ distance = h / cos(angle);
+ nodes.push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0)); // 53
+ // Node 54 (corner)
+ nodes.push_back(Vector3d(h, L / 2.0, 0.0)); // 54
+
+ // Node 55..60
+ angle = startAngle + 10.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 6, L, radius, &nodes);
+ // Node 61 (we have distance.sin(angle) = L / 2)
+ distance = L / (2.0 * sin(angle));
+ nodes.push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0)); // 61
+
+ // Node 62..66
+ angle = startAngle + 11.0 * deltaAngle;
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 5, L, radius, &nodes);
+ // Node 67 (we have distance.sin(angle) = L / 2)
+ distance = L / (2.0 * sin(angle));
+ nodes.push_back(Vector3d(distance * cos(angle), distance * sin(angle), 0.0)); // 67
+
+ // Node 69..73
+ angle = startAngle + 12.0 * deltaAngle; // (= M_PI / 2)
+ membranePlateWithSemiCircularHoleAddNodesForAngle(angle, 6, L, radius, &nodes);
+
+ std::vector<size_t> fixedNodes;
+ fixedNodes.push_back(54);
+ fixedNodes.push_back(61);
+ fixedNodes.push_back(67);
+ fixedNodes.push_back(73);
+
+ setNodePositions(nodes, fixedNodes);
+
+ const int numTriangles = 110;
+ std::array<std::array<size_t, 3>, numTriangles> trianglesNodeIds;
+ size_t triangleId = 0;
+ defineTriangleStrips(0, 6, 6, &trianglesNodeIds, &triangleId);
+ defineTriangleStrips(6, 12, 6, &trianglesNodeIds, &triangleId);
+ {
+ std::array<size_t, 3> triangle = {{11, 18, 17}};
+ trianglesNodeIds[triangleId++] = triangle;
+ }
+ defineTriangleStrips(12, 19, 7, &trianglesNodeIds, &triangleId);
+ defineTriangleStrips(19, 26, 5, &trianglesNodeIds, &triangleId);
+ {
+ std::array<size_t, 3> triangle = {{23, 24, 30}};
+ trianglesNodeIds[triangleId++] = triangle;
+ }
+ defineTriangleStrips(26, 31, 4, &trianglesNodeIds, &triangleId);
+ {
+ std::array<size_t, 3> triangle = {{29, 30, 34}};
+ trianglesNodeIds[triangleId++] = triangle;
+ }
+ defineTriangleStrips(31, 35, 4, &trianglesNodeIds, &triangleId);
+ defineTriangleStrips(35, 39, 4, &trianglesNodeIds, &triangleId);
+ defineTriangleStrips(39, 43, 4, &trianglesNodeIds, &triangleId);
+ {
+ std::array<size_t, 3> triangle = {{42, 47, 46}};
+ trianglesNodeIds[triangleId++] = triangle;
+ }
+ defineTriangleStrips(43, 48, 5, &trianglesNodeIds, &triangleId);
+ {
+ std::array<size_t, 3> triangle = {{47, 53, 52}};
+ trianglesNodeIds[triangleId++] = triangle;
+ }
+ defineTriangleStrips(48, 55, 7, &trianglesNodeIds, &triangleId);
+ defineTriangleStrips(55, 62, 6, &trianglesNodeIds, &triangleId);
+ {
+ std::array<size_t, 3> triangle = {{60, 61, 67}};
+ trianglesNodeIds[triangleId++] = triangle;
+ }
+ defineTriangleStrips(62, 68, 6, &trianglesNodeIds, &triangleId);
+
+ for (size_t triangleId = 0; triangleId < numTriangles; triangleId++)
+ {
+ addTriangle(trianglesNodeIds[triangleId], youngModulus, poissonRatio, thickness);
+ }
+
+ addPunctualLoad( 5, Vector3d(0, -1.0 * kipToNewton, 0));
+ addPunctualLoad(11, Vector3d(0, -1.0 * kipToNewton, 0));
+ addPunctualLoad(18, Vector3d(0, -1.0 * kipToNewton, 0));
+ addPunctualLoad(25, Vector3d(0, -1.0 * kipToNewton, 0));
+
+ solve();
+
+ EXPECT_NEAR( 0.001545 * inchToMeter, getUx(0), epsilonPlateWithSemiCirculatHole);
+ EXPECT_NEAR(-0.003132 * inchToMeter, getUy(0), epsilonPlateWithSemiCirculatHole);
+ EXPECT_NEAR( 0.004213 * inchToMeter, getUx(5), epsilonPlateWithSemiCirculatHole);
+ EXPECT_NEAR(-0.003355 * inchToMeter, getUy(5), epsilonPlateWithSemiCirculatHole);
+}
+
+TEST_F(Fem2DMechanicalValidationTests, PlateBendingSquarePlateMeshPatternATest)
+{
+ const double youngModulus = 3600 * ksiToPascal;
+ const double poissonRatio = 0.2;
+ const double thickness = 6.0 * inchToMeter;
+ const double L = 144.0 * inchToMeter;
+ const double deltaL = L / 8.0;
+
+ std::vector<Vector3d> nodes;
+ std::vector<size_t> fixedNodes;
+ for (size_t Y = 0; Y < 9; Y++)
+ {
+ for (size_t X = 0; X < 9; X++)
+ {
+ nodes.push_back(Vector3d(-L / 2.0 + deltaL * X, - L / 2.0 + deltaL * Y, 0.0));
+ }
+ }
+ for (size_t i = 0; i < 9; i++)
+ {
+ // 1st edge along X
+ fixedNodes.push_back(i); // Nodes 0..8
+ // last edge along X
+ fixedNodes.push_back(9 * 8 + i); // Nodes 72..80
+ }
+ for (size_t i = 1; i < 8; i++)
+ {
+ // 1st edge along Y
+ fixedNodes.push_back(9 * i); // Nodes 9, 18, 27, 36, 45, 54, 63
+ // last edge along Y
+ fixedNodes.push_back(9 * i + 8); // Nodes 17, 26, 35, 44, 53, 62
+ }
+ setNodePositions(nodes, fixedNodes);
+
+ const int numTriangles = 128;
+ std::array<std::array<size_t, 3>, numTriangles> trianglesNodeIds;
+ size_t triangleId = 0;
+ for(size_t Y = 0; Y < 8; Y++)
+ {
+ for(size_t X = 0; X < 8; X++)
+ {
+ std::array<size_t, 3> triangle1 = {{Y * 9 + X, Y * 9 + (X + 1), (Y + 1) * 9 + (X + 1)}};
+ trianglesNodeIds[triangleId++] = triangle1;
+ std::array<size_t, 3> triangle2 = {{Y * 9 + X, (Y + 1) * 9 + (X + 1), (Y + 1) * 9 + X}};
+ trianglesNodeIds[triangleId++] = triangle2;
+ }
+ }
+ for (size_t triangleId = 0; triangleId < numTriangles; triangleId++)
+ {
+ addTriangle(trianglesNodeIds[triangleId], youngModulus, poissonRatio, thickness);
+ }
+
+ addUniformSurfaceLoad(Vector3d(0.0, 0.0, -0.1) * ksiToPascal);
+
+ solve();
+
+ EXPECT_NEAR(-0.82920 * inchToMeter, getUz(40), epsilonPlateBending);
+ EXPECT_NEAR(-0.304909 * inchToMeter, getUz(20), epsilonPlateBending);
+}
+
+TEST_F(Fem2DMechanicalValidationTests, PlateBendingSquarePlateMeshPatternBTest)
+{
+ const double youngModulus = 3600 * ksiToPascal;
+ const double poissonRatio = 0.2;
+ const double thickness = 6.0 * inchToMeter;
+ const double L = 144.0 * inchToMeter;
+ const double deltaL = L / 8.0;
+
+ std::vector<Vector3d> nodes;
+ std::vector<size_t> fixedNodes;
+ for (size_t Y = 0; Y < 9; Y++)
+ {
+ for (size_t X = 0; X < 9; X++)
+ {
+ nodes.push_back(Vector3d(-L / 2.0 + deltaL * X, - L / 2.0 + deltaL * Y, 0.0));
+ }
+ }
+ for (size_t i = 0; i < 9; i++)
+ {
+ // 1st edge along X
+ fixedNodes.push_back(i); // Nodes 0..8
+ // last edge along X
+ fixedNodes.push_back(9 * 8 + i); // Nodes 72..80
+ }
+ for (size_t i = 1; i < 8; i++)
+ {
+ // 1st edge along Y
+ fixedNodes.push_back(9 * i); // Nodes 9, 18, 27, 36, 45, 54, 63
+ // last edge along Y
+ fixedNodes.push_back(9 * i + 8); // Nodes 17, 26, 35, 44, 53, 62
+ }
+ setNodePositions(nodes, fixedNodes);
+
+ const int numTriangles = 128;
+ std::array<std::array<size_t, 3>, numTriangles> trianglesNodeIds;
+ size_t triangleId = 0;
+ for(size_t Y = 0; Y < 8; Y++)
+ {
+ for(size_t X = 0; X < 8; X++)
+ {
+ std::array<size_t, 3> triangle1 = {{Y * 9 + X, Y * 9 + (X + 1), (Y + 1) * 9 + X}};
+ trianglesNodeIds[triangleId++] = triangle1;
+ std::array<size_t, 3> triangle2 = {{Y * 9 + (X + 1), (Y + 1) * 9 + (X + 1), (Y + 1) * 9 + X}};
+ trianglesNodeIds[triangleId++] = triangle2;
+ }
+ }
+ for (size_t triangleId = 0; triangleId < numTriangles; triangleId++)
+ {
+ addTriangle(trianglesNodeIds[triangleId], youngModulus, poissonRatio, thickness);
+ }
+
+ addUniformSurfaceLoad(Vector3d(0.0, 0.0, -0.1) * ksiToPascal);
+
+ solve();
+
+ EXPECT_NEAR(-0.82920 * inchToMeter, getUz(40), epsilonPlateBending);
+ EXPECT_NEAR(-0.30010 * inchToMeter, getUz(20), epsilonPlateBending);
+}
+
+TEST_F(Fem2DMechanicalValidationTests, CantileverPlateTest)
+{
+ const double youngModulus = 3600 * ksiToPascal;
+ const double poissonRatio = 0.2;
+ const double thickness = 6.0 * inchToMeter;
+ const double L = 96.0 * inchToMeter;
+ const double deltaL = L / 8;
+
+ std::vector<Vector3d> nodes;
+ std::vector<size_t> fixedNodes;
+ for (size_t Y = 0; Y < 9; Y++)
+ {
+ for (size_t X = 0; X < 9; X++)
+ {
+ nodes.push_back(Vector3d(-L / 2.0 + deltaL * X, - L / 2.0 + deltaL * Y, 0.0));
+ }
+ }
+ for (size_t i = 0; i < 9; i++)
+ {
+ // 1st edge along X
+ fixedNodes.push_back(i); // Nodes 0..8
+ }
+ setNodePositions(nodes, fixedNodes);
+
+ const int numTriangles = 128;
+ std::array<std::array<size_t, 3>, numTriangles> trianglesNodeIds;
+ size_t triangleId = 0;
+ for(size_t Y = 0; Y < 8; Y++)
+ {
+ for(size_t X = 0; X < 8; X++)
+ {
+ std::array<size_t, 3> triangle1 = {{Y * 9 + X, Y * 9 + (X + 1), (Y + 1) * 9 + X}};
+ trianglesNodeIds[triangleId++] = triangle1;
+ std::array<size_t, 3> triangle2 = {{Y * 9 + (X + 1), (Y + 1) * 9 + (X + 1), (Y + 1) * 9 + X}};
+ trianglesNodeIds[triangleId++] = triangle2;
+ }
+ }
+ for (size_t triangleId = 0; triangleId < numTriangles; triangleId++)
+ {
+ addTriangle(trianglesNodeIds[triangleId], youngModulus, poissonRatio, thickness);
+ }
+
+ addUniformSurfaceLoad(Vector3d(0.0, 0.0, -0.01) * ksiToPascal);
+
+ solve();
+
+ // Expect 5% error max (in the Thesis, the author gets 4.96% error)
+ const double epsilonNode76 = 1.68787 * inchToMeter * 0.05;
+ EXPECT_NEAR(-1.68787 * inchToMeter, getUz(76), epsilonNode76);
+
+ // Expect 5% error max (in the Thesis, the author gets 4.98% error)
+ const double epsilonNode40 = 0.599412 * inchToMeter * 0.05;
+ EXPECT_NEAR(-0.599412 * inchToMeter, getUz(40), epsilonNode40);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem2DPlyReaderDelegateTests.cpp b/SurgSim/Physics/UnitTests/Fem2DPlyReaderDelegateTests.cpp
new file mode 100644
index 0000000..1e3f5ea
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem2DPlyReaderDelegateTests.cpp
@@ -0,0 +1,91 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/Fem2DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+using SurgSim::Math::Vector3d;
+using SurgSim::DataStructures::PlyReader;
+
+TEST(Fem2DRepresentationReaderTests, DelegateTest)
+{
+ auto femRepresentation = std::make_shared<Fem2DRepresentation>("Representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+
+ femRepresentation->setFilename("PlyReaderTests/Fem2D.ply");
+ ASSERT_TRUE(femRepresentation->initialize(runtime));
+
+ // Vertices
+ ASSERT_EQ(6u, femRepresentation->getNumDofPerNode());
+ ASSERT_EQ(6u * 6u, femRepresentation->getNumDof());
+
+ Vector3d vertex0(1.0, 1.0, -1.0);
+ Vector3d vertex5(0.999999, -1.000001, 1.0);
+
+ EXPECT_TRUE(vertex0.isApprox(femRepresentation->getInitialState()->getPosition(0)));
+ EXPECT_TRUE(vertex5.isApprox(femRepresentation->getInitialState()->getPosition(5)));
+
+ // Number of triangles
+ ASSERT_EQ(3u, femRepresentation->getNumFemElements());
+
+ std::array<size_t, 3> triangle0 = {0, 1, 2};
+ std::array<size_t, 3> triangle2 = {3, 4, 5};
+
+ EXPECT_TRUE(std::equal(std::begin(triangle0), std::end(triangle0),
+ std::begin(femRepresentation->getFemElement(0)->getNodeIds())));
+ EXPECT_TRUE(std::equal(std::begin(triangle2), std::end(triangle2),
+ std::begin(femRepresentation->getFemElement(2)->getNodeIds())));
+
+ // Boundary conditions
+ ASSERT_EQ(2u * 6u, femRepresentation->getInitialState()->getNumBoundaryConditions());
+
+ // Boundary condition 0 is on node 8
+ size_t boundaryNode0 = 3;
+ size_t boundaryNode1 = 2;
+
+ EXPECT_EQ(6 * boundaryNode0, femRepresentation->getInitialState()->getBoundaryConditions().at(0));
+ EXPECT_EQ(6 * boundaryNode0 + 1, femRepresentation->getInitialState()->getBoundaryConditions().at(1));
+ EXPECT_EQ(6 * boundaryNode0 + 2, femRepresentation->getInitialState()->getBoundaryConditions().at(2));
+ EXPECT_EQ(6 * boundaryNode1, femRepresentation->getInitialState()->getBoundaryConditions().at(6));
+ EXPECT_EQ(6 * boundaryNode1 + 1, femRepresentation->getInitialState()->getBoundaryConditions().at(7));
+ EXPECT_EQ(6 * boundaryNode1 + 2, femRepresentation->getInitialState()->getBoundaryConditions().at(8));
+
+ // Material
+ for (size_t i = 0; i < femRepresentation->getNumFemElements(); ++i)
+ {
+ auto fem = femRepresentation->getFemElement(i);
+ EXPECT_DOUBLE_EQ(0.2, fem->getMassDensity());
+ EXPECT_DOUBLE_EQ(0.3, fem->getPoissonRatio());
+ EXPECT_DOUBLE_EQ(0.4, fem->getYoungModulus());
+
+ auto fem2DTriganle = std::dynamic_pointer_cast<SurgSim::Physics::Fem2DElementTriangle>(fem);
+ ASSERT_NE(nullptr, fem2DTriganle);
+ EXPECT_DOUBLE_EQ(0.1, fem2DTriganle->getThickness());
+ }
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem2DRepresentationLocalizationTest.cpp b/SurgSim/Physics/UnitTests/Fem2DRepresentationLocalizationTest.cpp
new file mode 100644
index 0000000..fa37fe6
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem2DRepresentationLocalizationTest.cpp
@@ -0,0 +1,235 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem2DRepresentationLocalization.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+void addTriangle(Fem2DRepresentation *fem, std::array<size_t, 3> nodes,
+ const SurgSim::Math::OdeState& state, double thickness = 0.01,
+ double massDensity = 1.0, double poissonRatio = 0.1, double youngModulus = 1.0)
+{
+ auto element = std::make_shared<Fem2DElementTriangle>(nodes);
+ element->setThickness(thickness);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ element->initialize(state);
+ fem->addFemElement(element);
+}
+
+class Fem2DRepresentationLocalizationTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::getSubVector;
+
+ m_fem = std::make_shared<Fem2DRepresentation>("Fem2dRepresentation");
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 4);
+
+ auto& x = state->getPositions();
+ getSubVector(x, 0, 6).segment<3>(0) = Vector3d(-1.0, 0.0, 0.0);
+ getSubVector(x, 1, 6).segment<3>(0) = Vector3d( 0.0,-1.0, 0.0);
+ getSubVector(x, 2, 6).segment<3>(0) = Vector3d( 1.0, 0.0, 0.0);
+ getSubVector(x, 3, 6).segment<3>(0) = Vector3d( 0.0, 1.0, 0.0);
+
+ // Define Triangles
+ {
+ std::array<size_t, 3> nodes = {{0, 2, 1}};
+ addTriangle(m_fem.get(), nodes, *state);
+ }
+
+ {
+ std::array<size_t, 3> nodes = {{0, 3, 2}};
+ addTriangle(m_fem.get(), nodes, *state);
+ }
+
+ m_fem->setInitialState(state);
+ m_fem->setLocalActive(true);
+
+ m_validLocalPosition.index = 1;
+ m_validLocalPosition.coordinate = SurgSim::Math::Vector::Zero(3);
+ m_validLocalPosition.coordinate[0] = 0.4;
+ m_validLocalPosition.coordinate[1] = 0.6;
+
+ m_invalidIndexLocalPosition.index = 3;
+ m_validLocalPosition.coordinate = SurgSim::Math::Vector::Zero(3);
+ m_validLocalPosition.coordinate[0] = 0.4;
+ m_validLocalPosition.coordinate[1] = 0.6;
+
+ m_invalidCoordinateLocalPosition.index = 1;
+ m_invalidCoordinateLocalPosition.coordinate = SurgSim::Math::Vector::Zero(3);
+ m_invalidCoordinateLocalPosition.coordinate[0] = 0.6;
+ m_invalidCoordinateLocalPosition.coordinate[1] = 0.6;
+ }
+
+ void TearDown()
+ {
+ }
+
+ std::shared_ptr<Fem2DRepresentation> m_fem;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_validLocalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_invalidIndexLocalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_invalidCoordinateLocalPosition;
+};
+
+TEST_F(Fem2DRepresentationLocalizationTest, ConstructorTest)
+{
+ ASSERT_THROW(std::make_shared<Fem2DRepresentationLocalization>(m_fem, m_invalidIndexLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_THROW(std::make_shared<Fem2DRepresentationLocalization>(m_fem, m_invalidCoordinateLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_NO_THROW(std::make_shared<Fem2DRepresentationLocalization>(m_fem, m_validLocalPosition););
+}
+
+TEST_F(Fem2DRepresentationLocalizationTest, SetGetRepresentation)
+{
+ Fem2DRepresentationLocalization localization(m_fem, m_validLocalPosition);
+
+ EXPECT_NE(nullptr, localization.getRepresentation());
+ EXPECT_EQ(m_fem, localization.getRepresentation());
+
+ EXPECT_EQ(1u, localization.getLocalPosition().index);
+ EXPECT_TRUE(localization.getLocalPosition().coordinate.isApprox(m_validLocalPosition.coordinate));
+
+ localization.setRepresentation(nullptr);
+ EXPECT_EQ(nullptr, localization.getRepresentation());
+ localization.setRepresentation(m_fem);
+ EXPECT_EQ(m_fem, localization.getRepresentation());
+
+ SurgSim::DataStructures::IndexedLocalCoordinate m_otherValidLocalPosition;
+ m_otherValidLocalPosition.index = 0;
+ m_otherValidLocalPosition.coordinate = SurgSim::Math::Vector::Zero(3);
+ m_otherValidLocalPosition.coordinate[1] = 1.0;
+
+ localization.setLocalPosition(m_otherValidLocalPosition);
+ EXPECT_EQ(m_otherValidLocalPosition.index, localization.getLocalPosition().index);
+ EXPECT_TRUE(localization.getLocalPosition().coordinate.isApprox(m_otherValidLocalPosition.coordinate));
+}
+
+TEST_F(Fem2DRepresentationLocalizationTest, SetGetLocalization)
+{
+ using SurgSim::Math::Vector4d;
+ using SurgSim::Math::Vector3d;
+
+ {
+ SCOPED_TRACE("Uninitialized Representation");
+
+ // Uninitialized Representation
+ EXPECT_THROW(std::make_shared<Fem2DRepresentationLocalization>(nullptr, m_validLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Incorrectly formed natural coordinate");
+
+ // Incorrectly formed natural coordinate
+ auto localization = std::make_shared<Fem2DRepresentationLocalization>(m_fem, m_validLocalPosition);
+ EXPECT_THROW(localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector3d(0.89, 0.54, 0.45))),
+ SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector4d(1.0, 0.0, 0.0, 0.0))),
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Out of bounds element Id");
+
+ // Out of bounds element Id
+ auto localization = std::make_shared<Fem2DRepresentationLocalization>(m_fem, m_validLocalPosition);
+ EXPECT_THROW(localization->setLocalPosition(IndexedLocalCoordinate(6u, Vector3d(1.0, 0.0, 0.0))),
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("valid");
+
+ auto localization = std::make_shared<Fem2DRepresentationLocalization>(m_fem, m_validLocalPosition);
+ EXPECT_NO_THROW(localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector3d(0.2, 0.7, 0.1))));
+ EXPECT_EQ(1u, localization->getLocalPosition().index);
+ EXPECT_TRUE(Vector3d(0.2, 0.7, 0.1).isApprox(localization->getLocalPosition().coordinate));
+ }
+}
+
+TEST_F(Fem2DRepresentationLocalizationTest, CalculatePositionTest)
+{
+ using SurgSim::Math::Vector;
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::Vector2d;
+
+ auto localization = std::make_shared<Fem2DRepresentationLocalization>(m_fem, m_validLocalPosition);
+
+ // Test triangle 1: nodes 0, 1, 2
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE(Vector3d(-1.0, 0.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE(Vector3d(1.0, 0.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector3d(0.0, 0.0, 1.0)));
+ EXPECT_TRUE(Vector3d(0.0,-1.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test triangle 2: nodes 0, 1, 2
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE(Vector3d(-1.0, 0.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE(Vector3d( 0.0, 1.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector3d(0.0, 0.0, 1.0)));
+ EXPECT_TRUE(Vector3d( 1.0, 0.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Advanced tests
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector3d(0.31, 0.35, 0.34)));
+ // 0.31 * (-1.0, 0.0, 0.0) => (-0.31, 0.0 , 0.0)
+ // + 0.35 * ( 1.0, 0.0, 0.0) => ( 0.35, 0.0 , 0.0)
+ // + 0.34 * ( 0.0,-1.0, 0.0) => ( 0.0 ,-0.34, 0.0)
+ // = ( 0.04,-0.34, 0.0)
+ EXPECT_TRUE(Vector3d(0.04, -0.34, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector3d(0.80, 0.05, 0.15)));
+ // 0.80 * (-1.0, 0.0, 0.0) => (-0.80, 0.0 , 0.0)
+ // + 0.05 * ( 0.0, 1.0, 0.0) => ( 0.0 , 0.05, 0.0)
+ // + 0.15 * ( 1.0, 0.0, 0.0) => ( 0.15, 0.0 , 0.0)
+ // = (-0.65, 0.05, 0.0)
+ EXPECT_TRUE(Vector3d(-0.65, 0.05, 0.0).isApprox(localization->calculatePosition(), epsilon));
+}
+
+} // namespace SurgSim
+} // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/Fem2DRepresentationTests.cpp b/SurgSim/Physics/UnitTests/Fem2DRepresentationTests.cpp
new file mode 100644
index 0000000..1ab700e
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem2DRepresentationTests.cpp
@@ -0,0 +1,216 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Fem2DRepresentationTests.cpp
+/// This file tests the functionalities of the class Fem2DRepresentation.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem2DRepresentationLocalization.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST(Fem2DRepresentationTests, ConstructorTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<Fem2DRepresentation> fem = std::make_shared<Fem2DRepresentation>("Fem2D");});
+}
+
+TEST(Fem2DRepresentationTests, GetTypeTest)
+{
+ std::shared_ptr<Fem2DRepresentation> fem = std::make_shared<Fem2DRepresentation>("Fem2D");
+ EXPECT_EQ(REPRESENTATION_TYPE_FEM2D, fem->getType());
+}
+
+TEST(Fem2DRepresentationTests, GetNumDofPerNodeTest)
+{
+ std::shared_ptr<Fem2DRepresentation> fem = std::make_shared<Fem2DRepresentation>("Fem2D");
+ EXPECT_EQ(6u, fem->getNumDofPerNode());
+}
+
+TEST(Fem2DRepresentationTests, TransformInitialStateTest)
+{
+ using SurgSim::Math::Vector;
+
+ std::shared_ptr<Fem2DRepresentation> fem = std::make_shared<Fem2DRepresentation>("Fem2D");
+
+ const size_t numNodes = 3;
+ const size_t numDofPerNode = fem->getNumDofPerNode();
+ const size_t numDof = numDofPerNode * numNodes;
+
+ SurgSim::Math::RigidTransform3d initialPose;
+ SurgSim::Math::Quaterniond q(1.0, 2.0, 3.0, 4.0);
+ SurgSim::Math::Vector3d t(1.0, 2.0, 3.0);
+ q.normalize();
+ initialPose = SurgSim::Math::makeRigidTransform(q, t);
+ fem->setLocalPose(initialPose);
+
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(numDofPerNode, numNodes);
+ Vector x = Vector::LinSpaced(numDof, 1.0, static_cast<double>(numDof));
+ Vector v = Vector::Ones(numDof);
+ Vector a = Vector::Ones(numDof) * 2.0;
+ initialState->getPositions() = x;
+ initialState->getVelocities() = v;
+ fem->setInitialState(initialState);
+
+ Vector expectedX = x, expectedV = v, expectedA = a;
+ for (size_t nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ expectedX.segment<3>(numDofPerNode * nodeId) = initialPose * x.segment<3>(numDofPerNode * nodeId);
+ expectedV.segment<3>(numDofPerNode * nodeId) = initialPose.linear() * v.segment<3>(numDofPerNode * nodeId);
+ expectedA.segment<3>(numDofPerNode * nodeId) = initialPose.linear() * a.segment<3>(numDofPerNode * nodeId);
+ }
+
+ // Initialize the component
+ ASSERT_TRUE(fem->initialize(std::make_shared<SurgSim::Framework::Runtime>()));
+ // Wake-up the component => apply the pose to the initial state
+ ASSERT_TRUE(fem->wakeUp());
+
+ EXPECT_TRUE(fem->getInitialState()->getPositions().isApprox(expectedX));
+ EXPECT_TRUE(fem->getInitialState()->getVelocities().isApprox(expectedV));
+}
+
+TEST(Fem2DRepresentationTests, ExternalForceAPITest)
+{
+ std::shared_ptr<Fem2DRepresentation> fem = std::make_shared<Fem2DRepresentation>("Fem");
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(6, 4);
+
+ // External force vector not initialized until the initial state has been set (it contains the #dof...)
+ EXPECT_EQ(0, fem->getExternalGeneralizedForce().size());
+ EXPECT_EQ(0, fem->getExternalGeneralizedStiffness().rows());
+ EXPECT_EQ(0, fem->getExternalGeneralizedStiffness().cols());
+ EXPECT_EQ(0, fem->getExternalGeneralizedDamping().rows());
+ EXPECT_EQ(0, fem->getExternalGeneralizedDamping().cols());
+
+ fem->setInitialState(initialState);
+
+ // Vector initialized (properly sized and zeroed)
+ EXPECT_NE(0, fem->getExternalGeneralizedForce().size());
+ EXPECT_NE(0, fem->getExternalGeneralizedStiffness().rows());
+ EXPECT_NE(0, fem->getExternalGeneralizedStiffness().cols());
+ EXPECT_NE(0, fem->getExternalGeneralizedDamping().rows());
+ EXPECT_NE(0, fem->getExternalGeneralizedDamping().cols());
+ EXPECT_EQ(fem->getNumDof(), fem->getExternalGeneralizedForce().size());
+ EXPECT_EQ(fem->getNumDof(), fem->getExternalGeneralizedStiffness().cols());
+ EXPECT_EQ(fem->getNumDof(), fem->getExternalGeneralizedStiffness().rows());
+ EXPECT_EQ(fem->getNumDof(), fem->getExternalGeneralizedDamping().cols());
+ EXPECT_EQ(fem->getNumDof(), fem->getExternalGeneralizedDamping().rows());
+ EXPECT_TRUE(fem->getExternalGeneralizedForce().isZero());
+ EXPECT_TRUE(fem->getExternalGeneralizedStiffness().isZero());
+ EXPECT_TRUE(fem->getExternalGeneralizedDamping().isZero());
+
+ std::array<size_t, 3> element1NodeIds = {{0, 1, 2}};
+ auto element1 = std::make_shared<Fem2DElementTriangle>(element1NodeIds);
+ fem->addFemElement(element1);
+ std::array<size_t, 3> element2NodeIds = {{1, 2, 3}};
+ auto element2 = std::make_shared<Fem2DElementTriangle>(element2NodeIds);
+ fem->addFemElement(element2);
+
+ SurgSim::DataStructures::IndexedLocalCoordinate femRepCoordinate;
+ femRepCoordinate.index = 0;
+ femRepCoordinate.coordinate = SurgSim::Math::Vector::Zero(3);
+ femRepCoordinate.coordinate[0] = 1.0;
+ auto localization = std::make_shared<Fem2DRepresentationLocalization>(fem, femRepCoordinate);
+ auto wrongLocalizationType = std::make_shared<MockLocalization>();
+
+ Vector FLocalWrongSize = Vector::Ones(2 * fem->getNumDofPerNode());
+ Matrix KLocalWrongSize = Matrix::Ones(3 * fem->getNumDofPerNode(), 3 * fem->getNumDofPerNode());
+ Matrix DLocalWrongSize = Matrix::Ones(4 * fem->getNumDofPerNode(), 4 * fem->getNumDofPerNode());
+ Vector Flocal = Vector::LinSpaced(fem->getNumDofPerNode(), -3.12, 4.09);
+ Matrix Klocal = Matrix::Ones(fem->getNumDofPerNode(), fem->getNumDofPerNode()) * 0.34;
+ Matrix Dlocal = Klocal + Matrix::Identity(fem->getNumDofPerNode(), fem->getNumDofPerNode());
+ Vector F = Vector::Zero(fem->getNumDof());
+ F.segment(0, fem->getNumDofPerNode()) = Flocal;
+ Matrix K = Matrix::Zero(fem->getNumDof(), fem->getNumDof());
+ K.block(0, 0, fem->getNumDofPerNode(), fem->getNumDofPerNode()) = Klocal;
+ Matrix D = Matrix::Zero(fem->getNumDof(), fem->getNumDof());
+ D.block(0, 0, fem->getNumDofPerNode(), fem->getNumDofPerNode()) = Dlocal;
+
+ // Test invalid localization nullptr
+ ASSERT_THROW(fem->addExternalGeneralizedForce(nullptr, Flocal),
+ SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(fem->addExternalGeneralizedForce(nullptr, Flocal, Klocal, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid localization type
+ ASSERT_THROW(fem->addExternalGeneralizedForce(wrongLocalizationType, Flocal),
+ SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(fem->addExternalGeneralizedForce(wrongLocalizationType, Flocal, Klocal, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid force size
+ ASSERT_THROW(fem->addExternalGeneralizedForce(localization, FLocalWrongSize),
+ SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(fem->addExternalGeneralizedForce(localization, FLocalWrongSize, Klocal, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid stiffness size
+ ASSERT_THROW(fem->addExternalGeneralizedForce(localization, Flocal, KLocalWrongSize, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid damping size
+ ASSERT_THROW(fem->addExternalGeneralizedForce(localization, Flocal, Klocal, DLocalWrongSize),
+ SurgSim::Framework::AssertionFailure);
+
+ // Test valid call to addExternalGeneralizedForce
+ fem->addExternalGeneralizedForce(localization, Flocal, Klocal, Dlocal);
+ EXPECT_FALSE(fem->getExternalGeneralizedForce().isZero());
+ EXPECT_FALSE(fem->getExternalGeneralizedStiffness().isZero());
+ EXPECT_FALSE(fem->getExternalGeneralizedDamping().isZero());
+ EXPECT_TRUE(fem->getExternalGeneralizedForce().isApprox(F));
+ EXPECT_TRUE(fem->getExternalGeneralizedStiffness().isApprox(K));
+ EXPECT_TRUE(fem->getExternalGeneralizedDamping().isApprox(D));
+
+ // Test valid call to addExternalGeneralizedForce to add things up
+ fem->addExternalGeneralizedForce(localization, Flocal, Klocal, Dlocal);
+ EXPECT_TRUE(fem->getExternalGeneralizedForce().isApprox(2.0 * F));
+ EXPECT_TRUE(fem->getExternalGeneralizedStiffness().isApprox(2.0 * K));
+ EXPECT_TRUE(fem->getExternalGeneralizedDamping().isApprox(2.0 * D));
+}
+
+TEST(Fem2DRepresentationTests, SerializationTest)
+{
+ auto fem2DRepresentation = std::make_shared<SurgSim::Physics::Fem2DRepresentation>("Test-Fem2D");
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*fem2DRepresentation));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Physics::Fem2DRepresentation"];
+ EXPECT_EQ(10u, data.size());
+
+ std::shared_ptr<Fem2DRepresentation> newRepresentation;
+ ASSERT_NO_THROW(newRepresentation =
+ std::dynamic_pointer_cast<Fem2DRepresentation>(node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ ASSERT_NE(nullptr, newRepresentation);
+
+ EXPECT_EQ("SurgSim::Physics::Fem2DRepresentation", newRepresentation->getClassName());
+ EXPECT_EQ(REPRESENTATION_TYPE_FEM2D, newRepresentation->getType());
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem3DElementCorotationalTetrahedronTests.cpp b/SurgSim/Physics/UnitTests/Fem3DElementCorotationalTetrahedronTests.cpp
new file mode 100644
index 0000000..588c7fc
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DElementCorotationalTetrahedronTests.cpp
@@ -0,0 +1,531 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <array>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h"
+
+using SurgSim::Physics::Fem3DElementCorotationalTetrahedron;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Matrix;
+
+namespace
+{
+const double epsilonAddForce = 1e-8;
+const double epsilonAddMatVec = 1e-10;
+};
+
+class MockFem3DElementCorotationalTet : public Fem3DElementCorotationalTetrahedron
+{
+public:
+ MockFem3DElementCorotationalTet(std::array<size_t, 4> nodeIds)
+ : Fem3DElementCorotationalTetrahedron(nodeIds)
+ {
+ }
+
+ const Eigen::Matrix<double,12 ,1>& getInitialPosition() const
+ {
+ return m_x0;
+ }
+
+ const SurgSim::Math::Matrix33d& getRotation() const
+ {
+ return m_rotation;
+ }
+
+ const Eigen::Matrix<double, 12, 12>& getMassMatrix() const
+ {
+ return m_M;
+ }
+
+ const Eigen::Matrix<double, 12, 12>& getRotatedStiffness() const
+ {
+ return m_corotationalStiffnessMatrix;
+ }
+
+ const Eigen::Matrix<double, 12, 12>& getNonRotatedStiffness() const
+ {
+ return m_K;
+ }
+
+ const SurgSim::Math::Matrix44d& getVinverse() const
+ {
+ return m_Vinverse;
+ }
+
+ void setupInitialParams(const SurgSim::Math::OdeState &state,
+ double massDensity,
+ double poissonRatio,
+ double youngModulus)
+ {
+ setMassDensity(massDensity);
+ setPoissonRatio(poissonRatio);
+ setYoungModulus(youngModulus);
+ initialize(state);
+ }
+};
+
+class Fem3DElementCorotationalTetrahedronTests : public ::testing::Test
+{
+public:
+ std::array<size_t, 4> m_nodeIds;
+ std::vector<size_t> m_nodeIdsAsVector;
+ SurgSim::Math::OdeState m_restState, m_state;
+ double m_rho, m_E, m_nu;
+
+ SurgSim::Math::Matrix33d m_rotation;
+ Eigen::Matrix<double, 12, 12> m_R12x12;
+ Vector3d m_translation;
+
+ virtual void SetUp() override
+ {
+ m_nodeIds[0] = 3;
+ m_nodeIds[1] = 1;
+ m_nodeIds[2] = 14;
+ m_nodeIds[3] = 9;
+ m_nodeIdsAsVector.assign(m_nodeIds.cbegin(), m_nodeIds.cend());
+
+ m_restState.setNumDof(3, 15);
+ Vector& x0 = m_restState.getPositions();
+ std::array<Vector3d, 4> points = {{Vector3d(0.0, 0.0, 0.0), Vector3d(1.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0), Vector3d(0.0, 0.0, 1.0) }};
+
+ // Tet is aligned with the axis (X,Y,Z), centered on (0.0, 0.0, 0.0), embedded in a cube of size 1
+ for (size_t nodeId = 0; nodeId < 4; ++nodeId)
+ {
+ SurgSim::Math::getSubVector(x0, m_nodeIds[nodeId], 3) = points[nodeId];
+ }
+
+ m_rho = 1000.0;
+ m_E = 1e6;
+ m_nu = 0.45;
+
+ Vector3d axis(1.1,-2.2,-3.3);
+ axis.normalize();
+ m_rotation = SurgSim::Math::makeRotationMatrix(3.1415, axis);
+ m_R12x12 = Eigen::Matrix<double, 12, 12>::Zero();
+ for (size_t nodeId = 0; nodeId < 4; ++nodeId)
+ {
+ m_R12x12.block<3, 3>(3* nodeId, 3* nodeId) = m_rotation;
+ }
+ m_translation = Vector3d(1.2, 2.3, 3.4);
+ }
+};
+
+namespace
+{
+template <class T, int MOpt>
+void defineCurrentState(const SurgSim::Math::OdeState& x0, SurgSim::Math::OdeState* x,
+ const Eigen::Transform<T, 3, MOpt>& t, bool addSmallDeformation)
+{
+ std::array<Vector3d, 3> delta = {{Vector3d(0.01, -0.02, 0.005),
+ Vector3d(-0.01, -0.01, -0.03), Vector3d(0.0, -0.015, 0.03)}};
+
+ *x = x0;
+ for (size_t nodeId = 0; nodeId < x0.getNumNodes(); ++nodeId)
+ {
+ (*x).getPositions().segment<3>(3 * nodeId) = t * x0.getPositions().segment<3>(3 * nodeId);
+ if (addSmallDeformation)
+ {
+ (*x).getPositions().segment<3>(3 * nodeId) += delta[nodeId % 3];
+ }
+ }
+}
+// Duplicates 4 times a 3x3 matrix on the diagonal blocks
+Eigen::Matrix<double, 12, 12> make12x12(const Eigen::Matrix<double, 3, 3>& R)
+{
+ Eigen::Matrix<double, 12, 12> res = Eigen::Matrix<double, 12, 12>::Zero();
+ for (size_t nodeId = 0; nodeId < 4; nodeId++)
+ {
+ res.block<3, 3>(3 * nodeId, 3 * nodeId) = R;
+ }
+ return res;
+}
+// Duplicates 4 times a 3x3 matrix on the diagonal blocks, with the 3x3 row-major matrix stored in a vector
+Eigen::Matrix<double, 12, 12> make12x12(const Eigen::Matrix<double, 9, 1>& R)
+{
+ return make12x12(Eigen::Matrix<double, 3, 3>(R.data()));
+}
+}; // anonymous namespace
+
+TEST_F(Fem3DElementCorotationalTetrahedronTests, ConstructorTest)
+{
+ ASSERT_NO_THROW({MockFem3DElementCorotationalTet tet(m_nodeIds);});
+ ASSERT_NO_THROW({MockFem3DElementCorotationalTet* tet = new MockFem3DElementCorotationalTet(m_nodeIds);
+ delete tet;});
+ ASSERT_NO_THROW({std::shared_ptr<MockFem3DElementCorotationalTet> tet =
+ std::make_shared<MockFem3DElementCorotationalTet>(m_nodeIds);});
+}
+
+TEST_F(Fem3DElementCorotationalTetrahedronTests, InitializeTest)
+{
+ MockFem3DElementCorotationalTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ EXPECT_TRUE(tet.getRotation().isIdentity());
+
+ EXPECT_TRUE(tet.getRotatedStiffness().isApprox(tet.getNonRotatedStiffness()));
+
+ // V^1 = (a b c d)^-1
+ // (1 1 1 1)
+ SurgSim::Math::Matrix44d expectedV = SurgSim::Math::Matrix44d::Ones();
+ for (size_t axis = 0; axis < 3; ++axis)
+ {
+ expectedV(axis, 0) = m_restState.getPosition(m_nodeIds[0])(axis);
+ expectedV(axis, 1) = m_restState.getPosition(m_nodeIds[1])(axis);
+ expectedV(axis, 2) = m_restState.getPosition(m_nodeIds[2])(axis);
+ expectedV(axis, 3) = m_restState.getPosition(m_nodeIds[3])(axis);
+ }
+ EXPECT_TRUE(tet.getVinverse().isApprox(expectedV.inverse()));
+}
+
+TEST_F(Fem3DElementCorotationalTetrahedronTests, UpdateTest)
+{
+ using SurgSim::Math::skew;
+ using SurgSim::Math::makeSkewSymmetricMatrix;
+
+ Eigen::Transform<double, 3, Eigen::Affine> transformation;
+
+ {
+ SCOPED_TRACE("No rotation, no translation");
+ transformation.linear().setIdentity();
+ transformation.translation().setZero();
+ defineCurrentState(m_restState, &m_state, transformation, false);
+
+ MockFem3DElementCorotationalTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ ASSERT_NO_THROW(tet.update(m_state));
+ EXPECT_TRUE(tet.getRotation().isIdentity());
+ EXPECT_TRUE(tet.getRotatedStiffness().isApprox(tet.getNonRotatedStiffness()));
+ }
+
+ {
+ SCOPED_TRACE("Pure translation");
+ transformation.linear().setIdentity();
+ transformation.translation() = m_translation;
+ defineCurrentState(m_restState, &m_state, transformation, false);
+
+ MockFem3DElementCorotationalTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ ASSERT_NO_THROW(tet.update(m_state));
+ EXPECT_TRUE(tet.getRotation().isIdentity());
+ EXPECT_TRUE(tet.getRotatedStiffness().isApprox(tet.getNonRotatedStiffness()));
+ }
+
+ {
+ SCOPED_TRACE("Pure rotation");
+ transformation.linear() = m_rotation;
+ transformation.translation().setZero();
+ defineCurrentState(m_restState, &m_state, transformation, false);
+
+ MockFem3DElementCorotationalTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ ASSERT_NO_THROW(tet.update(m_state));
+ EXPECT_TRUE(tet.getRotation().isApprox(m_rotation));
+ EXPECT_TRUE(tet.getRotatedStiffness().isApprox(
+ m_R12x12 * tet.getNonRotatedStiffness() * m_R12x12.transpose()));
+ }
+
+ {
+ SCOPED_TRACE("Translation + Pure Rotation");
+ transformation.linear() = m_rotation;
+ transformation.translation() = m_translation;
+ defineCurrentState(m_restState, &m_state, transformation, false);
+
+ MockFem3DElementCorotationalTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ ASSERT_NO_THROW(tet.update(m_state));
+ EXPECT_TRUE(tet.getRotation().isApprox(m_rotation));
+ EXPECT_TRUE(tet.getRotatedStiffness().isApprox(
+ m_R12x12 * tet.getNonRotatedStiffness() * m_R12x12.transpose()));
+ }
+
+ {
+ SCOPED_TRACE("Affine transform : Translation + rotation * scaling");
+ Eigen::Matrix<double, 3, 3> R = m_rotation;
+ Eigen::Matrix<double, 12, 12> R12x12 = make12x12(R);
+ Eigen::UniformScaling<double> S = Eigen::Scaling(45.3);
+ transformation.linear() = R * S;
+ transformation.translation() = m_translation;
+ defineCurrentState(m_restState, &m_state, transformation, false);
+
+ MockFem3DElementCorotationalTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ ASSERT_NO_THROW(tet.update(m_state));
+ EXPECT_TRUE(tet.getRotation().isApprox(m_rotation));
+
+ // Extra stiffness terms
+ Eigen::Matrix<double, 9, 12> dFdX = Eigen::Matrix<double, 9, 12>::Zero();
+ for (size_t nodeId = 0; nodeId < 4; ++nodeId)
+ {
+ Vector3d ni(tet.getVinverse().row(nodeId).segment<3>(0));
+ dFdX.col(3 * nodeId + 0).segment<3>(0) = ni;
+ dFdX.col(3 * nodeId + 1).segment<3>(3) = ni;
+ dFdX.col(3 * nodeId + 2).segment<3>(6) = ni;
+ }
+
+ Eigen::Matrix<double, 9, 9> dRdF = Eigen::Matrix<double, 9, 9>::Zero();
+ SurgSim::Math::Matrix33d G = (2.0 * S.factor() * SurgSim::Math::Matrix33d::Identity()) * R.transpose();
+ Vector3d e[3] = {Vector3d::UnitX(), Vector3d::UnitY(), Vector3d::UnitZ()};
+ for (size_t i = 0; i < 3; ++i)
+ {
+ for (size_t j = 0; j < 3; ++j)
+ {
+ Vector3d wij = G.inverse() * 2.0 * skew((R.transpose() * e[i] * e[j].transpose()).eval());
+ SurgSim::Math::Matrix33d dRdFij = makeSkewSymmetricMatrix(wij) * R;
+ dRdF.col(3 * i + j).segment<3>(0) = dRdFij.col(0);
+ dRdF.col(3 * i + j).segment<3>(3) = dRdFij.col(1);
+ dRdF.col(3 * i + j).segment<3>(6) = dRdFij.col(2);
+ }
+ }
+
+ Eigen::Matrix<double, 9, 12> dRdX = dRdF * dFdX;
+
+ Eigen::Matrix<double, 12, 12> expectedStiffness = R12x12 * tet.getNonRotatedStiffness() * R12x12.transpose();
+ const Eigen::Matrix<double, 12, 12>& Krest = tet.getNonRotatedStiffness();
+ Eigen::Matrix<double, 12, 1> x;
+ SurgSim::Math::getSubVector(m_state.getPositions(), tet.getNodeIds(), 3, &x);
+ for (size_t dofId = 0; dofId < 12; ++dofId)
+ {
+ const Eigen::Matrix<double, 12, 12> dRdxl12x12 = make12x12(Eigen::Matrix<double, 9, 1>(dRdX.col(dofId)));
+ expectedStiffness.col(dofId) += dRdxl12x12 * Krest * (R12x12.transpose() * x - tet.getInitialPosition());
+ expectedStiffness.col(dofId) += R12x12 * Krest * dRdxl12x12.transpose() * x;
+ }
+ EXPECT_TRUE(tet.getRotatedStiffness().isApprox(expectedStiffness));
+ }
+}
+
+namespace
+{
+void testAddStiffness(MockFem3DElementCorotationalTet* tet,
+ const SurgSim::Math::OdeState& state0,
+ const SurgSim::Math::RigidTransform3d& t,
+ double scale)
+{
+ SurgSim::Math::OdeState state;
+
+ defineCurrentState(state0, &state, t, false);
+ ASSERT_NO_THROW(tet->update(state));
+
+ Matrix expectedK = Matrix::Zero(state0.getNumDof(), state0.getNumDof());
+ SurgSim::Math::addSubMatrix(scale * tet->getRotatedStiffness(), tet->getNodeIds(), 3, &expectedK);
+
+ Matrix K = Matrix::Zero(state0.getNumDof(), state0.getNumDof());
+ tet->addStiffness(state, &K, scale);
+
+ EXPECT_TRUE(K.isApprox(expectedK));
+}
+}; // anonymous namespace
+
+TEST_F(Fem3DElementCorotationalTetrahedronTests, AddStiffnessTest)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::RigidTransform3d;
+
+ MockFem3DElementCorotationalTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ {
+ SCOPED_TRACE("Without rotation, scale 1.0");
+ testAddStiffness(&tet, m_restState, RigidTransform3d::Identity(), 1.0);
+ }
+
+ {
+ SCOPED_TRACE("Without rotation, scale 0.4");
+ testAddStiffness(&tet, m_restState, RigidTransform3d::Identity(), 0.4);
+ }
+
+ {
+ SCOPED_TRACE("With rotation, scale 1.0");
+ testAddStiffness(&tet, m_restState, makeRigidTransform(m_rotation, Vector3d::Zero()), 1.0);
+ }
+
+ {
+ SCOPED_TRACE("With rotation, scale 0.4");
+ testAddStiffness(&tet, m_restState, makeRigidTransform(m_rotation, Vector3d::Zero()), 0.4);
+ }
+}
+
+namespace
+{
+void testAddForce(MockFem3DElementCorotationalTet* tet,
+ const SurgSim::Math::OdeState& state0,
+ const SurgSim::Math::RigidTransform3d& t,
+ bool addLocalDeformation)
+{
+ SurgSim::Math::OdeState statet;
+ SurgSim::Math::Matrix K = tet->getNonRotatedStiffness();
+
+ // F = -RK(R^t.x - x0)
+ Eigen::Matrix<double, 12, 1> x, x0;
+ SurgSim::Math::getSubVector(state0.getPositions(), tet->getNodeIds(), 3, &x0);
+ defineCurrentState(state0, &statet, t, addLocalDeformation);
+ ASSERT_NO_THROW(tet->update(statet));
+ SurgSim::Math::getSubVector(statet.getPositions(), tet->getNodeIds(), 3, &x);
+
+ // Note that the element rotation is not necessarily the RigidTransform rotation
+ // If addDeformation is true, it will add pseudo-random variation that will affect the rigid motion slightly
+ SurgSim::Math::Matrix33d R = tet->getRotation();
+ Eigen::Matrix<double, 12, 12> R12x12 = Eigen::Matrix<double, 12, 12>::Zero();
+ for (size_t nodeId = 0; nodeId < 4; ++nodeId)
+ {
+ R12x12.block<3, 3>(3 * nodeId, 3 * nodeId) = R;
+ }
+
+ SurgSim::Math::Vector expectedF;
+ expectedF.resize(statet.getNumDof());
+ expectedF.setZero();
+ Eigen::Matrix<double, 12 ,1> f = - R12x12 * K * R12x12.transpose() * (x - (R12x12 * x0));
+ SurgSim::Math::addSubVector(f, tet->getNodeIds(), 3, &expectedF);
+
+ EXPECT_TRUE(tet->getRotation().isApprox(R));
+ EXPECT_TRUE(tet->getNonRotatedStiffness().isApprox(K));
+ EXPECT_TRUE(tet->getInitialPosition().isApprox(x0));
+ {
+ SCOPED_TRACE("Scale 1.0");
+ SurgSim::Math::Vector F;
+ F.resize(statet.getNumDof());
+ F.setZero();
+ tet->addForce(statet, &F);
+ EXPECT_LT((F - expectedF).norm(), epsilonAddForce);
+ if (!addLocalDeformation)
+ {
+ EXPECT_TRUE(F.isZero(epsilonAddForce));
+ }
+ }
+
+ {
+ SCOPED_TRACE("Scale 0.4");
+ SurgSim::Math::Vector F;
+ F.resize(statet.getNumDof());
+ F.setZero();
+ tet->addForce(statet, &F, 0.4);
+ EXPECT_LT((F - 0.4 * expectedF).norm(), epsilonAddForce);
+ if (!addLocalDeformation)
+ {
+ EXPECT_TRUE(F.isZero(epsilonAddForce));
+ }
+ }
+}
+}; // anonymous namespace
+
+TEST_F(Fem3DElementCorotationalTetrahedronTests, AddForceTest)
+{
+ MockFem3DElementCorotationalTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ SurgSim::Math::RigidTransform3d transformation;
+ transformation.linear() = m_rotation;
+ transformation.translation() = m_translation;
+
+ {
+ SCOPED_TRACE("No deformation, no rigid transformation");
+ testAddForce(&tet, m_restState, SurgSim::Math::RigidTransform3d::Identity(), false);
+ }
+
+ {
+ SCOPED_TRACE("No deformation, rigid transformation");
+ testAddForce(&tet, m_restState, transformation, false);
+ }
+
+ {
+ SCOPED_TRACE("Deformation, no rigid transformation");
+ testAddForce(&tet, m_restState, SurgSim::Math::RigidTransform3d::Identity(), true);
+ }
+
+ {
+ SCOPED_TRACE("Deformation, rigid transformation");
+ testAddForce(&tet, m_restState, transformation, true);
+ }
+}
+
+TEST_F(Fem3DElementCorotationalTetrahedronTests, AddMatVecTest)
+{
+ MockFem3DElementCorotationalTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ SurgSim::Math::RigidTransform3d transformation;
+ transformation.linear() = m_rotation;
+ transformation.translation() = m_translation;
+
+ SurgSim::Math::OdeState state;
+ defineCurrentState(m_restState, &state, transformation, true);
+ ASSERT_NO_THROW(tet.update(state));
+
+ Eigen::Matrix<double, 12, 12> M = tet.getMassMatrix();
+ Eigen::Matrix<double, 12, 12> K = tet.getRotatedStiffness();
+
+ SurgSim::Math::Vector ones = SurgSim::Math::Vector::Ones(state.getNumDof());
+
+ {
+ SCOPED_TRACE("Mass only");
+
+ SurgSim::Math::Vector result = SurgSim::Math::Vector::Zero(state.getNumDof());
+ tet.addMatVec(state, 1.4, 0.0, 0.0, ones, &result);
+
+ SurgSim::Math::Vector expectedResult = SurgSim::Math::Vector::Zero(state.getNumDof());
+ Eigen::Matrix<double, 12, 1> f = 1.4 * M * SurgSim::Math::Vector::Ones(12);
+ SurgSim::Math::addSubVector(f, m_nodeIdsAsVector, 3, &expectedResult);
+
+ EXPECT_TRUE(result.isApprox(expectedResult));
+ }
+
+ {
+ SCOPED_TRACE("Damping only");
+
+ SurgSim::Math::Vector result = SurgSim::Math::Vector::Zero(state.getNumDof());
+ tet.addMatVec(state, 0.0, 1.5, 0.0, ones, &result);
+
+ EXPECT_TRUE(result.isZero());
+ }
+
+ {
+ SCOPED_TRACE("Stiffness only");
+
+ SurgSim::Math::Vector result = SurgSim::Math::Vector::Zero(state.getNumDof());
+ tet.addMatVec(state, 0.0, 0.0, 1.6, ones, &result);
+
+ SurgSim::Math::Vector expectedResult = SurgSim::Math::Vector::Zero(state.getNumDof());
+ Eigen::Matrix<double, 12, 1> f = 1.6 * K * SurgSim::Math::Vector::Ones(12);
+ SurgSim::Math::addSubVector(f, m_nodeIdsAsVector, 3, &expectedResult);
+
+ EXPECT_TRUE(result.isApprox(expectedResult));
+ }
+
+ {
+ SCOPED_TRACE("Mass/Damping/Stiffness");
+
+ SurgSim::Math::Vector result = SurgSim::Math::Vector::Zero(state.getNumDof());
+ tet.addMatVec(state, 1.4, 1.5, 1.6, ones, &result);
+
+ SurgSim::Math::Vector expectedResult = SurgSim::Math::Vector::Zero(state.getNumDof());
+ Eigen::Matrix<double, 12, 1> f = (1.4 * M + 1.6 * K) * SurgSim::Math::Vector::Ones(12);
+ SurgSim::Math::addSubVector(f, m_nodeIdsAsVector, 3, &expectedResult);
+
+ EXPECT_TRUE(result.isApprox(expectedResult, epsilonAddMatVec));
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/Fem3DElementCubeTests.cpp b/SurgSim/Physics/UnitTests/Fem3DElementCubeTests.cpp
new file mode 100644
index 0000000..977f2f0
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DElementCubeTests.cpp
@@ -0,0 +1,915 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <array>
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+
+using SurgSim::Math::Matrix;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::Fem3DElementCube;
+
+namespace
+{
+/// Epsilon used in this unit test, resulting from a trial and error test.
+const double epsilon = 2.6e-9;
+};
+
+class MockFem3DElementCube : public Fem3DElementCube
+{
+public:
+ MockFem3DElementCube(std::array<size_t, 8> nodeIds) : Fem3DElementCube(nodeIds)
+ {
+ }
+
+ double getRestVolume() const
+ {
+ return m_restVolume;
+ }
+
+ double evaluateN(int i, double epsilon, double eta, double mu) const
+ {
+ return shapeFunction(i, epsilon, eta, mu);
+ }
+
+ double evaluatedNidEpsilon(int i, double epsilon, double eta, double mu) const
+ {
+ return dShapeFunctiondepsilon(i, epsilon, eta, mu);
+ }
+
+ double evaluatedNidEta(int i, double epsilon, double eta, double mu) const
+ {
+ return dShapeFunctiondeta(i, epsilon, eta, mu);
+ }
+
+ double evaluatedNidMu(int i, double epsilon, double eta, double mu) const
+ {
+ return dShapeFunctiondmu(i, epsilon, eta, mu);
+ }
+
+ const Eigen::Matrix<double, 24, 1>& getInitialPosition() const
+ {
+ return m_elementRestPosition;
+ }
+};
+
+class Fem3DElementCubeTests : public ::testing::Test
+{
+public:
+ std::array<size_t, 8> m_nodeIds;
+ SurgSim::Math::OdeState m_restState;
+ double m_expectedVolume;
+ Eigen::Matrix<double, 24, 1> m_expectedX0;
+ double m_rho, m_E, m_nu;
+ SurgSim::Math::Matrix m_expectedMassMatrix, m_expectedDampingMatrix, m_expectedStiffnessMatrix;
+ SurgSim::Math::Vector m_vectorOnes;
+
+ std::shared_ptr<MockFem3DElementCube> getCubeElement(const std::array<size_t, 8>& nodeIds)
+ {
+ std::shared_ptr<MockFem3DElementCube> element;
+
+ element = std::make_shared<MockFem3DElementCube>(nodeIds);
+ element->setYoungModulus(m_E);
+ element->setPoissonRatio(m_nu);
+ element->setMassDensity(m_rho);
+
+ return element;
+ }
+
+ void computeExpectedStiffnessMatrix(std::vector<size_t> nodeIdsVectorForm)
+ {
+ using SurgSim::Math::getSubMatrix;
+ using SurgSim::Math::addSubMatrix;
+
+ Eigen::Matrix<double, 24, 24> K;
+ K.setZero();
+ {
+ // Expected stiffness matrix given in
+ // "Physically-Based Simulation of Objects Represented by Surface Meshes"
+ // Muller, Techner, Gross, CGI 2004
+ // NOTE a bug in the paper, the sub matrix K44 should have a diagonal of 'd'
+ double h = 1.0;
+ double a = h * m_E * (1.0 - m_nu) / ((1.0 + m_nu) * (1.0 - 2.0 * m_nu));
+ double b = h * m_E * (m_nu) / ((1.0 + m_nu) * (1.0 - 2.0 * m_nu));
+ double c = h * m_E / (2.0 * (1.0 + m_nu));
+
+ double d = (a + 2.0 * c) / 9.0;
+ double e = (b + c) / 12.0;
+ double n = -e;
+ // Fill up the diagonal sub-matrices (3x3)
+ getSubMatrix(K, 0, 0, 3, 3).setConstant(e);
+ getSubMatrix(K, 0, 0, 3, 3).diagonal().setConstant(d);
+
+ getSubMatrix(K, 1, 1, 3, 3).setConstant(n);
+ getSubMatrix(K, 1, 1, 3, 3).diagonal().setConstant(d);
+ getSubMatrix(K, 1, 1, 3, 3)(1,2) = e;
+ getSubMatrix(K, 1, 1, 3, 3)(2,1) = e;
+
+ getSubMatrix(K, 2, 2, 3, 3).setConstant(n);
+ getSubMatrix(K, 2, 2, 3, 3).diagonal().setConstant(d);
+ getSubMatrix(K, 2, 2, 3, 3)(0,1) = e;
+ getSubMatrix(K, 2, 2, 3, 3)(1,0) = e;
+
+ getSubMatrix(K, 3, 3, 3, 3).setConstant(n);
+ getSubMatrix(K, 3, 3, 3, 3).diagonal().setConstant(d);
+ getSubMatrix(K, 3, 3, 3, 3)(0,2) = e;
+ getSubMatrix(K, 3, 3, 3, 3)(2,0) = e;
+
+ getSubMatrix(K, 4, 4, 3, 3).setConstant(n);
+ getSubMatrix(K, 4, 4, 3, 3).diagonal().setConstant(d);
+ //getSubMatrix(K, 4, 4, 3, 3)(0,0) = e; // BUG IN THE PAPER !!!
+ getSubMatrix(K, 4, 4, 3, 3)(0,1) = e;
+ getSubMatrix(K, 4, 4, 3, 3)(1,0) = e;
+
+ getSubMatrix(K, 5, 5, 3, 3).setConstant(n);
+ getSubMatrix(K, 5, 5, 3, 3).diagonal().setConstant(d);
+ getSubMatrix(K, 5, 5, 3, 3)(0,2) = e;
+ getSubMatrix(K, 5, 5, 3, 3)(2,0) = e;
+
+ getSubMatrix(K, 6, 6, 3, 3).setConstant(e);
+ getSubMatrix(K, 6, 6, 3, 3).diagonal().setConstant(d);
+
+ getSubMatrix(K, 7, 7, 3, 3).setConstant(n);
+ getSubMatrix(K, 7, 7, 3, 3).diagonal().setConstant(d);
+ getSubMatrix(K, 7, 7, 3, 3)(1,2) = e;
+ getSubMatrix(K, 7, 7, 3, 3)(2,1) = e;
+
+ // Edges
+ {
+ double d1 = (-a + c) / 9.0;
+ double d2 = (a - c) / 18.0;
+ double e1 = (b - c) / 12.0;
+ double e2 = (b + c) / 24.0;
+ double n1 = -e1;
+ double n2 = -e2;
+
+ // Edge in x-direction
+ getSubMatrix(K, 0, 1, 3, 3)(0, 0) = d1;
+ getSubMatrix(K, 0, 1, 3, 3)(0, 1) = e1;
+ getSubMatrix(K, 0, 1, 3, 3)(0, 2) = e1;
+ getSubMatrix(K, 0, 1, 3, 3)(1, 0) = n1;
+ getSubMatrix(K, 0, 1, 3, 3)(1, 1) = d2;
+ getSubMatrix(K, 0, 1, 3, 3)(1, 2) = e2;
+ getSubMatrix(K, 0, 1, 3, 3)(2, 0) = n1;
+ getSubMatrix(K, 0, 1, 3, 3)(2, 1) = e2;
+ getSubMatrix(K, 0, 1, 3, 3)(2, 2) = d2;
+
+ getSubMatrix(K, 2, 3, 3, 3) = getSubMatrix(K, 0, 1, 3, 3);
+ getSubMatrix(K, 2, 3, 3, 3)(0, 2) = n1;
+ getSubMatrix(K, 2, 3, 3, 3)(1, 2) = n2;
+ getSubMatrix(K, 2, 3, 3, 3)(2, 0) = e1;
+ getSubMatrix(K, 2, 3, 3, 3)(2, 1) = n2;
+
+ getSubMatrix(K, 4, 5, 3, 3) = getSubMatrix(K, 2, 3, 3, 3);
+
+ getSubMatrix(K, 6, 7, 3, 3) = getSubMatrix(K, 0, 1, 3, 3);
+
+ // Edge in y-direction
+ getSubMatrix(K, 0, 3, 3, 3)(0, 0) = d2;
+ getSubMatrix(K, 0, 3, 3, 3)(0, 1) = n1;
+ getSubMatrix(K, 0, 3, 3, 3)(0, 2) = e2;
+ getSubMatrix(K, 0, 3, 3, 3)(1, 0) = e1;
+ getSubMatrix(K, 0, 3, 3, 3)(1, 1) = d1;
+ getSubMatrix(K, 0, 3, 3, 3)(1, 2) = e1;
+ getSubMatrix(K, 0, 3, 3, 3)(2, 0) = e2;
+ getSubMatrix(K, 0, 3, 3, 3)(2, 1) = n1;
+ getSubMatrix(K, 0, 3, 3, 3)(2, 2) = d2;
+
+ getSubMatrix(K, 1, 2, 3, 3) = getSubMatrix(K, 0, 3, 3, 3);
+ getSubMatrix(K, 1, 2, 3, 3)(0, 1) = e1;
+ getSubMatrix(K, 1, 2, 3, 3)(0, 2) = n2;
+ getSubMatrix(K, 1, 2, 3, 3)(1, 0) = n1;
+ getSubMatrix(K, 1, 2, 3, 3)(2, 0) = n2;
+
+ getSubMatrix(K, 4, 7, 3, 3) = getSubMatrix(K, 1, 2, 3, 3);
+ getSubMatrix(K, 4, 7, 3, 3)(0, 1) = n1;
+ getSubMatrix(K, 4, 7, 3, 3)(1, 0) = e1;
+ getSubMatrix(K, 4, 7, 3, 3)(1, 2) = n1;
+ getSubMatrix(K, 4, 7, 3, 3)(2, 1) = e1;
+
+ getSubMatrix(K, 5, 6, 3, 3) = getSubMatrix(K, 0, 3, 3, 3);
+ getSubMatrix(K, 5, 6, 3, 3)(0, 1) = e1;
+ getSubMatrix(K, 5, 6, 3, 3)(1, 0) = n1;
+ getSubMatrix(K, 5, 6, 3, 3)(1, 2) = n1;
+ getSubMatrix(K, 5, 6, 3, 3)(2, 1) = e1;
+
+ // Edge in z-direction
+ getSubMatrix(K, 0, 4, 3, 3)(0, 0) = d2;
+ getSubMatrix(K, 0, 4, 3, 3)(0, 1) = e2;
+ getSubMatrix(K, 0, 4, 3, 3)(0, 2) = n1;
+ getSubMatrix(K, 0, 4, 3, 3)(1, 0) = e2;
+ getSubMatrix(K, 0, 4, 3, 3)(1, 1) = d2;
+ getSubMatrix(K, 0, 4, 3, 3)(1, 2) = n1;
+ getSubMatrix(K, 0, 4, 3, 3)(2, 0) = e1;
+ getSubMatrix(K, 0, 4, 3, 3)(2, 1) = e1;
+ getSubMatrix(K, 0, 4, 3, 3)(2, 2) = d1;
+
+ getSubMatrix(K, 1, 5, 3, 3) = getSubMatrix(K, 0, 4, 3, 3);
+ getSubMatrix(K, 1, 5, 3, 3)(0, 1) = n2;
+ getSubMatrix(K, 1, 5, 3, 3)(0, 2) = e1;
+ getSubMatrix(K, 1, 5, 3, 3)(1, 0) = n2;
+ getSubMatrix(K, 1, 5, 3, 3)(2, 0) = n1;
+
+ getSubMatrix(K, 2, 6, 3, 3) = getSubMatrix(K, 0, 4, 3, 3);
+ getSubMatrix(K, 2, 6, 3, 3)(0, 2) = e1;
+ getSubMatrix(K, 2, 6, 3, 3)(1, 2) = e1;
+ getSubMatrix(K, 2, 6, 3, 3)(2, 0) = n1;
+ getSubMatrix(K, 2, 6, 3, 3)(2, 1) = n1;
+
+ getSubMatrix(K, 3, 7, 3, 3) = getSubMatrix(K, 0, 4, 3, 3);
+ getSubMatrix(K, 3, 7, 3, 3)(0, 1) = n2;
+ getSubMatrix(K, 3, 7, 3, 3)(1, 0) = n2;
+ getSubMatrix(K, 3, 7, 3, 3)(1, 2) = e1;
+ getSubMatrix(K, 3, 7, 3, 3)(2, 1) = n1;
+ }
+
+ // Faces diagonals
+ {
+ double d1 = (-2.0 * a - c) / 36.0;
+ double d2 = (a - 4.0 * c) / 36.0;
+ double e1 = (b + c) / 12.0;
+ double e2 = (b - c) / 24.0;
+ double n1 = -e1;
+ double n2 = -e2;
+
+ getSubMatrix(K, 0, 5, 3, 3)(0, 0) = d1;
+ getSubMatrix(K, 0, 5, 3, 3)(0, 1) = e2;
+ getSubMatrix(K, 0, 5, 3, 3)(0, 2) = n1;
+ getSubMatrix(K, 0, 5, 3, 3)(1, 0) = n2;
+ getSubMatrix(K, 0, 5, 3, 3)(1, 1) = d2;
+ getSubMatrix(K, 0, 5, 3, 3)(1, 2) = n2;
+ getSubMatrix(K, 0, 5, 3, 3)(2, 0) = n1;
+ getSubMatrix(K, 0, 5, 3, 3)(2, 1) = e2;
+ getSubMatrix(K, 0, 5, 3, 3)(2, 2) = d1;
+
+ getSubMatrix(K, 1, 4, 3, 3) = getSubMatrix(K, 0, 5, 3, 3);
+ getSubMatrix(K, 1, 4, 3, 3)(0, 1) = n2;
+ getSubMatrix(K, 1, 4, 3, 3)(0, 2) = e1;
+ getSubMatrix(K, 1, 4, 3, 3)(1, 0) = e2;
+ getSubMatrix(K, 1, 4, 3, 3)(2, 0) = e1;
+
+ getSubMatrix(K, 2, 7, 3, 3) = getSubMatrix(K, 0, 5, 3, 3);
+ getSubMatrix(K, 2, 7, 3, 3)(0, 2) = e1;
+ getSubMatrix(K, 2, 7, 3, 3)(1, 2) = e2;
+ getSubMatrix(K, 2, 7, 3, 3)(2, 0) = e1;
+ getSubMatrix(K, 2, 7, 3, 3)(2, 1) = n2;
+
+ getSubMatrix(K, 3, 6, 3, 3) = getSubMatrix(K, 0, 5, 3, 3);
+ getSubMatrix(K, 3, 6, 3, 3)(0, 1) = n2;
+ getSubMatrix(K, 3, 6, 3, 3)(1, 0) = e2;
+ getSubMatrix(K, 3, 6, 3, 3)(1, 2) = e2;
+ getSubMatrix(K, 3, 6, 3, 3)(2, 1) = n2;
+
+ getSubMatrix(K, 1, 6, 3, 3)(0, 0) = d2;
+ getSubMatrix(K, 1, 6, 3, 3)(0, 1) = e2;
+ getSubMatrix(K, 1, 6, 3, 3)(0, 2) = e2;
+ getSubMatrix(K, 1, 6, 3, 3)(1, 0) = n2;
+ getSubMatrix(K, 1, 6, 3, 3)(1, 1) = d1;
+ getSubMatrix(K, 1, 6, 3, 3)(1, 2) = n1;
+ getSubMatrix(K, 1, 6, 3, 3)(2, 0) = n2;
+ getSubMatrix(K, 1, 6, 3, 3)(2, 1) = n1;
+ getSubMatrix(K, 1, 6, 3, 3)(2, 2) = d1;
+
+ getSubMatrix(K, 2, 5, 3, 3) = getSubMatrix(K, 1, 6, 3, 3);
+ getSubMatrix(K, 2, 5, 3, 3)(0, 1) = n2;
+ getSubMatrix(K, 2, 5, 3, 3)(1, 0) = e2;
+ getSubMatrix(K, 2, 5, 3, 3)(1, 2) = e1;
+ getSubMatrix(K, 2, 5, 3, 3)(2, 1) = e1;
+
+ getSubMatrix(K, 0, 7, 3, 3) = getSubMatrix(K, 1, 6, 3, 3);
+ getSubMatrix(K, 0, 7, 3, 3)(0, 1) = n2;
+ getSubMatrix(K, 0, 7, 3, 3)(0, 2) = n2;
+ getSubMatrix(K, 0, 7, 3, 3)(1, 0) = e2;
+ getSubMatrix(K, 0, 7, 3, 3)(2, 0) = e2;
+
+ getSubMatrix(K, 3, 4, 3, 3) = getSubMatrix(K, 1, 6, 3, 3);
+ getSubMatrix(K, 3, 4, 3, 3)(0, 2) = n2;
+ getSubMatrix(K, 3, 4, 3, 3)(1, 2) = e1;
+ getSubMatrix(K, 3, 4, 3, 3)(2, 0) = e2;
+ getSubMatrix(K, 3, 4, 3, 3)(2, 1) = e1;
+
+ getSubMatrix(K, 0, 2, 3, 3)(0, 0) = d1;
+ getSubMatrix(K, 0, 2, 3, 3)(0, 1) = n1;
+ getSubMatrix(K, 0, 2, 3, 3)(0, 2) = e2;
+ getSubMatrix(K, 0, 2, 3, 3)(1, 0) = n1;
+ getSubMatrix(K, 0, 2, 3, 3)(1, 1) = d1;
+ getSubMatrix(K, 0, 2, 3, 3)(1, 2) = e2;
+ getSubMatrix(K, 0, 2, 3, 3)(2, 0) = n2;
+ getSubMatrix(K, 0, 2, 3, 3)(2, 1) = n2;
+ getSubMatrix(K, 0, 2, 3, 3)(2, 2) = d2;
+
+ getSubMatrix(K, 1, 3, 3, 3) = getSubMatrix(K, 0, 2, 3, 3);
+ getSubMatrix(K, 1, 3, 3, 3)(0, 1) = e1;
+ getSubMatrix(K, 1, 3, 3, 3)(0, 2) = n2;
+ getSubMatrix(K, 1, 3, 3, 3)(1, 0) = e1;
+ getSubMatrix(K, 1, 3, 3, 3)(2, 0) = e2;
+
+ getSubMatrix(K, 4, 6, 3, 3) = getSubMatrix(K, 0, 2, 3, 3);
+ getSubMatrix(K, 4, 6, 3, 3)(0, 2) = n2;
+ getSubMatrix(K, 4, 6, 3, 3)(1, 2) = n2;
+ getSubMatrix(K, 4, 6, 3, 3)(2, 0) = e2;
+ getSubMatrix(K, 4, 6, 3, 3)(2, 1) = e2;
+
+ getSubMatrix(K, 5, 7, 3, 3) = getSubMatrix(K, 0, 2, 3, 3);
+ getSubMatrix(K, 5, 7, 3, 3)(0, 1) = e1;
+ getSubMatrix(K, 5, 7, 3, 3)(1, 0) = e1;
+ getSubMatrix(K, 5, 7, 3, 3)(1, 2) = n2;
+ getSubMatrix(K, 5, 7, 3, 3)(2, 1) = e2;
+ }
+
+ // Cube diagonals
+ {
+ double d = (-a - 2.0 * c) / 36.0;
+ double e = (b + c) / 24.0;
+ double n = -e;
+
+ getSubMatrix(K, 0, 6, 3, 3).setConstant(n);
+ getSubMatrix(K, 0, 6, 3, 3).diagonal().setConstant(d);
+
+ getSubMatrix(K, 1, 7, 3, 3).setConstant(e);
+ getSubMatrix(K, 1, 7, 3, 3).diagonal().setConstant(d);
+ getSubMatrix(K, 1, 7, 3, 3)(1, 2) = n;
+ getSubMatrix(K, 1, 7, 3, 3)(2, 1) = n;
+
+ getSubMatrix(K, 2, 4, 3, 3).setConstant(e);
+ getSubMatrix(K, 2, 4, 3, 3).diagonal().setConstant(d);
+ getSubMatrix(K, 2, 4, 3, 3)(0, 1) = n;
+ getSubMatrix(K, 2, 4, 3, 3)(1, 0) = n;
+
+ getSubMatrix(K, 3, 5, 3, 3).setConstant(e);
+ getSubMatrix(K, 3, 5, 3, 3).diagonal().setConstant(d);
+ getSubMatrix(K, 3, 5, 3, 3)(0, 2) = n;
+ getSubMatrix(K, 3, 5, 3, 3)(2, 0) = n;
+ }
+
+ // Use symmetry to complete the triangular inferior part of K
+ K.triangularView<Eigen::StrictlyLower>().setZero();
+ K += K.triangularView<Eigen::StrictlyUpper>().adjoint();
+ }
+ addSubMatrix(K, nodeIdsVectorForm, 3 , &m_expectedStiffnessMatrix);
+ }
+
+ void computeExpectedMassMatrix(std::vector<size_t> nodeIdsVectorForm)
+ {
+ using SurgSim::Math::addSubMatrix;
+
+ Eigen::Matrix<double, 24, 24> M;
+ M.setZero();
+
+ // "Physically-Based Simulation of Objects Represented by Surface Meshes"
+ // Muller, Techner, Gross, CGI 2004
+ // Given the shape functions they defined on Appendix A for the cube we are testing
+ // We can derive the mass matrix M = \int_V rho N^T.N dV
+ // N being a matrix (3x24) of shape functions
+ // cf documentation
+
+ double a = 1.0 / 27.0;
+ double b = a / 2.0;
+ double c = a / 4.0;
+ double d = a / 8.0;
+
+ M.diagonal().setConstant(a);
+
+ M.block(0, 3, 21, 21).diagonal().setConstant(b);
+ M.block(3*3, 3*4, 3, 3).diagonal().setConstant(c); // block (3, 4)
+
+ M.block(0, 6, 18, 18).diagonal().setConstant(c);
+ M.block(3*2, 3*4, 6, 6).diagonal().setConstant(d); // block (2, 4) and block (3, 5)
+
+ M.block(0, 9, 15, 15).diagonal().setConstant(c);
+ M.block(3*0, 3*3, 3, 3).diagonal().setConstant(b); // block (0, 3)
+ M.block(3*4, 3*7, 3, 3).diagonal().setConstant(b); // block (4, 7)
+
+ M.block(0, 12, 12, 12).diagonal().setConstant(b);
+
+ M.block(0, 15, 9, 9).diagonal().setConstant(c);
+
+ M.block(0, 18, 6, 6).diagonal().setConstant(d);
+
+ M.block(0, 21, 3, 3).diagonal().setConstant(c);
+
+ // Symmetry
+ for (size_t row = 0; row < 24; ++row)
+ {
+ for (size_t col = row+1; col < 24; ++col)
+ {
+ M(col, row) = M(row, col);
+ }
+ }
+
+ M *= m_rho;
+ addSubMatrix(M, nodeIdsVectorForm, 3 , &m_expectedMassMatrix);
+ }
+
+ virtual void SetUp() override
+ {
+ using SurgSim::Math::getSubVector;
+ using SurgSim::Math::getSubMatrix;
+ using SurgSim::Math::addSubMatrix;
+
+ m_restState.setNumDof(3, 8);
+ Vector& x0 = m_restState.getPositions();
+
+ // Cube is aligned with the axis (X,Y,Z), centered on (0.0, 0.0, 0.0), of size 1
+ // 2*-----------*3
+ // / /|
+ // 6*-----------*7 | ^ y
+ // | | | |
+ // | 0 | *1 *->x
+ // | | / /
+ // 4*-----------*5 z
+ getSubVector(x0, 0, 3) = Vector3d(-0.5,-0.5,-0.5);
+ getSubVector(x0, 1, 3) = Vector3d( 0.5,-0.5,-0.5);
+ getSubVector(x0, 2, 3) = Vector3d(-0.5, 0.5,-0.5);
+ getSubVector(x0, 3, 3) = Vector3d( 0.5, 0.5,-0.5);
+ getSubVector(x0, 4, 3) = Vector3d(-0.5,-0.5, 0.5);
+ getSubVector(x0, 5, 3) = Vector3d( 0.5,-0.5, 0.5);
+ getSubVector(x0, 6, 3) = Vector3d(-0.5, 0.5, 0.5);
+ getSubVector(x0, 7, 3) = Vector3d( 0.5, 0.5, 0.5);
+
+ // Ordering following the description in
+ // "Physically-Based Simulation of Objects Represented by Surface Meshes"
+ // Muller, Techner, Gross, CGI 2004
+ std::array<size_t, 8> tmpNodeIds = {{0, 1, 3, 2, 4, 5, 7, 6}};
+ m_nodeIds = tmpNodeIds;
+
+ // Useful for assembly helper function
+ std::vector<size_t> nodeIdsVectorForm(tmpNodeIds.begin(), tmpNodeIds.end());
+
+ // Build the expected x0 vector
+ for (size_t i = 0; i < 8; i++)
+ {
+ getSubVector(m_expectedX0, i, 3) = getSubVector(x0, m_nodeIds[i], 3);
+ }
+
+ // The cube has a size of 1, so its volume is 1m^3
+ m_expectedVolume = 1.0;
+
+ m_rho = 1000.0;
+ m_E = 1e6;
+ m_nu = 0.45;
+
+ m_expectedMassMatrix.resize(3*8, 3*8);
+ m_expectedMassMatrix.setZero();
+ m_expectedDampingMatrix.resize(3*8, 3*8);
+ m_expectedDampingMatrix.setZero();
+ m_expectedStiffnessMatrix.resize(3*8, 3*8);
+ m_expectedStiffnessMatrix.setZero();
+ m_vectorOnes.resize(3*8);
+ m_vectorOnes.setConstant(1.0);
+
+ computeExpectedMassMatrix(nodeIdsVectorForm);
+ m_expectedDampingMatrix.setZero();
+ computeExpectedStiffnessMatrix(nodeIdsVectorForm);
+ }
+
+ // This method tests all node permutations for both face definition (2 groups of 4 indices)
+ // keeping their ordering intact (CW or CCW)
+ void testNodeOrderingAllPermutations(const SurgSim::Math::OdeState& m_restState,
+ size_t id0, size_t id1, size_t id2, size_t id3,
+ size_t id4, size_t id5, size_t id6, size_t id7,
+ bool expectThrow)
+ {
+ std::array<size_t, 4> face1 = {{id0, id1, id2, id3}};
+ std::array<size_t, 4> face2 = {{id4, id5, id6, id7}};
+
+ // Shuffle the faces to create all the possible permutations
+ for (size_t face1Permutation = 0; face1Permutation < 4; face1Permutation++)
+ {
+ for (size_t face2Permutation = 0; face2Permutation < 4; face2Permutation++)
+ {
+ std::array<size_t, 8> ids;
+ for (size_t index = 0; index < 4; index++)
+ {
+ ids[ index] = face1[(index + face1Permutation) % 4];
+ ids[4 + index] = face2[(index + face2Permutation) % 4];
+ }
+
+ // Test this permutation
+ if (expectThrow)
+ {
+ EXPECT_ANY_THROW({auto cube = getCubeElement(ids); cube->initialize(m_restState);});
+ }
+ else
+ {
+ EXPECT_NO_THROW({auto cube = getCubeElement(ids); cube->initialize(m_restState);});
+ }
+ }
+ }
+ }
+};
+
+extern void testSize(const Vector& v, int expectedSize);
+extern void testSize(const Matrix& m, int expectedRows, int expectedCols);
+
+TEST_F(Fem3DElementCubeTests, ConstructorTest)
+{
+ ASSERT_NO_THROW({MockFem3DElementCube cube(m_nodeIds);});
+ ASSERT_NO_THROW({MockFem3DElementCube* cube = new MockFem3DElementCube(m_nodeIds); delete cube;});
+ ASSERT_NO_THROW({std::shared_ptr<MockFem3DElementCube> cube =
+ std::make_shared<MockFem3DElementCube>(m_nodeIds);});
+}
+
+TEST_F(Fem3DElementCubeTests, InitializeTest)
+{
+ {
+ SCOPED_TRACE("Invalid node ids");
+
+ std::array<size_t, 8> invalidNodeIds = {{0, 1, 2, 3, 4, 10, 9, 8}};
+ ASSERT_NO_THROW({auto cube = getCubeElement(invalidNodeIds);});
+ auto cube = getCubeElement(invalidNodeIds);
+ ASSERT_THROW(cube->initialize(m_restState), SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Valid node ids");
+
+ ASSERT_NO_THROW({auto cube = getCubeElement(m_nodeIds);});
+ auto cube = getCubeElement(m_nodeIds);
+ ASSERT_NO_THROW(cube->initialize(m_restState));
+ }
+}
+
+TEST_F(Fem3DElementCubeTests, NodeIdsTest)
+{
+ Fem3DElementCube cube(m_nodeIds);
+ EXPECT_EQ(8u, cube.getNumNodes());
+ EXPECT_EQ(8u, cube.getNodeIds().size());
+ for (int i = 0; i < 8; i++)
+ {
+ EXPECT_EQ(m_nodeIds[i], cube.getNodeId(i));
+ EXPECT_EQ(m_nodeIds[i], cube.getNodeIds()[i]);
+ }
+}
+
+TEST_F(Fem3DElementCubeTests, VolumeTest)
+{
+ {
+ SCOPED_TRACE("Volume valid and positive");
+
+ auto cube = getCubeElement(m_nodeIds);
+ cube->initialize(m_restState); // rest volume is computed by the initialize method
+ EXPECT_NEAR(cube->getRestVolume(), m_expectedVolume, 1e-10);
+ EXPECT_NEAR(cube->getVolume(m_restState), m_expectedVolume, 1e-10);
+ }
+
+ {
+ SCOPED_TRACE("Volume valid but negative");
+
+ std::array<size_t, 8> nodeIds = {{0, 1, 3, 2, 4, 6, 7, 5}};
+ auto cube = getCubeElement(nodeIds);
+ ASSERT_THROW(cube->getVolume(m_restState), SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Volume invalid, degenerated cube");
+
+ // Copy the 1st 4 points over the 4 following points, so the cube degenerate to a square
+ m_restState.getPositions().segment<3 * 4>(12) = m_restState.getPositions().segment<3 * 4>(0);
+ auto cube = getCubeElement(m_nodeIds);
+ ASSERT_THROW(cube->getVolume(m_restState), SurgSim::Framework::AssertionFailure);
+ }
+}
+
+TEST_F(Fem3DElementCubeTests, NodeOrderingTest)
+{
+ // Any definition starting with a 1st face defined CW
+ // followed by the opposite face defined CCW is valid.
+
+ // Front face CW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 6, 7, 5, 4, 2, 3, 1, 0, false);
+ // Front face CW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 6, 7, 5, 4, 2, 0, 1, 3, true);
+ // Front face CCW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 6, 4, 5, 7, 2, 3, 1, 0, true);
+ // Front face CCW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 6, 4, 5, 7, 2, 0, 1, 3, true);
+
+ // Back face CW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 0, 1, 3, 2, 4, 5, 7, 6, false);
+ // Back face CW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 0, 1, 3, 2, 4, 6, 7, 5, true);
+ // Back face CCW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 0, 2, 3, 1, 4, 5, 7, 6, true);
+ // Back face CCW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 0, 2, 3, 1, 4, 6, 7, 5, true);
+
+ // Top face CW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 2, 3, 7, 6, 0, 1, 5, 4, false);
+ // Top face CW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 2, 3, 7, 6, 0, 4, 5, 1, true);
+ // Top face CCW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 2, 6, 7, 3, 0, 1, 5, 4, true);
+ // Top face CCW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 2, 6, 7, 3, 0, 4, 5, 1, true);
+
+ // Bottom face CW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 0, 4, 5, 1, 6, 7, 3, 2, false);
+ // Bottom face CW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 0, 4, 5, 1, 6, 2, 3, 7, true);
+ // Bottom face CCW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 0, 1, 5, 4, 6, 7, 3, 2, true);
+ // Bottom face CCW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 0, 1, 5, 4, 6, 2, 3, 7, true);
+
+ // Right face CW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 3, 1, 5, 7, 2, 0, 4, 6, false);
+ // Right face CW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 3, 1, 5, 7, 2, 6, 4, 0, true);
+ // Right face CCW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 3, 7, 5, 1, 2, 0, 4, 6, true);
+ // Right face CCW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 3, 7, 5, 1, 2, 6, 4, 0, true);
+
+ // Left face CW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 0, 2, 6, 4, 1, 3, 7, 5, false);
+ // Left face CW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 0, 2, 6, 4, 1, 5, 7, 3, true);
+ // Left face CCW, opposite face CCW
+ testNodeOrderingAllPermutations(m_restState, 0, 4, 6, 2, 1, 3, 7, 5, true);
+ // Left face CCW, opposite face CW
+ testNodeOrderingAllPermutations(m_restState, 0, 4, 6, 2, 1, 5, 7, 3, true);
+}
+
+TEST_F(Fem3DElementCubeTests, ShapeFunctionsTest)
+{
+ using SurgSim::Math::getSubVector;
+
+ auto cube = getCubeElement(m_nodeIds);
+ cube->initialize(m_restState);
+
+ EXPECT_TRUE(cube->getInitialPosition().isApprox(m_expectedX0)) <<
+ "x0 = " << cube->getInitialPosition().transpose() << std::endl << "x0 expected = " << m_expectedX0.transpose();
+
+ // We should have by construction:
+ // { N0(p0) = 1 N1(p0)=N2(p0)=N3(p0)=0
+ // { N1(p1) = 1 N1(p1)=N2(p1)=N3(p1)=0
+ // { N2(p2) = 1 N1(p2)=N2(p2)=N3(p2)=0
+ // { N3(p3) = 1 N1(p3)=N2(p3)=N3(p3)=0
+ Vector3d p[8];
+ for (size_t nodeId = 0; nodeId < 8; ++nodeId)
+ {
+ // retrieving the points from expectedX0 with indices 0..7
+ // which is equivalent to
+ // retrieving the points from m_restState with indices m_nodeIds[0]..m_nodeIds[7]
+ p[nodeId] = getSubVector(m_expectedX0, nodeId, 3);
+ }
+ double Ni_p0[8], Ni_p1[8], Ni_p2[8], Ni_p3[8], Ni_p4[8], Ni_p5[8], Ni_p6[8], Ni_p7[8];
+ for (int i = 0; i < 8; i++)
+ {
+ Ni_p0[i] = cube->evaluateN(i, -1.0, -1.0, -1.0);
+ Ni_p1[i] = cube->evaluateN(i, +1.0, -1.0, -1.0);
+ Ni_p2[i] = cube->evaluateN(i, +1.0, +1.0, -1.0);
+ Ni_p3[i] = cube->evaluateN(i, -1.0, +1.0, -1.0);
+ Ni_p4[i] = cube->evaluateN(i, -1.0, -1.0, +1.0);
+ Ni_p5[i] = cube->evaluateN(i, +1.0, -1.0, +1.0);
+ Ni_p6[i] = cube->evaluateN(i, +1.0, +1.0, +1.0);
+ Ni_p7[i] = cube->evaluateN(i, -1.0, +1.0, +1.0);
+ }
+ EXPECT_NEAR(Ni_p0[0], 1.0, 1e-12);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ if (i == 0) continue;
+ EXPECT_NEAR(Ni_p0[i], 0.0, 1e-12);
+ }
+
+ EXPECT_NEAR(Ni_p1[1], 1.0, 1e-12);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ if (i == 1) continue;
+ EXPECT_NEAR(Ni_p1[i], 0.0, 1e-12);
+ }
+
+ EXPECT_NEAR(Ni_p2[2], 1.0, 1e-12);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ if (i == 2) continue;
+ EXPECT_NEAR(Ni_p2[i], 0.0, 1e-12);
+ }
+
+ EXPECT_NEAR(Ni_p3[3], 1.0, 1e-12);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ if (i == 3) continue;
+ EXPECT_NEAR(Ni_p3[i], 0.0, 1e-12);
+ }
+
+ EXPECT_NEAR(Ni_p4[4], 1.0, 1e-12);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ if (i == 4) continue;
+ EXPECT_NEAR(Ni_p4[i], 0.0, 1e-12);
+ }
+
+ EXPECT_NEAR(Ni_p5[5], 1.0, 1e-12);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ if (i == 5) continue;
+ EXPECT_NEAR(Ni_p5[i], 0.0, 1e-12);
+ }
+
+ EXPECT_NEAR(Ni_p6[6], 1.0, 1e-12);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ if (i == 6) continue;
+ EXPECT_NEAR(Ni_p6[i], 0.0, 1e-12);
+ }
+
+ EXPECT_NEAR(Ni_p7[7], 1.0, 1e-12);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ if (i == 7) continue;
+ EXPECT_NEAR(Ni_p7[i], 0.0, 1e-12);
+ }
+
+ // We should have the relation sum(Ni(x,y,z) = 1) for all points in the volume
+ // We verify that relation by sampling the tetrahedron volume
+ for (double epsilon = -1.0; epsilon <= 1.0; epsilon+=0.1)
+ {
+ for (double eta = -1.0; eta <= 1.0; eta+=0.1)
+ {
+ for (double mu = -1.0; mu <= 1.0; mu+=0.1)
+ {
+ double Ni_p[8];
+ double sum = 0.0;
+ for (int i = 0; i < 8; i++)
+ {
+ Ni_p[i] = cube->evaluateN(i, epsilon, eta, mu);
+ sum += Ni_p[i];
+ }
+ EXPECT_NEAR(sum, 1.0, 1e-10) <<
+ " for epsilon = " << epsilon << ", eta = " << eta << ", mu = " << mu << std::endl <<
+ " N0(epsilon,eta,mu) = " << Ni_p[0] << " N1(epsilon,eta,mu) = " << Ni_p[1] <<
+ " N2(epsilon,eta,mu) = " << Ni_p[2] << " N3(epsilon,eta,mu) = " << Ni_p[3] <<
+ " N4(epsilon,eta,mu) = " << Ni_p[4] << " N5(epsilon,eta,mu) = " << Ni_p[5] <<
+ " N6(epsilon,eta,mu) = " << Ni_p[6] << " N7(epsilon,eta,mu) = " << Ni_p[7];
+ }
+ }
+ }
+}
+
+TEST_F(Fem3DElementCubeTests, CoordinateTests)
+{
+ auto cube = getCubeElement(m_nodeIds);
+ cube->initialize(m_restState);
+
+ {
+ // Non-normalize node
+ SurgSim::Math::Vector nodePositions(8);
+ nodePositions << 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+ EXPECT_FALSE(cube->isValidCoordinate(nodePositions));
+
+ // Node with point which is outside of the cube
+ nodePositions << 1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+ EXPECT_FALSE(cube->isValidCoordinate(nodePositions));
+
+ // Normal node
+ nodePositions << 0.5, 0.25, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0;
+ EXPECT_TRUE(cube->isValidCoordinate(nodePositions));
+
+ }
+
+ {
+ // Node with more than 8 coordinate points
+ SurgSim::Math::Vector nodePositions(9);
+ nodePositions << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+ EXPECT_FALSE(cube->isValidCoordinate(nodePositions));
+ }
+
+ {
+ // Node with some coordinates less than 0 but greater than epsilon and
+ // some greater than 1 and less than (1 + epsilon).
+ SurgSim::Math::Vector nodePositions(8);
+ nodePositions << -1e-11, 1.0 + 1e-11, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+ EXPECT_TRUE(cube->isValidCoordinate(nodePositions));
+ }
+
+ // Test computeCartesianCoordinate.
+ {
+ // Compute central point of the cube
+ SurgSim::Math::Vector nodePositions(8);
+ nodePositions << 0.125, 0.125, 0.125, 0.125, 0.125, 0.125, 0.125, 0.125;
+ EXPECT_TRUE(cube->isValidCoordinate(nodePositions));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(0.0, 0.0, 0.0).isApprox(
+ cube->computeCartesianCoordinate(m_restState, nodePositions), epsilon));
+ }
+
+ {
+ SurgSim::Math::Vector nodePositions(8);
+ nodePositions << 0.01, 0.07, 0.11, 0.05, 0.0, 0.23, 0.13, 0.4;
+ EXPECT_TRUE(cube->isValidCoordinate(nodePositions));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(0.04, 0.19, 0.26).isApprox(
+ cube->computeCartesianCoordinate(m_restState, nodePositions), epsilon));
+ // 0.01 * (-0.5,-0.5,-0.5) => (-0.005, -0.005, -0.005)
+ // 0.07 * ( 0.5,-0.5,-0.5) => ( 0.035, -0.035, -0.035)
+ // 0.11 * ( 0.5, 0.5,-0.5) => ( 0.055, 0.055, -0.055)
+ // 0.05 * (-0.5, 0.5,-0.5) => (-0.025, 0.025, -0.025)
+ // 0.0 * (-0.5,-0.5, 0.5) => (-0.00, -0.00, 0.00)
+ // 0.23 * ( 0.5,-0.5, 0.5) => ( 0.115, -0.115, 0.115)
+ // 0.13 * ( 0.5, 0.5, 0.5) => ( 0.065, 0.065, 0.065)
+ // 0.4 * (-0.5, 0.5, 0.5) => (-0.20, 0.20, 0.20)
+ // = ( 0.04, 0.19, 0.26)
+ }
+
+ // Test computeNaturalCoordinate.
+ EXPECT_THROW(cube->computeNaturalCoordinate(m_restState, SurgSim::Math::Vector3d(0.0, 0.0, 0.0)),
+ SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(Fem3DElementCubeTests, ForceAndMatricesTest)
+{
+ using SurgSim::Math::getSubVector;
+
+ auto cube = getCubeElement(m_nodeIds);
+ cube->initialize(m_restState);
+
+ SurgSim::Math::Vector forceVector(3*8);
+ SurgSim::Math::Matrix massMatrix(3*8, 3*8);
+ SurgSim::Math::Matrix dampingMatrix(3*8, 3*8);
+ SurgSim::Math::Matrix stiffnessMatrix(3*8, 3*8);
+
+ forceVector.setZero();
+ massMatrix.setZero();
+ dampingMatrix.setZero();
+ stiffnessMatrix.setZero();
+
+ // No force should be produced when in rest state (x = x0) => F = K.(x-x0) = 0
+ cube->addForce(m_restState, &forceVector);
+ EXPECT_TRUE(forceVector.isZero());
+
+ cube->addMass(m_restState, &massMatrix);
+ EXPECT_TRUE(massMatrix.isApprox(m_expectedMassMatrix)) <<
+ "Expected mass matrix :" << std::endl << m_expectedMassMatrix << std::endl << std::endl <<
+ "Mass matrix :" << std::endl << massMatrix << std::endl << std::endl <<
+ "Error on the mass matrix is " << std::endl << m_expectedMassMatrix - massMatrix << std::endl;
+
+ cube->addDamping(m_restState, &dampingMatrix);
+ EXPECT_TRUE(dampingMatrix.isApprox(m_expectedDampingMatrix));
+
+ cube->addStiffness(m_restState, &stiffnessMatrix);
+ EXPECT_TRUE(stiffnessMatrix.isApprox(m_expectedStiffnessMatrix)) <<
+ "Error on the stiffness matrix is " << std::endl << m_expectedStiffnessMatrix - stiffnessMatrix << std::endl;
+
+ forceVector.setZero();
+ massMatrix.setZero();
+ dampingMatrix.setZero();
+ stiffnessMatrix.setZero();
+
+ cube->addFMDK(m_restState, &forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
+ EXPECT_TRUE(forceVector.isZero());
+ EXPECT_TRUE(massMatrix.isApprox(m_expectedMassMatrix));
+ EXPECT_TRUE(dampingMatrix.isApprox(m_expectedDampingMatrix));
+ EXPECT_TRUE(stiffnessMatrix.isApprox(m_expectedStiffnessMatrix));
+
+ // Test addMatVec API with Mass component only
+ forceVector.setZero();
+ cube->addMatVec(m_restState, 1.0, 0.0, 0.0, m_vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 3 * 8; rowId++)
+ {
+ EXPECT_NEAR(m_expectedMassMatrix.row(rowId).sum(), forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with Damping component only
+ forceVector.setZero();
+ cube->addMatVec(m_restState, 0.0, 1.0, 0.0, m_vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 3 * 8; rowId++)
+ {
+ EXPECT_NEAR(m_expectedDampingMatrix.row(rowId).sum(), forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with Stiffness component only
+ forceVector.setZero();
+ cube->addMatVec(m_restState, 0.0, 0.0, 1.0, m_vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 3 * 8; rowId++)
+ {
+ EXPECT_NEAR(m_expectedStiffnessMatrix.row(rowId).sum(), forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with mix Mass/Damping/Stiffness components
+ forceVector.setZero();
+ cube->addMatVec(m_restState, 1.0, 2.0, 3.0, m_vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 3 * 8; rowId++)
+ {
+ double expectedCoef = 1.0 * m_expectedMassMatrix.row(rowId).sum() +
+ 2.0 * m_expectedDampingMatrix.row(rowId).sum() +
+ 3.0 * m_expectedStiffnessMatrix.row(rowId).sum();
+ EXPECT_NEAR(expectedCoef, forceVector[rowId], epsilon);
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/Fem3DElementTetrahedronTests.cpp b/SurgSim/Physics/UnitTests/Fem3DElementTetrahedronTests.cpp
new file mode 100644
index 0000000..badf494
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DElementTetrahedronTests.cpp
@@ -0,0 +1,535 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <array>
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+
+using SurgSim::Physics::Fem3DElementTetrahedron;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Matrix;
+
+namespace
+{
+/// Useful function to compute a shape function
+/// \param i ith shape function
+/// \param ai, bi, ci, di The shape functions parameters
+/// \param p The point to evaluate the shape function at
+/// \return Ni(p) = 1/6V . (ai + bi.px + ci.py + di.pz)
+double N(size_t i, double V, double *ai, double *bi, double *ci, double *di, const Vector3d& p)
+{
+ double inv6V = 1.0 / (6.0 * V);
+ return inv6V * (ai[i] + bi[i] * p[0] + ci[i] * p[1] + di[i] * p[2]);
+}
+
+const double epsilon = 1e-9;
+};
+
+class MockFem3DElementTet : public Fem3DElementTetrahedron
+{
+public:
+ MockFem3DElementTet(std::array<size_t, 4> nodeIds) : Fem3DElementTetrahedron(nodeIds)
+ {
+ }
+
+ double getRestVolume() const
+ {
+ return m_restVolume;
+ }
+
+ void getShapeFunction(int i, double *ai, double *bi, double *ci, double *di) const
+ {
+ *ai = m_ai[i];
+ *bi = m_bi[i];
+ *ci = m_ci[i];
+ *di = m_di[i];
+ }
+
+ const Eigen::Matrix<double, 12, 1>& getInitialPosition() const
+ {
+ return m_x0;
+ }
+
+ void setupInitialParams(const SurgSim::Math::OdeState &state,
+ double massDensity,
+ double poissonRatio,
+ double youngModulus)
+ {
+ setMassDensity(massDensity);
+ setPoissonRatio(poissonRatio);
+ setYoungModulus(youngModulus);
+ initialize(state);
+ }
+};
+
+class Fem3DElementTetrahedronTests : public ::testing::Test
+{
+public:
+ std::array<size_t, 4> m_nodeIds;
+ SurgSim::Math::OdeState m_restState;
+ double m_expectedVolume;
+ Eigen::Matrix<double, 12, 1> m_expectedX0;
+ double m_rho, m_E, m_nu;
+ SurgSim::Math::Matrix m_expectedMassMatrix, m_expectedDampingMatrix;
+ SurgSim::Math::Matrix m_expectedStiffnessMatrix, m_expectedStiffnessMatrix2;
+ SurgSim::Math::Vector m_vectorOnes;
+
+ virtual void SetUp() override
+ {
+ using SurgSim::Math::getSubVector;
+ using SurgSim::Math::getSubMatrix;
+ using SurgSim::Math::addSubMatrix;
+
+ m_nodeIds[0] = 3;
+ m_nodeIds[1] = 1;
+ m_nodeIds[2] = 14;
+ m_nodeIds[3] = 9;
+ std::vector<size_t> m_nodeIdsVectorForm; // Useful for assembly helper function
+ m_nodeIdsVectorForm.push_back(m_nodeIds[0]);
+ m_nodeIdsVectorForm.push_back(m_nodeIds[1]);
+ m_nodeIdsVectorForm.push_back(m_nodeIds[2]);
+ m_nodeIdsVectorForm.push_back(m_nodeIds[3]);
+
+ m_restState.setNumDof(3, 15);
+ Vector& x0 = m_restState.getPositions();
+ // Tet is aligned with the axis (X,Y,Z), centered on (0.1, 1.2, 2.3), embedded in a cube of size 1
+ getSubVector(m_expectedX0, 0, 3) = getSubVector(x0, m_nodeIds[0], 3) = Vector3d(0.1, 1.2, 2.3);
+ getSubVector(m_expectedX0, 1, 3) = getSubVector(x0, m_nodeIds[1], 3) = Vector3d(1.1, 1.2, 2.3);
+ getSubVector(m_expectedX0, 2, 3) = getSubVector(x0, m_nodeIds[2], 3) = Vector3d(0.1, 2.2, 2.3);
+ getSubVector(m_expectedX0, 3, 3) = getSubVector(x0, m_nodeIds[3], 3) = Vector3d(0.1, 1.2, 3.3);
+
+ // The tet is part of a cube of size 1x1x1 (it occupies 1/6 of the cube's volume)
+ m_expectedVolume = 1.0 / 6.0;
+
+ m_rho = 1000.0;
+ m_E = 1e6;
+ m_nu = 0.45;
+
+ m_expectedMassMatrix.resize(3*15, 3*15);
+ m_expectedMassMatrix.setZero();
+ m_expectedDampingMatrix.resize(3*15, 3*15);
+ m_expectedDampingMatrix.setZero();
+ m_expectedStiffnessMatrix.resize(3*15, 3*15);
+ m_expectedStiffnessMatrix.setZero();
+ m_expectedStiffnessMatrix2.resize(3*15, 3*15);
+ m_expectedStiffnessMatrix2.setZero();
+ m_vectorOnes.resize(3*15);
+ m_vectorOnes.setConstant(1.0);
+
+ Eigen::Matrix<double, 12, 12> M;
+ M.setZero();
+ {
+ M.diagonal().setConstant(2.0);
+ M.block(0, 3, 9, 9).diagonal().setConstant(1.0);
+ M.block(0, 6, 6, 6).diagonal().setConstant(1.0);
+ M.block(0, 9, 3, 3).diagonal().setConstant(1.0);
+ M.block(3, 0, 9, 9).diagonal().setConstant(1.0);
+ M.block(6, 0, 6, 6).diagonal().setConstant(1.0);
+ M.block(9, 0, 3, 3).diagonal().setConstant(1.0);
+ }
+ M *= m_rho * m_expectedVolume / 20.0;
+ addSubMatrix(M, m_nodeIdsVectorForm, 3 , &m_expectedMassMatrix);
+
+ m_expectedDampingMatrix.setZero();
+
+ Eigen::Matrix<double, 12, 12> K;
+ K.setZero();
+ {
+ // Calculation done by hand from
+ // http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf
+ // ai = {}
+ // bi = {-1 1 0 0}
+ // ci = {-1 0 1 0}
+ // di = {-1 0 0 1}
+ Eigen::Matrix<double, 6, 12> B;
+ Eigen::Matrix<double, 6, 6> E;
+
+ B.setZero();
+ B(0, 0) = -1; B(0, 3) = 1;
+ B(1, 1) = -1; B(1, 7) = 1;
+ B(2, 2) = -1; B(2, 11) = 1;
+ B(3, 0) = -1; B(3, 1) = -1; B(3, 4) = 1; B(3, 6) = 1;
+ B(4, 1) = -1; B(4, 2) = -1; B(4, 8) = 1; B(4, 10) = 1;
+ B(5, 0) = -1; B(5, 2) = -1; B(5, 5) = 1; B(5, 9) = 1;
+ B *= 1.0 / (6.0 * m_expectedVolume);
+
+ E.setZero();
+ E.block(0, 0, 3, 3).setConstant(m_nu);
+ E.block(0, 0, 3, 3).diagonal().setConstant(1.0 - m_nu);
+ E.block(3, 3, 3, 3).diagonal().setConstant(0.5 - m_nu);
+ E *= m_E / (( 1.0 + m_nu) * (1.0 - 2.0 * m_nu));
+
+ K = m_expectedVolume * B.transpose() * E * B;
+ }
+ addSubMatrix(K, m_nodeIdsVectorForm, 3 , &m_expectedStiffnessMatrix);
+
+ // Expecte stiffness matrix given for our case in:
+ // http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf
+ double E = m_E / (12.0*(1.0 - 2.0*m_nu)*(1.0 + m_nu));
+ double n0 = 1.0 - 2.0 * m_nu;
+ double n1 = 1.0 - m_nu;
+ K.setZero();
+
+ // Fill up the upper triangle part first (without diagonal elements)
+ K(0, 1) = K(0, 2) = K(1, 2) = 1.0;
+
+ K(0, 3) = -2.0 * n1; K(0, 4) = -n0; K(0, 5) = -n0;
+ K(1, 3) = -2.0 * m_nu; K(1, 4) = -n0;
+ K(2, 3) = -2.0 * m_nu; K(2, 5) = -n0;
+
+ K(0, 6) = - n0; K(0, 7) = -2.0 * m_nu;
+ K(1, 6) = - n0; K(1, 7) = -2.0 * n1; K(1, 8) = - n0;
+ K(2, 7) = - 2.0 * m_nu; K(2, 8) = -n0;
+
+ K(0, 9) = - n0; K(0, 11) = -2.0 * m_nu;
+ K(1, 10) = - n0; K(1, 11) = -2.0 * m_nu;
+ K(2, 9) = - n0; K(2, 10) = - n0; K(2, 11) = -2.0 * n1;
+
+ K(3, 7) = K(3, 11) = 2.0 * m_nu;
+ K(4, 6) = n0;
+ K(5, 9) = n0;
+ K(7, 11) = 2.0 * m_nu;
+ K(8, 10) = n0;
+
+ K += K.transpose().eval(); // symmetric part (do not forget the .eval() !)
+
+ K.block(0,0,3,3).diagonal().setConstant(4.0 - 6.0 * m_nu); // diagonal elements
+ K.block(3,3,9,9).diagonal().setConstant(n0); // diagonal elements
+ K(3, 3) = K(7, 7) = K(11, 11) = 2.0 * n1; // diagonal elements
+
+ K *= E;
+
+ addSubMatrix(K, m_nodeIdsVectorForm, 3 , &m_expectedStiffnessMatrix2);
+ }
+};
+
+extern void testSize(const Vector& v, int expectedSize);
+extern void testSize(const Matrix& m, int expectedRows, int expectedCols);
+
+TEST_F(Fem3DElementTetrahedronTests, ConstructorTest)
+{
+ ASSERT_NO_THROW({MockFem3DElementTet tet(m_nodeIds);});
+ ASSERT_NO_THROW({MockFem3DElementTet* tet = new MockFem3DElementTet(m_nodeIds); delete tet;});
+ ASSERT_NO_THROW({std::shared_ptr<MockFem3DElementTet> tet =
+ std::make_shared<MockFem3DElementTet>(m_nodeIds);});
+}
+
+TEST_F(Fem3DElementTetrahedronTests, NodeIdsTest)
+{
+ Fem3DElementTetrahedron tet(m_nodeIds);
+ EXPECT_EQ(4u, tet.getNumNodes());
+ EXPECT_EQ(4u, tet.getNodeIds().size());
+ for (int i = 0; i < 4; i++)
+ {
+ EXPECT_EQ(m_nodeIds[i], tet.getNodeId(i));
+ EXPECT_EQ(m_nodeIds[i], tet.getNodeIds()[i]);
+ }
+}
+
+TEST_F(Fem3DElementTetrahedronTests, VolumeTest)
+{
+ MockFem3DElementTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ EXPECT_NEAR(tet.getRestVolume(), m_expectedVolume, 1e-10);
+ EXPECT_NEAR(tet.getVolume(m_restState), m_expectedVolume, 1e-10);
+}
+
+TEST_F(Fem3DElementTetrahedronTests, CoordinateTests)
+{
+ Fem3DElementTetrahedron element(m_nodeIds);
+ Vector3d expectedA(0.1, 1.2, 2.3);
+ Vector3d expectedB(1.1, 1.2, 2.3);
+ Vector3d expectedC(0.1, 2.2, 2.3);
+ Vector3d expectedD(0.1, 1.2, 3.3);
+
+ Vector validNaturalCoordinate(4);
+ Vector validNaturalCoordinate2(4);
+ Vector invalidNaturalCoordinateSumNot1(4);
+ Vector invalidNaturalCoordinateNegativeValue(4);
+ Vector invalidNaturalCoordinateBiggerThan1Value(4);
+ Vector invalidNaturalCoordinateSize3(3), invalidNaturalCoordinateSize5(5);
+
+ validNaturalCoordinate << 0.4, 0.3, 0.2, 0.1;
+ validNaturalCoordinate2 << -1e-11, 1.0 + 1e-11, 0.0, 0.0;
+ invalidNaturalCoordinateSumNot1 << 0.1, 0.1, 0.1, 0.1;
+ invalidNaturalCoordinateNegativeValue << 0.7, 0.7, -0.5, 0.1;
+ invalidNaturalCoordinateBiggerThan1Value << 1.4, 0.6, -1.2, 0.2;
+ invalidNaturalCoordinateSize3 << 0.4, 0.4, 0.2;
+ invalidNaturalCoordinateSize5 << 0.2, 0.2, 0.2, 0.2, 0.2;
+ EXPECT_TRUE(element.isValidCoordinate(validNaturalCoordinate));
+ EXPECT_TRUE(element.isValidCoordinate(validNaturalCoordinate2));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateSumNot1));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateNegativeValue));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateBiggerThan1Value));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateSize3));
+ EXPECT_FALSE(element.isValidCoordinate(invalidNaturalCoordinateSize5));
+
+ Vector naturalCoordinateA(4), naturalCoordinateB(4), naturalCoordinateC(4), naturalCoordinateD(4);
+ Vector naturalCoordinateMiddle(4);
+ Vector ptA, ptB, ptC, ptD, ptMiddle;
+ naturalCoordinateA << 1.0, 0.0, 0.0, 0.0;
+ naturalCoordinateB << 0.0, 1.0, 0.0, 0.0;
+ naturalCoordinateC << 0.0, 0.0, 1.0, 0.0;
+ naturalCoordinateD << 0.0, 0.0, 0.0, 1.0;
+ naturalCoordinateMiddle << 1.0 / 4.0, 1.0 / 4.0, 1.0 / 4.0, 1 / 4.0;
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateBiggerThan1Value), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateNegativeValue), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize3), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize5), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSumNot1), \
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(ptA = element.computeCartesianCoordinate(m_restState, naturalCoordinateA));
+ EXPECT_NO_THROW(ptB = element.computeCartesianCoordinate(m_restState, naturalCoordinateB));
+ EXPECT_NO_THROW(ptC = element.computeCartesianCoordinate(m_restState, naturalCoordinateC));
+ EXPECT_NO_THROW(ptD = element.computeCartesianCoordinate(m_restState, naturalCoordinateD));
+ EXPECT_NO_THROW(ptMiddle = element.computeCartesianCoordinate(m_restState, naturalCoordinateMiddle));
+ EXPECT_TRUE(ptA.isApprox(expectedA));
+ EXPECT_TRUE(ptB.isApprox(expectedB));
+ EXPECT_TRUE(ptC.isApprox(expectedC));
+ EXPECT_TRUE(ptD.isApprox(expectedD));
+ EXPECT_TRUE(ptMiddle.isApprox((expectedA + expectedB + expectedC + expectedD) / 4.0));
+
+ // Test computeNaturalCoordinate.
+ SurgSim::Math::Vector2d cartesian2d(1, 0);
+ SurgSim::Math::Vector4d cartesian4d(1, 0, 0, 0);
+ EXPECT_THROW(element.computeNaturalCoordinate(m_restState, cartesian2d), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(element.computeNaturalCoordinate(m_restState, cartesian4d), SurgSim::Framework::AssertionFailure);
+
+ std::vector<SurgSim::Math::Vector4d> listOfNaturalCoordinates;
+ listOfNaturalCoordinates.push_back(SurgSim::Math::Vector4d(1, 0, 0, 0));
+ listOfNaturalCoordinates.push_back(SurgSim::Math::Vector4d(0, 1, 0, 0));
+ listOfNaturalCoordinates.push_back(SurgSim::Math::Vector4d(0, 0, 1, 0));
+ listOfNaturalCoordinates.push_back(SurgSim::Math::Vector4d(0, 0, 0, 1));
+ listOfNaturalCoordinates.push_back(SurgSim::Math::Vector4d(0.354623, 0.768423, 0.12457, 0.327683));
+ listOfNaturalCoordinates.push_back(SurgSim::Math::Vector4d(1.354623, 2.768423, 3.12457, 4.327683));
+ listOfNaturalCoordinates.push_back(SurgSim::Math::Vector4d(0.546323, 2.435323, -69.3422, 345.423));
+ listOfNaturalCoordinates.push_back(SurgSim::Math::Vector4d(0.352346, 3.3424, 9.325324, 5.32432));
+ listOfNaturalCoordinates.push_back(SurgSim::Math::Vector4d(0.623543, 9.2345, 2.45346, 1.645745));
+ listOfNaturalCoordinates.push_back(SurgSim::Math::Vector4d(0.356234, 435.234, 32545.234, 9534.2123));
+
+ SurgSim::Math::Vector4d input, calculated(0, 0, 0, 0);
+ Vector cartesian;
+ for (auto testCase = listOfNaturalCoordinates.begin(); testCase != listOfNaturalCoordinates.end(); ++testCase)
+ {
+ input = (*testCase).cwiseAbs();
+ input /= input.sum();
+ EXPECT_NO_THROW(cartesian = element.computeCartesianCoordinate(m_restState, input));
+ EXPECT_NO_THROW(calculated = element.computeNaturalCoordinate(m_restState, cartesian););
+ EXPECT_TRUE(input.isApprox(calculated));
+ }
+}
+
+TEST_F(Fem3DElementTetrahedronTests, ShapeFunctionsTest)
+{
+ using SurgSim::Math::getSubVector;
+
+ MockFem3DElementTet tet(m_nodeIds);
+ tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
+
+ EXPECT_TRUE(tet.getInitialPosition().isApprox(m_expectedX0)) <<
+ "x0 = " << tet.getInitialPosition().transpose() << std::endl << "x0 expected = " << m_expectedX0.transpose();
+
+ double ai[4], bi[4], ci[4], di[4];
+ double sumAi = 0.0;
+ for (int i = 0; i < 4; i++)
+ {
+ tet.getShapeFunction(i, &(ai[i]), &(bi[i]), &(ci[i]), &(di[i]));
+ sumAi += ai[i];
+ }
+ EXPECT_DOUBLE_EQ(6.0 * m_expectedVolume, sumAi);
+
+ // Ni(x,y,z) = 1/6V . (ai + bi.x + ci.y + di.z)
+
+ // We should have by construction:
+ // { N0(p0) = 1 N1(p0)=N2(p0)=N3(p0)=0
+ // { N1(p1) = 1 N1(p1)=N2(p1)=N3(p1)=0
+ // { N2(p2) = 1 N1(p2)=N2(p2)=N3(p2)=0
+ // { N3(p3) = 1 N1(p3)=N2(p3)=N3(p3)=0
+ const Vector3d p0 = getSubVector(m_expectedX0, 0, 3);
+ const Vector3d p1 = getSubVector(m_expectedX0, 1, 3);
+ const Vector3d p2 = getSubVector(m_expectedX0, 2, 3);
+ const Vector3d p3 = getSubVector(m_expectedX0, 3, 3);
+ double Ni_p0[4], Ni_p1[4], Ni_p2[4], Ni_p3[4];
+ for (int i = 0; i < 4; i++)
+ {
+ Ni_p0[i] = N(i, m_expectedVolume, ai, bi, ci, di, p0);
+ Ni_p1[i] = N(i, m_expectedVolume, ai, bi, ci, di, p1);
+ Ni_p2[i] = N(i, m_expectedVolume, ai, bi, ci, di, p2);
+ Ni_p3[i] = N(i, m_expectedVolume, ai, bi, ci, di, p3);
+ }
+ EXPECT_NEAR(Ni_p0[0], 1.0, 1e-12);
+ EXPECT_NEAR(Ni_p0[1], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p0[2], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p0[3], 0.0, 1e-12);
+
+ EXPECT_NEAR(Ni_p1[0], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p1[1], 1.0, 1e-12);
+ EXPECT_NEAR(Ni_p1[2], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p1[3], 0.0, 1e-12);
+
+ EXPECT_NEAR(Ni_p2[0], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p2[1], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p2[2], 1.0, 1e-12);
+ EXPECT_NEAR(Ni_p2[3], 0.0, 1e-12);
+
+ EXPECT_NEAR(Ni_p3[0], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p3[1], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p3[2], 0.0, 1e-12);
+ EXPECT_NEAR(Ni_p3[3], 1.0, 1e-12);
+
+ // We should have the relation sum(Ni(x,y,z) = 1) for all points in the volume
+ // We verify that relation by sampling the tetrahedron volume
+ for (double sp0p1 = 0; sp0p1 <= 1.0; sp0p1+=0.1)
+ {
+ for (double sp0p2 = 0; sp0p1 + sp0p2 <= 1.0; sp0p2+=0.1)
+ {
+ for (double sp0p3 = 0; sp0p1 + sp0p2 + sp0p3 <= 1.0; sp0p3+=0.1)
+ {
+ Vector3d p = p0 + sp0p1 * (p1 - p0) + sp0p2 * (p2 - p0) + sp0p3 * (p3 - p0);
+ double Ni_p[4];
+ for (int i = 0; i < 4; i++)
+ {
+ Ni_p[i] = N(i, m_expectedVolume, ai, bi, ci, di, p);
+ }
+ EXPECT_NEAR(Ni_p[0] + Ni_p[1] + Ni_p[2] + Ni_p[3], 1.0, 1e-10) <<
+ " for sp0p1 = " << sp0p1 << ", sp0p2 = " << sp0p2 << ", sp0p3 = " << sp0p3 << std::endl <<
+ " N0(x,y,z) = " << Ni_p[0] << " N1(x,y,z) = " << Ni_p[1] <<
+ " N2(x,y,z) = " << Ni_p[2] << " N3(x,y,z) = " << Ni_p[3];
+ }
+ }
+ }
+}
+
+TEST_F(Fem3DElementTetrahedronTests, ForceAndMatricesTest)
+{
+ using SurgSim::Math::getSubVector;
+
+ MockFem3DElementTet tet(m_nodeIds);
+
+ // Test the various mode of failure related to the physical parameters
+ // This has been already tested in FemElementTests, but this is to make sure this method is called properly
+ // So the same behavior should be expected
+ {
+ // Mass density not set
+ ASSERT_ANY_THROW(tet.initialize(m_restState));
+
+ // Poisson Ratio not set
+ tet.setMassDensity(-1234.56);
+ ASSERT_ANY_THROW(tet.initialize(m_restState));
+
+ // Young modulus not set
+ tet.setPoissonRatio(0.55);
+ ASSERT_ANY_THROW(tet.initialize(m_restState));
+
+ // Invalid mass density
+ tet.setYoungModulus(-4321.33);
+ ASSERT_ANY_THROW(tet.initialize(m_restState));
+
+ // Invalid Poisson ratio
+ tet.setMassDensity(m_rho);
+ ASSERT_ANY_THROW(tet.initialize(m_restState));
+
+ // Invalid Young modulus
+ tet.setPoissonRatio(m_nu);
+ ASSERT_ANY_THROW(tet.initialize(m_restState));
+
+ tet.setYoungModulus(m_E);
+ ASSERT_NO_THROW(tet.initialize(m_restState));
+ }
+
+ SurgSim::Math::Vector forceVector(3*15);
+ SurgSim::Math::Matrix massMatrix(3*15, 3*15);
+ SurgSim::Math::Matrix dampingMatrix(3*15, 3*15);
+ SurgSim::Math::Matrix stiffnessMatrix(3*15, 3*15);
+
+ forceVector.setZero();
+ massMatrix.setZero();
+ dampingMatrix.setZero();
+ stiffnessMatrix.setZero();
+
+ // Make sure that the 2 ways of computing the expected stiffness matrix gives the same result
+ EXPECT_TRUE(m_expectedStiffnessMatrix.isApprox(m_expectedStiffnessMatrix2));
+
+ // No force should be produced when in rest state (x = x0) => F = K.(x-x0) = 0
+ tet.addForce(m_restState, &forceVector);
+ EXPECT_TRUE(forceVector.isZero());
+
+ tet.addMass(m_restState, &massMatrix);
+ EXPECT_TRUE(massMatrix.isApprox(m_expectedMassMatrix));
+
+ tet.addDamping(m_restState, &dampingMatrix);
+ EXPECT_TRUE(dampingMatrix.isApprox(m_expectedDampingMatrix));
+
+ tet.addStiffness(m_restState, &stiffnessMatrix);
+ EXPECT_TRUE(stiffnessMatrix.isApprox(m_expectedStiffnessMatrix));
+ EXPECT_TRUE(stiffnessMatrix.isApprox(m_expectedStiffnessMatrix2));
+
+ forceVector.setZero();
+ massMatrix.setZero();
+ dampingMatrix.setZero();
+ stiffnessMatrix.setZero();
+
+ tet.addFMDK(m_restState, &forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
+ EXPECT_TRUE(forceVector.isZero());
+ EXPECT_TRUE(massMatrix.isApprox(m_expectedMassMatrix));
+ EXPECT_TRUE(dampingMatrix.isApprox(m_expectedDampingMatrix));
+ EXPECT_TRUE(stiffnessMatrix.isApprox(m_expectedStiffnessMatrix));
+ EXPECT_TRUE(stiffnessMatrix.isApprox(m_expectedStiffnessMatrix2));
+
+ // Test addMatVec API with Mass component only
+ forceVector.setZero();
+ tet.addMatVec(m_restState, 1.0, 0.0, 0.0, m_vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 3 * 15; rowId++)
+ {
+ EXPECT_NEAR(m_expectedMassMatrix.row(rowId).sum(), forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with Damping component only
+ forceVector.setZero();
+ tet.addMatVec(m_restState, 0.0, 1.0, 0.0, m_vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 3 * 15; rowId++)
+ {
+ EXPECT_NEAR(m_expectedDampingMatrix.row(rowId).sum(), forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with Stiffness component only
+ forceVector.setZero();
+ tet.addMatVec(m_restState, 0.0, 0.0, 1.0, m_vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 3 * 15; rowId++)
+ {
+ EXPECT_NEAR(m_expectedStiffnessMatrix.row(rowId).sum(), forceVector[rowId], epsilon);
+ }
+ // Test addMatVec API with mix Mass/Damping/Stiffness components
+ forceVector.setZero();
+ tet.addMatVec(m_restState, 1.0, 2.0, 3.0, m_vectorOnes, &forceVector);
+ for (int rowId = 0; rowId < 3 * 15; rowId++)
+ {
+ double expectedCoef = 1.0 * m_expectedMassMatrix.row(rowId).sum() +
+ 2.0 * m_expectedDampingMatrix.row(rowId).sum() +
+ 3.0 * m_expectedStiffnessMatrix.row(rowId).sum();
+ EXPECT_NEAR(expectedCoef, forceVector[rowId], epsilon);
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/Fem3DPlyReaderDelegateTests.cpp b/SurgSim/Physics/UnitTests/Fem3DPlyReaderDelegateTests.cpp
new file mode 100644
index 0000000..156464c
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DPlyReaderDelegateTests.cpp
@@ -0,0 +1,158 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/Fem3DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::DataStructures::PlyReader;
+
+namespace
+{
+std::string findFile(std::string filename)
+{
+ const SurgSim::Framework::ApplicationData data("config.txt");
+
+ return data.findFile(filename);
+}
+}
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST(Fem3DRepresentationReaderTests, TetrahedronMeshDelegateTest)
+{
+ auto fem = std::make_shared<Fem3DRepresentation>("Representation");
+
+ std::string path = findFile("PlyReaderTests/Tetrahedron.ply");
+ ASSERT_TRUE(!path.empty());
+ PlyReader reader(path);
+ auto delegate = std::make_shared<Fem3DPlyReaderDelegate>(fem);
+
+ ASSERT_TRUE(reader.parseWithDelegate(delegate));
+
+ // Vertices
+ ASSERT_EQ(3u, fem->getNumDofPerNode());
+ ASSERT_EQ(3u * 26u, fem->getNumDof());
+
+ Vector3d vertex0(1.0, 1.0, -1.0);
+ Vector3d vertex25(-1.0, -1.0, 1.0);
+
+ EXPECT_TRUE(vertex0.isApprox(fem->getInitialState()->getPosition(0)));
+ EXPECT_TRUE(vertex25.isApprox(fem->getInitialState()->getPosition(25)));
+
+ // Tetrahedrons
+ ASSERT_EQ(12u, fem->getNumFemElements());
+
+ std::array<size_t, 4> tetrahedron0 = {0, 1, 2, 3};
+ std::array<size_t, 4> tetrahedron2 = {10, 25, 11, 9};
+
+ EXPECT_TRUE(std::equal(std::begin(tetrahedron0), std::end(tetrahedron0),
+ std::begin(fem->getFemElement(0)->getNodeIds())));
+ EXPECT_TRUE(std::equal(std::begin(tetrahedron2), std::end(tetrahedron2),
+ std::begin(fem->getFemElement(11)->getNodeIds())));
+
+ // Boundary conditions
+ ASSERT_EQ(24u, fem->getInitialState()->getNumBoundaryConditions());
+
+ // Boundary condition 0 is on node 8
+ size_t boundaryNode0 = 8;
+ size_t boundaryNode7 = 11;
+
+ EXPECT_EQ(3 * boundaryNode0, fem->getInitialState()->getBoundaryConditions().at(0));
+ EXPECT_EQ(3 * boundaryNode0 + 1, fem->getInitialState()->getBoundaryConditions().at(1));
+ EXPECT_EQ(3 * boundaryNode0 + 2, fem->getInitialState()->getBoundaryConditions().at(2));
+ EXPECT_EQ(3 * boundaryNode7, fem->getInitialState()->getBoundaryConditions().at(21));
+ EXPECT_EQ(3 * boundaryNode7 + 1, fem->getInitialState()->getBoundaryConditions().at(22));
+ EXPECT_EQ(3 * boundaryNode7 + 2, fem->getInitialState()->getBoundaryConditions().at(23));
+ // Material
+ auto fem2 = fem->getFemElement(2);
+ EXPECT_DOUBLE_EQ(0.1432, fem2->getMassDensity());
+ EXPECT_DOUBLE_EQ(0.224, fem2->getPoissonRatio());
+ EXPECT_DOUBLE_EQ(0.472, fem2->getYoungModulus());
+
+ auto fem8 = fem->getFemElement(8);
+ EXPECT_DOUBLE_EQ(0.1432, fem8->getMassDensity());
+ EXPECT_DOUBLE_EQ(0.224, fem8->getPoissonRatio());
+ EXPECT_DOUBLE_EQ(0.472, fem8->getYoungModulus());
+}
+
+TEST(Fem3DRepresentationReaderTests, Fem3DCubePlyReadTest)
+{
+ auto fem = std::make_shared<Fem3DRepresentation>("Representation");
+
+ std::string path = findFile("PlyReaderTests/Fem3DCube.ply");
+ ASSERT_TRUE(!path.empty());
+ PlyReader reader(path);
+ auto delegate = std::make_shared<Fem3DPlyReaderDelegate>(fem);
+
+ ASSERT_TRUE(reader.parseWithDelegate(delegate));
+
+ // Vertices
+ ASSERT_EQ(3u, fem->getNumDofPerNode());
+ ASSERT_EQ(3u * 10u, fem->getNumDof());
+
+ Vector3d vertex0(1.0, 1.0, 1.0);
+ Vector3d vertex5(2.0, 2.0, 2.0);
+
+ EXPECT_TRUE(vertex0.isApprox(fem->getInitialState()->getPosition(0)));
+ EXPECT_TRUE(vertex5.isApprox(fem->getInitialState()->getPosition(5)));
+
+ // Cubes
+ ASSERT_EQ(3u, fem->getNumFemElements());
+
+ std::array<size_t, 8> cube0 = {0, 1, 2, 3, 4, 5, 6, 7};
+ std::array<size_t, 8> cube2 = {3, 4, 5, 0, 2, 6, 8, 7};
+
+ EXPECT_TRUE(std::equal(std::begin(cube0), std::end(cube0), std::begin(fem->getFemElement(0)->getNodeIds())));
+ EXPECT_TRUE(std::equal(std::begin(cube2), std::end(cube2), std::begin(fem->getFemElement(2)->getNodeIds())));
+
+ // Boundary conditions
+ ASSERT_EQ(3u* 2u, fem->getInitialState()->getNumBoundaryConditions());
+
+ // Boundary condition 0 is on node 9
+ size_t boundaryNode0 = 9;
+ size_t boundaryNode1 = 5;
+
+ EXPECT_EQ(3 * boundaryNode0, fem->getInitialState()->getBoundaryConditions().at(0));
+ EXPECT_EQ(3 * boundaryNode0 + 1, fem->getInitialState()->getBoundaryConditions().at(1));
+ EXPECT_EQ(3 * boundaryNode0 + 2, fem->getInitialState()->getBoundaryConditions().at(2));
+ EXPECT_EQ(3 * boundaryNode1, fem->getInitialState()->getBoundaryConditions().at(3));
+ EXPECT_EQ(3 * boundaryNode1 + 1, fem->getInitialState()->getBoundaryConditions().at(4));
+ EXPECT_EQ(3 * boundaryNode1 + 2, fem->getInitialState()->getBoundaryConditions().at(5));
+
+ // Material
+ auto fem2 = fem->getFemElement(2);
+ EXPECT_DOUBLE_EQ(0.2, fem2->getMassDensity());
+ EXPECT_DOUBLE_EQ(0.3, fem2->getPoissonRatio());
+ EXPECT_DOUBLE_EQ(0.4, fem2->getYoungModulus());
+
+ auto fem1 = fem->getFemElement(1);
+ EXPECT_DOUBLE_EQ(0.2, fem1->getMassDensity());
+ EXPECT_DOUBLE_EQ(0.3, fem1->getPoissonRatio());
+ EXPECT_DOUBLE_EQ(0.4, fem1->getYoungModulus());
+}
+
+} // Physics
+} // SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem3DRepresentationBilateral3DTests.cpp b/SurgSim/Physics/UnitTests/Fem3DRepresentationBilateral3DTests.cpp
new file mode 100644
index 0000000..e7362cb
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DRepresentationBilateral3DTests.cpp
@@ -0,0 +1,237 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/MlcpConstraintType.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentationBilateral3D.h"
+#include "SurgSim/Physics/Fem3DRepresentationLocalization.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+
+namespace
+{
+const double epsilon = 1e-10;
+const double dt = 1e-3;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+static std::shared_ptr<Fem3DElementTetrahedron> makeTetrahedron(size_t node0,
+ size_t node1,
+ size_t node2,
+ size_t node3,
+ double massDensity,
+ double poissonRatio,
+ double youngModulus)
+{
+ std::array<size_t, 4> nodes = {node0, node1, node2, node3};
+ auto element = std::make_shared<Fem3DElementTetrahedron>(nodes);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ return element;
+}
+
+static std::shared_ptr<Fem3DRepresentation> getTestingFem3d(const std::string &name,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ auto fem = std::make_shared<Fem3DRepresentation>(name);
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(3, 6);
+
+ // Place coordinates at
+ // ( 0.00, 0.00, 0.00) + (0.24, -0.43, 0.55) + ( 0.06, -0.14, -0.15) = ( 0.30, -0.57, 0.40)
+ // ( 0.00, 1.00, -1.00) + (0.24, -0.43, 0.55) + (-0.18, 0.06, 0.13) = ( 0.06, 0.63, -0.32)
+ // (-1.00, 1.00, 0.00) + (0.24, -0.43, 0.55) + (-0.15, 0.15, 0.17) = (-0.91, 0.72, 0.72)
+ // ( 0.00, 1.00, 0.00) + (0.24, -0.43, 0.55) + ( 0.11, -0.05, -0.05) = ( 0.35, 0.52, 0.50)
+ // ( 1.00, 1.00, 0.00) + (0.24, -0.43, 0.55) + (-0.10, 0.09, 0.16) = ( 1.14, 0.66, 0.71)
+ // ( 1.00, 0.00, -1.00) + (0.24, -0.43, 0.55) + (-0.22, 0.12, -0.09) = ( 1.02, -0.31, -0.54)
+
+ state->getPositions().segment<3>(0 * 3) = Vector3d( 0.30, -0.57, 0.40);
+ state->getPositions().segment<3>(1 * 3) = Vector3d( 0.06, 0.63, -0.32);
+ state->getPositions().segment<3>(2 * 3) = Vector3d(-0.91, 0.72, 0.72);
+ state->getPositions().segment<3>(3 * 3) = Vector3d( 0.35, 0.52, 0.50);
+ state->getPositions().segment<3>(4 * 3) = Vector3d( 1.14, 0.66, 0.71);
+ state->getPositions().segment<3>(5 * 3) = Vector3d( 1.02, -0.31, -0.54);
+
+ fem->addFemElement(makeTetrahedron(0, 1, 2, 3, massDensity, poissonRatio, youngModulus));
+ fem->addFemElement(makeTetrahedron(0, 1, 3, 4, massDensity, poissonRatio, youngModulus));
+ fem->addFemElement(makeTetrahedron(0, 1, 4, 5, massDensity, poissonRatio, youngModulus));
+
+ fem->setInitialState(state);
+ fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ fem->wakeUp();
+
+ fem->setIsGravityEnabled(false);
+
+ fem->beforeUpdate(dt);
+ fem->update(dt);
+
+ return fem;
+}
+
+TEST(Fem3DRepresentationBilateral3DTests, Constructor)
+{
+ ASSERT_NO_THROW(
+ { Fem3DRepresentationBilateral3D constraint; });
+}
+
+TEST(Fem3DRepresentationBilateral3DTests, Constants)
+{
+ Fem3DRepresentationBilateral3D constraint;
+
+ EXPECT_EQ(SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT, constraint.getMlcpConstraintType());
+ EXPECT_EQ(SurgSim::Physics::REPRESENTATION_TYPE_FEM3D, constraint.getRepresentationType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(Fem3DRepresentationBilateral3DTests, BuildMlcpBasic)
+{
+ // Whitebox test which validates ConstraintImplementation::build's output parameter, MlcpPhysicsProblem. It assumes
+ // CHt and HCHt can be correctly built given H, so it does not neccessarily construct the physical parameters
+ // neccessary to supply a realistic C. It only checks H and b.
+ Fem3DRepresentationBilateral3D constraint;
+
+ Vector3d actual;
+
+ // Setup parameters for Fem3DRepresentationBilateral3D::build
+ auto localization = std::make_shared<Fem3DRepresentationLocalization>(
+ getTestingFem3d("representation"),
+ SurgSim::DataStructures::IndexedLocalCoordinate(2u, Vector4d(0.0, 0.0, 1.0, 0.0)));
+
+ actual = localization->calculatePosition();
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(18, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ // Compare results
+ Eigen::Matrix<double, 3, 1> violation = actual;
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ Eigen::Matrix<double, 3, 18> H = Eigen::Matrix<double, 3, 18>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::setSubMatrix(1.0 * dt * identity, 0, 4, 3, 3, &H);
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(Fem3DRepresentationBilateral3DTests, BuildMlcp)
+{
+ // Whitebox test which validates ConstraintImplementation::build's output parameter, MlcpPhysicsProblem. It assumes
+ // CHt and HCHt can be correctly built given H, so it does not neccessarily construct the physical parameters
+ // neccessary to supply a realistic C. It only checks H and b.
+ Fem3DRepresentationBilateral3D constraint;
+
+ Vector3d actual;
+
+ // Setup parameters for Fem3DRepresentationBilateral3D::build
+ auto localization = std::make_shared<Fem3DRepresentationLocalization>(
+ getTestingFem3d("representation"),
+ SurgSim::DataStructures::IndexedLocalCoordinate(2u, Vector4d(0.11, 0.02, 0.33, 0.54)));
+
+ actual = localization->calculatePosition();
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(18, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ // Compare results
+ Eigen::Matrix<double, 3, 1> violation = actual;
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ Eigen::Matrix<double, 3, 18> H = Eigen::Matrix<double, 3, 18>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::setSubMatrix(0.11 * dt * identity, 0, 0, 3, 3, &H);
+ SurgSim::Math::setSubMatrix(0.02 * dt * identity, 0, 1, 3, 3, &H);
+ SurgSim::Math::setSubMatrix(0.33 * dt * identity, 0, 4, 3, 3, &H);
+ SurgSim::Math::setSubMatrix(0.54 * dt * identity, 0, 5, 3, 3, &H);
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(Fem3DRepresentationBilateral3DTests, BuildMlcpTwoStep)
+{
+ // Whitebox test which validates ConstraintImplementation::build's output parameter, MlcpPhysicsProblem. It assumes
+ // CHt and HCHt can be correctly built given H, so it does not neccessarily construct the physical parameters
+ // neccessary to supply a realistic C. It only checks H and b.
+ Fem3DRepresentationBilateral3D constraint;
+
+ Vector3d actual;
+ Vector3d desired;
+
+ // Setup parameters for Fem3DRepresentationBilateral3D::build
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(18, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ auto localization = std::make_shared<Fem3DRepresentationLocalization>(
+ getTestingFem3d("representation"),
+ SurgSim::DataStructures::IndexedLocalCoordinate(2u, Vector4d(0.11, 0.02, 0.33, 0.54)));
+ actual = localization->calculatePosition();
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ localization->setLocalPosition(
+ SurgSim::DataStructures::IndexedLocalCoordinate(1u, Vector4d(0.32, 0.05, 0.14, 0.49)));
+ desired = localization->calculatePosition();
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_NEGATIVE_SIDE));
+
+ // Compare results
+ Eigen::Matrix<double, 3, 1> violation = actual - desired;
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ Eigen::Matrix<double, 3, 18> H = Eigen::Matrix<double, 3, 18>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::addSubMatrix( 0.11 * dt * identity, 0, 0, 3, 3, &H);
+ SurgSim::Math::addSubMatrix( 0.02 * dt * identity, 0, 1, 3, 3, &H);
+ SurgSim::Math::addSubMatrix( 0.33 * dt * identity, 0, 4, 3, 3, &H);
+ SurgSim::Math::addSubMatrix( 0.54 * dt * identity, 0, 5, 3, 3, &H);
+ SurgSim::Math::addSubMatrix(-0.32 * dt * identity, 0, 0, 3, 3, &H);
+ SurgSim::Math::addSubMatrix(-0.05 * dt * identity, 0, 1, 3, 3, &H);
+ SurgSim::Math::addSubMatrix(-0.14 * dt * identity, 0, 3, 3, 3, &H);
+ SurgSim::Math::addSubMatrix(-0.49 * dt * identity, 0, 4, 3, 3, &H);
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem3DRepresentationContactTests.cpp b/SurgSim/Physics/UnitTests/Fem3DRepresentationContactTests.cpp
new file mode 100644
index 0000000..77335f9
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DRepresentationContactTests.cpp
@@ -0,0 +1,275 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentationContact.h"
+#include "SurgSim/Physics/Fem3DRepresentationLocalization.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::Framework::Runtime;
+using SurgSim::Physics::ContactConstraintData;
+using SurgSim::Physics::Fem3DRepresentation;
+using SurgSim::Physics::Fem3DRepresentationContact;
+using SurgSim::Physics::Fem3DRepresentationLocalization;
+using SurgSim::Physics::Fem3DElementTetrahedron;
+using SurgSim::Physics::MlcpPhysicsProblem;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+ const double dt = 1e-3;
+};
+
+static void addTetraheadron(Fem3DRepresentation *fem,
+ size_t node0,
+ size_t node1,
+ size_t node2,
+ size_t node3,
+ const SurgSim::Math::OdeState& state,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ std::array<size_t, 4> nodes = {node0, node1, node2, node3};
+ auto element = std::make_shared<Fem3DElementTetrahedron>(nodes);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ element->initialize(state);
+ fem->addFemElement(element);
+}
+
+class Fem3DRepresentationContactTests : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // Define plane with normal 'n' pointing against gravity.
+ m_n = Vector3d(0.8539, 0.6289, -0.9978);
+ m_n.normalize();
+
+ // Create mock FEM
+ m_fem = std::make_shared<Fem3DRepresentation>("Fem3dRepresentation");
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(3, 6);
+
+ // Place coordinates at
+ // ( 0.00, 0.00, 0.00) + (0.24, -0.43, 0.55) + ( 0.06, -0.14, -0.15) = ( 0.30, -0.57, 0.40)
+ // ( 0.00, 1.00, -1.00) + (0.24, -0.43, 0.55) + (-0.18, 0.06, 0.13) = ( 0.06, 0.63, -0.32)
+ // (-1.00, 1.00, 0.00) + (0.24, -0.43, 0.55) + (-0.15, 0.15, 0.17) = (-0.91, 0.72, 0.72)
+ // ( 0.00, 1.00, 0.00) + (0.24, -0.43, 0.55) + ( 0.11, -0.05, -0.05) = ( 0.35, 0.52, 0.50)
+ // ( 1.00, 1.00, 0.00) + (0.24, -0.43, 0.55) + (-0.10, 0.09, 0.16) = ( 1.14, 0.66, 0.71)
+ // ( 1.00, 0.00, -1.00) + (0.24, -0.43, 0.55) + (-0.22, 0.12, -0.09) = ( 1.02, -0.31, -0.54)
+
+ state->getPositions().segment<3>(0 * 3) = Vector3d( 0.30, -0.57, 0.40);
+ state->getPositions().segment<3>(1 * 3) = Vector3d( 0.06, 0.63, -0.32);
+ state->getPositions().segment<3>(2 * 3) = Vector3d(-0.91, 0.72, 0.72);
+ state->getPositions().segment<3>(3 * 3) = Vector3d( 0.35, 0.52, 0.50);
+ state->getPositions().segment<3>(4 * 3) = Vector3d( 1.14, 0.66, 0.71);
+ state->getPositions().segment<3>(5 * 3) = Vector3d( 1.02, -0.31, -0.54);
+
+ addTetraheadron(m_fem.get(), 0, 1, 2, 3, *state);
+ addTetraheadron(m_fem.get(), 0, 1, 3, 4, *state);
+ addTetraheadron(m_fem.get(), 0, 1, 4, 5, *state);
+
+ m_fem->setInitialState(state);
+ m_fem->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER);
+ m_fem->setLocalActive(true);
+
+ m_fem->initialize(std::make_shared<Runtime>());
+ m_fem->wakeUp();
+
+ // Update model by one timestep
+ m_fem->beforeUpdate(dt);
+ m_fem->update(dt);
+
+ }
+
+ void setContactAt(const IndexedLocalCoordinate &coord)
+ {
+ m_coord = coord;
+ m_localization = std::make_shared<Fem3DRepresentationLocalization>(m_fem, m_coord);
+
+ // Calculate position at state before "m_fem->update(dt)" was called.
+ double distance = -m_localization->calculatePosition(0.0).dot(m_n);
+ m_constraintData.setPlaneEquation(m_n, distance);
+ }
+
+ std::shared_ptr<Fem3DRepresentation> m_fem;
+ std::shared_ptr<Fem3DRepresentationLocalization> m_localization;
+
+ IndexedLocalCoordinate m_coord;
+ Vector3d m_n;
+ ContactConstraintData m_constraintData;
+};
+
+TEST_F(Fem3DRepresentationContactTests, ConstructorTest)
+{
+ ASSERT_NO_THROW({
+ Fem3DRepresentationContact femContact;
+ });
+}
+
+TEST_F(Fem3DRepresentationContactTests, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<Fem3DRepresentationContact>();
+
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, implementation->getMlcpConstraintType());
+ EXPECT_EQ(SurgSim::Physics::REPRESENTATION_TYPE_FEM3D, implementation->getRepresentationType());
+ EXPECT_EQ(1u, implementation->getNumDof());
+}
+
+TEST_F(Fem3DRepresentationContactTests, BuildMlcpTest)
+{
+ // This test verifies the build mlcp function works for a simple case.
+
+ auto implementation = std::make_shared<Fem3DRepresentationContact>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 1, 1);
+
+ // Apply constraint purely to the first node of the 0th tetrahedron.
+ IndexedLocalCoordinate coord(0, Vector4d(1.0, 0.0, 0.0, 0.0));
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ EXPECT_NEAR(-9.81 * dt * dt * m_n[1], mlcpPhysicsProblem.b[0], epsilon);
+
+ Eigen::Matrix<double, 1, 18> H = Eigen::Matrix<double, 1, 18>::Zero();
+ SurgSim::Math::setSubVector(dt * m_n, 0, 3, &H);
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ Eigen::Matrix<double, 18, 18> C = dt * m_fem->computeM(*m_fem->getPreviousState()).inverse();
+
+ EXPECT_NEAR_EIGEN(C * H.transpose(), mlcpPhysicsProblem.CHt, epsilon);
+
+ EXPECT_NEAR_EIGEN(H * C * H.transpose(), mlcpPhysicsProblem.A, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST_F(Fem3DRepresentationContactTests, BuildMlcpCoordinateTest)
+{
+ // This test verifies the build mlcp function works for a more complicated case with different nodes.
+
+ auto implementation = std::make_shared<Fem3DRepresentationContact>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 1, 1);
+
+ // Apply constraint to all nodes of an fem.
+ IndexedLocalCoordinate coord(1, Vector4d(0.25, 0.33, 0.28, 0.14));
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ EXPECT_NEAR(-9.81 * dt * dt * m_n[1], mlcpPhysicsProblem.b[0], epsilon);
+
+ Eigen::Matrix<double, 1, 18> H = Eigen::Matrix<double, 1, 18>::Zero();
+ SurgSim::Math::setSubVector(0.25 * dt * m_n, 0, 3, &H);
+ SurgSim::Math::setSubVector(0.33 * dt * m_n, 1, 3, &H);
+ SurgSim::Math::setSubVector(0.28 * dt * m_n, 3, 3, &H);
+ SurgSim::Math::setSubVector(0.14 * dt * m_n, 4, 3, &H);
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ Eigen::Matrix<double, 18, 18> C = dt * m_fem->computeM(*m_fem->getPreviousState()).inverse();
+
+ EXPECT_NEAR_EIGEN(C * H.transpose(), mlcpPhysicsProblem.CHt, epsilon);
+
+ EXPECT_NEAR_EIGEN(H * C * H.transpose(), mlcpPhysicsProblem.A, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST_F(Fem3DRepresentationContactTests, BuildMlcpIndiciesTest)
+{
+ // This test verifies the build mlcp function works given a hypothetical, preexisting mlcp problem.
+
+ auto implementation = std::make_shared<Fem3DRepresentationContact>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof() + 5, 2, 1);
+
+ // Suppose 5 dof and 1 constraint are defined elsewhere. Then H, CHt, HCHt, and b are prebuilt.
+ Eigen::Matrix<double, 1, 5> localH;
+ localH <<
+ 0.9478, -0.3807, 0.5536, -0.6944, 0.1815;
+ mlcpPhysicsProblem.H.block<1, 5>(0, 0) = localH;
+
+ Eigen::Matrix<double, 5, 5> localC;
+ localC <<
+ -0.2294, 0.5160, 0.2520, 0.5941, -0.4854,
+ 0.1233, -0.4433, 0.3679, 0.9307, 0.2600,
+ 0.1988, 0.6637, -0.7591, 0.1475, 0.8517,
+ -0.5495, -0.4305, 0.3162, -0.7862, 0.7627,
+ -0.5754, 0.4108, 0.8445, -0.5565, 0.7150;
+ localC = localC * localC.transpose(); // force to be symmetric
+
+ Eigen::Matrix<double, 5, 1> localCHt = localC * localH.transpose();
+ mlcpPhysicsProblem.CHt.block<5, 1>(0, 0) = localCHt;
+
+ mlcpPhysicsProblem.A.block<1, 1>(0, 0) = localH * localCHt;
+
+ mlcpPhysicsProblem.b.block<1, 1>(0, 0)[0] = 0.6991;
+
+ // Place mass-spring at 5th dof and 1th constraint.
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ // Apply constraint to all nodes of an fem.
+ IndexedLocalCoordinate coord(1, Vector4d(0.25, 0.33, 0.28, 0.14));
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ EXPECT_NEAR(-9.81 * dt * dt * m_n[1], mlcpPhysicsProblem.b[indexOfConstraint], epsilon);
+
+ Eigen::Matrix<double, 1, 18> H = Eigen::Matrix<double, 1, 18>::Zero();
+ SurgSim::Math::setSubVector(0.25 * dt * m_n, 0, 3, &H);
+ SurgSim::Math::setSubVector(0.33 * dt * m_n, 1, 3, &H);
+ SurgSim::Math::setSubVector(0.28 * dt * m_n, 3, 3, &H);
+ SurgSim::Math::setSubVector(0.14 * dt * m_n, 4, 3, &H);
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H.block(indexOfConstraint, indexOfRepresentation, 1, 18), epsilon);
+
+ Eigen::Matrix<double, 18, 18> C = dt * m_fem->computeM(*m_fem->getPreviousState()).inverse();
+
+ EXPECT_NEAR_EIGEN(
+ C * H.transpose(), mlcpPhysicsProblem.CHt.block(indexOfRepresentation, indexOfConstraint, 18, 1), epsilon);
+
+ EXPECT_NEAR_EIGEN(
+ H * C * H.transpose(), mlcpPhysicsProblem.A.block(indexOfConstraint, indexOfConstraint, 1, 1), epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
diff --git a/SurgSim/Physics/UnitTests/Fem3DRepresentationLocalizationTest.cpp b/SurgSim/Physics/UnitTests/Fem3DRepresentationLocalizationTest.cpp
new file mode 100644
index 0000000..38bfec0
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DRepresentationLocalizationTest.cpp
@@ -0,0 +1,371 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentationLocalization.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::Math::getSubVector;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+void addTetraheadron(Fem3DRepresentation *fem, std::array<size_t, 4> nodes,
+ const SurgSim::Math::OdeState& state, double massDensity = 1.0,
+ double poissonRatio = 0.1, double youngModulus = 1.0)
+{
+ auto element = std::make_shared<Fem3DElementTetrahedron>(nodes);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ element->initialize(state);
+ fem->addFemElement(element);
+}
+
+class Fem3DRepresentationLocalizationTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ using SurgSim::Math::Vector3d;
+
+ m_fem = std::make_shared<Fem3DRepresentation>("Fem3dRepresentation");
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(3, 6);
+
+ auto& x = state->getPositions();
+ getSubVector(x, 0, 3) = Vector3d( 0.0, 0.0, 0.0);
+ getSubVector(x, 1, 3) = Vector3d( 0.0, 1.0, -1.0);
+ getSubVector(x, 2, 3) = Vector3d(-1.0, 1.0, 0.0);
+ getSubVector(x, 3, 3) = Vector3d( 0.0, 1.0, 0.0);
+ getSubVector(x, 4, 3) = Vector3d( 1.0, 1.0, 0.0);
+ getSubVector(x, 5, 3) = Vector3d( 1.0, 0.0, -1.0);
+
+ // Define Tetrahedrons
+ {
+ std::array<size_t, 4> nodes = {{0, 1, 2, 3}};
+ addTetraheadron(m_fem.get(), nodes, *state);
+ }
+
+ {
+ std::array<size_t, 4> nodes = {{0, 1, 3, 4}};
+ addTetraheadron(m_fem.get(), nodes, *state);
+ }
+
+ {
+ std::array<size_t, 4> nodes = {{0, 1, 4, 5}};
+ addTetraheadron(m_fem.get(), nodes, *state);
+ }
+
+ m_fem->setInitialState(state);
+ m_fem->setLocalActive(true);
+
+ // FEMRepresentation for Fem3DElementCube
+ m_fem3DCube = std::make_shared<Fem3DRepresentation>("Fem3dCubeRepresentation");
+ auto restState = std::make_shared<SurgSim::Math::OdeState>();
+ restState->setNumDof(3, 8);
+
+ auto& x0 = restState->getPositions();
+ getSubVector(x0, 0, 3) = Vector3d(-1.0,-1.0,-1.0);
+ getSubVector(x0, 1, 3) = Vector3d( 1.0,-1.0,-1.0);
+ getSubVector(x0, 2, 3) = Vector3d(-1.0, 1.0,-1.0);
+ getSubVector(x0, 3, 3) = Vector3d( 1.0, 1.0,-1.0);
+ getSubVector(x0, 4, 3) = Vector3d(-1.0,-1.0, 1.0);
+ getSubVector(x0, 5, 3) = Vector3d( 1.0,-1.0, 1.0);
+ getSubVector(x0, 6, 3) = Vector3d(-1.0, 1.0, 1.0);
+ getSubVector(x0, 7, 3) = Vector3d( 1.0, 1.0, 1.0);
+
+ // Define Cube
+ {
+ std::array<size_t, 8> node0 = {{0, 1, 3, 2, 4, 5, 7, 6}};
+ std::shared_ptr<Fem3DElementCube> femElement = std::make_shared<Fem3DElementCube>(node0);
+ femElement->setMassDensity(1.0);
+ femElement->setPoissonRatio(0.1);
+ femElement->setYoungModulus(1.0);
+
+ m_fem3DCube->addFemElement(femElement);
+ }
+ m_fem3DCube->setInitialState(restState);
+ m_fem3DCube->setLocalActive(true);
+
+ m_validLocalPosition.index = 1;
+ m_validLocalPosition.coordinate = SurgSim::Math::Vector::Zero(4);
+ m_validLocalPosition.coordinate[0] = 0.4;
+ m_validLocalPosition.coordinate[1] = 0.6;
+
+ m_validLocalPositionForCube.index = 0;
+ m_validLocalPositionForCube.coordinate = SurgSim::Math::Vector::Zero(8);
+ m_validLocalPositionForCube.coordinate[0] = 0.4;
+ m_validLocalPositionForCube.coordinate[1] = 0.6;
+
+ m_invalidIndexLocalPosition.index = 3;
+ m_validLocalPosition.coordinate = SurgSim::Math::Vector::Zero(4);
+ m_validLocalPosition.coordinate[0] = 0.4;
+ m_validLocalPosition.coordinate[1] = 0.6;
+
+ m_invalidCoordinateLocalPosition.index = 1;
+ m_invalidCoordinateLocalPosition.coordinate = SurgSim::Math::Vector::Zero(4);
+ m_invalidCoordinateLocalPosition.coordinate[0] = 0.6;
+ m_invalidCoordinateLocalPosition.coordinate[1] = 0.6;
+ }
+
+ void TearDown()
+ {
+ }
+
+ std::shared_ptr<Fem3DRepresentation> m_fem;
+ std::shared_ptr<Fem3DRepresentation> m_fem3DCube;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_validLocalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_invalidIndexLocalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_invalidCoordinateLocalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_validLocalPositionForCube;
+};
+
+TEST_F(Fem3DRepresentationLocalizationTest, ConstructorTest)
+{
+ ASSERT_THROW(std::make_shared<Fem3DRepresentationLocalization>(m_fem, m_invalidIndexLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_THROW(std::make_shared<Fem3DRepresentationLocalization>(m_fem, m_invalidCoordinateLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_NO_THROW(std::make_shared<Fem3DRepresentationLocalization>(m_fem, m_validLocalPosition););
+}
+
+TEST_F(Fem3DRepresentationLocalizationTest, SetGetRepresentation)
+{
+ Fem3DRepresentationLocalization localization(m_fem, m_validLocalPosition);
+
+ EXPECT_NE(nullptr, localization.getRepresentation());
+ EXPECT_EQ(m_fem, localization.getRepresentation());
+
+ EXPECT_EQ(1u, localization.getLocalPosition().index);
+ EXPECT_TRUE(localization.getLocalPosition().coordinate.isApprox(m_validLocalPosition.coordinate));
+
+ localization.setRepresentation(nullptr);
+ EXPECT_EQ(nullptr, localization.getRepresentation());
+ localization.setRepresentation(m_fem);
+ EXPECT_EQ(m_fem, localization.getRepresentation());
+
+ SurgSim::DataStructures::IndexedLocalCoordinate m_otherValidLocalPosition;
+ m_otherValidLocalPosition.index = 0;
+ m_otherValidLocalPosition.coordinate = SurgSim::Math::Vector::Zero(4);
+ m_otherValidLocalPosition.coordinate[1] = 1.0;
+
+ localization.setLocalPosition(m_otherValidLocalPosition);
+ EXPECT_EQ(m_otherValidLocalPosition.index, localization.getLocalPosition().index);
+ EXPECT_TRUE(localization.getLocalPosition().coordinate.isApprox(m_otherValidLocalPosition.coordinate));
+}
+
+TEST_F(Fem3DRepresentationLocalizationTest, SetGetLocalization)
+{
+ using SurgSim::Math::Vector4d;
+ using SurgSim::Math::Vector3d;
+
+ {
+ SCOPED_TRACE("Uninitialized Representation");
+
+ // Uninitialized Representation
+ EXPECT_THROW(std::make_shared<Fem3DRepresentationLocalization>(nullptr, m_validLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Incorrectly formed natural coordinate");
+
+ // Incorrectly formed natural coordinate
+ auto localization = std::make_shared<Fem3DRepresentationLocalization>(m_fem, m_validLocalPosition);
+ EXPECT_THROW(localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector4d(0.89, 0.54, 0.45, 0.64))),
+ SurgSim::Framework::AssertionFailure);
+
+ EXPECT_THROW(localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector3d(1.0, 0.0, 0.0))),
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Out of bounds element Id");
+
+ // Out of bounds element Id
+ auto localization = std::make_shared<Fem3DRepresentationLocalization>(m_fem, m_validLocalPosition);
+ EXPECT_THROW(localization->setLocalPosition(IndexedLocalCoordinate(6u, Vector4d(1.0, 0.0, 0.0, 0.0))),
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("valid");
+
+ auto localization = std::make_shared<Fem3DRepresentationLocalization>(m_fem, m_validLocalPosition);
+ EXPECT_NO_THROW(localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector4d(0.2, 0.6, 0.1, 0.1))));
+ EXPECT_EQ(1u, localization->getLocalPosition().index);
+ EXPECT_TRUE(localization->getLocalPosition().coordinate.isApprox(Vector4d(0.2, 0.6, 0.1, 0.1)));
+ }
+}
+
+TEST_F(Fem3DRepresentationLocalizationTest, CalculatePositionTest)
+{
+ using SurgSim::Math::Vector;
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::Vector4d;
+
+ auto localization = std::make_shared<Fem3DRepresentationLocalization>(m_fem, m_validLocalPosition);
+
+ // Test tetrahedron 1: nodes 0, 1, 2, 3
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector4d(1.0, 0.0, 0.0, 0.0)));
+ EXPECT_TRUE(Vector3d(0.0, 0.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector4d(0.0, 1.0, 0.0, 0.0)));
+ EXPECT_TRUE(Vector3d(0.0, 1.0, -1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector4d(0.0, 0.0, 1.0, 0.0)));
+ EXPECT_TRUE(Vector3d(-1.0, 1.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector4d(0.0, 0.0, 0.0, 1.0)));
+ EXPECT_TRUE(Vector3d(0.0, 1.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test tetrahedron 2: nodes 0, 1, 3, 4
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector4d(1.0, 0.0, 0.0, 0.0)));
+ EXPECT_TRUE(Vector3d(0.0, 0.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector4d(0.0, 1.0, 0.0, 0.0)));
+ EXPECT_TRUE(Vector3d(0.0, 1.0, -1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector4d(0.0, 0.0, 1.0, 0.0)));
+ EXPECT_TRUE(Vector3d(0.0, 1.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector4d(0.0, 0.0, 0.0, 1.0)));
+ EXPECT_TRUE(Vector3d(1.0, 1.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test tetrahedron 3: nodes 0, 1, 4, 5
+ localization->setLocalPosition(IndexedLocalCoordinate(2u, Vector4d(1.0, 0.0, 0.0, 0.0)));
+ EXPECT_TRUE(Vector3d(0.0, 0.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(2u, Vector4d(0.0, 1.0, 0.0, 0.0)));
+ EXPECT_TRUE(Vector3d(0.0, 1.0, -1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(2u, Vector4d(0.0, 0.0, 1.0, 0.0)));
+ EXPECT_TRUE(Vector3d(1.0, 1.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(2u, Vector4d(0.0, 0.0, 0.0, 1.0)));
+ EXPECT_TRUE(Vector3d(1.0, 0.0, -1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Advanced tests
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector4d(0.31, 0.03, 0.19, 0.47)));
+ // 0.31 * ( 0.0, 0.0, 0.0) => ( 0.0, 0.0, 0.0 )
+ // + 0.03 * ( 0.0, 1.0, -1.0) => ( 0.0, 0.03, -0.03)
+ // + 0.19 * (-1.0, 1.0, 0.0) => (-0.19, 0.19, 0.0 )
+ // + 0.47 * ( 0.0, 1.0, 0.0) => ( 0.0, 0.47, 0.0 )
+ // = (-0.19, 0.69, -0.03)
+ EXPECT_TRUE(Vector3d(-0.19, 0.69, -0.03).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector4d(0.05, 0.81, 0.06, 0.08)));
+ // 0.05 * ( 0.0, 0.0, 0.0) => (0.0, 0.0, 0.0 )
+ // + 0.81 * ( 0.0, 1.0, -1.0) => (0.0, 0.81, -0.81)
+ // + 0.06 * ( 0.0, 1.0, 0.0) => (0.0, 0.06, 0.0 )
+ // + 0.08 * ( 1.0, 1.0, 0.0) => (0.08, 0.08, 0.0 )
+ // = (0.08, 0.95, -0.81)
+ EXPECT_TRUE(Vector3d(0.08, 0.95, -0.81).isApprox(localization->calculatePosition(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(2u, Vector4d(0.11, 0.15, 0.67, 0.07)));
+ // 0.11 * ( 0.0, 0.0, 0.0) => (0.0, 0.0, 0.0 )
+ // + 0.15 * ( 0.0, 1.0, -1.0) => (0.0, 0.15, -0.15)
+ // + 0.67 * ( 1.0, 1.0, 0.0) => (0.67, 0.67, 0.0 )
+ // + 0.07 * ( 1.0, 0.0, -1.0) => (0.07, 0.0, -0.07)
+ // = (0.74, 0.82, -0.22)
+ EXPECT_TRUE(Vector3d(0.74, 0.82, -0.22).isApprox(localization->calculatePosition(), epsilon));
+}
+
+TEST_F(Fem3DRepresentationLocalizationTest, CalculatePositionTest3DCube)
+{
+ auto localization = std::make_shared<Fem3DRepresentationLocalization>(m_fem3DCube, m_validLocalPositionForCube);
+ SurgSim::Math::Vector cubeNodes(8);
+
+ // Test central node
+ cubeNodes << 0.125, 0.125, 0.125, 0.125, 0.125, 0.125, 0.125, 0.125;
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, cubeNodes));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(0.0, 0.0, 0.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test node 0:
+ cubeNodes << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, cubeNodes));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(-1.0, -1.0, -1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test node 1:
+ cubeNodes << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, cubeNodes));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(1.0, -1.0, -1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test node 2:
+ cubeNodes << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, cubeNodes));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(1.0, 1.0, -1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test node 3:
+ cubeNodes << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0;
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, cubeNodes));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(-1.0, 1.0, -1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test node 4:
+ cubeNodes << 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, cubeNodes));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(-1.0, -1.0, 1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test node 5:
+ cubeNodes << 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0;
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, cubeNodes));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(1.0, -1.0, 1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test node 6:
+ cubeNodes << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0;
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, cubeNodes));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(1.0, 1.0, 1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Test node 7:
+ cubeNodes << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, cubeNodes));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(-1.0, 1.0, 1.0).isApprox(localization->calculatePosition(), epsilon));
+
+ // Advantage test
+ cubeNodes << 0.03, 0.09, 0.07, 0.05, 0.1, 0.13, 0.26, 0.27;
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, cubeNodes));
+ EXPECT_TRUE(SurgSim::Math::Vector3d(0.1, 0.3, 0.52).isApprox(localization->calculatePosition(), epsilon));
+ // 0.03 * (-1.0,-1.0,-1.0) => (-0.03, -0.03, -0.03)
+ // 0.09 * ( 1.0,-1.0,-1.0) => ( 0.09, -0.09, -0.09)
+ // 0.07 * ( 1.0, 1.0,-1.0) => ( 0.07, 0.07, -0.07)
+ // 0.05 * (-1.0, 1.0,-1.0) => (-0.05, 0.05, -0.05)
+ // 0.1 * (-1.0,-1.0, 1.0) => (-0.1, -0.1, 0.1)
+ // 0.13 * ( 1.0,-1.0, 1.0) => ( 0.13, -0.13, 0.13)
+ // 0.26 * ( 1.0, 1.0, 1.0) => ( 0.26, 0.26, 0.26)
+ // 0.27 * (-1.0, 1.0, 1.0) => (-0.27, 0.27, 0.27)
+ // = ( 0.1, 0.3, 0.52)
+}
+
+} // namespace SurgSim
+} // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/Fem3DRepresentationTests.cpp b/SurgSim/Physics/UnitTests/Fem3DRepresentationTests.cpp
new file mode 100644
index 0000000..9f9a8fb
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DRepresentationTests.cpp
@@ -0,0 +1,375 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 Fem3DRepresentationTests.cpp
+/// This file tests the non-abstract functionalities of the base class FemRepresentation
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h"
+#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Runtime.h" //< Used to initialize the Component Fem3DRepresentation
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentationLocalization.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+class Fem3DRepresentationTests : public ::testing::Test
+{
+public:
+ void SetUp() override
+ {
+ m_numNodes = 10;
+ }
+
+ void createFem()
+ {
+ m_fem = std::make_shared<Fem3DRepresentation>("Fem3d");
+
+ m_initialState = std::make_shared<SurgSim::Math::OdeState>();
+ m_initialState->setNumDof(m_fem->getNumDofPerNode(), m_numNodes);
+ m_initialState->getPositions().setLinSpaced(1.23, 9.43);
+ m_initialState->getVelocities().setLinSpaced(-0.94, 1.09);
+
+ SurgSim::Math::Quaterniond q(1.0, 2.0, 3.0, 4.0);
+ SurgSim::Math::Vector3d t(1.0, 2.0, 3.0);
+ q.normalize();
+ m_initialPose = SurgSim::Math::makeRigidTransform(q, t);
+ }
+
+ void addFemElement()
+ {
+ std::array<size_t, 4> elementNodeIds = {{0, 1, 2, 3}};
+ std::shared_ptr<Fem3DElementTetrahedron> element = std::make_shared<Fem3DElementTetrahedron>(elementNodeIds);
+ element->setYoungModulus(1e9);
+ element->setPoissonRatio(0.45);
+ element->setMassDensity(1000.0);
+ m_fem->addFemElement(element);
+ }
+
+ void createLocalization()
+ {
+ SurgSim::DataStructures::IndexedLocalCoordinate femRepCoordinate;
+ femRepCoordinate.index = 0;
+ femRepCoordinate.coordinate = SurgSim::Math::Vector::Zero(4);
+ femRepCoordinate.coordinate[0] = 1.0;
+ m_localization = std::make_shared<Fem3DRepresentationLocalization>(m_fem, femRepCoordinate);
+
+ m_wrongLocalizationType = std::make_shared<MockLocalization>();
+ m_wrongLocalizationType->setRepresentation(m_fem);
+ }
+
+protected:
+ size_t m_numNodes;
+ std::shared_ptr<Fem3DRepresentation> m_fem;
+ std::shared_ptr<SurgSim::Math::OdeState> m_initialState;
+ SurgSim::Math::RigidTransform3d m_initialPose;
+ std::shared_ptr<Fem3DRepresentationLocalization> m_localization;
+ std::shared_ptr<MockLocalization> m_wrongLocalizationType;
+};
+
+TEST_F(Fem3DRepresentationTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(std::shared_ptr<Fem3DRepresentation> fem = std::make_shared<Fem3DRepresentation>("Fem3D"));
+}
+
+TEST_F(Fem3DRepresentationTests, GetTypeTest)
+{
+ createFem();
+ EXPECT_EQ(REPRESENTATION_TYPE_FEM3D, m_fem->getType());
+}
+
+TEST_F(Fem3DRepresentationTests, GetNumDofPerNodeTest)
+{
+ createFem();
+ EXPECT_EQ(3u, m_fem->getNumDofPerNode());
+}
+
+TEST_F(Fem3DRepresentationTests, TransformInitialStateTest)
+{
+ using SurgSim::Math::Vector;
+
+ createFem();
+ m_fem->setLocalPose(m_initialPose);
+ m_fem->setInitialState(m_initialState);
+
+ Vector expectedX = Vector::Zero(m_fem->getNumDof()), expectedV = Vector::Zero(m_fem->getNumDof());
+ for (size_t nodeId = 0; nodeId < m_numNodes; nodeId++)
+ {
+ expectedX.segment<3>(m_fem->getNumDofPerNode() * nodeId) =
+ m_initialPose * m_initialState->getPosition(nodeId);
+ expectedV.segment<3>(m_fem->getNumDofPerNode() * nodeId) =
+ m_initialPose.linear() * m_initialState->getVelocity(nodeId);
+ }
+
+ // Initialize the component
+ ASSERT_TRUE(m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>()));
+ // Wake-up the component => apply the pose to the initial state
+ ASSERT_TRUE(m_fem->wakeUp());
+
+ EXPECT_TRUE(m_fem->getInitialState()->getPositions().isApprox(expectedX));
+ EXPECT_TRUE(m_fem->getInitialState()->getVelocities().isApprox(expectedV));
+}
+
+TEST_F(Fem3DRepresentationTests, SetGetFilenameTest)
+{
+ createFem();
+ ASSERT_NO_THROW(m_fem->setFilename("Data/PlyReaderTests/Tetrahedron.ply"));
+ ASSERT_NO_THROW(m_fem->getFilename());
+ ASSERT_EQ("Data/PlyReaderTests/Tetrahedron.ply", m_fem->getFilename());
+}
+
+TEST_F(Fem3DRepresentationTests, DoInitializeTest)
+{
+ {
+ SCOPED_TRACE("Initialize with a valid file name");
+ createFem();
+ m_fem->setFilename("PlyReaderTests/Tetrahedron.ply");
+ // Fem3DRepresentation::initialize() will call Fem3DRepresentation::doInitialize(), which should load the file.
+ ASSERT_NO_THROW(ASSERT_TRUE(m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>("config.txt"))));
+ EXPECT_EQ(3u, m_fem->getNumDofPerNode());
+ EXPECT_EQ(3u * 26u, m_fem->getNumDof());
+ EXPECT_EQ(24u, m_fem->getInitialState()->getNumBoundaryConditions());
+ }
+
+ {
+ SCOPED_TRACE("Initialize with an invalid file name");
+ createFem();
+ m_fem->setFilename("Non existent fake name");
+ EXPECT_FALSE(m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>("config.txt")));
+ }
+
+ {
+ SCOPED_TRACE("Initialize with file name not set");
+ createFem();
+ // Fem3DRepresentation will not try to load file, but FemRepresentation::doInitialize() will throw.
+ EXPECT_ANY_THROW(m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>("config.txt")));
+ }
+
+ {
+ SCOPED_TRACE("Initialization called on object instance");
+ Fem3DRepresentation fem("fem3d");
+ fem.setFilename("PlyReaderTests/Tetrahedron.ply");
+ // It throws because within doInitialize(), 'this' Fem3DRepresentation will be passed as a shared_ptr<> to
+ // the Fem3DRepresentationPlyReaderDelegate.
+ EXPECT_ANY_THROW(fem.initialize(std::make_shared<SurgSim::Framework::Runtime>("config.txt")));
+ }
+
+ {
+ SCOPED_TRACE("Loading file with incorrect PLY format");
+ createFem();
+ m_fem->setFilename("PlyReaderTests/WrongPlyTetrahedron.ply");
+ EXPECT_FALSE(m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>("config.txt")));
+ }
+
+ {
+ SCOPED_TRACE("Loading file with incorrect data");
+ createFem();
+ m_fem->setFilename("PlyReaderTests/WrongDataTetrahedron.ply");
+ EXPECT_ANY_THROW(m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>("config.txt")));
+ }
+}
+
+TEST_F(Fem3DRepresentationTests, CreateLocalizationTest)
+{
+ using SurgSim::DataStructures::EmptyData;
+
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ createFem();
+ ASSERT_NO_THROW(m_fem->setFilename("Geometry/wound_deformable.ply"));
+
+ std::string path = runtime->getApplicationData()->findFile("Geometry/wound_deformable.ply");
+ std::shared_ptr<SurgSim::DataStructures::TriangleMeshPlain> triangleMesh =
+ SurgSim::DataStructures::loadTriangleMesh(path);
+
+ // Create the collision mesh for the surface of the finite element model
+ auto collisionRepresentation = std::make_shared<DeformableCollisionRepresentation>("Collision");
+ collisionRepresentation->setMesh(std::make_shared<SurgSim::DataStructures::TriangleMesh>(*triangleMesh));
+ m_fem->setCollisionRepresentation(collisionRepresentation);
+
+ bool wokeUp = false;
+ ASSERT_TRUE(m_fem->initialize(runtime));
+ EXPECT_NO_THROW(wokeUp = m_fem->wakeUp(););
+ EXPECT_TRUE(wokeUp);
+
+ auto& meshTriangles = triangleMesh->getTriangles();
+ size_t triangleId = 0;
+ SurgSim::Math::Vector3d centroid;
+ for (auto triangle = meshTriangles.cbegin(); triangle != meshTriangles.cend(); ++triangle, ++triangleId)
+ {
+ std::array<size_t, 3> triangleNodeIds = triangle->verticesId;
+ centroid = triangleMesh->getVertexPosition(triangleNodeIds[0]);
+ centroid += triangleMesh->getVertexPosition(triangleNodeIds[1]);
+ centroid += triangleMesh->getVertexPosition(triangleNodeIds[2]);
+ centroid /= 3.0;
+
+ // Test the localization with each of the triangle vertices and the triangle centroid.
+ std::array<SurgSim::Math::Vector3d, 4> points = {centroid,
+ triangleMesh->getVertexPosition(triangleNodeIds[0]),
+ triangleMesh->getVertexPosition(triangleNodeIds[1]),
+ triangleMesh->getVertexPosition(triangleNodeIds[2])
+ };
+
+ std::array<SurgSim::Math::Vector3d, 4> barycentricCoordinates = {SurgSim::Math::Vector3d::Ones() / 3.0,
+ SurgSim::Math::Vector3d::UnitX(),
+ SurgSim::Math::Vector3d::UnitY(),
+ SurgSim::Math::Vector3d::UnitZ()};
+
+ auto barycentricCoordinate = barycentricCoordinates.cbegin();
+ for (auto point = points.cbegin(); point != points.cend(); ++point, ++barycentricCoordinate)
+ {
+ SurgSim::DataStructures::IndexedLocalCoordinate triangleLocalPosition(triangleId, *barycentricCoordinate);
+ SurgSim::DataStructures::Location location(triangleLocalPosition);
+ std::shared_ptr<SurgSim::Physics::Fem3DRepresentationLocalization> localization;
+
+ EXPECT_NO_THROW(localization =
+ std::dynamic_pointer_cast<SurgSim::Physics::Fem3DRepresentationLocalization>(
+ m_fem->createLocalization(location)););
+ EXPECT_TRUE(localization != nullptr);
+
+ SurgSim::Math::Vector globalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate coordinate = localization->getLocalPosition();
+ EXPECT_NO_THROW(globalPosition =
+ m_fem->getFemElement(coordinate.index)->computeCartesianCoordinate(
+ *m_fem->getCurrentState(), coordinate.coordinate););
+ EXPECT_EQ(3, globalPosition.size());
+ EXPECT_TRUE(globalPosition.isApprox(*point));
+ }
+ }
+}
+
+TEST_F(Fem3DRepresentationTests, ExternalForceAPITest)
+{
+ createFem();
+
+ // External force vector not initialized until the initial state has been set (it contains the #dof...)
+ EXPECT_EQ(0, m_fem->getExternalGeneralizedForce().size());
+ EXPECT_EQ(0, m_fem->getExternalGeneralizedStiffness().rows());
+ EXPECT_EQ(0, m_fem->getExternalGeneralizedStiffness().cols());
+ EXPECT_EQ(0, m_fem->getExternalGeneralizedDamping().rows());
+ EXPECT_EQ(0, m_fem->getExternalGeneralizedDamping().cols());
+
+ m_fem->setInitialState(m_initialState);
+
+ // Vector initialized (properly sized and zeroed)
+ EXPECT_NE(0, m_fem->getExternalGeneralizedForce().size());
+ EXPECT_NE(0, m_fem->getExternalGeneralizedStiffness().rows());
+ EXPECT_NE(0, m_fem->getExternalGeneralizedStiffness().cols());
+ EXPECT_NE(0, m_fem->getExternalGeneralizedDamping().rows());
+ EXPECT_NE(0, m_fem->getExternalGeneralizedDamping().cols());
+ EXPECT_EQ(m_fem->getNumDof(), m_fem->getExternalGeneralizedForce().size());
+ EXPECT_EQ(m_fem->getNumDof(), m_fem->getExternalGeneralizedStiffness().cols());
+ EXPECT_EQ(m_fem->getNumDof(), m_fem->getExternalGeneralizedStiffness().rows());
+ EXPECT_EQ(m_fem->getNumDof(), m_fem->getExternalGeneralizedDamping().cols());
+ EXPECT_EQ(m_fem->getNumDof(), m_fem->getExternalGeneralizedDamping().rows());
+ EXPECT_TRUE(m_fem->getExternalGeneralizedForce().isZero());
+ EXPECT_TRUE(m_fem->getExternalGeneralizedStiffness().isZero());
+ EXPECT_TRUE(m_fem->getExternalGeneralizedDamping().isZero());
+
+ addFemElement();
+ createLocalization();
+
+ Vector FLocalWrongSize = Vector::Ones(2 * m_fem->getNumDofPerNode());
+ Matrix KLocalWrongSize = Matrix::Ones(3 * m_fem->getNumDofPerNode(), 3 * m_fem->getNumDofPerNode());
+ Matrix DLocalWrongSize = Matrix::Ones(4 * m_fem->getNumDofPerNode(), 4 * m_fem->getNumDofPerNode());
+ Vector Flocal = Vector::LinSpaced(m_fem->getNumDofPerNode(), -3.12, 4.09);
+ Matrix Klocal = Matrix::Ones(m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) * 0.34;
+ Matrix Dlocal = Klocal + Matrix::Identity(m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode());
+ Vector F = Vector::Zero(m_fem->getNumDof());
+ F.segment(0, m_fem->getNumDofPerNode()) = Flocal;
+ Matrix K = Matrix::Zero(m_fem->getNumDof(), m_fem->getNumDof());
+ K.block(0, 0, m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) = Klocal;
+ Matrix D = Matrix::Zero(m_fem->getNumDof(), m_fem->getNumDof());
+ D.block(0, 0, m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) = Dlocal;
+
+ // Test invalid localization nullptr
+ ASSERT_THROW(m_fem->addExternalGeneralizedForce(nullptr, Flocal),
+ SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(m_fem->addExternalGeneralizedForce(nullptr, Flocal, Klocal, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid localization type
+ ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_wrongLocalizationType, Flocal),
+ SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_wrongLocalizationType, Flocal, Klocal, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid force size
+ ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_localization, FLocalWrongSize),
+ SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_localization, FLocalWrongSize, Klocal, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid stiffness size
+ ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_localization, Flocal, KLocalWrongSize, Dlocal),
+ SurgSim::Framework::AssertionFailure);
+ // Test invalid damping size
+ ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_localization, Flocal, Klocal, DLocalWrongSize),
+ SurgSim::Framework::AssertionFailure);
+
+ // Test valid call to addExternalGeneralizedForce
+ m_fem->addExternalGeneralizedForce(m_localization, Flocal, Klocal, Dlocal);
+ EXPECT_FALSE(m_fem->getExternalGeneralizedForce().isZero());
+ EXPECT_FALSE(m_fem->getExternalGeneralizedStiffness().isZero());
+ EXPECT_FALSE(m_fem->getExternalGeneralizedDamping().isZero());
+ EXPECT_TRUE(m_fem->getExternalGeneralizedForce().isApprox(F));
+ EXPECT_TRUE(m_fem->getExternalGeneralizedStiffness().isApprox(K));
+ EXPECT_TRUE(m_fem->getExternalGeneralizedDamping().isApprox(D));
+
+ // Test valid call to addExternalGeneralizedForce to add things up
+ m_fem->addExternalGeneralizedForce(m_localization, Flocal, Klocal, Dlocal);
+ EXPECT_TRUE(m_fem->getExternalGeneralizedForce().isApprox(2.0 * F));
+ EXPECT_TRUE(m_fem->getExternalGeneralizedStiffness().isApprox(2.0 * K));
+ EXPECT_TRUE(m_fem->getExternalGeneralizedDamping().isApprox(2.0 * D));
+}
+
+TEST_F(Fem3DRepresentationTests, SerializationTest)
+{
+ auto fem3DRepresentation = std::make_shared<SurgSim::Physics::Fem3DRepresentation>("Test-Fem3D");
+ const std::string filename = "TestFilename";
+ fem3DRepresentation->setFilename(filename);
+ auto collisionRepresentation = std::make_shared<DeformableCollisionRepresentation>("Collision");
+ fem3DRepresentation->setCollisionRepresentation(collisionRepresentation);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*fem3DRepresentation));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Physics::Fem3DRepresentation"];
+ EXPECT_EQ(10u, data.size());
+
+ std::shared_ptr<Fem3DRepresentation> newRepresentation;
+ ASSERT_NO_THROW(newRepresentation = std::dynamic_pointer_cast<Fem3DRepresentation>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_EQ("SurgSim::Physics::Fem3DRepresentation", newRepresentation->getClassName());
+ EXPECT_EQ(filename, newRepresentation->getValue<std::string>("Filename"));
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/FemElementTests.cpp b/SurgSim/Physics/UnitTests/FemElementTests.cpp
new file mode 100644
index 0000000..90c0d0c
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FemElementTests.cpp
@@ -0,0 +1,226 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::Physics::FemElement;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Matrix;
+
+void testSize(const Vector& v, size_t expectedSize)
+{
+ EXPECT_EQ(expectedSize, v.size());
+}
+
+void testSize(const Matrix& m, size_t expectedRows, size_t expectedCols)
+{
+ EXPECT_EQ(expectedRows, m.rows());
+ EXPECT_EQ(expectedCols, m.cols());
+}
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST(FemElementTests, GetSetAddMethods)
+{
+ MockFemElement femElement;
+
+ // Initial setup (numDofPerNode set), no nodes defined yet, density = 0
+ EXPECT_EQ(3u, femElement.getNumDofPerNode());
+ EXPECT_EQ(0u, femElement.getNumNodes());
+ EXPECT_EQ(0, femElement.getNodeIds().size());
+ EXPECT_DOUBLE_EQ(0.0, femElement.getMassDensity());
+ EXPECT_DOUBLE_EQ(0.0, femElement.getYoungModulus());
+ EXPECT_DOUBLE_EQ(0.0, femElement.getPoissonRatio());
+
+ // Test Set/Get Young modulus
+ femElement.setYoungModulus(4455.33);
+ EXPECT_DOUBLE_EQ(4455.33, femElement.getYoungModulus());
+ femElement.setYoungModulus(0.0);
+ EXPECT_DOUBLE_EQ(0.0, femElement.getYoungModulus());
+
+ // Test Set/Get Poisson ratio
+ femElement.setPoissonRatio(0.45);
+ EXPECT_DOUBLE_EQ(0.45, femElement.getPoissonRatio());
+ femElement.setPoissonRatio(0.0);
+ EXPECT_DOUBLE_EQ(0.0, femElement.getPoissonRatio());
+
+ // Test Set/Get mass density
+ femElement.setMassDensity(2343.13);
+ EXPECT_DOUBLE_EQ(2343.13, femElement.getMassDensity());
+ femElement.setMassDensity(0.0);
+ EXPECT_DOUBLE_EQ(0.0, femElement.getMassDensity());
+
+ // Test GetMass
+ SurgSim::Math::OdeState fakeState;
+ femElement.setMassDensity(0.0);
+ EXPECT_DOUBLE_EQ(0.0, femElement.getMass(fakeState));
+ femElement.setMassDensity(1.14);
+ EXPECT_DOUBLE_EQ(1.14, femElement.getMass(fakeState));
+ femElement.setMassDensity(434.55);
+ EXPECT_DOUBLE_EQ(434.55, femElement.getMass(fakeState));
+
+ // Add 1 node
+ femElement.addNode(0);
+ EXPECT_EQ(3u, femElement.getNumDofPerNode());
+ EXPECT_EQ(1u, femElement.getNumNodes());
+ EXPECT_EQ(1, femElement.getNodeIds().size());
+ EXPECT_EQ(0, femElement.getNodeIds()[0]);
+ EXPECT_EQ(0, femElement.getNodeId(0));
+
+ // Add 1 more node
+ femElement.addNode(9);
+ EXPECT_EQ(3u, femElement.getNumDofPerNode());
+ EXPECT_EQ(2u, femElement.getNumNodes());
+ EXPECT_EQ(2, femElement.getNodeIds().size());
+ EXPECT_EQ(0, femElement.getNodeIds()[0]);
+ EXPECT_EQ(0, femElement.getNodeId(0));
+ EXPECT_EQ(9, femElement.getNodeIds()[1]);
+ EXPECT_EQ(9, femElement.getNodeId(1));
+}
+
+TEST(FemElementTests, InitializeMethods)
+{
+ MockFemElement femElement;
+ SurgSim::Math::OdeState fakeState;
+
+ // Mass density not set
+ ASSERT_ANY_THROW(femElement.initialize(fakeState));
+
+ // Poisson Ratio not set
+ femElement.setMassDensity(-1234.56);
+ ASSERT_ANY_THROW(femElement.initialize(fakeState));
+
+ // Young modulus not set
+ femElement.setPoissonRatio(0.55);
+ ASSERT_ANY_THROW(femElement.initialize(fakeState));
+
+ // Invalid mass density
+ femElement.setYoungModulus(-4321.33);
+ ASSERT_ANY_THROW(femElement.initialize(fakeState));
+
+ // Invalid Poisson ratio
+ femElement.setMassDensity(1234.56);
+ ASSERT_ANY_THROW(femElement.initialize(fakeState));
+
+ // Invalid Young modulus
+ femElement.setPoissonRatio(0.499);
+ ASSERT_ANY_THROW(femElement.initialize(fakeState));
+
+ femElement.setYoungModulus(4321.33);
+ ASSERT_NO_THROW(femElement.initialize(fakeState));
+}
+
+TEST(FemElementTests, UpdateTest)
+{
+ MockFemElement femElement;
+ SurgSim::Math::OdeState state;
+
+ // By default, FemElement are considered elements of linear deformation,
+ // therefore no update is required, they return simply true.
+ EXPECT_TRUE(femElement.update(state));
+}
+
+void checkValidCoordinate(const MockFemElement& femElement, double v0, bool expected)
+{
+ Vector naturalCoordinate(1);
+ naturalCoordinate << v0;
+ EXPECT_EQ(expected, femElement.isValidCoordinate(naturalCoordinate));
+}
+
+void checkValidCoordinate(const MockFemElement& femElement, double v0, double v1, bool expected)
+{
+ Vector naturalCoordinate(2);
+ naturalCoordinate << v0, v1;
+ EXPECT_EQ(expected, femElement.isValidCoordinate(naturalCoordinate));
+}
+
+void checkValidCoordinate(const MockFemElement& femElement, double v0, double v1, double v2, bool expected)
+{
+ Vector naturalCoordinate(3);
+ naturalCoordinate << v0, v1, v2;
+ EXPECT_EQ(expected, femElement.isValidCoordinate(naturalCoordinate));
+}
+
+void checkValidCoordinate(const MockFemElement& femElement, double v0, double v1, double v2, double v3, bool expected)
+{
+ Vector naturalCoordinate(4);
+ naturalCoordinate << v0, v1, v2, v3;
+ EXPECT_EQ(expected, femElement.isValidCoordinate(naturalCoordinate));
+}
+
+TEST(FemElementTests, IsValidCoordinate)
+{
+ MockFemElement femElement;
+ femElement.addNode(0);
+ double e = 1e-11;
+
+ checkValidCoordinate(femElement, 1.0, true);
+ checkValidCoordinate(femElement, 1.0 + e, true);
+ checkValidCoordinate(femElement, 1.0 - e, true);
+ checkValidCoordinate(femElement, 1.01, false);
+ checkValidCoordinate(femElement, -1.01, false);
+ checkValidCoordinate(femElement, 0.7, false);
+
+ femElement.addNode(1);
+
+ checkValidCoordinate(femElement, 1.0, 0.0, true);
+ checkValidCoordinate(femElement, 1.0 + e, 0.0, true);
+ checkValidCoordinate(femElement, 1.0 + e, 0.0 - e, true);
+ checkValidCoordinate(femElement, 1.0 - e, 0.0 + e, true);
+ checkValidCoordinate(femElement, 0.5, 0.5, true);
+ checkValidCoordinate(femElement, 0.5 + e, 0.5 + e, true);
+ checkValidCoordinate(femElement, 0.5, 0.51, false);
+ checkValidCoordinate(femElement, 1.0, false);
+ checkValidCoordinate(femElement, -0.01, 1.01, false);
+
+ femElement.addNode(2);
+
+ checkValidCoordinate(femElement, 1.0, 0.0, 0.0, true);
+ checkValidCoordinate(femElement, 1.0 + e, 0.0, 0.0, true);
+ checkValidCoordinate(femElement, 1.0 + e, 0.0 - e, 0.0, true);
+ checkValidCoordinate(femElement, 1.0 - e, 0.0 + e, e, true);
+ checkValidCoordinate(femElement, 0.5, 0.5, e, true);
+ checkValidCoordinate(femElement, 0.5 + e, 0.5 + e, -e, true);
+ checkValidCoordinate(femElement, 0.5, 0.41, 0.1, false);
+ checkValidCoordinate(femElement, 1.0, 0.0, false);
+ checkValidCoordinate(femElement, -0.01, 1.01, e, false);
+
+ femElement.addNode(3);
+
+ checkValidCoordinate(femElement, 1.0, 0.0, 0.0, 0.0, true);
+ checkValidCoordinate(femElement, 1.0 + e, 0.0, 0.0, 0.0, true);
+ checkValidCoordinate(femElement, 1.0 + e, 0.0 - e, 0.0, 0.0, true);
+ checkValidCoordinate(femElement, 1.0 - e, 0.0 + e, e, 0.0, true);
+ checkValidCoordinate(femElement, 0.5, 0.5, e, 0.0, true);
+ checkValidCoordinate(femElement, 0.5 + e, 0.5 + e, 0.0, -e, true);
+ checkValidCoordinate(femElement, 0.5, 0.0, 0.41, 0.1, false);
+ checkValidCoordinate(femElement, 0.0, 1.0, 0.0, false);
+ checkValidCoordinate(femElement, -0.01, 0.0, 1.01, e, false);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/FemRepresentationParametersTest.cpp b/SurgSim/Physics/UnitTests/FemRepresentationParametersTest.cpp
new file mode 100644
index 0000000..4379332
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FemRepresentationParametersTest.cpp
@@ -0,0 +1,183 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "SurgSim/Physics/FemRepresentationParameters.h"
+
+namespace
+{
+ const double epsilon = 1e-10;
+}
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST(FemRepresentationParametersTest, ConstructorTest)
+{
+ ASSERT_NO_THROW( {FemRepresentationParameters femRepresentationParam;});
+}
+
+TEST(FemRepresentationParametersTest, DefaultValueTest)
+{
+ // Create the base rigid representation state
+ std::shared_ptr<FemRepresentationParameters> femRepresentationParam =
+ std::make_shared<FemRepresentationParameters>();
+
+ // Mass density [default = 0]
+ EXPECT_NEAR(0.0, femRepresentationParam->getDensity(), epsilon);
+ // Rayleigh damping mass parameter [default = 0]
+ EXPECT_NEAR(0.0, femRepresentationParam->getRayleighDampingMass(), epsilon);
+ // Rayleigh damping stiffness parameter [default = 0]
+ EXPECT_NEAR(0.0, femRepresentationParam->getRayleighDampingStiffness(), epsilon);
+ // Young modulus [default = 0]
+ EXPECT_NEAR(0.0, femRepresentationParam->getYoungModulus(), epsilon);
+ // Poisson ratio [default = 0]
+ EXPECT_NEAR(0.0, femRepresentationParam->getPoissonRatio(), epsilon);
+ // isValid [default = false]
+ EXPECT_FALSE(femRepresentationParam->isValid());
+}
+
+TEST(FemRepresentationParametersTest, SetGetValidTest)
+{
+ // Create the base rigid representation state
+ std::shared_ptr<FemRepresentationParameters> femRepresentationParam =
+ std::make_shared<FemRepresentationParameters>();
+
+ // Set proper density, young modulus and poisson ratio to test validy flag
+ femRepresentationParam->setDensity(12.52);
+ EXPECT_NEAR(12.52, femRepresentationParam->getDensity(), epsilon);
+ EXPECT_FALSE(femRepresentationParam->isValid());
+ femRepresentationParam->setYoungModulus(10e7);
+ EXPECT_NEAR(10e7, femRepresentationParam->getYoungModulus(), epsilon);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+ femRepresentationParam->setPoissonRatio(0.4);
+ EXPECT_NEAR(0.4, femRepresentationParam->getPoissonRatio(), epsilon);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+
+ // Test Rayleigh damping mass
+ femRepresentationParam->setRayleighDampingMass(13.21);
+ EXPECT_NEAR(13.21, femRepresentationParam->getRayleighDampingMass(), epsilon);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+ femRepresentationParam->setRayleighDampingMass(-13.21);
+ EXPECT_NEAR(-13.21, femRepresentationParam->getRayleighDampingMass(), epsilon);
+ EXPECT_FALSE(femRepresentationParam->isValid());
+ femRepresentationParam->setRayleighDampingMass(0.0);
+ EXPECT_NEAR(0.0, femRepresentationParam->getRayleighDampingMass(), epsilon);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+
+ // Test Rayleigh damping stiffness
+ femRepresentationParam->setRayleighDampingStiffness(3.87);
+ EXPECT_NEAR(3.87, femRepresentationParam->getRayleighDampingStiffness(), epsilon);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+ femRepresentationParam->setRayleighDampingStiffness(-3.87);
+ EXPECT_NEAR(-3.87, femRepresentationParam->getRayleighDampingStiffness(), epsilon);
+ EXPECT_FALSE(femRepresentationParam->isValid());
+ femRepresentationParam->setRayleighDampingStiffness(0.0);
+ EXPECT_NEAR(0.0, femRepresentationParam->getRayleighDampingStiffness(), epsilon);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+
+ // Test validity flag some more
+ femRepresentationParam->setDensity(0.0);
+ EXPECT_FALSE(femRepresentationParam->isValid());
+ femRepresentationParam->setDensity(1.0);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+ femRepresentationParam->setYoungModulus(0.0);
+ EXPECT_FALSE(femRepresentationParam->isValid());
+ femRepresentationParam->setYoungModulus(1.0);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+ femRepresentationParam->setPoissonRatio(-2.0);
+ EXPECT_FALSE(femRepresentationParam->isValid());
+ femRepresentationParam->setPoissonRatio(-1.0);
+ EXPECT_FALSE(femRepresentationParam->isValid());
+ femRepresentationParam->setPoissonRatio(-0.99);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+ femRepresentationParam->setPoissonRatio(0.0);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+ femRepresentationParam->setPoissonRatio(0.499);
+ EXPECT_TRUE(femRepresentationParam->isValid());
+ femRepresentationParam->setPoissonRatio(0.5);
+ EXPECT_FALSE(femRepresentationParam->isValid());
+ femRepresentationParam->setPoissonRatio(1.0);
+ EXPECT_FALSE(femRepresentationParam->isValid());
+}
+
+
+TEST(FemRepresentationParametersTest, BoundaryConditionsTest)
+{
+ // Create the base rigid representation state
+ std::shared_ptr<FemRepresentationParameters> femRepresentationParam =
+ std::make_shared<FemRepresentationParameters>();
+
+ // Add 1 by 1
+ EXPECT_EQ(0u, femRepresentationParam->getBoundaryConditions().size());
+ EXPECT_TRUE(femRepresentationParam->addBoundaryCondition(0));
+ EXPECT_EQ(1u, femRepresentationParam->getBoundaryConditions().size());
+ EXPECT_TRUE(femRepresentationParam->addBoundaryCondition(1));
+ EXPECT_EQ(2u, femRepresentationParam->getBoundaryConditions().size());
+ EXPECT_FALSE(femRepresentationParam->addBoundaryCondition(0));
+ EXPECT_EQ(2u, femRepresentationParam->getBoundaryConditions().size());
+ EXPECT_TRUE(femRepresentationParam->addBoundaryCondition(10));
+ EXPECT_EQ(3u, femRepresentationParam->getBoundaryConditions().size());
+
+ // Remove 1 by 1
+ EXPECT_TRUE(femRepresentationParam->removeBoundaryCondition(10));
+ EXPECT_EQ(2u, femRepresentationParam->getBoundaryConditions().size());
+ EXPECT_FALSE(femRepresentationParam->removeBoundaryCondition(5));
+ EXPECT_EQ(2u, femRepresentationParam->getBoundaryConditions().size());
+ EXPECT_TRUE(femRepresentationParam->removeBoundaryCondition(0));
+ EXPECT_EQ(1u, femRepresentationParam->getBoundaryConditions().size());
+ EXPECT_TRUE(femRepresentationParam->removeBoundaryCondition(1));
+ EXPECT_EQ(0u, femRepresentationParam->getBoundaryConditions().size());
+
+ // Add all
+ std::vector<size_t> data;
+ data.push_back(0);
+ data.push_back(1);
+ data.push_back(2);
+ data.push_back(3);
+ data.push_back(2); // duplicate !
+ EXPECT_EQ(4u, femRepresentationParam->addBoundaryConditions(data));
+ EXPECT_EQ(4u, femRepresentationParam->getBoundaryConditions().size());
+
+ // Clear all
+ femRepresentationParam->clearBoundaryConditions();
+ EXPECT_EQ(0u, femRepresentationParam->getBoundaryConditions().size());
+
+ /// Boundary condition mass property
+ femRepresentationParam->setBoundaryConditionMass(0.0);
+ EXPECT_NEAR(0.0, femRepresentationParam->getBoundaryConditionMass(), epsilon);
+ femRepresentationParam->setBoundaryConditionMass(1.2);
+ EXPECT_NEAR(1.2, femRepresentationParam->getBoundaryConditionMass(), epsilon);
+ femRepresentationParam->setBoundaryConditionMass(0.0);
+ EXPECT_NEAR(0.0, femRepresentationParam->getBoundaryConditionMass(), epsilon);
+
+ /// Boundary condition stiffness property
+ femRepresentationParam->setBoundaryConditionInverseMass(0.0);
+ EXPECT_NEAR(0.0, femRepresentationParam->getBoundaryConditionInverseMass(), epsilon);
+ femRepresentationParam->setBoundaryConditionInverseMass(1.2);
+ EXPECT_NEAR(1.2, femRepresentationParam->getBoundaryConditionInverseMass(), epsilon);
+ femRepresentationParam->setBoundaryConditionInverseMass(0.0);
+ EXPECT_NEAR(0.0, femRepresentationParam->getBoundaryConditionInverseMass(), epsilon);
+}
+
+}; // Physics
+
+}; // SurgSim
diff --git a/SurgSim/Physics/UnitTests/FemRepresentationTests.cpp b/SurgSim/Physics/UnitTests/FemRepresentationTests.cpp
new file mode 100644
index 0000000..1e0340e
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FemRepresentationTests.cpp
@@ -0,0 +1,562 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 FemRepresentationTests.cpp
+/// This file tests the non-abstract functionalities of the base class FemRepresentation
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Framework/Runtime.h" //< Used to initialize the Component Fem3DRepresentation
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+class FemRepresentationTests : public ::testing::Test
+{
+public:
+ double m_rho;
+ double m_nu;
+ double m_E;
+ double m_dt;
+ double m_rayleighDampingMassParameter;
+ double m_rayleighDampingStiffnessParameter;
+
+ Vector m_expectedFemElementsForce;
+ Vector m_expectedRayleighDampingForce;
+ Vector m_expectedGravityForce;
+ Matrix m_expectedMass;
+ Matrix m_expectedDamping;
+ Matrix m_expectedRayleighDamping;
+ Matrix m_expectedStiffness;
+
+ std::shared_ptr<MockFemRepresentation> m_fem;
+ std::shared_ptr<SurgSim::Math::OdeState> m_initialState;
+
+protected:
+ virtual void SetUp() override
+ {
+ m_rho = 2000.0;
+ m_nu = 0.45;
+ m_E = 1e6;
+ m_dt = 1e-3;
+ m_rayleighDampingMassParameter = 0.452;
+ m_rayleighDampingStiffnessParameter = 0.242;
+
+ m_fem = std::make_shared<MockFemRepresentation>("MockFem");
+
+ m_initialState = std::make_shared<SurgSim::Math::OdeState>();
+ m_initialState->setNumDof(m_fem->getNumDofPerNode(), 3);
+ m_initialState->getVelocities().setOnes(); // v = (1...1) to test damping
+ m_fem->setInitialState(m_initialState);
+
+ std::shared_ptr<MockFemElement> element01 = std::make_shared<MockFemElement>();
+ element01->setMassDensity(m_rho);
+ element01->setPoissonRatio(m_nu);
+ element01->setYoungModulus(m_E);
+ element01->addNode(0);
+ element01->addNode(1);
+ m_fem->addFemElement(element01);
+
+ std::shared_ptr<MockFemElement> element12 = std::make_shared<MockFemElement>();
+ element12->setMassDensity(m_rho);
+ element12->setPoissonRatio(m_nu);
+ element12->setYoungModulus(m_E);
+ element12->addNode(1);
+ element12->addNode(2);
+ m_fem->addFemElement(element12);
+
+ // Mass should be a diagonal matrix with diagonal (1 1 1 2 2 2 1 1 1)
+ m_expectedMass = Matrix::Zero(9, 9);
+ m_expectedMass.diagonal() << 1.0, 1.0, 1.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.0;
+
+ // Damping should be a diagonal matrix with diagonal (2 2 2 4 4 4 2 2 2)
+ m_expectedDamping = Matrix::Zero(9, 9);
+ m_expectedDamping.diagonal() << 2.0, 2.0, 2.0, 4.0, 4.0, 4.0, 2.0, 2.0, 2.0;
+
+ // Stiffness should be a diagonal matrix with diagonal (3 3 3 6 6 6 3 3 3)
+ m_expectedStiffness = Matrix::Zero(9, 9);
+ m_expectedStiffness.diagonal() << 3.0, 3.0, 3.0, 6.0, 6.0, 6.0, 3.0, 3.0, 3.0;
+
+ // Rayleigh damping = alpha.M + beta.K M and K are diagonals, so the resulting matrix is too
+ m_expectedRayleighDamping = m_expectedMass * m_rayleighDampingMassParameter;
+ m_expectedRayleighDamping += m_expectedStiffness * m_rayleighDampingStiffnessParameter;
+
+ // FemElements force should be (1 2 3 4 5 6 0 0 0) + (0 0 0 1 2 3 4 5 6) = (1 2 3 5 7 9 4 5 6)
+ m_expectedFemElementsForce.resize(9);
+ m_expectedFemElementsForce << 1.0, 2.0, 3.0, 5.0, 7.0, 9.0, 4.0, 5.0, 6.0;
+
+ // Gravity force should be m.gravity for each node of each element
+ m_expectedGravityForce = Vector::Zero(9);
+ SurgSim::Math::Vector3d g(0.0, -9.81, 0.0);
+ m_expectedGravityForce.segment<3>(0) += g * element01->getMass(*m_initialState) / 2.0;
+ m_expectedGravityForce.segment<3>(3) += g * element01->getMass(*m_initialState) / 2.0;
+ m_expectedGravityForce.segment<3>(3) += g * element12->getMass(*m_initialState) / 2.0;
+ m_expectedGravityForce.segment<3>(6) += g * element12->getMass(*m_initialState) / 2.0;
+
+ // Rayleigh damping force should be -(alpha.M + beta.K).(1...1)^t
+ // with (alpha.M + beta.K) a diagonal matrix
+ m_expectedRayleighDampingForce = -m_expectedRayleighDamping.diagonal();
+ }
+};
+
+TEST_F(FemRepresentationTests, ConstructorTest)
+{
+ ASSERT_NO_THROW({MockFemRepresentation fem("name");});
+ ASSERT_NO_THROW({MockFemRepresentation* fem = new MockFemRepresentation("name"); delete fem;});
+ ASSERT_NO_THROW({std::shared_ptr<MockFemRepresentation> fem = std::make_shared<MockFemRepresentation>("name");});
+}
+
+TEST_F(FemRepresentationTests, AddSetGetTest)
+{
+ MockFemRepresentation fem("name");
+
+ /// Add/Get FemElement
+ EXPECT_EQ(0u, fem.getNumFemElements());
+ ASSERT_ANY_THROW(fem.getFemElement(0));
+ ASSERT_ANY_THROW(fem.getFemElement(1));
+
+ std::shared_ptr<MockFemElement> element = std::make_shared<MockFemElement>();
+ element->setMassDensity(m_rho);
+ element->setPoissonRatio(m_nu);
+ element->setYoungModulus(m_E);
+ ASSERT_NO_THROW(fem.addFemElement(element));
+ EXPECT_EQ(1u, fem.getNumFemElements());
+ EXPECT_NE(nullptr, fem.getFemElement(0));
+ ASSERT_ANY_THROW(fem.getFemElement(1));
+
+ element = std::make_shared<MockFemElement>();
+ element->setMassDensity(m_rho);
+ element->setPoissonRatio(m_nu);
+ element->setYoungModulus(m_E);
+ ASSERT_NO_THROW(fem.addFemElement(element));
+ EXPECT_EQ(2u, fem.getNumFemElements());
+ EXPECT_NE(nullptr, fem.getFemElement(0));
+ EXPECT_NE(nullptr, fem.getFemElement(1));
+
+ /// Test gets the total mass of the fem (Each MockFemElement has a fixed volume of 1m^3)
+ EXPECT_DOUBLE_EQ(2.0 * m_rho, fem.getTotalMass());
+
+ /// Test sets/gets the Rayleigh stiffness parameter
+ EXPECT_DOUBLE_EQ(0.0, fem.getRayleighDampingStiffness());
+ fem.setRayleighDampingStiffness(13.45);
+ EXPECT_DOUBLE_EQ(13.45, fem.getRayleighDampingStiffness());
+
+ /// Test sets/gets the Rayleigh mass parameter
+ EXPECT_DOUBLE_EQ(0.0, fem.getRayleighDampingMass());
+ fem.setRayleighDampingMass(43.99);
+ EXPECT_DOUBLE_EQ(43.99, fem.getRayleighDampingMass());
+}
+
+TEST_F(FemRepresentationTests, IsValidCoordinateTest)
+{
+ using SurgSim::DataStructures::IndexedLocalCoordinate;
+
+ MockFemRepresentation fem("name");
+
+ std::shared_ptr<MockFemElement> element = std::make_shared<MockFemElement>();
+ element->setMassDensity(m_rho);
+ element->setPoissonRatio(m_nu);
+ element->setYoungModulus(m_E);
+ ASSERT_NO_THROW(fem.addFemElement(element));
+
+ // Set up the fem element to have a valid natural coordinate.
+ element->addNode(0);
+ element->addNode(1);
+ Vector validNaturalCoordinate(2);
+ validNaturalCoordinate << 1.0, 0.0;
+
+ // Only the elementId is tested (by passing a valid natural coordinate).
+ // The validation of the natural coordinate is tested in the respective FemElement's UnitTest.
+ IndexedLocalCoordinate invalidCoordinates(1, validNaturalCoordinate);
+ EXPECT_FALSE(fem.isValidCoordinate(invalidCoordinates));
+ IndexedLocalCoordinate validCoordinates(0, validNaturalCoordinate);
+ EXPECT_TRUE(fem.isValidCoordinate(validCoordinates));
+}
+
+TEST_F(FemRepresentationTests, BeforeUpdateTest)
+{
+ MockFemRepresentation fem("name");
+
+ // Throw exception (no FemElement)
+ ASSERT_ANY_THROW(fem.beforeUpdate(m_dt));
+
+ std::shared_ptr<MockFemElement> element = std::make_shared<MockFemElement>();
+ element->setMassDensity(m_rho);
+ element->setPoissonRatio(m_nu);
+ element->setYoungModulus(m_E);
+ fem.addFemElement(element);
+
+ // Throw exception (no initialState setup yet)
+ ASSERT_ANY_THROW(fem.beforeUpdate(m_dt));
+
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(fem.getNumDofPerNode(), 8);
+ fem.setInitialState(initialState);
+
+ ASSERT_NO_THROW(fem.beforeUpdate(m_dt));
+}
+
+TEST_F(FemRepresentationTests, AfterUpdateTest)
+{
+ {
+ SCOPED_TRACE("Valid FemElements");
+
+ m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m_fem->wakeUp();
+
+ ASSERT_TRUE(m_fem->isActive());
+ ASSERT_NO_THROW(m_fem->beforeUpdate(m_dt));
+ ASSERT_TRUE(m_fem->isActive());
+ ASSERT_NO_THROW(m_fem->update(m_dt));
+ ASSERT_TRUE(m_fem->isActive());
+ // After update should backup the currentState into finalState and update all FemElement
+ ASSERT_NO_THROW(m_fem->afterUpdate(m_dt));
+ ASSERT_FALSE(*m_fem->getFinalState() == *m_fem->getInitialState());
+ ASSERT_TRUE(*m_fem->getFinalState() == *m_fem->getCurrentState());
+ ASSERT_TRUE(m_fem->isActive());
+ }
+ {
+ SCOPED_TRACE("Invalid FemElements");
+ MockFemRepresentation fem("name");
+ std::shared_ptr<InvalidMockFemElement> element = std::make_shared<InvalidMockFemElement>();
+ element->setMassDensity(m_rho);
+ element->setPoissonRatio(m_nu);
+ element->setYoungModulus(m_E);
+ // MockFemRepresentation does not connect any nodeIds by default, we need to provide them manually
+ // to construct a fake element. If none is provided, the element does not have any structure, any support,
+ // it would have no mass, no force and no stiffness/mass/damping matrix.
+ // Therefore the system would be undefined.
+ element->addNode(0);
+ element->addNode(1);
+ element->addNode(2);
+ fem.addFemElement(element);
+
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(fem.getNumDofPerNode(), 3);
+ fem.setInitialState(initialState);
+
+ fem.initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ fem.wakeUp();
+
+ ASSERT_TRUE(fem.isActive());
+ ASSERT_NO_THROW(fem.beforeUpdate(m_dt));
+ ASSERT_TRUE(fem.isActive());
+ ASSERT_NO_THROW(fem.update(m_dt));
+ ASSERT_TRUE(fem.isActive());
+ // After update should catch the invalid element update and deactivate the representation
+ ASSERT_NO_THROW(fem.afterUpdate(m_dt));
+ ASSERT_FALSE(fem.isActive());
+ }
+}
+
+TEST_F(FemRepresentationTests, ComputesWithNoGravityAndNoDampingTest)
+{
+ using SurgSim::Math::Vector3d;
+
+ Vector *F;
+ Matrix *M, *D, *K;
+
+ // No gravity, no Rayleigh damping
+ // computeF tests addFemElementsForce
+ m_fem->setIsGravityEnabled(false);
+ m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m_fem->wakeUp();
+
+ Vector expectedF = m_expectedFemElementsForce;
+
+ {
+ SCOPED_TRACE("Without external force");
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeF(*m_initialState).isApprox(expectedF)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeD(*m_initialState).isApprox(m_expectedDamping)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeK(*m_initialState).isApprox(m_expectedStiffness)));
+ // Test combo method computeFMDK
+ EXPECT_NO_THROW(m_fem->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedF));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness));
+ }
+
+ {
+ SCOPED_TRACE("With external force");
+
+ std::shared_ptr<MockDeformableRepresentationLocalization> localization =
+ std::make_shared<MockDeformableRepresentationLocalization>();
+ localization->setRepresentation(m_fem);
+ localization->setLocalNode(0);
+ Vector FextLocal = Vector::Ones(m_fem->getNumDofPerNode());
+ Matrix KextLocal = Matrix::Ones(m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode());
+ Matrix DextLocal = KextLocal + Matrix::Identity(m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode());
+ Vector Fext = Vector::Zero(m_fem->getNumDof());
+ Fext.segment(0, m_fem->getNumDofPerNode()) = FextLocal;
+ Matrix Kext = Matrix::Zero(m_fem->getNumDof(), m_fem->getNumDof());
+ Kext.block(0, 0, m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) = KextLocal;
+ Matrix Dext = Matrix::Zero(m_fem->getNumDof(), m_fem->getNumDof());
+ Dext.block(0, 0, m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) = DextLocal;
+ m_fem->addExternalGeneralizedForce(localization, FextLocal, KextLocal, DextLocal);
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeF(*m_initialState).isApprox(expectedF + Fext)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeD(*m_initialState).isApprox(m_expectedDamping + Dext)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeK(*m_initialState).isApprox(m_expectedStiffness + Kext)));
+ EXPECT_NO_THROW(m_fem->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedF + Fext));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + Dext));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness + Kext));
+ }
+}
+
+TEST_F(FemRepresentationTests, ComputesWithNoGravityAndDampingTest)
+{
+ using SurgSim::Math::Vector3d;
+
+ Vector *F;
+ Matrix *M, *D, *K;
+
+ // No gravity, Rayleigh damping
+ // computeF tests addFemElementsForce and addRayleighDampingForce
+ m_fem->setIsGravityEnabled(false);
+ m_fem->setRayleighDampingMass(m_rayleighDampingMassParameter);
+ m_fem->setRayleighDampingStiffness(m_rayleighDampingStiffnessParameter);
+ m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m_fem->wakeUp();
+
+ SurgSim::Math::Vector expectedF = m_expectedFemElementsForce + m_expectedRayleighDampingForce;
+
+ {
+ SCOPED_TRACE("Without external force");
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeF(*m_initialState).isApprox(expectedF)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeD(*m_initialState).isApprox(
+ m_expectedDamping + m_expectedRayleighDamping)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeK(*m_initialState).isApprox(m_expectedStiffness)));
+ // Test combo method computeFMDK
+ EXPECT_NO_THROW(m_fem->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedF));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + m_expectedRayleighDamping));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness));
+ }
+
+ {
+ SCOPED_TRACE("With external force");
+
+ std::shared_ptr<MockDeformableRepresentationLocalization> localization =
+ std::make_shared<MockDeformableRepresentationLocalization>();
+ localization->setRepresentation(m_fem);
+ localization->setLocalNode(0);
+ Vector FextLocal = Vector::Ones(m_fem->getNumDofPerNode());
+ Matrix KextLocal = Matrix::Ones(m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode());
+ Matrix DextLocal = KextLocal + Matrix::Identity(m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode());
+ Vector Fext = Vector::Zero(m_fem->getNumDof());
+ Fext.segment(0, m_fem->getNumDofPerNode()) = FextLocal;
+ Matrix Kext = Matrix::Zero(m_fem->getNumDof(), m_fem->getNumDof());
+ Kext.block(0, 0, m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) = KextLocal;
+ Matrix Dext = Matrix::Zero(m_fem->getNumDof(), m_fem->getNumDof());
+ Dext.block(0, 0, m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) = DextLocal;
+ m_fem->addExternalGeneralizedForce(localization, FextLocal, KextLocal, DextLocal);
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeF(*m_initialState).isApprox(expectedF + Fext)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeD(*m_initialState).isApprox(m_expectedDamping +
+ m_expectedRayleighDamping + Dext)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeK(*m_initialState).isApprox(m_expectedStiffness + Kext)));
+ EXPECT_NO_THROW(m_fem->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedF + Fext));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + m_expectedRayleighDamping + Dext));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness + Kext));
+ }
+}
+
+TEST_F(FemRepresentationTests, ComputesWithGravityAndNoDampingTest)
+{
+ using SurgSim::Math::Vector3d;
+
+ Vector *F;
+ Matrix *M, *D, *K;
+
+ // Gravity, no Rayleigh damping
+ // computeF tests addFemElementsForce and addGravityForce
+ m_fem->setIsGravityEnabled(true);
+ m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m_fem->wakeUp();
+
+ SurgSim::Math::Vector expectedF = m_expectedFemElementsForce + m_expectedGravityForce;
+
+ {
+ SCOPED_TRACE("Without external force");
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeF(*m_initialState).isApprox(expectedF)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeD(*m_initialState).isApprox(m_expectedDamping)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeK(*m_initialState).isApprox(m_expectedStiffness)));
+ // Test combo method computeFMDK
+ EXPECT_NO_THROW(m_fem->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedF));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness));
+ }
+
+ {
+ SCOPED_TRACE("With external force");
+
+ std::shared_ptr<MockDeformableRepresentationLocalization> localization =
+ std::make_shared<MockDeformableRepresentationLocalization>();
+ localization->setRepresentation(m_fem);
+ localization->setLocalNode(0);
+ Vector FextLocal = Vector::Ones(m_fem->getNumDofPerNode());
+ Matrix KextLocal = Matrix::Ones(m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode());
+ Matrix DextLocal = KextLocal + Matrix::Identity(m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode());
+ Vector Fext = Vector::Zero(m_fem->getNumDof());
+ Fext.segment(0, m_fem->getNumDofPerNode()) = FextLocal;
+ Matrix Kext = Matrix::Zero(m_fem->getNumDof(), m_fem->getNumDof());
+ Kext.block(0, 0, m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) = KextLocal;
+ Matrix Dext = Matrix::Zero(m_fem->getNumDof(), m_fem->getNumDof());
+ Dext.block(0, 0, m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) = DextLocal;
+ m_fem->addExternalGeneralizedForce(localization, FextLocal, KextLocal, DextLocal);
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeF(*m_initialState).isApprox(expectedF + Fext)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeD(*m_initialState).isApprox(m_expectedDamping + Dext)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeK(*m_initialState).isApprox(m_expectedStiffness + Kext)));
+ EXPECT_NO_THROW(m_fem->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedF + Fext));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + Dext));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness + Kext));
+ }
+}
+
+TEST_F(FemRepresentationTests, ComputesWithGravityAndDampingTest)
+{
+ using SurgSim::Math::Vector3d;
+
+ Vector *F;
+ Matrix *M, *D, *K;
+
+ // Gravity, Rayleigh damping
+ // computeF tests addFemElementsForce, addRayleighDampingForce and addGravityForce
+ m_fem->setIsGravityEnabled(true);
+ m_fem->setRayleighDampingMass(m_rayleighDampingMassParameter);
+ m_fem->setRayleighDampingStiffness(m_rayleighDampingStiffnessParameter);
+ m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m_fem->wakeUp();
+
+ SurgSim::Math::Vector expectedF = m_expectedFemElementsForce + m_expectedRayleighDampingForce +
+ m_expectedGravityForce;
+
+ {
+ SCOPED_TRACE("Without external force");
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeF(*m_initialState).isApprox(expectedF)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeD(*m_initialState).isApprox(
+ m_expectedDamping + m_expectedRayleighDamping)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeK(*m_initialState).isApprox(m_expectedStiffness)));
+ // Test combo method computeFMDK
+ EXPECT_NO_THROW(m_fem->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedF));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + m_expectedRayleighDamping));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness));
+ }
+
+ {
+ SCOPED_TRACE("With external force");
+
+ std::shared_ptr<MockDeformableRepresentationLocalization> localization =
+ std::make_shared<MockDeformableRepresentationLocalization>();
+ localization->setRepresentation(m_fem);
+ localization->setLocalNode(0);
+ Vector FextLocal = Vector::Ones(m_fem->getNumDofPerNode());
+ Matrix KextLocal = Matrix::Ones(m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode());
+ Matrix DextLocal = KextLocal + Matrix::Identity(m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode());
+ Vector Fext = Vector::Zero(m_fem->getNumDof());
+ Fext.segment(0, m_fem->getNumDofPerNode()) = FextLocal;
+ Matrix Kext = Matrix::Zero(m_fem->getNumDof(), m_fem->getNumDof());
+ Kext.block(0, 0, m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) = KextLocal;
+ Matrix Dext = Matrix::Zero(m_fem->getNumDof(), m_fem->getNumDof());
+ Dext.block(0, 0, m_fem->getNumDofPerNode(), m_fem->getNumDofPerNode()) = DextLocal;
+ m_fem->addExternalGeneralizedForce(localization, FextLocal, KextLocal, DextLocal);
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeF(*m_initialState).isApprox(expectedF + Fext)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeD(*m_initialState).isApprox(m_expectedDamping +
+ m_expectedRayleighDamping + Dext)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_fem->computeK(*m_initialState).isApprox(m_expectedStiffness + Kext)));
+ EXPECT_NO_THROW(m_fem->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedF + Fext));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + m_expectedRayleighDamping + Dext));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness + Kext));
+ }
+}
+
+TEST_F(FemRepresentationTests, DoInitializeTest)
+{
+ using SurgSim::Framework::Runtime;
+
+ MockFemRepresentation fem("name");
+
+ // Setup the initial state
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(fem.getNumDofPerNode(), 8);
+ fem.setInitialState(initialState);
+
+ // Add one element
+ std::shared_ptr<MockFemElement> element = std::make_shared<MockFemElement>();
+ element->setMassDensity(m_rho);
+ element->setPoissonRatio(m_nu);
+ element->setYoungModulus(m_E);
+ element->addNode(0);
+ element->addNode(1);
+ element->addNode(2);
+ fem.addFemElement(element);
+
+ // Initialize the component, this will call FemRepresentation::doInitialize
+ fem.initialize(std::make_shared<Runtime>());
+
+ // At this point, all FemElement and m_massPerNode should have been initialized
+ // We have 8 nodes, 1 element connecting nodes (0 1 2)
+ // The mass should be evenly distributed on the 3 first nodes, and 0 for all others.
+ std::shared_ptr<MockFemElement> femElement;
+ ASSERT_NO_THROW(femElement = std::static_pointer_cast<MockFemElement>(fem.getFemElement(0)));
+ ASSERT_TRUE(femElement->isInitialized());
+ ASSERT_EQ(8u, fem.getMassPerNode().size());
+ ASSERT_EQ(m_rho, fem.getTotalMass());
+ for (size_t nodeId = 0; nodeId < 3; ++nodeId)
+ {
+ ASSERT_EQ(m_rho / 3.0, fem.getMassPerNode()[nodeId]);
+ }
+ for (size_t nodeId = 3; nodeId < 8; ++nodeId)
+ {
+ ASSERT_EQ(0u, fem.getMassPerNode()[nodeId]);
+ }
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/FixedRepresentationBilateral3DTests.cpp b/SurgSim/Physics/UnitTests/FixedRepresentationBilateral3DTests.cpp
new file mode 100644
index 0000000..2739abc
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FixedRepresentationBilateral3DTests.cpp
@@ -0,0 +1,116 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "SurgSim/Math/MlcpConstraintType.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/FixedRepresentationBilateral3D.h"
+#include "SurgSim/Physics/FixedRepresentationLocalization.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+const double epsilon = 1e-10;
+const double dt = 1e-3;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST(FixedRepresentationBilateral3DTests, Constructor)
+{
+ ASSERT_NO_THROW(
+ { FixedRepresentationBilateral3D constraint; });
+}
+
+TEST(FixedRepresentationBilateral3DTests, Constants)
+{
+ FixedRepresentationBilateral3D constraint;
+
+ EXPECT_EQ(SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT, constraint.getMlcpConstraintType());
+ EXPECT_EQ(SurgSim::Physics::REPRESENTATION_TYPE_FIXED, constraint.getRepresentationType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(FixedRepresentationBilateral3DTests, BuildMlcp)
+{
+ // Whitebox test which validates ConstraintImplementation::build's output parameter, MlcpPhysicsProblem. It assumes
+ // CHt and HCHt can be correctly built given H, so it does not neccessarily construct the physical parameters
+ // neccessary to supply a realistic C. It only checks H and b.
+ FixedRepresentationBilateral3D constraint;
+
+ Vector3d actual = Vector3d(8.0, 6.4, 3.5);
+
+ // Setup parameters for FixedRepresentationBilateral3D::build
+ auto localization
+ = std::make_shared<FixedRepresentationLocalization>(std::make_shared<FixedRepresentation>("representation"));
+ localization->setLocalPosition(actual);
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(0, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ // Compare results
+ Eigen::Matrix<double, 3, 1> violation = actual;
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(FixedRepresentationBilateral3DTests, BuildMlcpTwoStep)
+{
+ // Whitebox test which validates ConstraintImplementation::build's output parameter, MlcpPhysicsProblem. It assumes
+ // CHt and HCHt can be correctly built given H, so it does not neccessarily construct the physical parameters
+ // neccessary to supply a realistic C. It only checks H and b.
+ FixedRepresentationBilateral3D constraint;
+
+ Vector3d actual = Vector3d(8.0, 6.4, 3.5);
+ Vector3d desired = Vector3d(3.0, 7.7, 0.0);
+
+ // Setup parameters for FixedRepresentationBilateral3D::build
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(0, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ auto localization
+ = std::make_shared<FixedRepresentationLocalization>(std::make_shared<FixedRepresentation>("representation"));
+
+ localization->setLocalPosition(actual);
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ localization->setLocalPosition(desired);
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_NEGATIVE_SIDE));
+
+ // Compare results
+ Eigen::Matrix<double, 3, 1> violation = actual - desired;
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/FixedRepresentationContactTests.cpp b/SurgSim/Physics/UnitTests/FixedRepresentationContactTests.cpp
new file mode 100644
index 0000000..deffeaa
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FixedRepresentationContactTests.cpp
@@ -0,0 +1,87 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/FixedRepresentationContact.h"
+#include "SurgSim/Physics/FixedRepresentationLocalization.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationContact.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+TEST (FixedRepresentationContactTests, SetGet_BuildMlcp_Test)
+{
+ SurgSim::Math::Vector3d n(0.0, 1.0, 0.0);
+ double d = 0.0;
+ double violation = -0.01;
+
+ SurgSim::Math::Vector3d contactPosition = -n * (d - violation);
+ SurgSim::Math::RigidTransform3d poseFixed;
+ poseFixed.setIdentity();
+
+ std::shared_ptr<FixedRepresentation> fixed = std::make_shared<FixedRepresentation>("Fixed");
+ fixed->setLocalActive(true);
+ fixed->setIsGravityEnabled(false);
+ fixed->setLocalPose(poseFixed);
+
+ std::shared_ptr<FixedRepresentationLocalization> loc = std::make_shared<FixedRepresentationLocalization>(fixed);
+ loc->setLocalPosition(contactPosition);
+ std::shared_ptr<FixedRepresentationContact> implementation = std::make_shared<FixedRepresentationContact>();
+
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, implementation->getMlcpConstraintType());
+ EXPECT_EQ(1u, implementation->getNumDof());
+
+ ContactConstraintData constraintData;
+ constraintData.setPlaneEquation(n, d);
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(fixed->getNumDof(), 1, 1);
+
+ // Fill up the Mlcp
+ double dt = 1e-3;
+ implementation->build(dt, constraintData, loc, &mlcpPhysicsProblem,
+ 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ // b should be exactly the violation
+ EXPECT_NEAR(violation, mlcpPhysicsProblem.b[0], epsilon);
+
+ // Constraint H should be [] (a fixed representation has no dof !)
+
+ // ConstraintTypes should contain 0 entry as it is setup by the constraint and not the ConstraintImplementation
+ // This way, the constraint can verify that both ConstraintImplementation are the same type
+ ASSERT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/FixedRepresentationLocalizationTest.cpp b/SurgSim/Physics/UnitTests/FixedRepresentationLocalizationTest.cpp
new file mode 100644
index 0000000..215b8a9
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FixedRepresentationLocalizationTest.cpp
@@ -0,0 +1,99 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+using SurgSim::Physics::FixedRepresentation;
+using SurgSim::Physics::RigidRepresentationLocalization;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+class FixedRepresentationLocalizationTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ SurgSim::Math::Quaterniond q;
+ SurgSim::Math::Vector3d t;
+
+ q.coeffs().setRandom();
+ q.normalize();
+ t.setRandom();
+ m_initialTransformation = SurgSim::Math::makeRigidTransform(q, t);
+
+ do
+ {
+ q.coeffs().setRandom();
+ q.normalize();
+ t.setRandom();
+ m_currentTransformation = SurgSim::Math::makeRigidTransform(q, t);
+ } while (m_initialTransformation.isApprox(m_currentTransformation));
+
+ m_identityTransformation.setIdentity();
+ }
+
+ void TearDown()
+ {
+ }
+
+ // Fixed representation initialization pose
+ SurgSim::Math::RigidTransform3d m_initialTransformation;
+
+ // Fixed representation current pose
+ SurgSim::Math::RigidTransform3d m_currentTransformation;
+
+ // Identity pose (no translation/rotation)
+ SurgSim::Math::RigidTransform3d m_identityTransformation;
+};
+
+TEST_F(FixedRepresentationLocalizationTest, GetPositionTest)
+{
+ // Create the rigid body
+ std::shared_ptr<FixedRepresentation> fixedRepresentation =
+ std::make_shared<FixedRepresentation>("FixedRepresentation");
+
+ // Activate the rigid body and setup its initial pose
+ fixedRepresentation->setLocalActive(true);
+ fixedRepresentation->setLocalPose(m_initialTransformation);
+ fixedRepresentation->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ fixedRepresentation->wakeUp();
+
+ RigidRepresentationLocalization localization = RigidRepresentationLocalization(fixedRepresentation);
+ ASSERT_EQ(fixedRepresentation, localization.getRepresentation());
+
+ SurgSim::Math::Vector3d origin = m_initialTransformation.translation();
+ SurgSim::Math::Vector3d zero = SurgSim::Math::Vector3d::Zero();
+ localization.setLocalPosition(zero);
+ EXPECT_TRUE(localization.getLocalPosition().isZero(epsilon));
+ EXPECT_TRUE(localization.calculatePosition().isApprox(origin, epsilon));
+
+ SurgSim::Math::Vector3d position = SurgSim::Math::Vector3d::Random();
+ localization.setLocalPosition(position);
+ EXPECT_TRUE(localization.getLocalPosition().isApprox(position, epsilon));
+ EXPECT_FALSE(localization.calculatePosition().isApprox(origin, epsilon));
+}
diff --git a/SurgSim/Physics/UnitTests/FixedRepresentationTest.cpp b/SurgSim/Physics/UnitTests/FixedRepresentationTest.cpp
new file mode 100644
index 0000000..d1565ad
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FixedRepresentationTest.cpp
@@ -0,0 +1,229 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationState.h"
+
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::SphereShape;
+using SurgSim::Physics::Representation;
+using SurgSim::Physics::FixedRepresentation;
+using SurgSim::Physics::RigidCollisionRepresentation;
+using SurgSim::Physics::RigidRepresentationState;
+
+class FixedRepresentationTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ Quaterniond q;
+ Vector3d t;
+
+ q.coeffs().setRandom();
+ q.normalize();
+ t.setRandom();
+ m_initialTransformation = SurgSim::Math::makeRigidTransform(q, t);
+
+ do
+ {
+ q.coeffs().setRandom();
+ q.normalize();
+ t.setRandom();
+ m_currentTransformation = SurgSim::Math::makeRigidTransform(q, t);
+ } while (m_initialTransformation.isApprox(m_currentTransformation));
+
+ m_identityTransformation.setIdentity();
+
+ m_fixedRepresentation = std::make_shared<FixedRepresentation>("FixedRepresentation");
+ m_element = std::make_shared<BasicSceneElement>("element");
+ m_element->addComponent(m_fixedRepresentation);
+ }
+
+ std::shared_ptr<FixedRepresentation> m_fixedRepresentation;
+ std::shared_ptr<BasicSceneElement> m_element;
+
+ // Fixed representation initialization pose
+ RigidTransform3d m_initialTransformation;
+
+ // Fixed representation current pose
+ RigidTransform3d m_currentTransformation;
+
+ // Identity pose (no translation/rotation)
+ RigidTransform3d m_identityTransformation;
+};
+
+TEST_F(FixedRepresentationTest, ConstructorTest)
+{
+ ASSERT_NO_THROW(FixedRepresentation fixedRepresentation("FixedRepresentation"));
+}
+
+TEST_F(FixedRepresentationTest, ResetStateTest)
+{
+ m_fixedRepresentation->setLocalActive(false);
+ m_fixedRepresentation->setIsGravityEnabled(false);
+ m_fixedRepresentation->setLocalPose(m_initialTransformation);
+
+ m_element->initialize();
+ m_fixedRepresentation->wakeUp();
+
+ // Initial = Current = Previous = m_initialTransformation
+ EXPECT_TRUE(m_fixedRepresentation->getInitialState().getPose().isApprox(m_initialTransformation));
+ EXPECT_TRUE(m_fixedRepresentation->getCurrentState() == m_fixedRepresentation->getInitialState());
+ EXPECT_TRUE(m_fixedRepresentation->getPreviousState() == m_fixedRepresentation->getInitialState());
+
+ m_fixedRepresentation->setLocalPose(m_currentTransformation);
+ m_fixedRepresentation->beforeUpdate(1.0);
+ m_fixedRepresentation->update(1.0);
+ m_fixedRepresentation->afterUpdate(1.0);
+ // update is supposed to backup current in previous and set current
+ // Therefore it should not affect initial
+ // Initial = Previous = m_initialTransformation
+ // Current = m_currentTransformation
+ EXPECT_TRUE(m_fixedRepresentation->getCurrentState() != m_fixedRepresentation->getInitialState());
+ EXPECT_TRUE(m_fixedRepresentation->getPreviousState() == m_fixedRepresentation->getInitialState());
+ EXPECT_TRUE(m_fixedRepresentation->getPreviousState() != m_fixedRepresentation->getCurrentState());
+ EXPECT_TRUE(m_fixedRepresentation->getCurrentState().getPose().isApprox(m_currentTransformation));
+
+ m_fixedRepresentation->setLocalPose(m_currentTransformation);
+ m_fixedRepresentation->beforeUpdate(1.0);
+ m_fixedRepresentation->update(1.0);
+ m_fixedRepresentation->afterUpdate(1.0);
+ // update is supposed to backup current in previous and set current
+ // Therefore it should not affect initial
+ // Initial = m_initialTransformation
+ // Previous = Current = m_currentTransformation
+ EXPECT_TRUE(m_fixedRepresentation->getCurrentState() != m_fixedRepresentation->getInitialState());
+ EXPECT_TRUE(m_fixedRepresentation->getPreviousState() != m_fixedRepresentation->getInitialState());
+ EXPECT_TRUE(m_fixedRepresentation->getPreviousState() == m_fixedRepresentation->getCurrentState());
+ EXPECT_TRUE(m_fixedRepresentation->getCurrentState().getPose().isApprox(m_currentTransformation));
+
+ std::shared_ptr<Representation> representation = m_fixedRepresentation;
+ // reset the representation (NOT THE FIXED REPRESENTATION, test polymorphism)
+ representation->resetState();
+
+ // isActive flag unchanged
+ EXPECT_FALSE(m_fixedRepresentation->isActive());
+ // isGravityEnable flag unchanged
+ EXPECT_FALSE(m_fixedRepresentation->isGravityEnabled());
+ // The current rigid state should be exactly the initial rigid state
+ EXPECT_TRUE(m_fixedRepresentation->getCurrentState() == m_fixedRepresentation->getInitialState());
+ EXPECT_TRUE(m_fixedRepresentation->getCurrentState().getPose().isApprox(m_initialTransformation));
+ EXPECT_TRUE(m_fixedRepresentation->getInitialState().getPose().isApprox(m_initialTransformation));
+ // The previous rigid state should be exactly the initial rigid state
+ EXPECT_TRUE(m_fixedRepresentation->getPreviousState() == m_fixedRepresentation->getInitialState());
+}
+
+TEST_F(FixedRepresentationTest, SetGetAndDefaultValueTest)
+{
+ // Get/Set active flag [default = true]
+ EXPECT_TRUE(m_fixedRepresentation->isActive());
+ m_fixedRepresentation->setLocalActive(false);
+ ASSERT_FALSE(m_fixedRepresentation->isActive());
+ m_fixedRepresentation->setLocalActive(true);
+ ASSERT_TRUE(m_fixedRepresentation->isActive());
+
+ // Get numDof = 0
+ ASSERT_EQ(0u, m_fixedRepresentation->getNumDof());
+
+ // Set/Get isGravityEnabled [default = true]
+ EXPECT_TRUE(m_fixedRepresentation->isGravityEnabled());
+ m_fixedRepresentation->setIsGravityEnabled(false);
+ ASSERT_FALSE(m_fixedRepresentation->isGravityEnabled());
+ m_fixedRepresentation->setIsGravityEnabled(true);
+ ASSERT_TRUE(m_fixedRepresentation->isGravityEnabled());
+}
+
+TEST_F(FixedRepresentationTest, UpdateTest)
+{
+ double dt = 1.0;
+
+ m_fixedRepresentation->setLocalPose(m_initialTransformation);
+ m_element->initialize();
+ m_fixedRepresentation->wakeUp();
+
+ m_fixedRepresentation->setLocalPose(m_currentTransformation);
+ m_fixedRepresentation->beforeUpdate(dt);
+ m_fixedRepresentation->update(dt);
+ m_fixedRepresentation->afterUpdate(dt);
+
+ EXPECT_TRUE(m_fixedRepresentation->getCurrentState() != m_fixedRepresentation->getPreviousState());
+ EXPECT_TRUE(m_fixedRepresentation->getCurrentState() != m_fixedRepresentation->getInitialState());
+ EXPECT_TRUE(m_fixedRepresentation->getPreviousState() == m_fixedRepresentation->getInitialState());
+}
+
+
+TEST_F(FixedRepresentationTest, SerializationTest)
+{
+
+ {
+ SCOPED_TRACE("Encode/Decode as shared_ptr<>, should be OK");
+ auto rigidRepresentation = std::make_shared<FixedRepresentation>("TestFixedRepresentation");
+ YAML::Node node;
+ ASSERT_NO_THROW(node =
+ YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::encode(rigidRepresentation));
+
+ std::shared_ptr<FixedRepresentation> newRepresentation;
+ EXPECT_NO_THROW(newRepresentation =
+ std::dynamic_pointer_cast<FixedRepresentation>(node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ EXPECT_NE(nullptr, newRepresentation);
+ }
+
+ {
+ SCOPED_TRACE("Encode a FixedRepresentation object without a shape, should throw.");
+ auto rigidRepresentation = std::make_shared<FixedRepresentation>("TestFixedRepresentation");
+ auto rigidCollisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("RigidCollisionRepresentation");
+
+ rigidRepresentation->setCollisionRepresentation(rigidCollisionRepresentation);
+
+ EXPECT_ANY_THROW(YAML::convert<SurgSim::Framework::Component>::encode(*rigidRepresentation));
+ }
+
+ {
+ SCOPED_TRACE("Encode a FixedRepresentation object with valid RigidCollisionRepresentation and shape, no throw");
+ auto rigidRepresentation = std::make_shared<FixedRepresentation>("TestFixedRepresentation");
+ auto rigidCollisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("RigidCollisionRepresentation");
+
+ rigidRepresentation->setCollisionRepresentation(rigidCollisionRepresentation);
+ rigidRepresentation->setShape(std::make_shared<SphereShape>(0.1));
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*rigidRepresentation));
+
+ std::shared_ptr<FixedRepresentation> newRepresentation;
+ newRepresentation =
+ std::dynamic_pointer_cast<FixedRepresentation>(node.as<std::shared_ptr<SurgSim::Framework::Component>>());
+ EXPECT_NE(nullptr, newRepresentation->getCollisionRepresentation());
+
+ auto newCollisionRepresentation =
+ std::dynamic_pointer_cast<RigidCollisionRepresentation>(newRepresentation->getCollisionRepresentation());
+ EXPECT_EQ(newRepresentation, newCollisionRepresentation->getRigidRepresentation());
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/FreeMotionTests.cpp b/SurgSim/Physics/UnitTests/FreeMotionTests.cpp
new file mode 100644
index 0000000..671f77f
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FreeMotionTests.cpp
@@ -0,0 +1,67 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 FreeMotionTests.cpp
+/// Simple Test for FreeMotion calculation
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/FreeMotion.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+using SurgSim::Physics::Representation;
+using SurgSim::Physics::RigidRepresentation;
+using SurgSim::Physics::FreeMotion;
+using SurgSim::Physics::PhysicsManagerState;
+
+TEST(FreeMotionTest, RunTest)
+{
+ std::vector<std::shared_ptr<Representation>> representations = std::vector<std::shared_ptr<Representation>>();
+ std::shared_ptr<RigidRepresentation> representation = std::make_shared<RigidRepresentation>("TestSphere");
+
+ representation->setDensity(700.0); // Wood
+ std::shared_ptr<SphereShape> shape = std::make_shared<SphereShape>(0.01); // 1cm Sphere
+ representation->setShape(shape);
+
+ representations.push_back(representation);
+
+ std::shared_ptr<PhysicsManagerState> state = std::make_shared<PhysicsManagerState>();
+ state->setRepresentations(representations);
+
+ FreeMotion computation;
+
+ representation->setIsGravityEnabled(false);
+ EXPECT_TRUE(representation->getCurrentState().getPose().translation().isZero());
+ state = computation.update(1.0,state);
+ EXPECT_TRUE(representation->getCurrentState().getPose().translation().isZero());
+
+ representation->setIsGravityEnabled(true);
+ state = computation.update(1.0,state);
+ EXPECT_FALSE(representation->getCurrentState().getPose().translation().isZero());
+}
diff --git a/SurgSim/Physics/UnitTests/LinearSpringTest.cpp b/SurgSim/Physics/UnitTests/LinearSpringTest.cpp
new file mode 100644
index 0000000..20b69b1
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/LinearSpringTest.cpp
@@ -0,0 +1,485 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/LinearSpring.h"
+
+using SurgSim::Math::Matrix;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::LinearSpring;
+
+namespace
+{
+const double epsilon = 1e-10;
+const double epsilonNumericalEvaluation = 1e-8;
+const double epsilonTestAgainstNumericalApproximation = 1e-7;
+
+Eigen::Matrix<double, 6, 6> KFormal(const Vector3d p0, const Vector3d p1,
+ const Vector3d v0, const Vector3d v1,
+ double l0, double stiffness, double damping)
+{
+ Vector3d u = p1 - p0;
+ double m_l = u.norm();
+ u /= m_l;
+ double lRatio = (m_l - l0) / m_l;
+ double vRatio = (v1 -v0).dot(u) / m_l;
+
+ Matrix33d K00 = Matrix33d::Identity() * (stiffness * lRatio + damping * vRatio);
+ K00 -= (u * u.transpose()) * (stiffness * (lRatio - 1.0) + 2.0 * damping * vRatio);
+ K00 += damping * (u * (v1 -v0).transpose()) / m_l;
+
+ Eigen::Matrix<double, 6, 6> K;
+ K.setZero();
+
+ // Assembly stage in K
+ SurgSim::Math::addSubMatrix( K00, 0, 0, 3, 3, &K);
+ SurgSim::Math::addSubMatrix(-K00, 0, 1, 3, 3, &K);
+ SurgSim::Math::addSubMatrix(-K00, 1, 0, 3, 3, &K);
+ SurgSim::Math::addSubMatrix( K00, 1, 1, 3, 3, &K);
+
+ return K;
+}
+
+Eigen::Matrix<double, 6, 6> DFormal(const Vector3d p0, const Vector3d p1,
+ const Vector3d v0, const Vector3d v1,
+ double l0, double stiffness, double damping)
+{
+ Vector3d u = p1 - p0;
+ u.normalize();
+ Matrix33d D00 = damping * (u * u.transpose());
+
+ // Assembly stage in D
+ Eigen::Matrix<double, 6, 6> D;
+ D.setZero();
+ SurgSim::Math::addSubMatrix( D00, 0, 0, 3, 3, &D);
+ SurgSim::Math::addSubMatrix(-D00, 0, 1, 3, 3, &D);
+ SurgSim::Math::addSubMatrix(-D00, 1, 0, 3, 3, &D);
+ SurgSim::Math::addSubMatrix( D00, 1, 1, 3, 3, &D);
+
+ return D;
+}
+
+double f(size_t axis, const Vector3d p0, const Vector3d p1,
+ const Vector3d v0, const Vector3d v1,
+ double l0, double stiffness, double damping)
+{
+ Vector3d u = p1 - p0;
+ double m_l = u.norm();
+ u /= m_l;
+ double elongationPosition = m_l - l0;
+ double elongationVelocity = (v1 - v0).dot(u);
+ if (axis <=2)
+ {
+ return (stiffness * elongationPosition + damping * elongationVelocity) * u[axis];
+ }
+ else
+ {
+ return -(stiffness * elongationPosition + damping * elongationVelocity) * u[axis - 3];
+ }
+}
+
+Eigen::Matrix<double, 6, 6> KNumerical(const Vector3d p0, const Vector3d p1,
+ const Vector3d v0, const Vector3d v1,
+ double l0, double stiffness, double damping)
+{
+ Eigen::Matrix<double, 6, 6> dfdx;
+
+ for (size_t row = 0; row < 6; row++)
+ {
+ for (size_t col = 0; col < 6; col++)
+ {
+ // dfrow/dxcol
+ // (1) f(x+delta) = f(x) + df/dx.delta + o(delta^2)
+ // (2) f(x-delta) = f(x) - df/dx.delta + o(delta^2)
+ // (1) - (2) f(x+delta) - f(x-delta) = 2df/dx.delta
+ // df/dx = (f(x+delta) - f(x-delta)) / 2delta
+ Eigen::Matrix<double, 6 ,1> delta6D;
+ delta6D.setZero();
+ delta6D[col] = epsilonNumericalEvaluation;
+ double f_plus_delta = f(row, p0 + delta6D.segment(0, 3), p1 + delta6D.segment(3, 3), v0, v1,
+ l0, stiffness, damping);
+ double f_minus_delta = f(row, p0 - delta6D.segment(0, 3), p1 - delta6D.segment(3, 3), v0, v1,
+ l0, stiffness, damping);
+ dfdx(row, col) = (f_plus_delta - f_minus_delta) / (2.0 * epsilonNumericalEvaluation);
+ }
+ }
+
+ return - dfdx;
+}
+
+Eigen::Matrix<double, 6, 6> DNumerical(const Vector3d p0, const Vector3d p1,
+ const Vector3d v0, const Vector3d v1,
+ double l0, double stiffness, double damping)
+{
+ Eigen::Matrix<double, 6, 6> dfdv;
+
+ for (size_t row = 0; row < 6; row++)
+ {
+ for (size_t col = 0; col < 6; col++)
+ {
+ // dfrow/dxcol
+ // (1) f(x+delta) = f(x) + df/dx.delta + o(delta^2)
+ // (2) f(x-delta) = f(x) - df/dx.delta + o(delta^2)
+ // (1) - (2) f(x+delta) - f(x-delta) = 2df/dx.delta
+ // df/dx = (f(x+delta) - f(x-delta)) / 2delta
+ Eigen::Matrix<double, 6 ,1> delta6D;
+ delta6D.setZero();
+ delta6D[col] = epsilonNumericalEvaluation;
+ double f_plus_delta = f(row, p0, p1, v0 + delta6D.segment(0, 3), v1 + delta6D.segment(3, 3),
+ l0, stiffness, damping);
+ double f_moins_delta = f(row, p0, p1, v0 - delta6D.segment(0, 3), v1 - delta6D.segment(3, 3),
+ l0, stiffness, damping);
+ dfdv(row, col) = (f_plus_delta - f_moins_delta) / (2.0 * epsilonNumericalEvaluation);
+ }
+ }
+
+ return - dfdv;
+}
+
+};
+
+TEST(LinearSpringTests, Constructor)
+{
+ ASSERT_NO_THROW({LinearSpring ls(0, 1);});
+ ASSERT_NO_THROW({LinearSpring *ls = new LinearSpring(0, 1); delete ls;});
+ ASSERT_NO_THROW({std::shared_ptr<LinearSpring> ls = std::make_shared<LinearSpring>(0, 1);});
+}
+
+TEST(LinearSpringTests, SetGetMethods)
+{
+ LinearSpring ls(0, 1);
+
+ // Stiffness getter/setter
+ ls.setStiffness(0.34);
+ ASSERT_DOUBLE_EQ(0.34, ls.getStiffness());
+
+ // Damping getter/setter
+ ls.setDamping(0.45);
+ ASSERT_DOUBLE_EQ(0.45, ls.getDamping());
+
+ // Rest length getter/setter
+ ls.setRestLength(1.23);
+ ASSERT_DOUBLE_EQ(1.23, ls.getRestLength());
+
+ // Operator ==/!= (with same node Ids)
+ LinearSpring ls2(0, 1);
+ ASSERT_TRUE(ls != ls2);
+ ls2.setStiffness(ls.getStiffness());
+ ASSERT_TRUE(ls != ls2);
+ ls2.setDamping(ls.getDamping());
+ ASSERT_TRUE(ls != ls2);
+ ls2.setRestLength(ls.getRestLength());
+ ASSERT_TRUE(ls == ls2);
+ ls2.setDamping(ls.getDamping() + 0.55);
+ ASSERT_TRUE(ls != ls2);
+ ls2.setDamping(ls.getDamping());
+ ls2.setStiffness(ls.getStiffness() + 0.23);
+ ASSERT_TRUE(ls != ls2);
+
+ // Operator ==/!= (with different node Ids)
+ LinearSpring ls3(0, 2);
+ ASSERT_TRUE(ls != ls3);
+ ls3.setStiffness(ls.getStiffness());
+ ASSERT_TRUE(ls != ls3);
+ ls3.setDamping(ls.getDamping());
+ ASSERT_TRUE(ls != ls3);
+ ls3.setRestLength(ls.getRestLength());
+ ASSERT_TRUE(ls != ls3);
+}
+
+TEST(LinearSpringTests, computeMethods)
+{
+ using SurgSim::Math::setSubVector;
+ using SurgSim::Math::setSubMatrix;
+
+ // Setup the spring
+ LinearSpring ls(0, 1);
+ ls.setStiffness(0.34);
+ ls.setDamping(0.45);
+ ls.setRestLength(1.23);
+
+ // Setup the state
+ SurgSim::Math::OdeState state;
+ state.setNumDof(3u, 2u);
+ setSubVector(Vector3d(0.0, 0.0, 0.0), 0, 3, &state.getPositions());
+ setSubVector(Vector3d(2.3, 4.1, 1.2), 1, 3, &state.getPositions());
+ setSubVector(Vector3d(0.1, 0.2, 0.5), 0, 3, &state.getVelocities());
+ setSubVector(Vector3d(-0.3, -0.14, 0.0), 1, 3, &state.getVelocities());
+
+ // Calculating spring force
+ Vector3d expectedF3D(0.45563016177577925, 0.81221028838291076, 0.23772008440475439);
+ Vector expectedF;
+ expectedF.resize(6u);
+ setSubVector(expectedF3D, 0, 3, &expectedF);
+ setSubVector(-expectedF3D, 1, 3, &expectedF);
+ Vector f(6);
+ f.setZero();
+ ls.addForce(state, &f);
+ EXPECT_TRUE(f.isApprox(expectedF)) << " F = " << f.transpose() << std::endl <<
+ "expectedF = " << expectedF.transpose() << std::endl;
+
+ // Calculate stiffness matrix
+ Matrix33d expectedStiffness33;
+ expectedStiffness33 << 0.224919570941728960, 0.064210544149390564, 0.0011847965179350647,
+ 0.047808674990512064, 0.312562344690556770, 0.0021120285754494608,
+ 0.013992782924052311, 0.033501153469247251, 0.1987182250423049600;
+ Matrix expectedK;
+ expectedK.resize(6u, 6u);
+ setSubMatrix( expectedStiffness33, 0, 0, 3, 3, &expectedK);
+ setSubMatrix(-expectedStiffness33, 0, 1, 3, 3, &expectedK);
+ setSubMatrix(-expectedStiffness33, 1, 0, 3, 3, &expectedK);
+ setSubMatrix( expectedStiffness33, 1, 1, 3, 3, &expectedK);
+ Matrix K(6, 6);
+ K.setZero();
+ ls.addStiffness(state, &K);
+ EXPECT_TRUE(K.isApprox(expectedK)) << " K = " << std::endl << K << std::endl <<
+ "expectedK = " << std::endl << expectedK << std::endl;
+
+ // Calculate damping matrix
+ Matrix33d expectedDamping33;
+ expectedDamping33 << 0.101125743415463040, 0.180267629566694950, 0.052761257434154628,
+ 0.180267629566694950, 0.321346644010195360, 0.094052676295666937,
+ 0.052761257434154628, 0.094052676295666937, 0.027527612574341543;
+ Matrix expectedD;
+ expectedD.resize(6u, 6u);
+ setSubMatrix( expectedDamping33, 0, 0, 3, 3, &expectedD);
+ setSubMatrix(-expectedDamping33, 0, 1, 3, 3, &expectedD);
+ setSubMatrix(-expectedDamping33, 1, 0, 3, 3, &expectedD);
+ setSubMatrix( expectedDamping33, 1, 1, 3, 3, &expectedD);
+ Matrix D(6, 6);
+ D.setZero();
+ ls.addDamping(state, &D);
+ EXPECT_TRUE(D.isApprox(expectedD)) << " D = " << std::endl << D << std::endl <<
+ "expectedD = " << std::endl << expectedD << std::endl;
+
+ // Compute all together
+ {
+ SCOPED_TRACE("Testing addFDK method call");
+ Vector f(6);
+ Matrix K(6, 6), D(6, 6);
+ f.setZero();
+ K.setZero();
+ D.setZero();
+ ls.addFDK(state, &f, &D, &K);
+ EXPECT_TRUE(f.isApprox(expectedF)) << " F = " << f.transpose() << std::endl <<
+ "expectedF = " << expectedF.transpose() << std::endl;
+ EXPECT_TRUE(K.isApprox(expectedK)) << " K = " << std::endl << K << std::endl <<
+ "expectedK = " << std::endl << expectedK << std::endl;
+ EXPECT_TRUE(D.isApprox(expectedD)) << " D = " << std::endl << D << std::endl <<
+ "expectedD = " << std::endl << expectedD << std::endl;
+ }
+
+ // Test addMatVec method
+ Vector ones(6), oneToSix(6);
+ ones.setOnes();
+ oneToSix.setLinSpaced(1.0, 6.0);
+
+ f.setZero();
+ ls.addMatVec(state, 1.0, 0.0, ones, &f);
+ {
+ SCOPED_TRACE("addMatVec(..., dampingFactor=1, stiffnessFactor=0, (1 1 1 1 1 1),...)");
+ for (size_t row = 0; row < 6; ++row)
+ {
+ EXPECT_NEAR(expectedD.row(row).sum(), f[row], epsilon) <<
+ "f[" << row << "] = " << f[row] <<
+ " expectedValue = " << expectedD.row(row).sum();
+ }
+ }
+ f.setZero();
+ ls.addMatVec(state, 1.0, 0.0, oneToSix, &f);
+ {
+ SCOPED_TRACE("addMatVec(..., dampingFactor=1, stiffnessFactor=0, (1 2 3 4 5 6),...)");
+ for (size_t row = 0; row < 6; ++row)
+ {
+ EXPECT_NEAR(expectedD.row(row).dot(oneToSix), f[row], epsilon) <<
+ "f[" << row << "] = " << f[row] <<
+ " expectedValue = " << expectedD.row(row).dot(oneToSix);
+ }
+ }
+ f.setZero();
+ ls.addMatVec(state, 0.0, 1.0, ones, &f);
+ {
+ SCOPED_TRACE("addMatVec(..., dampingFactor=0, stiffnessFactor=1, (1 1 1 1 1 1),...)");
+ for (size_t row = 0; row < 6; ++row)
+ {
+ EXPECT_NEAR(expectedK.row(row).sum(), f[row], epsilon) <<
+ "f[" << row << "] = " << f[row] <<
+ " expectedValue = " << expectedK.row(row).sum();
+ }
+ }
+ f.setZero();
+ ls.addMatVec(state, 0.0, 1.0, oneToSix, &f);
+ {
+ SCOPED_TRACE("addMatVec(..., dampingFactor=0, stiffnessFactor=1, (1 2 3 4 5 6),...)");
+ for (size_t row = 0; row < 6; ++row)
+ {
+ EXPECT_NEAR(expectedK.row(row).dot(oneToSix), f[row], epsilon) <<
+ "f[" << row << "] = " << f[row] <<
+ " expectedValue = " << expectedK.row(row).dot(oneToSix);
+ }
+ }
+
+ f.setZero();
+ ls.addMatVec(state, 1.4, 4.1, ones, &f);
+ {
+ SCOPED_TRACE("addMatVec(..., dampingFactor=1.4, stiffnessFactor=4.1, (1 1 1 1 1 1),...)");
+ for (size_t row = 0; row < 6; ++row)
+ {
+ EXPECT_NEAR(1.4 * expectedD.row(row).sum() + 4.1 * expectedK.row(row).sum(), f[row], epsilon) <<
+ "f[" << row << "] = " << f[row] <<
+ " expectedValue = " << 1.4 * expectedD.row(row).sum() + 4.1 * expectedK.row(row).sum();
+ }
+ }
+ f.setZero();
+ ls.addMatVec(state, 1.4, 4.1, oneToSix, &f);
+ {
+ SCOPED_TRACE("addMatVec(..., dampingFactor=1.4, stiffnessFactor=4.1, (1 2 3 4 5 6),...)");
+ for (size_t row = 0; row < 6; ++row)
+ {
+ EXPECT_NEAR(\
+ 1.4 * expectedD.row(row).dot(oneToSix) + 4.1 * expectedK.row(row).dot(oneToSix), f[row], epsilon) <<
+ "f[" << row << "] = " << f[row] <<
+ " expectedValue = " << 1.4 * expectedD.row(row).dot(oneToSix) + 4.1 * expectedK.row(row).dot(oneToSix);
+ }
+ }
+}
+
+TEST(LinearSpringTests, addStiffnessNumericalTest)
+{
+ Vector3d x0(0.1, 0.2, 0.3), x1(1.3, 1.2, 1.45), v0(0.1, 0.2, 0.3), v1(-0.1, 0.12, -0.45);
+ double restLength = 1.0, stiffness = 5.0, damping = 2.0;
+
+ // Setup the spring
+ LinearSpring ls(0, 1);
+ ls.setStiffness(stiffness);
+ ls.setDamping(damping);
+ ls.setRestLength(restLength);
+
+ // Setup the state
+ SurgSim::Math::OdeState state;
+ state.setNumDof(3, 2); // 2 nodes of 3DOF each
+ state.getPositions().segment(0, 3) = x0;
+ state.getPositions().segment(3, 3) = x1;
+ state.getVelocities().segment(0, 3) = v0;
+ state.getVelocities().segment(3, 3) = v1;
+
+ Matrix K(6, 6);
+ K.setZero();
+ ls.addStiffness(state, &K);
+
+ Eigen::Matrix<double, 6, 6> Knumeric = KNumerical(x0, x1, v0, v1, restLength, stiffness, damping);
+ Eigen::Matrix<double, 6, 6> Kformal = KFormal(x0, x1, v0, v1, restLength, stiffness, damping);
+ EXPECT_TRUE(Kformal.isApprox(Knumeric, epsilonTestAgainstNumericalApproximation)) << std::endl <<
+ "Kformal = " << std::endl << Kformal << std::endl <<
+ "Knumeric = " << std::endl << Knumeric << std::endl <<
+ "Kformal - Knumeric= " << std::endl << Kformal - Knumeric << std::endl;
+ EXPECT_TRUE(K.isApprox(Kformal)) << std::endl <<
+ "K = " << std::endl << K << std::endl <<
+ "Knumeric = " << std::endl << Knumeric << std::endl <<
+ "K - Knumeric= " << std::endl << K - Knumeric << std::endl;
+}
+
+TEST(LinearSpringTests, addDampingNumericalTest)
+{
+ Vector3d x0(0.1, 0.2, 0.3), x1(1.3, 1.2, 1.45), v0(0.1, 0.2, 0.3), v1(-0.1, 0.12, -0.45);
+ double restLength = 1.0, stiffness = 5.0, damping = 2.0;
+
+ // Setup the spring
+ LinearSpring ls(0, 1);
+ ls.setStiffness(stiffness);
+ ls.setDamping(damping);
+ ls.setRestLength(restLength);
+
+ // Setup the state
+ SurgSim::Math::OdeState state;
+ state.setNumDof(3, 2); // 2 nodes of 3DOF each
+ state.getPositions().segment(0, 3) = x0;
+ state.getPositions().segment(3, 3) = x1;
+ state.getVelocities().segment(0, 3) = v0;
+ state.getVelocities().segment(3, 3) = v1;
+
+ Matrix D(6, 6);
+ D.setZero();
+ ls.addDamping(state, &D);
+
+ Eigen::Matrix<double, 6, 6> Dnumeric = DNumerical(x0, x1, v0, v1, restLength, stiffness, damping);
+ Eigen::Matrix<double, 6, 6> Dformal = DFormal(x0, x1, v0, v1, restLength, stiffness, damping);
+ EXPECT_TRUE(Dformal.isApprox(Dnumeric, epsilonTestAgainstNumericalApproximation)) << std::endl <<
+ "Dformal = " << std::endl << Dformal << std::endl <<
+ "Dnumeric = " << std::endl << Dnumeric << std::endl <<
+ "Dformal - Dnumeric= " << std::endl << Dformal - Dnumeric << std::endl;
+ EXPECT_TRUE(D.isApprox(Dformal)) << std::endl <<
+ "D = " << std::endl << D << std::endl <<
+ "Dnumeric = " << std::endl << Dnumeric << std::endl <<
+ "D - Dnumeric= " << std::endl << D - Dnumeric << std::endl;
+}
+
+TEST(LinearSpringTests, addFDKNumericalTest)
+{
+ Vector3d x0(0.1, 0.2, 0.3), x1(1.3, 1.2, 1.45), v0(0.1, 0.2, 0.3), v1(-0.1, 0.12, -0.45);
+ double restLength = 1.0, stiffness = 5.0, damping = 2.0;
+
+ // Setup the spring
+ LinearSpring ls(0, 1);
+ ls.setStiffness(stiffness);
+ ls.setDamping(damping);
+ ls.setRestLength(restLength);
+
+ // Setup the state
+ SurgSim::Math::OdeState state;
+ state.setNumDof(3, 2); // 2 nodes of 3DOF each
+ state.getPositions().segment(0, 3) = x0;
+ state.getPositions().segment(3, 3) = x1;
+ state.getVelocities().segment(0, 3) = v0;
+ state.getVelocities().segment(3, 3) = v1;
+
+ Vector F(6);
+ Matrix K(6, 6);
+ Matrix D(6, 6);
+ F.setZero();
+ K.setZero();
+ D.setZero();
+ ls.addFDK(state, &F, &D, &K);
+
+ Eigen::Matrix<double, 6, 6> Knumeric = KNumerical(x0, x1, v0, v1, restLength, stiffness, damping);
+ Eigen::Matrix<double, 6, 6> Kformal = KFormal(x0, x1, v0, v1, restLength, stiffness, damping);
+ EXPECT_TRUE(Kformal.isApprox(Knumeric, epsilonTestAgainstNumericalApproximation)) << std::endl <<
+ "Kformal = " << std::endl << Kformal << std::endl <<
+ "Knumeric = " << std::endl << Knumeric << std::endl <<
+ "Kformal - Knumeric= " << std::endl << Kformal - Knumeric << std::endl;
+ EXPECT_TRUE(K.isApprox(Kformal)) << std::endl <<
+ "K = " << std::endl << K << std::endl <<
+ "Knumeric = " << std::endl << Knumeric << std::endl <<
+ "K - Knumeric= " << std::endl << K - Knumeric << std::endl;
+
+ Eigen::Matrix<double, 6, 6> Dnumeric = DNumerical(x0, x1, v0, v1, restLength, stiffness, damping);
+ Eigen::Matrix<double, 6, 6> Dformal = DFormal(x0, x1, v0, v1, restLength, stiffness, damping);
+ EXPECT_TRUE(Dformal.isApprox(Dnumeric, epsilonTestAgainstNumericalApproximation)) << std::endl <<
+ "Dformal = " << std::endl << Dformal << std::endl <<
+ "Dnumeric = " << std::endl << Dnumeric << std::endl <<
+ "Dformal - Dnumeric= " << std::endl << Dformal - Dnumeric << std::endl;
+ EXPECT_TRUE(D.isApprox(Dformal)) << std::endl <<
+ "D = " << std::endl << D << std::endl <<
+ "Dnumeric = " << std::endl << Dnumeric << std::endl <<
+ "D - Dnumeric= " << std::endl << D - Dnumeric << std::endl;
+}
diff --git a/SurgSim/Physics/UnitTests/MassSpringMechanicalValidationTests.cpp b/SurgSim/Physics/UnitTests/MassSpringMechanicalValidationTests.cpp
new file mode 100644
index 0000000..7f4da0f
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/MassSpringMechanicalValidationTests.cpp
@@ -0,0 +1,318 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/LinearSpring.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::LinearSpring;
+using SurgSim::Physics::MassSpringRepresentation;
+using SurgSim::Physics::MockMassSpring;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+class MassSpringMechanicalValidationTests : public ::testing::Test
+{
+public:
+ void SetUp() override
+ {
+ // Initialization values for the simulation
+ m_dt = 1e-3;
+
+ m_numNodes = 10;
+ m_totalMass = 0.1;
+ m_springStiffness = 1.0;
+ m_springDamping = 0.1;
+ m_rayleighDampingMass = 1e0;
+ m_rayleighDampingStiffness = 1e-3;
+ m_nodeBoundaryConditions.push_back(0);
+
+ // Poses
+ m_poseIdentity.setIdentity();
+ Quaterniond q;
+ q.coeffs().setRandom();
+ q.normalize();
+ Vector3d t;
+ t.setRandom();
+ m_poseRandom = SurgSim::Math::makeRigidTransform(q, t);
+ }
+
+ /// Run the energy test on a spring
+ /// \param scheme The numerical integration scheme to be used
+ /// \param expectedBehavior -1 if the energy should decrease, 1 if it should increase, 0 if stable
+ void runEnergyTest(SurgSim::Math::IntegrationScheme scheme, int expectedBehavior)
+ {
+ m_dt = 1e-3;
+
+ // 2 nodes, the 1st node fixed, no Rayleigh damping and no spring damping
+ // A spring oscillating, only subject to mass and stiffness
+ m_numNodes = 2;
+ m_totalMass = 0.1;
+ m_springStiffness = 1.0;
+ m_springDamping = 0.0;
+ m_rayleighDampingMass = 0.0;
+ m_rayleighDampingStiffness = 0.0;
+ m_nodeBoundaryConditions.clear();
+ m_nodeBoundaryConditions.push_back(0);
+
+ MockMassSpring m("MassSpring", m_poseRandom, m_numNodes, m_nodeBoundaryConditions, m_totalMass,
+ m_rayleighDampingMass, m_rayleighDampingStiffness, m_springStiffness, m_springDamping,
+ scheme);
+
+ m.initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m.wakeUp();
+
+ // Pull on the free mass, by simply making the initial length shorter (creating an extension right away)
+ std::static_pointer_cast<LinearSpring>(m.getSpring(0))->setRestLength(0.0);
+ m.setIsGravityEnabled(false);
+
+ Vector3d initialDelta = m.getCurrentState()->getPosition(1) - m.getCurrentState()->getPosition(0);
+ double initialEk = 0.5 * (m_totalMass/2.0) * m.getCurrentState()->getVelocity(1).squaredNorm();
+ double initialEs = 0.5 * m_springStiffness * initialDelta.squaredNorm();
+ double initialEnergy = initialEk + initialEs;
+
+ // Simulate 2 seconds of virtual time
+ double time = 0.0;
+ while(time < 2.0)
+ {
+ m.beforeUpdate(m_dt);
+ m.update(m_dt);
+ m.afterUpdate(m_dt);
+
+ Vector3d previousDelta = m.getPreviousState()->getPosition(1) - m.getPreviousState()->getPosition(0);
+ Vector3d currentDelta = m.getCurrentState()->getPosition(1) - m.getCurrentState()->getPosition(0);
+
+ // Calculate previous/current system energy (kinetic energy + spring energy)
+ double previousEk = 0.5 * (m_totalMass/2.0) * m.getPreviousState()->getVelocity(1).squaredNorm();
+ double previousEs = 0.5 * m_springStiffness * previousDelta.squaredNorm();
+ double previousEnergy = previousEk + previousEs;
+ double currentEk = 0.5 * (m_totalMass/2.0) * m.getCurrentState()->getVelocity(1).squaredNorm();
+ double currentEs = 0.5 * m_springStiffness * currentDelta.squaredNorm();
+ double currentEnergy = currentEk + currentEs;
+ if (expectedBehavior < 0)
+ {
+ EXPECT_LT(currentEnergy, previousEnergy);
+ }
+ else if (expectedBehavior > 0)
+ {
+ EXPECT_GT(currentEnergy, previousEnergy);
+ }
+ else
+ {
+ EXPECT_NEAR(currentEnergy, previousEnergy, 2.6e-6);
+ EXPECT_NEAR(currentEnergy, initialEnergy, 3e-4);
+ }
+
+ time += m_dt;
+ }
+ }
+
+protected:
+ /// Simulation parameters
+ double m_dt;
+
+ /// Number of nodes
+ size_t m_numNodes;
+ /// NodeIds boundary conditions
+ std::vector<size_t> m_nodeBoundaryConditions;
+ /// Total mass (in Kg)
+ double m_totalMass;
+ /// Spring stiffness and damping
+ double m_springStiffness, m_springDamping;
+ /// Rayleigh damping (mass and stiffness coefs)
+ double m_rayleighDampingMass, m_rayleighDampingStiffness;
+
+ /// IdentityPose and random pose
+ SurgSim::Math::RigidTransform3d m_poseIdentity, m_poseRandom;
+};
+
+TEST_F(MassSpringMechanicalValidationTests, NoGravityTest)
+{
+ // Note the use of identity pose to avoid small variation between the spring rest length and current length
+ MockMassSpring m("MassSpring", m_poseIdentity, m_numNodes, m_nodeBoundaryConditions, m_totalMass,
+ m_rayleighDampingMass, m_rayleighDampingStiffness, m_springStiffness, m_springDamping,
+ SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER);
+
+ m.setIsGravityEnabled(false);
+
+ m.initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m.wakeUp();
+
+ m.beforeUpdate(m_dt);
+ m.update(m_dt);
+ m.afterUpdate(m_dt);
+
+ EXPECT_EQ(*m.getInitialState(), *m.getFinalState());
+ EXPECT_EQ(*m.getInitialState(), *m.getCurrentState());
+ EXPECT_EQ(*m.getInitialState(), *m.getPreviousState());
+}
+
+TEST_F(MassSpringMechanicalValidationTests, OneSpringFrequencyTest)
+{
+ m_numNodes = 2;
+ m_totalMass = 0.1;
+ m_springStiffness = 1.0;
+ m_springDamping = 0.0;
+ m_rayleighDampingMass = 0.0;
+ m_rayleighDampingStiffness = 0.0;
+ m_nodeBoundaryConditions.clear();
+ m_nodeBoundaryConditions.push_back(0);
+
+ // Only the Modified Euler Explicit integration conserves the energy exactly
+ // (explicit adds energy to the system, implicit removes energy to the system)
+ MockMassSpring m("MassSpring", m_poseRandom, m_numNodes, m_nodeBoundaryConditions, m_totalMass,
+ m_rayleighDampingMass, m_rayleighDampingStiffness, m_springStiffness, m_springDamping,
+ SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER);
+
+ // Pull on the free mass, by simply making the initial length shorter (creating an extension right away)
+ std::static_pointer_cast<LinearSpring>(m.getSpring(0))->setRestLength(0.0);
+ m.setIsGravityEnabled(false);
+
+ m.initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m.wakeUp();
+
+ // Frequency = 1/(2PI) sqrt(k / m)
+ double f = 1.0/(2.0 * M_PI) * sqrt(m_springStiffness / (m_totalMass/2.0));
+ // Period = 1/f
+ double period = 1.0 / f;
+ // Let's look for a time step, factor of period, small enough for stability of Explicit Euler (~1Khz)
+ m_dt = period;
+ while(m_dt > 1e-3) m_dt *= 0.5;
+
+ // Simulate a single mass connected to a spring at the same time for comparison purpose
+ Vector3d x0 = SurgSim::Math::getSubVector(m.getInitialState()->getPositions(), 1, 3);
+ Vector3d x = x0;
+ Vector3d v = Vector3d::Zero();
+
+ double time = 0.0;
+ // Let's do all iterations (except the last 2) testing that the mass is NOT back yet to its original position
+ while(time < period - 2.0 * m_dt)
+ {
+ m.beforeUpdate(m_dt);
+ m.update(m_dt);
+ m.afterUpdate(m_dt);
+
+ // Manually simulate a single mass connected to a spring
+ Vector3d anchor = SurgSim::Math::getSubVector(m.getInitialState()->getPositions(), 0, 3);
+ Vector3d f = (m_springStiffness * (anchor - x)) / (m_totalMass/2.0);
+ v += f * m_dt;
+ x += v * m_dt;
+
+ const Vector3d& finalPosition = SurgSim::Math::getSubVector(m.getFinalState()->getPositions(), 1, 3);
+ const Vector3d& finalVelocity = SurgSim::Math::getSubVector(m.getFinalState()->getVelocities(), 1, 3);
+ const Vector3d& currentPosition = SurgSim::Math::getSubVector(m.getCurrentState()->getPositions(), 1, 3);
+ EXPECT_TRUE(finalPosition.isApprox(currentPosition));
+ EXPECT_TRUE(finalPosition.isApprox(x, 1e-8));
+ EXPECT_TRUE(finalVelocity.isApprox(v, 1e-8));
+
+ Vector3d deltaCompare = finalPosition - x0;
+ EXPECT_FALSE(deltaCompare.isZero(1e-11)) << "Error is " << deltaCompare.norm();
+ deltaCompare = currentPosition - x0;
+ EXPECT_FALSE(deltaCompare.isZero(1e-11)) << "Error is " << deltaCompare.norm();
+
+ time += m_dt;
+ }
+
+ // Let's do the last 2 iterations without testing as we are getting close enough to the solution
+ // that the test might become true before reaching the very last iteration
+ while(time < period)
+ {
+ m.beforeUpdate(m_dt);
+ m.update(m_dt);
+ m.afterUpdate(m_dt);
+ time += m_dt;
+ }
+
+ const Vector3d& finalPosition = SurgSim::Math::getSubVector(m.getFinalState()->getPositions(), 1, 3);
+ const Vector3d& currentPosition = SurgSim::Math::getSubVector(m.getCurrentState()->getPositions(), 1, 3);
+ Vector3d deltaCompare = finalPosition - x0;
+ EXPECT_TRUE(deltaCompare.isZero(2e-9)) << "Error is " << deltaCompare.norm();
+ deltaCompare = currentPosition - x0;
+ EXPECT_TRUE(deltaCompare.isZero(2e-9)) << "Error is " << deltaCompare.norm();
+}
+
+TEST_F(MassSpringMechanicalValidationTests, FallingTest)
+{
+ // No boundary conditions to let the model fall
+ m_nodeBoundaryConditions.clear();
+
+ MockMassSpring m("MassSpring", m_poseRandom, m_numNodes, m_nodeBoundaryConditions, m_totalMass,
+ m_rayleighDampingMass, m_rayleighDampingStiffness, m_springStiffness, m_springDamping,
+ SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER);
+
+ m.initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m.wakeUp();
+
+ // run few iterations of simulation...
+ for (int i = 0; i< 5; i++)
+ {
+ m.beforeUpdate(m_dt);
+ m.update(m_dt);
+ m.afterUpdate(m_dt);
+
+ const Vector& v = m.getFinalState()->getVelocities();
+
+ // Making sure that each mass has a velocity directed toward the gravity vector direction
+ // with no orthogonal components
+ for (size_t nodeId = 0; nodeId < m.getNumMasses(); nodeId++)
+ {
+ const Vector3d& vi = SurgSim::Math::getSubVector(v, nodeId, 3);
+ double vi_dot_g = vi.dot(m.getGravityVector());
+ EXPECT_GE(vi_dot_g, 0.0) << "v = " << vi.transpose() << " ; g = " << m.getGravityVector().transpose();
+ const Vector3d vi_cross_g = vi.cross(m.getGravityVector());
+ EXPECT_TRUE(vi_cross_g.isZero()) << "v = " << vi.transpose() << " ; g = " <<
+ m.getGravityVector().transpose() << " vi^g = " << vi_cross_g.transpose();
+ }
+ }
+}
+
+TEST_F(MassSpringMechanicalValidationTests, EnergyTest)
+{
+ {
+ SCOPED_TRACE("Testing energy increase for Euler explicit");
+
+ // For Euler explicit, the energy should increase
+ runEnergyTest(SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER, 1);
+ }
+
+ {
+ SCOPED_TRACE("Testing energy decrease for Euler implicit");
+
+ // For Euler implicit, the energy should decrease
+ runEnergyTest(SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER, -1);
+ }
+
+ {
+ SCOPED_TRACE("Testing energy stability for Modified Euler explicit");
+
+ // For modified Euler explicit , the energy should be stable
+ runEnergyTest(SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER, 0);
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/MassSpringRepresentationContactTest.cpp b/SurgSim/Physics/UnitTests/MassSpringRepresentationContactTest.cpp
new file mode 100644
index 0000000..319fdc2
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/MassSpringRepresentationContactTest.cpp
@@ -0,0 +1,259 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/MassSpring1DRepresentation.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/MassSpringRepresentationContact.h"
+#include "SurgSim/Physics/MassSpringRepresentationLocalization.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+
+namespace
+{
+ const double epsilon = 1e-10;
+ const double dt = 1e-3;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+using SurgSim::Math::Vector3d;
+
+class MassSpringRepresentationContactTest : public ::testing::Test
+{
+public:
+ void SetUp() {
+ // Define plane normal
+ m_n = Vector3d(0.8539, 0.6289, -0.9978);
+ m_n.normalize();
+
+ // Place spring at random location.
+ m_extremities.push_back(Vector3d(0.8799, -0.0871, 0.7468));
+ m_extremities.push_back(Vector3d(0.9040, -0.7074, 0.6783));
+
+ // Define physics representation of mass-spring using 1d helper function.
+ m_massSpring = std::make_shared<SurgSim::Blocks::MassSpring1DRepresentation>("MassSpring");
+ size_t numNodesPerDim[1] = {2};
+ m_massPerNode = 0.137;
+ std::vector<size_t> boundaryConditions;
+ m_massSpring->init1D(
+ m_extremities,
+ boundaryConditions,
+ numNodesPerDim[0] * m_massPerNode, // total mass (in Kg)
+ 100.0, // Stiffness stretching
+ 0.0, // Damping stretching
+ 10.0, // Stiffness bending
+ 0.0); // Damping bending
+
+ // Update position in only 1 timestep
+ // Forward euler for velocity, backward euler for position
+ m_massSpring->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER);
+
+ m_massSpring->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m_massSpring->wakeUp();
+
+ // Update model by one timestep
+ m_massSpring->beforeUpdate(dt);
+ m_massSpring->update(dt);
+
+ // Create localization helper class
+ m_localization = std::make_shared<MassSpringRepresentationLocalization>(m_massSpring);
+ }
+
+ void setContactAtNode(size_t nodeId)
+ {
+ m_nodeId = nodeId;
+ m_localization->setLocalNode(nodeId);
+
+ // Place plane at nodeId
+ double distance = -m_extremities[nodeId].dot(m_n);
+ m_constraintData.setPlaneEquation(m_n, distance);
+ }
+
+ Vector3d m_n;
+ ContactConstraintData m_constraintData;
+ size_t m_nodeId;
+
+ std::shared_ptr<SurgSim::Blocks::MassSpring1DRepresentation> m_massSpring;
+ double m_massPerNode;
+ std::vector<Vector3d> m_extremities;
+
+ std::shared_ptr<MassSpringRepresentationLocalization> m_localization;
+};
+
+TEST_F(MassSpringRepresentationContactTest, ConstructorTest)
+{
+ ASSERT_NO_THROW({ MassSpringRepresentationContact massSpring; });
+
+ ASSERT_NE(nullptr, std::make_shared<MassSpringRepresentationContact>());
+}
+
+TEST_F(MassSpringRepresentationContactTest, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<MassSpringRepresentationContact>();
+
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, implementation->getMlcpConstraintType());
+ EXPECT_EQ(SurgSim::Physics::REPRESENTATION_TYPE_MASSSPRING, implementation->getRepresentationType());
+ EXPECT_EQ(1u, implementation->getNumDof());
+}
+
+TEST_F(MassSpringRepresentationContactTest, BuildMlcpTest)
+{
+ // Define constraint (frictionless non-penetration)
+ auto implementation = std::make_shared<MassSpringRepresentationContact>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_massSpring->getNumDof(), 1, 1);
+
+ // Build MLCP for 0th node
+ setContactAtNode(0);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ // Expected results.
+ // At the initial time-step, the only force is gravity.
+ //
+ // F(0) = m * a(0)
+ // => a(0) = 1/m * F(0)
+ // = (0, -g, 0)
+ //
+ // Modified Explicit Euler:
+ // v(1) = v(0) + dv/dt|(t=0)*dt
+ // = v(0) + a(0)*dt
+ // = (0, -g*dt, 0)
+ //
+ // p(1) = p(0) + dp/dt|(t=1)*dt
+ // = p(0) + v(1)*dt
+ // = p(0) + (0, -g*dt^2, 0)
+ //
+ // The constraint violation is the position from the plane projected onto the constraint. Note, we have defined the
+ // plane to intersect with p(0). The constraint is
+ // U(t) = n^t.(p(t) - p(0)) >= 0
+ //
+ // U(1) = n^t.(p(1) - p(0))
+ // = (nx, ny, nz)^t.(0, -g*dt^2, 0)
+ // = -g*dt^2*ny
+ EXPECT_NEAR(-9.81 * dt * dt * m_n[1], mlcpPhysicsProblem.b[0], epsilon);
+
+ // By definition, H = dU/dp.
+ //
+ // dU/dt = (dU/dp).(dp/dt)
+ // = H.(dp/dt)
+ //
+ // dp = p(1) - p(0)
+ // => dp/dt = (p(1) - p(0)) / dt
+ //
+ // dU/dt = (U(1) - U(0)) / dt [Note U(0) = 0]
+ // = n^t.(p(1) - p(0)) / dt
+ // = n^t.(dp/dt)
+ // => H = n^t
+ EXPECT_NEAR(m_n[0], mlcpPhysicsProblem.H(0, 0), epsilon);
+ EXPECT_NEAR(m_n[1], mlcpPhysicsProblem.H(0, 1), epsilon);
+ EXPECT_NEAR(m_n[2], mlcpPhysicsProblem.H(0, 2), epsilon);
+
+ // We define C as the matrix which transforms F -> v (which differs from treatments which define it as F -> p)
+ // v(1) = v(0) + a(0)*dt
+ // = v(0) + 1/m*F(0)*dt
+ // => (v(1) - v(0)) = [dt/m] * F(0)
+ //
+ // Therefore C = dt/m
+ //
+ // We can directly calculate
+ // CHt = C * H^t
+ // = (dt/m) * (nx, ny, nz)
+ EXPECT_NEAR(dt / m_massPerNode * m_n[0], mlcpPhysicsProblem.CHt(0, 0), epsilon);
+ EXPECT_NEAR(dt / m_massPerNode * m_n[1], mlcpPhysicsProblem.CHt(1, 0), epsilon);
+ EXPECT_NEAR(dt / m_massPerNode * m_n[2], mlcpPhysicsProblem.CHt(2, 0), epsilon);
+
+ // And finally,
+ // HCHt = [nx ny nz] * (dt/m) * (nx, ny, nz)
+ // = (dt/m) * (nx*nx + ny*ny + nz*nz)
+ double calculatedA = mlcpPhysicsProblem.A.block<1, 1>(0, 0)[0]; // VS intellisense error workaround
+ EXPECT_NEAR(dt / m_massPerNode * (m_n[0] * m_n[0] + m_n[1] * m_n[1] + m_n[2] * m_n[2]), calculatedA, epsilon);
+
+ // ConstraintTypes should contain 0 entry as it is setup by the constraint and not the ConstraintImplementation
+ // This way, the constraint can verify that both ConstraintImplementation are the same type
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST_F(MassSpringRepresentationContactTest, BuildMlcpIndiciesTest)
+{
+ auto implementation = std::make_shared<MassSpringRepresentationContact>();
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(11, 2, 2);
+
+ // Suppose 5 dof and 1 constraint are defined elsewhere. Then H, CHt, HCHt, and b are prebuilt.
+ Eigen::Matrix<double, 1, 5> localH;
+ localH <<
+ 0.9478, -0.3807, 0.5536, -0.6944, 0.1815;
+ mlcpPhysicsProblem.H.block<1, 5>(0, 0) = localH;
+
+ Eigen::Matrix<double, 5, 5> localC;
+ localC <<
+ -0.2294, 0.5160, 0.2520, 0.5941, -0.4854,
+ 0.1233, -0.4433, 0.3679, 0.9307, 0.2600,
+ 0.1988, 0.6637, -0.7591, 0.1475, 0.8517,
+ -0.5495, -0.4305, 0.3162, -0.7862, 0.7627,
+ -0.5754, 0.4108, 0.8445, -0.5565, 0.7150;
+ localC = localC * localC.transpose(); // force to be symmetric
+
+ Eigen::Matrix<double, 5, 1> localCHt = localC * localH.transpose();
+ mlcpPhysicsProblem.CHt.block<5, 1>(0, 0) = localCHt;
+
+ mlcpPhysicsProblem.A.block<1, 1>(0, 0) = localH * localCHt;
+
+ mlcpPhysicsProblem.b.block<1, 1>(0, 0)[0] = 0.6991;
+
+ // Place mass-spring at 5th dof and 1th constraint.
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ setContactAtNode(1);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ // b -> E -> [#constraints, 1]
+ EXPECT_NEAR(-9.81 * dt * dt * m_n[1], mlcpPhysicsProblem.b[indexOfConstraint], epsilon);
+
+ // H -> [#constraints, #dof]
+ EXPECT_NEAR(m_n[0], mlcpPhysicsProblem.H(indexOfConstraint, indexOfRepresentation + 3 * m_nodeId + 0), epsilon);
+ EXPECT_NEAR(m_n[1], mlcpPhysicsProblem.H(indexOfConstraint, indexOfRepresentation + 3 * m_nodeId + 1), epsilon);
+ EXPECT_NEAR(m_n[2], mlcpPhysicsProblem.H(indexOfConstraint, indexOfRepresentation + 3 * m_nodeId + 2), epsilon);
+
+ // C -> [#dof, #dof]
+ // CHt -> [#dof, #constraints]
+ EXPECT_NEAR(dt / m_massPerNode * m_n[0],
+ mlcpPhysicsProblem.CHt(indexOfRepresentation + 3 * m_nodeId + 0, indexOfConstraint), epsilon);
+ EXPECT_NEAR(dt / m_massPerNode * m_n[1],
+ mlcpPhysicsProblem.CHt(indexOfRepresentation + 3 * m_nodeId + 1, indexOfConstraint), epsilon);
+ EXPECT_NEAR(dt / m_massPerNode * m_n[2],
+ mlcpPhysicsProblem.CHt(indexOfRepresentation + 3 * m_nodeId + 2, indexOfConstraint), epsilon);
+
+ // A -> HCHt -> [#constraints, #constraints]
+ double calculatedA = mlcpPhysicsProblem.A.block<1, 1>(indexOfConstraint, indexOfConstraint)[0];
+ EXPECT_NEAR(dt / m_massPerNode * (m_n[0] * m_n[0] + m_n[1] * m_n[1] + m_n[2] * m_n[2]), calculatedA, epsilon);
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/MassSpringRepresentationLocalizationTest.cpp b/SurgSim/Physics/UnitTests/MassSpringRepresentationLocalizationTest.cpp
new file mode 100644
index 0000000..c659032
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/MassSpringRepresentationLocalizationTest.cpp
@@ -0,0 +1,107 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Blocks/MassSpring1DRepresentation.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+#include "SurgSim/Physics/MassSpringRepresentationLocalization.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST (MassSpringRepresentationLocalizationTest, ConstructorTest)
+{
+ ASSERT_NO_THROW( {MassSpringRepresentationLocalization localization;});
+
+ ASSERT_NO_THROW(
+ {
+ auto massSpring = std::make_shared<MassSpringRepresentation>("MassSpringRepresentation");
+ MassSpringRepresentationLocalization localization(massSpring);
+ });
+}
+
+TEST (MassSpringRepresentationLocalizationTest, SetGetRepresentation)
+{
+ MassSpringRepresentationLocalization localization;
+ auto massSpring = std::make_shared<MassSpringRepresentation>("MassSpringRepresentation");
+
+ EXPECT_EQ(nullptr, localization.getRepresentation());
+
+ localization.setRepresentation(massSpring);
+ EXPECT_EQ(massSpring, localization.getRepresentation());
+
+ localization.setRepresentation(nullptr);
+ EXPECT_EQ(nullptr, localization.getRepresentation());
+}
+
+TEST (MassSpringRepresentationLocalizationTest, GetPositionTest)
+{
+ using SurgSim::Math::Vector3d;
+
+ // Create the mass spring
+ auto massSpring = std::make_shared<SurgSim::Blocks::MassSpring1DRepresentation>("MassSpring");
+ std::vector<Vector3d> extremities;
+ extremities.push_back(Vector3d(0,0,0));
+ extremities.push_back(Vector3d(1,0,0));
+ std::vector<size_t> boundaryConditions;
+ massSpring->init1D(
+ extremities,
+ boundaryConditions,
+ 0.1, // total mass (in Kg)
+ 100.0, // Stiffness stretching
+ 0.0, // Damping stretching
+ 10.0, // Stiffness bending
+ 0.0); // Damping bending
+
+ MassSpringRepresentationLocalization localization = MassSpringRepresentationLocalization(massSpring);
+
+ localization.setLocalNode(0);
+ ASSERT_EQ(0u, localization.getLocalNode());
+ ASSERT_TRUE(localization.calculatePosition().isZero(epsilon));
+
+ localization.setLocalNode(1);
+ ASSERT_EQ(1u, localization.getLocalNode());
+ ASSERT_TRUE(localization.calculatePosition().isApprox(Vector3d(1.0, 0.0, 0.0), epsilon));
+}
+
+TEST (MassSpringRepresentationLocalizationTest, IsValidRepresentationTest)
+{
+ MassSpringRepresentationLocalization localization;
+
+ EXPECT_TRUE(localization.isValidRepresentation(nullptr));
+ EXPECT_TRUE(localization.isValidRepresentation(std::make_shared<MassSpringRepresentation>("massSpring")));
+ EXPECT_TRUE(localization.isValidRepresentation(
+ std::make_shared<MockDescendent<MassSpringRepresentation>>("descendent")));
+
+ EXPECT_FALSE(localization.isValidRepresentation(std::make_shared<MockRepresentation>()));
+ EXPECT_FALSE(localization.isValidRepresentation(std::make_shared<RigidRepresentation>("rigidRepresentation")));
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/MassSpringRepresentationTests.cpp b/SurgSim/Physics/UnitTests/MassSpringRepresentationTests.cpp
new file mode 100644
index 0000000..71bf62c
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/MassSpringRepresentationTests.cpp
@@ -0,0 +1,595 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+#include "SurgSim/Physics/MassSpringRepresentationLocalization.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::Math::Vector;
+using SurgSim::Math::Matrix;
+using SurgSim::Physics::MassSpringRepresentation;
+using SurgSim::Physics::MassSpringRepresentationLocalization;
+using SurgSim::Physics::MockLocalization;
+using SurgSim::Physics::MockMassSpring;
+using SurgSim::Physics::MockSpring;
+
+
+namespace
+{
+const double epsilon = 1e-10;
+};
+
+class MassSpringRepresentationTests : public ::testing::Test
+{
+protected:
+ double m_dt;
+ double m_rayleighDampingMassParameter;
+ double m_rayleighDampingStiffnessParameter;
+
+ Vector m_expectedSpringsForce;
+ Vector m_expectedRayleighDampingForce;
+ Vector m_expectedGravityForce;
+ Matrix m_expectedMass;
+ Matrix m_expectedDamping;
+ Matrix m_expectedRayleighDamping;
+ Matrix m_expectedStiffness;
+
+ std::shared_ptr<MockMassSpring> m_massSpring;
+ std::shared_ptr<SurgSim::Math::OdeState> m_initialState;
+ std::shared_ptr<SurgSim::Physics::MassSpringRepresentationLocalization> m_localization;
+
+ virtual void SetUp() override
+ {
+ m_massSpring = std::make_shared<MockMassSpring>();
+
+ m_dt = 1e-3;
+ m_rayleighDampingMassParameter = 0.452;
+ m_rayleighDampingStiffnessParameter = 0.242;
+
+ m_initialState = std::make_shared<SurgSim::Math::OdeState>();
+ m_initialState->setNumDof(m_massSpring->getNumDofPerNode(), 3);
+ m_initialState->getVelocities().setOnes(); // v = (1...1) to test damping
+ m_massSpring->setInitialState(m_initialState);
+
+ // Mass should be a identity matrix (all masses are 1)
+ m_expectedMass = Matrix::Identity(9, 9);
+
+ // Damping should be a diagonal matrix with diagonal (2 2 2 4 4 4 2 2 2)
+ m_expectedDamping = Matrix::Zero(9, 9);
+ m_expectedDamping.diagonal() << 2.0, 2.0, 2.0, 4.0, 4.0, 4.0, 2.0, 2.0, 2.0;
+
+ // Stiffness should be a diagonal matrix with diagonal (3 3 3 6 6 6 3 3 3)
+ m_expectedStiffness = Matrix::Zero(9, 9);
+ m_expectedStiffness.diagonal() << 3.0, 3.0, 3.0, 6.0, 6.0, 6.0, 3.0, 3.0, 3.0;
+
+ // Rayleigh damping = alpha.M + beta.K with M and K are diagonals, so the resulting matrix is too
+ m_expectedRayleighDamping = m_expectedMass * m_rayleighDampingMassParameter;
+ m_expectedRayleighDamping += m_expectedStiffness * m_rayleighDampingStiffnessParameter;
+
+ // FemElements force should be (1 2 3 4 5 6 0 0 0) + (0 0 0 1 2 3 4 5 6) = (1 2 3 5 7 9 4 5 6)
+ m_expectedSpringsForce.resize(9);
+ m_expectedSpringsForce << 1.0, 2.0, 3.0, 5.0, 7.0, 9.0, 4.0, 5.0, 6.0;
+
+ // Gravity force should be m.gravity for each node of each element
+ m_expectedGravityForce = Vector::Zero(9);
+ SurgSim::Math::Vector3d g(0.0, -9.81, 0.0);
+ m_expectedGravityForce .segment<3>(0) += g; // Mass = 1 for each node
+ m_expectedGravityForce .segment<3>(3) += g; // Mass = 1 for each node
+ m_expectedGravityForce .segment<3>(6) += g; // Mass = 1 for each node
+
+ // Rayleigh damping force should be -(alpha.M + beta.K).(1...1)^t
+ // with (alpha.M + beta.K) a diagonal matrix
+ m_expectedRayleighDampingForce = -m_expectedRayleighDamping.diagonal();
+ }
+
+ void addSprings()
+ {
+ std::shared_ptr<MockSpring> element01 = std::make_shared<MockSpring>();
+ element01->addNode(0);
+ element01->addNode(1);
+ m_massSpring->addSpring(element01);
+
+ std::shared_ptr<MockSpring> element12 = std::make_shared<MockSpring>();
+ element12->addNode(1);
+ element12->addNode(2);
+ m_massSpring->addSpring(element12);
+ }
+
+ void addMasses()
+ {
+ m_massSpring->addMass(std::make_shared<SurgSim::Physics::Mass>(1.0));
+ m_massSpring->addMass(std::make_shared<SurgSim::Physics::Mass>(1.0));
+ m_massSpring->addMass(std::make_shared<SurgSim::Physics::Mass>(1.0));
+ }
+
+ void createLocalization()
+ {
+ m_localization = std::make_shared<SurgSim::Physics::MassSpringRepresentationLocalization>();
+ m_localization->setRepresentation(m_massSpring);
+ m_localization->setLocalNode(0);
+ }
+};
+
+TEST_F(MassSpringRepresentationTests, Constructor)
+{
+ ASSERT_NO_THROW({MassSpringRepresentation m("MassSpring");});
+
+ ASSERT_NO_THROW({MassSpringRepresentation* m = new MassSpringRepresentation("MassSpring"); delete m;});
+
+ ASSERT_NO_THROW({std::shared_ptr<MassSpringRepresentation> m =
+ std::make_shared<MassSpringRepresentation>("MassSpring");});
+}
+
+TEST_F(MassSpringRepresentationTests, SetGetMethods)
+{
+ using SurgSim::Math::setSubVector;
+ using SurgSim::Physics::Mass;
+ using SurgSim::Physics::LinearSpring;
+
+ std::shared_ptr<MassSpringRepresentation> massSpring = std::make_shared<MassSpringRepresentation>("MassSpring");
+
+ // set/get InitialPose
+ SurgSim::Math::RigidTransform3d poseRandom;
+ SurgSim::Math::Quaterniond q(1.0, 2.0, 3.0, 4.0);
+ SurgSim::Math::Vector3d t(1.0, 2.0, 3.0);
+ q.normalize();
+ poseRandom = SurgSim::Math::makeRigidTransform(q, t);
+ massSpring->setLocalPose(poseRandom);
+ EXPECT_TRUE(massSpring->getLocalPose().isApprox(poseRandom));
+
+ EXPECT_TRUE(massSpring->getPose().isApprox(poseRandom));
+
+ // get{NumDof | NumMasses | NumSprings} initial value is 0
+ EXPECT_EQ(0u, massSpring->getNumDof());
+ EXPECT_EQ(0u, massSpring->getNumMasses());
+ EXPECT_EQ(0u, massSpring->getNumSprings());
+
+ // setInitialState is part of DeformableRepresentation...already tested !
+ std::shared_ptr<SurgSim::Math::OdeState> state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(3, 2);
+ state->getPositions().setRandom();
+ massSpring->setInitialState(state);
+
+ // addMass/getNumMasses/getMass
+ std::shared_ptr<Mass> mass0 = std::make_shared<Mass>(1.0);
+ massSpring->addMass(mass0);
+ std::shared_ptr<Mass> mass1 = std::make_shared<Mass>(1.1);
+ massSpring->addMass(mass1);
+ EXPECT_EQ(2u, massSpring->getNumMasses());
+ EXPECT_EQ(mass0, massSpring->getMass(0));
+ EXPECT_EQ(*mass0, *massSpring->getMass(0));
+ EXPECT_EQ(mass1, massSpring->getMass(1));
+ EXPECT_EQ(*mass1, *massSpring->getMass(1));
+
+ // addSpring/getNumSprings/getSpring
+ std::shared_ptr<LinearSpring> spring0 = std::make_shared<LinearSpring>(0, 1);
+ spring0->setStiffness(1.0);
+ spring0->setDamping(1.0);
+ spring0->setRestLength(1.0);
+ massSpring->addSpring(spring0);
+ EXPECT_EQ(1u, massSpring->getNumSprings());
+ EXPECT_EQ(spring0, massSpring->getSpring(0));
+ EXPECT_EQ(*spring0, *massSpring->getSpring(0));
+
+ // getTotalMass
+ EXPECT_DOUBLE_EQ(1.0 + 1.1, massSpring->getTotalMass());
+
+ // set/get RayleighDamping{Mass|Stiffness}
+ massSpring->setRayleighDampingMass(5.5);
+ EXPECT_DOUBLE_EQ(5.5, massSpring->getRayleighDampingMass());
+ massSpring->setRayleighDampingStiffness(5.4);
+ EXPECT_DOUBLE_EQ(5.4, massSpring->getRayleighDampingStiffness());
+
+ // set/get Type
+ EXPECT_EQ(SurgSim::Physics::REPRESENTATION_TYPE_MASSSPRING, massSpring->getType());
+}
+
+TEST_F(MassSpringRepresentationTests, BeforeUpdateTest)
+{
+ const double dt = 1e-3;
+
+ std::shared_ptr<MassSpringRepresentation> massSpring = std::make_shared<MassSpringRepresentation>("MassSpring");
+
+ // Missing initial state, masses and springs
+ EXPECT_THROW(massSpring->beforeUpdate(dt), SurgSim::Framework::AssertionFailure);
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(massSpring->getNumDofPerNode(), 3);
+ massSpring->setInitialState(initialState);
+
+ // Missing springs and 2 masses
+ EXPECT_THROW(massSpring->beforeUpdate(dt), SurgSim::Framework::AssertionFailure);
+ massSpring->addSpring(std::make_shared<SurgSim::Physics::LinearSpring>(0, 1));
+
+ // Missing 3 masses
+ EXPECT_THROW(massSpring->beforeUpdate(dt), SurgSim::Framework::AssertionFailure);
+ massSpring->addMass(std::make_shared<SurgSim::Physics::Mass>(1.2));
+
+ // Missing 2 masses
+ EXPECT_THROW(massSpring->beforeUpdate(dt), SurgSim::Framework::AssertionFailure);
+ massSpring->addMass(std::make_shared<SurgSim::Physics::Mass>(1.2));
+
+ // Missing 1 mass
+ EXPECT_THROW(massSpring->beforeUpdate(dt), SurgSim::Framework::AssertionFailure);
+ massSpring->addMass(std::make_shared<SurgSim::Physics::Mass>(1.2));
+
+ EXPECT_NO_THROW(massSpring->beforeUpdate(dt));
+}
+
+TEST_F(MassSpringRepresentationTests, TransformInitialStateTest)
+{
+ using SurgSim::Math::Vector;
+
+ const size_t numNodes = 2;
+ const size_t numDofPerNode = m_massSpring->getNumDofPerNode();
+ const size_t numDof = numDofPerNode * numNodes;
+
+ SurgSim::Math::RigidTransform3d initialPose;
+ SurgSim::Math::Quaterniond q(1.0, 2.0, 3.0, 4.0);
+ SurgSim::Math::Vector3d t(1.0, 2.0, 3.0);
+ q.normalize();
+ initialPose = SurgSim::Math::makeRigidTransform(q, t);
+ m_massSpring->setLocalPose(initialPose);
+
+ std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(numDofPerNode, numNodes);
+ Vector x = Vector::LinSpaced(numDof, 1.0, static_cast<double>(numDof));
+ Vector v = Vector::Ones(numDof);
+ initialState->getPositions() = x;
+ initialState->getVelocities() = v;
+ m_massSpring->setInitialState(initialState);
+
+ Vector expectedX = x, expectedV = v;
+ for (size_t nodeId = 0; nodeId < numNodes; nodeId++)
+ {
+ expectedX.segment<3>(numDofPerNode * nodeId) = initialPose * x.segment<3>(numDofPerNode * nodeId);
+ expectedV.segment<3>(numDofPerNode * nodeId) = initialPose.linear() * v.segment<3>(numDofPerNode * nodeId);
+ }
+
+ // Initialize the component
+ ASSERT_TRUE(m_massSpring->initialize(std::make_shared<SurgSim::Framework::Runtime>()));
+ // Wake-up the component => apply the pose to the initial state
+ ASSERT_TRUE(m_massSpring->wakeUp());
+
+ EXPECT_TRUE(m_massSpring->getInitialState()->getPositions().isApprox(expectedX));
+ EXPECT_TRUE(m_massSpring->getInitialState()->getVelocities().isApprox(expectedV));
+}
+
+TEST_F(MassSpringRepresentationTests, ExternalForceAPITest)
+{
+ using SurgSim::Math::Vector;
+ using SurgSim::Math::Matrix;
+
+ std::shared_ptr<MockMassSpring> massSpring = std::make_shared<MockMassSpring>();
+ std::shared_ptr<MassSpringRepresentationLocalization> localization =
+ std::make_shared<MassSpringRepresentationLocalization>();
+ std::shared_ptr<MockLocalization> wrongLocalizationType =
+ std::make_shared<MockLocalization>();
+
+ // External force vector not initialized until the initial state has been set (it contains the #dof...)
+ EXPECT_EQ(0, massSpring->getExternalForce().size());
+ EXPECT_EQ(0, massSpring->getExternalStiffness().size());
+ EXPECT_EQ(0, massSpring->getExternalDamping().size());
+
+ massSpring->setInitialState(m_initialState);
+
+ massSpring->addMass(std::make_shared<SurgSim::Physics::Mass>(1.0));
+ massSpring->addMass(std::make_shared<SurgSim::Physics::Mass>(1.0));
+ massSpring->addSpring(std::make_shared<SurgSim::Physics::LinearSpring>(0, 1));
+
+ // Vector initialized (properly sized and zeroed)
+ EXPECT_NE(0, massSpring->getExternalForce().size());
+ EXPECT_NE(0, massSpring->getExternalStiffness().size());
+ EXPECT_NE(0, massSpring->getExternalDamping().size());
+ EXPECT_EQ(massSpring->getNumDof(), massSpring->getExternalForce().size());
+ EXPECT_EQ(massSpring->getNumDof(), massSpring->getExternalStiffness().cols());
+ EXPECT_EQ(massSpring->getNumDof(), massSpring->getExternalStiffness().rows());
+ EXPECT_EQ(massSpring->getNumDof(), massSpring->getExternalDamping().cols());
+ EXPECT_EQ(massSpring->getNumDof(), massSpring->getExternalDamping().rows());
+ EXPECT_TRUE(massSpring->getExternalForce().isZero());
+ EXPECT_TRUE(massSpring->getExternalStiffness().isZero());
+ EXPECT_TRUE(massSpring->getExternalDamping().isZero());
+
+ localization->setRepresentation(massSpring);
+ localization->setLocalNode(0);
+ wrongLocalizationType->setRepresentation(massSpring);
+
+ Vector F = Vector::LinSpaced(massSpring->getNumDofPerNode(), -3.12, 4.09);
+ Matrix K = Matrix::Ones(massSpring->getNumDofPerNode(), massSpring->getNumDofPerNode()) * 0.34;
+ Matrix D = K + Matrix::Identity(massSpring->getNumDofPerNode(), massSpring->getNumDofPerNode());
+ Vector expectedF = Vector::Zero(massSpring->getNumDof());
+ expectedF.segment(0, massSpring->getNumDofPerNode()) = F;
+ Matrix expectedK = Matrix::Zero(massSpring->getNumDof(), massSpring->getNumDof());
+ expectedK.block(0, 0, massSpring->getNumDofPerNode(), massSpring->getNumDofPerNode()) = K;
+ Matrix expectedD = Matrix::Zero(massSpring->getNumDof(), massSpring->getNumDof());
+ expectedD.block(0, 0, massSpring->getNumDofPerNode(), massSpring->getNumDofPerNode()) = D;
+
+ ASSERT_THROW(massSpring->addExternalGeneralizedForce(nullptr, F, K, D),
+ SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(massSpring->addExternalGeneralizedForce(wrongLocalizationType, F, K, D),
+ SurgSim::Framework::AssertionFailure);
+
+ massSpring->addExternalGeneralizedForce(localization, F, K, D);
+ EXPECT_FALSE(massSpring->getExternalForce().isZero());
+ EXPECT_FALSE(massSpring->getExternalStiffness().isZero());
+ EXPECT_FALSE(massSpring->getExternalDamping().isZero());
+ EXPECT_TRUE(massSpring->getExternalForce().isApprox(expectedF));
+ EXPECT_TRUE(massSpring->getExternalStiffness().isApprox(expectedK));
+ EXPECT_TRUE(massSpring->getExternalDamping().isApprox(expectedD));
+
+ massSpring->addExternalGeneralizedForce(localization, F, K, D);
+ EXPECT_TRUE(massSpring->getExternalForce().isApprox(2.0 * expectedF));
+ EXPECT_TRUE(massSpring->getExternalStiffness().isApprox(2.0 * expectedK));
+ EXPECT_TRUE(massSpring->getExternalDamping().isApprox(2.0 * expectedD));
+}
+
+TEST_F(MassSpringRepresentationTests, ComputesWithNoGravityAndNoDampingTest)
+{
+ using SurgSim::Math::Vector3d;
+
+ Vector *F;
+ Matrix *M, *D, *K;
+
+ addMasses();
+ addSprings();
+ createLocalization();
+
+ // No gravity, no Rayleigh damping
+ // computeF tests addFemElementsForce
+ m_massSpring->setIsGravityEnabled(false);
+ m_massSpring->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m_massSpring->wakeUp();
+
+ {
+ SCOPED_TRACE("Without external force");
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeF(*m_initialState).isApprox(m_expectedSpringsForce)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeD(*m_initialState).isApprox(m_expectedDamping)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeK(*m_initialState).isApprox(m_expectedStiffness)));
+ // Test combo method computeFMDK
+ EXPECT_NO_THROW(m_massSpring->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(m_expectedSpringsForce));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness));
+ }
+
+ size_t numDofPerNode = m_massSpring->getNumDofPerNode();
+ size_t numDof = m_massSpring->getNumDof();
+
+ {
+ SCOPED_TRACE("With external force");
+
+ Vector externalLocalForce = Vector::LinSpaced(numDofPerNode, 1.1, 4.56);
+ Matrix externalLocalK = Matrix::Ones(numDofPerNode, numDofPerNode);
+ Matrix externalLocalD = 3.1 * externalLocalK + Matrix::Identity(numDofPerNode, numDofPerNode);
+ Vector externalForce = Vector::Zero(numDof);
+ Matrix externalK = Matrix::Zero(numDof, numDof);
+ Matrix externalD = Matrix::Zero(numDof, numDof);
+ externalForce.segment(0, numDofPerNode) = externalLocalForce;
+ externalK.block(0, 0, numDofPerNode, numDofPerNode) = externalLocalK;
+ externalD.block(0, 0, numDofPerNode, numDofPerNode) = externalLocalD;
+ m_massSpring->addExternalGeneralizedForce(m_localization, externalLocalForce, externalLocalK, externalLocalD);
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeF(*m_initialState).isApprox(
+ m_expectedSpringsForce + externalForce)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeK(*m_initialState).isApprox(
+ m_expectedStiffness + externalK)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeD(*m_initialState).isApprox(m_expectedDamping + externalD)));
+ EXPECT_NO_THROW(m_massSpring->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(m_expectedSpringsForce + externalForce));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness + externalK));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + externalD));
+ }
+}
+
+TEST_F(MassSpringRepresentationTests, ComputesWithNoGravityAndDampingTest)
+{
+ using SurgSim::Math::Vector3d;
+
+ Vector *F;
+ Matrix *M, *D, *K;
+
+ addMasses();
+ addSprings();
+ createLocalization();
+
+ // No gravity, Rayleigh damping
+ // computeF tests addFemElementsForce and addRayleighDampingForce
+ m_massSpring->setIsGravityEnabled(false);
+ m_massSpring->setRayleighDampingMass(m_rayleighDampingMassParameter);
+ m_massSpring->setRayleighDampingStiffness(m_rayleighDampingStiffnessParameter);
+ m_massSpring->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m_massSpring->wakeUp();
+
+ SurgSim::Math::Vector expectedForce = m_expectedSpringsForce + m_expectedRayleighDampingForce;
+
+ {
+ SCOPED_TRACE("Without external force");
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeF(*m_initialState).isApprox(expectedForce)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeD(*m_initialState).isApprox(
+ m_expectedDamping + m_expectedRayleighDamping)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeK(*m_initialState).isApprox(m_expectedStiffness)));
+ // Test combo method computeFMDK
+ EXPECT_NO_THROW(m_massSpring->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedForce));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + m_expectedRayleighDamping));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness));
+ }
+
+ size_t numDofPerNode = m_massSpring->getNumDofPerNode();
+ size_t numDof = m_massSpring->getNumDof();
+
+ {
+ SCOPED_TRACE("With external force");
+
+ Vector externalLocalForce = Vector::LinSpaced(numDofPerNode, 1.1, 4.56);
+ Matrix externalLocalK = Matrix::Ones(numDofPerNode, numDofPerNode);
+ Matrix externalLocalD = 3.1 * externalLocalK + Matrix::Identity(numDofPerNode, numDofPerNode);
+ Vector externalForce = Vector::Zero(numDof);
+ Matrix externalK = Matrix::Zero(numDof, numDof);
+ Matrix externalD = Matrix::Zero(numDof, numDof);
+ externalForce.segment(0, numDofPerNode) = externalLocalForce;
+ externalK.block(0, 0, numDofPerNode, numDofPerNode) = externalLocalK;
+ externalD.block(0, 0, numDofPerNode, numDofPerNode) = externalLocalD;
+ m_massSpring->addExternalGeneralizedForce(m_localization, externalLocalForce, externalLocalK, externalLocalD);
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeF(*m_initialState).isApprox(expectedForce + externalForce)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeK(*m_initialState).isApprox(
+ m_expectedStiffness + externalK)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeD(*m_initialState).isApprox(
+ m_expectedDamping + m_expectedRayleighDamping + externalD)));
+ EXPECT_NO_THROW(m_massSpring->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedForce + externalForce));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness + externalK));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + m_expectedRayleighDamping + externalD));
+ }
+}
+
+TEST_F(MassSpringRepresentationTests, ComputesWithGravityAndNoDampingTest)
+{
+ using SurgSim::Math::Vector3d;
+
+ Vector *F;
+ Matrix *M, *D, *K;
+
+ addMasses();
+ addSprings();
+ createLocalization();
+
+ // Gravity, no Rayleigh damping
+ // computeF tests addFemElementsForce and addGravityForce
+ m_massSpring->setIsGravityEnabled(true);
+ m_massSpring->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m_massSpring->wakeUp();
+
+ SurgSim::Math::Vector expectedForce = m_expectedSpringsForce + m_expectedGravityForce;
+
+ {
+ SCOPED_TRACE("Without external force");
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeF(*m_initialState).isApprox(expectedForce)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeD(*m_initialState).isApprox(m_expectedDamping)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeK(*m_initialState).isApprox(m_expectedStiffness)));
+ // Test combo method computeFMDK
+ EXPECT_NO_THROW(m_massSpring->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedForce));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness));
+ }
+
+ size_t numDofPerNode = m_massSpring->getNumDofPerNode();
+ size_t numDof = m_massSpring->getNumDof();
+
+ {
+ SCOPED_TRACE("With external force");
+
+ Vector externalLocalForce = Vector::LinSpaced(numDofPerNode, 1.1, 4.56);
+ Matrix externalLocalK = Matrix::Ones(numDofPerNode, numDofPerNode);
+ Matrix externalLocalD = 3.1 * externalLocalK + Matrix::Identity(numDofPerNode, numDofPerNode);
+ Vector externalForce = Vector::Zero(numDof);
+ Matrix externalK = Matrix::Zero(numDof, numDof);
+ Matrix externalD = Matrix::Zero(numDof, numDof);
+ externalForce.segment(0, numDofPerNode) = externalLocalForce;
+ externalK.block(0, 0, numDofPerNode, numDofPerNode) = externalLocalK;
+ externalD.block(0, 0, numDofPerNode, numDofPerNode) = externalLocalD;
+ m_massSpring->addExternalGeneralizedForce(m_localization, externalLocalForce, externalLocalK, externalLocalD);
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeF(*m_initialState).isApprox(expectedForce + externalForce)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeK(*m_initialState).isApprox(
+ m_expectedStiffness + externalK)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeD(*m_initialState).isApprox(m_expectedDamping + externalD)));
+ EXPECT_NO_THROW(m_massSpring->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedForce + externalForce));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness + externalK));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + externalD));
+ }
+}
+
+TEST_F(MassSpringRepresentationTests, ComputesWithGravityAndDampingTest)
+{
+ using SurgSim::Math::Vector3d;
+
+ Vector *F;
+ Matrix *M, *D, *K;
+
+ addMasses();
+ addSprings();
+ createLocalization();
+
+ // Gravity, Rayleigh damping
+ // computeF tests addFemElementsForce, addRayleighDampingForce and addGravityForce
+ m_massSpring->setIsGravityEnabled(true);
+ m_massSpring->setRayleighDampingMass(m_rayleighDampingMassParameter);
+ m_massSpring->setRayleighDampingStiffness(m_rayleighDampingStiffnessParameter);
+ m_massSpring->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ m_massSpring->wakeUp();
+
+ SurgSim::Math::Vector expectedForce = m_expectedSpringsForce + m_expectedRayleighDampingForce +
+ m_expectedGravityForce;
+
+ {
+ SCOPED_TRACE("Without external force");
+
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeF(*m_initialState).isApprox(expectedForce)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeM(*m_initialState).isApprox(m_expectedMass)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeD(*m_initialState).isApprox(
+ m_expectedDamping + m_expectedRayleighDamping)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeK(*m_initialState).isApprox(m_expectedStiffness)));
+ // Test combo method computeFMDK
+ EXPECT_NO_THROW(m_massSpring->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedForce));
+ EXPECT_TRUE((*M).isApprox(m_expectedMass));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + m_expectedRayleighDamping));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness));
+ }
+
+ size_t numDofPerNode = m_massSpring->getNumDofPerNode();
+ size_t numDof = m_massSpring->getNumDof();
+
+ {
+ SCOPED_TRACE("With external force");
+
+ Vector externalLocalForce = Vector::LinSpaced(numDofPerNode, 1.1, 4.56);
+ Matrix externalLocalK = Matrix::Ones(numDofPerNode, numDofPerNode);
+ Matrix externalLocalD = 3.1 * externalLocalK + Matrix::Identity(numDofPerNode, numDofPerNode);
+ Vector externalForce = Vector::Zero(numDof);
+ Matrix externalK = Matrix::Zero(numDof, numDof);
+ Matrix externalD = Matrix::Zero(numDof, numDof);
+ externalForce.segment(0, numDofPerNode) = externalLocalForce;
+ externalK.block(0, 0, numDofPerNode, numDofPerNode) = externalLocalK;
+ externalD.block(0, 0, numDofPerNode, numDofPerNode) = externalLocalD;
+ m_massSpring->addExternalGeneralizedForce(m_localization, externalLocalForce, externalLocalK, externalLocalD);
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeF(*m_initialState).isApprox(expectedForce + externalForce)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeK(*m_initialState).isApprox(
+ m_expectedStiffness + externalK)));
+ EXPECT_NO_THROW(EXPECT_TRUE(m_massSpring->computeD(*m_initialState).isApprox(
+ m_expectedDamping + m_expectedRayleighDamping + externalD)));
+ EXPECT_NO_THROW(m_massSpring->computeFMDK(*m_initialState, &F, &M, &D, &K));
+ EXPECT_TRUE((*F).isApprox(expectedForce + externalForce));
+ EXPECT_TRUE((*K).isApprox(m_expectedStiffness + externalK));
+ EXPECT_TRUE((*D).isApprox(m_expectedDamping + m_expectedRayleighDamping + externalD));
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/MassTest.cpp b/SurgSim/Physics/UnitTests/MassTest.cpp
new file mode 100644
index 0000000..d7adfca
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/MassTest.cpp
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "SurgSim/Physics/Mass.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Physics::Mass;
+using SurgSim::Math::Vector3d;
+
+TEST(MassTests, Constructor)
+{
+ ASSERT_NO_THROW({Mass m(1.3);});
+
+ ASSERT_NO_THROW({Mass* m = new Mass(1.3); delete m;});
+
+ ASSERT_NO_THROW({std::shared_ptr<Mass> m = std::make_shared<Mass>(0.3);});
+}
+
+TEST(MassTests, SetGetMethodsTest)
+{
+ Mass m(1.3);
+
+ EXPECT_DOUBLE_EQ(1.3, m.getMass());
+ m.setMass(0.0);
+ EXPECT_DOUBLE_EQ(0.0, m.getMass());
+ m.setMass(-1.1);
+ EXPECT_DOUBLE_EQ(-1.1, m.getMass());
+ m.setMass(13.45);
+ EXPECT_DOUBLE_EQ(13.45, m.getMass());
+}
+
+
+TEST(MassTests, ComparisonTests)
+{
+ Mass m1(1.3), m2, m3(1.3);
+
+ EXPECT_FALSE(m1 == m2);
+ EXPECT_TRUE(m1 == m3);
+ EXPECT_FALSE(m2 == m3);
+ m2 = m1;
+ EXPECT_TRUE(m1 == m2);
+ EXPECT_TRUE(m1 == m3);
+ EXPECT_TRUE(m2 == m3);
+ m3.setMass(m3.getMass() + 3.4);
+ EXPECT_TRUE(m1 == m2);
+ EXPECT_FALSE(m1 == m3);
+ EXPECT_FALSE(m2 == m3);
+}
diff --git a/SurgSim/Physics/UnitTests/MockObjects.cpp b/SurgSim/Physics/UnitTests/MockObjects.cpp
new file mode 100644
index 0000000..7d7d683
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/MockObjects.cpp
@@ -0,0 +1,585 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+#include "SurgSim/Physics/FemPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::MockRepresentation, MockRepresentation);
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::MockDeformableRepresentation,
+ MockDeformableRepresentation);
+
+MockRepresentation::MockRepresentation(const std::string& name) :
+ Representation(name),
+ m_preUpdateCount(0),
+ m_updateCount(0),
+ m_postUpdateCount(0)
+{
+}
+
+MockRepresentation::~MockRepresentation()
+{
+}
+
+RepresentationType MockRepresentation::getType() const
+{
+ return REPRESENTATION_TYPE_FIXED;
+}
+
+void MockRepresentation::beforeUpdate(double dt)
+{
+ m_preUpdateCount++;
+}
+
+void MockRepresentation::update(double dt)
+{
+ m_updateCount++;
+}
+
+void MockRepresentation::afterUpdate(double dt)
+{
+ m_postUpdateCount++;
+}
+
+int MockRepresentation::getPreUpdateCount() const
+{
+ return m_preUpdateCount;
+}
+
+int MockRepresentation::getUpdateCount() const
+{
+ return m_updateCount;
+}
+
+int MockRepresentation::getPostUpdateCount() const
+{
+ return m_postUpdateCount;
+}
+
+
+MockRigidRepresentation::MockRigidRepresentation() : RigidRepresentation("MockRigidRepresentation")
+{
+}
+
+RigidRepresentationState& MockRigidRepresentation::getInitialState()
+{
+ return m_initialState;
+}
+
+RigidRepresentationState& MockRigidRepresentation::getCurrentState()
+{
+ return m_currentState;
+}
+
+RigidRepresentationState& MockRigidRepresentation::getPreviousState()
+{
+ return m_previousState;
+}
+
+
+MockDeformableRepresentation::MockDeformableRepresentation(const std::string& name) :
+ SurgSim::Physics::DeformableRepresentation(name)
+{
+ this->m_numDofPerNode = 3;
+ m_F = Vector::LinSpaced(3, 1.0, 3.0);
+ m_M = Matrix::Identity(3, 3);
+ m_D = Matrix::Identity(3, 3);
+ m_K = Matrix::Identity(3, 3);
+}
+
+RepresentationType MockDeformableRepresentation::getType() const
+{
+ return SurgSim::Physics::REPRESENTATION_TYPE_INVALID;
+}
+
+void MockDeformableRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K,
+ const SurgSim::Math::Matrix& D)
+{
+ std::shared_ptr<MockDeformableRepresentationLocalization> loc =
+ std::dynamic_pointer_cast<MockDeformableRepresentationLocalization>(localization);
+
+ m_externalGeneralizedForce.segment<3>(3 * loc->getLocalNode()) += generalizedForce;
+ m_externalGeneralizedStiffness.block<3, 3>(3 * loc->getLocalNode(), 3 * loc->getLocalNode()) += K;
+ m_externalGeneralizedDamping.block<3, 3>(3 * loc->getLocalNode(), 3 * loc->getLocalNode()) += D;
+}
+
+Vector& MockDeformableRepresentation::computeF(const OdeState& state)
+{
+ return m_F;
+}
+
+const Matrix& MockDeformableRepresentation::computeM(const OdeState& state)
+{
+ return m_M;
+}
+
+const Matrix& MockDeformableRepresentation::computeD(const OdeState& state)
+{
+ return m_D;
+}
+
+const Matrix& MockDeformableRepresentation::computeK(const OdeState& state)
+{
+ return m_K;
+}
+
+void MockDeformableRepresentation::computeFMDK(const OdeState& state,
+ Vector** f,
+ Matrix** M,
+ Matrix** D,
+ Matrix** K)
+{
+ *f = &m_F;
+ *M = &m_M;
+ *D = &m_D;
+ *K = &m_K;
+}
+
+void MockDeformableRepresentation::transformState(std::shared_ptr<OdeState> state, const RigidTransform3d& transform)
+{
+ using SurgSim::Math::setSubVector;
+ using SurgSim::Math::getSubVector;
+
+ Vector& x = state->getPositions();
+ Vector& v = state->getVelocities();
+ for (size_t nodeId = 0; nodeId < state->getNumNodes(); nodeId++)
+ {
+ Vector3d xi = getSubVector(x, nodeId, 3);
+ Vector3d xiTransformed = transform * xi;
+ setSubVector(xiTransformed, nodeId, 3, &x);
+
+ Vector3d vi = getSubVector(v, nodeId, 3);
+ Vector3d viTransformed = transform.linear() * vi;
+ setSubVector(viTransformed, nodeId, 3, &v);
+ }
+}
+
+
+MockSpring::MockSpring() : SurgSim::Physics::Spring()
+{
+ m_F = Vector::LinSpaced(6, 1.0, 6.0);
+ m_D = Matrix::Identity(6, 6) * 2.0;
+ m_K = Matrix::Identity(6, 6) * 3.0;
+}
+
+void MockSpring::addNode(size_t nodeId)
+{
+ this->m_nodeIds.push_back(nodeId);
+}
+
+void MockSpring::addForce(const OdeState& state, Vector* F, double scale)
+{
+ SurgSim::Math::addSubVector(scale * m_F, m_nodeIds, 3, F);
+}
+
+void MockSpring::addDamping(const OdeState& state, Matrix* D, double scale)
+{
+ SurgSim::Math::addSubMatrix(scale * m_D, m_nodeIds, 3, D);
+}
+
+void MockSpring::addStiffness(const OdeState& state, Matrix* K, double scale)
+{
+ SurgSim::Math::addSubMatrix(scale * m_K, m_nodeIds, 3, K);
+}
+
+void MockSpring::addFDK(const OdeState& state, Vector* f, Matrix* D, Matrix* K)
+{
+ addForce(state, f);
+ addDamping(state, D);
+ addStiffness(state, K);
+}
+
+void MockSpring::addMatVec(const OdeState& state, double alphaD, double alphaK, const Vector& x, Vector* F)
+{
+ Vector xLocal(3 * m_nodeIds.size()), fLocal;
+ SurgSim::Math::getSubVector(x, m_nodeIds, 3, &xLocal);
+ fLocal = (alphaD * m_D + alphaK * m_K) * xLocal;
+ SurgSim::Math::addSubVector(fLocal, m_nodeIds, 3, F);
+}
+
+MockMassSpring::MockMassSpring(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& pose,
+ size_t numNodes, std::vector<size_t> nodeBoundaryConditions,
+ double totalMass,
+ double rayleighDampingMass, double rayleighDampingStiffness,
+ double springStiffness, double springDamping,
+ SurgSim::Math::IntegrationScheme integrationScheme) :
+ SurgSim::Physics::MassSpringRepresentation(name)
+{
+ using SurgSim::Math::getSubVector;
+ using SurgSim::Math::setSubVector;
+ using SurgSim::Physics::Mass;
+ using SurgSim::Physics::LinearSpring;
+
+ // Note: setLocalPose MUST be called before WakeUp to be effective !
+ setLocalPose(pose);
+
+ std::shared_ptr<SurgSim::Math::OdeState> state;
+ state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(3, numNodes);
+ for (size_t i = 0; i < numNodes; i++)
+ {
+ Vector3d p(static_cast<double>(i)/static_cast<double>(numNodes), 0, 0);
+ setSubVector(p, i, 3, &state->getPositions());
+ addMass(std::make_shared<Mass>(totalMass / numNodes));
+ }
+ for (auto bc = std::begin(nodeBoundaryConditions); bc != std::end(nodeBoundaryConditions); bc++)
+ {
+ state->addBoundaryCondition(*bc);
+ }
+ for (size_t i = 0; i < numNodes - 1; i++)
+ {
+ std::shared_ptr<LinearSpring> spring = std::make_shared<LinearSpring>(i, i+1);
+ spring->setDamping(springDamping);
+ spring->setStiffness(springStiffness);
+ const Vector3d& xi = getSubVector(state->getPositions(), i, 3);
+ const Vector3d& xj = getSubVector(state->getPositions(), i+1, 3);
+ spring->setRestLength( (xj - xi).norm() );
+ addSpring(spring);
+ }
+ setInitialState(state);
+ setIntegrationScheme(integrationScheme);
+ setRayleighDampingMass(rayleighDampingMass);
+ setRayleighDampingStiffness(rayleighDampingStiffness);
+}
+
+MockMassSpring::~MockMassSpring()
+{
+}
+
+const Vector3d& MockMassSpring::getGravityVector() const
+{
+ return getGravity();
+}
+
+
+MockFemElement::MockFemElement() : FemElement(), m_isInitialized(false)
+{
+ setNumDofPerNode(3);
+}
+
+void MockFemElement::addNode(size_t nodeId)
+{
+ this->m_nodeIds.push_back(nodeId);
+}
+
+double MockFemElement::getVolume(const OdeState& state) const
+{
+ return 1;
+}
+
+void MockFemElement::addForce(const OdeState& state, Vector* F, double scale)
+{
+ SurgSim::Math::addSubVector(scale * m_F, m_nodeIds, 3, F);
+}
+
+void MockFemElement::addMass(const OdeState& state, Matrix* M, double scale)
+{
+ SurgSim::Math::addSubMatrix(scale * m_M, m_nodeIds, 3, M);
+}
+
+void MockFemElement::addDamping(const OdeState& state, Matrix* D, double scale)
+{
+ SurgSim::Math::addSubMatrix(scale * m_D, m_nodeIds, 3, D);
+}
+
+void MockFemElement::addStiffness(const OdeState& state, Matrix* K, double scale)
+{
+ SurgSim::Math::addSubMatrix(scale * m_K, m_nodeIds, 3, K);
+}
+
+void MockFemElement::addFMDK(const OdeState& state, Vector* f, Matrix* M, Matrix* D, Matrix* K)
+{
+ addForce(state, f);
+ addMass(state, M);
+ addDamping(state, D);
+ addStiffness(state, K);
+}
+
+void MockFemElement::addMatVec(const OdeState& state, double alphaM, double alphaD, double alphaK,
+ const Vector& x, Vector* F)
+{
+ Vector xLocal(3 * m_nodeIds.size()), fLocal;
+ SurgSim::Math::getSubVector(x, m_nodeIds, 3, &xLocal);
+ fLocal = (alphaM * m_M + alphaD * m_D + alphaK * m_K) * xLocal;
+ SurgSim::Math::addSubVector(fLocal, m_nodeIds, 3, F);
+}
+
+Vector MockFemElement::computeCartesianCoordinate(const OdeState& state, const Vector &barycentricCoordinate) const
+{
+ return SurgSim::Math::Vector3d::Zero();
+}
+
+Vector MockFemElement::computeNaturalCoordinate(const OdeState& state, const Vector &globalCoordinate) const
+{
+ return SurgSim::Math::Vector3d::Zero();
+}
+
+void MockFemElement::initialize(const OdeState& state)
+{
+ FemElement::initialize(state);
+ const size_t numDof = 3 * m_nodeIds.size();
+ m_F = Vector::LinSpaced(numDof, 1.0, static_cast<double>(numDof));
+ m_M = Matrix::Identity(numDof, numDof) * 1.0;
+ m_D = Matrix::Identity(numDof, numDof) * 2.0;
+ m_K = Matrix::Identity(numDof, numDof) * 3.0;
+ m_isInitialized = true;
+}
+
+bool MockFemElement::isInitialized() const
+{
+ return m_isInitialized;
+}
+
+bool InvalidMockFemElement::update(const SurgSim::Math::OdeState& state)
+{
+ return false;
+}
+
+
+MockFemRepresentation::MockFemRepresentation(const std::string& name) : FemRepresentation(name)
+{
+ this->m_numDofPerNode = 3;
+}
+
+MockFemRepresentation::~MockFemRepresentation()
+{
+}
+
+void MockFemRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K,
+ const SurgSim::Math::Matrix& D)
+{
+ std::shared_ptr<MockDeformableRepresentationLocalization> loc =
+ std::dynamic_pointer_cast<MockDeformableRepresentationLocalization>(localization);
+
+ size_t numDofPerNode = getNumDofPerNode();
+ m_externalGeneralizedForce.segment(numDofPerNode * loc->getLocalNode(), numDofPerNode) += generalizedForce;
+ m_externalGeneralizedStiffness.block(numDofPerNode * loc->getLocalNode(), numDofPerNode * loc->getLocalNode(),
+ numDofPerNode, numDofPerNode) += K;
+ m_externalGeneralizedDamping.block(numDofPerNode * loc->getLocalNode(), numDofPerNode * loc->getLocalNode(),
+ numDofPerNode, numDofPerNode) += D;
+}
+
+std::shared_ptr<FemPlyReaderDelegate> MockFemRepresentation::getDelegate()
+{
+ return nullptr;
+}
+
+RepresentationType MockFemRepresentation::getType() const
+{
+ return REPRESENTATION_TYPE_INVALID;
+}
+
+std::shared_ptr<OdeSolver> MockFemRepresentation::getOdeSolver() const
+{
+ return this->m_odeSolver;
+}
+
+const std::vector<double>& MockFemRepresentation::getMassPerNode() const
+{
+ return m_massPerNode;
+}
+
+void MockFemRepresentation::transformState(std::shared_ptr<OdeState> state, const RigidTransform3d& transform)
+{
+}
+
+MockFem1DRepresentation::MockFem1DRepresentation(const std::string& name) : SurgSim::Physics::Fem1DRepresentation(name)
+{
+}
+
+const std::shared_ptr<OdeSolver> MockFem1DRepresentation::getOdeSolver() const
+{
+ return this->m_odeSolver;
+}
+
+
+MockFixedConstraintBilateral3D::MockFixedConstraintBilateral3D() : ConstraintImplementation()
+{
+}
+
+MockFixedConstraintBilateral3D::~MockFixedConstraintBilateral3D()
+{
+}
+
+SurgSim::Math::MlcpConstraintType MockFixedConstraintBilateral3D::getMlcpConstraintType() const
+{
+ return SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT;
+}
+
+RepresentationType MockFixedConstraintBilateral3D::getRepresentationType() const
+{
+ return REPRESENTATION_TYPE_FIXED;
+}
+
+size_t MockFixedConstraintBilateral3D::doGetNumDof() const
+{
+ return 3;
+}
+
+void MockFixedConstraintBilateral3D::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+}
+
+
+MockRigidConstraintBilateral3D::MockRigidConstraintBilateral3D() : ConstraintImplementation()
+{
+}
+
+MockRigidConstraintBilateral3D::~MockRigidConstraintBilateral3D()
+{
+}
+
+SurgSim::Math::MlcpConstraintType MockRigidConstraintBilateral3D::getMlcpConstraintType() const
+{
+ return SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT;
+}
+
+RepresentationType MockRigidConstraintBilateral3D::getRepresentationType() const
+{
+ return REPRESENTATION_TYPE_RIGID;
+}
+
+size_t MockRigidConstraintBilateral3D::doGetNumDof() const
+{
+ return 3;
+}
+
+void MockRigidConstraintBilateral3D::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+}
+
+
+MockLocalization::MockLocalization() : Localization()
+{
+}
+
+MockLocalization::MockLocalization(std::shared_ptr<Representation> representation) : Localization(representation)
+{
+}
+
+Vector3d MockLocalization::doCalculatePosition(double time)
+{
+ return SurgSim::Math::Vector3d::Zero();
+}
+
+
+SurgSim::Math::MlcpConstraintType MockConstraintImplementation::getMlcpConstraintType() const
+{
+ return SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT;
+}
+
+RepresentationType MockConstraintImplementation::getRepresentationType() const
+{
+ return SurgSim::Physics::REPRESENTATION_TYPE_FIXED;
+}
+
+size_t MockConstraintImplementation::doGetNumDof() const
+{
+ return 1;
+}
+
+void MockConstraintImplementation::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+}
+
+MockVirtualToolCoupler::MockVirtualToolCoupler() : VirtualToolCoupler("Mock Virtual Tool Coupler")
+{
+}
+
+const SurgSim::DataStructures::OptionalValue<double>& MockVirtualToolCoupler::getOptionalLinearStiffness() const
+{
+ return VirtualToolCoupler::getOptionalLinearStiffness();
+}
+
+const SurgSim::DataStructures::OptionalValue<double>& MockVirtualToolCoupler::getOptionalLinearDamping() const
+{
+ return VirtualToolCoupler::getOptionalLinearDamping();
+}
+
+const SurgSim::DataStructures::OptionalValue<double>& MockVirtualToolCoupler::getOptionalAngularStiffness() const
+{
+ return VirtualToolCoupler::getOptionalAngularStiffness();
+}
+
+const SurgSim::DataStructures::OptionalValue<double>& MockVirtualToolCoupler::getOptionalAngularDamping() const
+{
+ return VirtualToolCoupler::getOptionalAngularDamping();
+}
+
+const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>&
+MockVirtualToolCoupler::getOptionalAttachmentPoint() const
+{
+ return VirtualToolCoupler::getOptionalAttachmentPoint();
+}
+
+void MockVirtualToolCoupler::setOptionalLinearStiffness(const SurgSim::DataStructures::OptionalValue<double>& val)
+{
+ VirtualToolCoupler::setOptionalLinearStiffness(val);
+}
+
+void MockVirtualToolCoupler::setOptionalLinearDamping(const SurgSim::DataStructures::OptionalValue<double>& val)
+{
+ VirtualToolCoupler::setOptionalLinearDamping(val);
+}
+
+void MockVirtualToolCoupler::setOptionalAngularStiffness(const SurgSim::DataStructures::OptionalValue<double>& val)
+{
+ VirtualToolCoupler::setOptionalAngularStiffness(val);
+}
+
+void MockVirtualToolCoupler::setOptionalAngularDamping(const SurgSim::DataStructures::OptionalValue<double>& val)
+{
+ VirtualToolCoupler::setOptionalAngularDamping(val);
+}
+
+void MockVirtualToolCoupler::setOptionalAttachmentPoint(
+ const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& val)
+{
+ VirtualToolCoupler::setOptionalAttachmentPoint(val);
+}
+
+const SurgSim::DataStructures::DataGroup& MockVirtualToolCoupler::getOutputData() const
+{
+ return m_outputData;
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/UnitTests/MockObjects.h b/SurgSim/Physics/UnitTests/MockObjects.h
new file mode 100644
index 0000000..472c3d5
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/MockObjects.h
@@ -0,0 +1,428 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_UNITTESTS_MOCKOBJECTS_H
+#define SURGSIM_PHYSICS_UNITTESTS_MOCKOBJECTS_H
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeSolver.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+#include "SurgSim/Physics/DeformableRepresentation.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+#include "SurgSim/Physics/LinearSpring.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/Mass.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+#include "SurgSim/Physics/MassSpringRepresentationLocalization.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/VirtualToolCoupler.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+using SurgSim::Math::Matrix;
+using SurgSim::Math::OdeSolver;
+using SurgSim::Math::OdeState;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+
+class MockRepresentation : public Representation
+{
+protected:
+ int m_preUpdateCount;
+ int m_updateCount;
+ int m_postUpdateCount;
+
+public:
+ explicit MockRepresentation(const std::string& name = "MockRepresention");
+
+ virtual ~MockRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::MockRepresentation);
+
+ virtual RepresentationType getType() const override;
+
+ /// Preprocessing done before the update call
+ /// \param dt The time step (in seconds)
+ virtual void beforeUpdate(double dt) override;
+
+ /// Update the representation state to the current time step
+ /// \param dt The time step (in seconds)
+ virtual void update(double dt) override;
+
+ /// Postprocessing done after the update call
+ /// \param dt The time step (in seconds)
+ virtual void afterUpdate(double dt) override;
+
+ int getPreUpdateCount() const;
+
+ int getUpdateCount() const;
+
+ int getPostUpdateCount() const;
+};
+
+class MockRigidRepresentation : public RigidRepresentation
+{
+public:
+ MockRigidRepresentation();
+
+ // Non constant access to the states
+ RigidRepresentationState& getInitialState();
+ RigidRepresentationState& getCurrentState();
+ RigidRepresentationState& getPreviousState();
+};
+
+class MockDeformableRepresentationLocalization : public SurgSim::Physics::Localization
+{
+public:
+ MockDeformableRepresentationLocalization(){}
+
+ explicit MockDeformableRepresentationLocalization(std::shared_ptr<Representation> representation) : Localization()
+ {
+ setRepresentation(representation);
+ }
+
+ virtual ~MockDeformableRepresentationLocalization(){}
+
+ void setLocalNode(size_t nodeID){ m_nodeID = nodeID; }
+
+ const size_t& getLocalNode() const { return m_nodeID; }
+
+ virtual bool isValidRepresentation(std::shared_ptr<Representation> representation) override
+ {
+ std::shared_ptr<DeformableRepresentation> defRepresentation =
+ std::dynamic_pointer_cast<DeformableRepresentation>(representation);
+
+ // Allows to reset the representation to nullptr ...
+ return (defRepresentation != nullptr || representation == nullptr);
+ }
+
+private:
+ virtual SurgSim::Math::Vector3d doCalculatePosition(double time) override
+ {
+ std::shared_ptr<DeformableRepresentation> defRepresentation =
+ std::static_pointer_cast<DeformableRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(defRepresentation != nullptr) << "Deformable Representation is null, it was probably not" <<
+ " initialized";
+ SURGSIM_ASSERT((0.0 <= time) && (time <= 1.0)) << "Time must be between 0.0 and 1.0 inclusive";
+
+ const SurgSim::Math::Vector3d& currentPoint = defRepresentation->getCurrentState()->getPosition(m_nodeID);
+ const SurgSim::Math::Vector3d& previousPoint = defRepresentation->getPreviousState()->getPosition(m_nodeID);
+
+ return SurgSim::Math::interpolate(previousPoint, currentPoint, time);
+ }
+
+ size_t m_nodeID;
+};
+
+class MockDeformableRepresentation : public SurgSim::Physics::DeformableRepresentation
+{
+public:
+ explicit MockDeformableRepresentation(const std::string& name = "MockDeformableRepresentation");
+
+ /// Query the representation type
+ /// \return the RepresentationType for this representation
+ /// \note DeformableRepresentation is abstract because there is really no deformable behind this class !
+ /// \note For the test, we simply set the type to INVALID
+ virtual SurgSim::Physics::RepresentationType getType() const override;
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::MockDeformableRepresentation);
+
+ virtual void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K,
+ const SurgSim::Math::Matrix& D) override;
+
+ /// OdeEquation API (empty) is not tested here as DeformableRep does not provide an implementation
+ /// This API will be tested in derived classes when the API will be provided
+ virtual Vector& computeF(const OdeState& state) override;
+
+ /// OdeEquation API (empty) is not tested here as DeformableRep does not provide an implementation
+ /// This API will be tested in derived classes when the API will be provided
+ virtual const Matrix& computeM(const OdeState& state) override;
+
+ /// OdeEquation API (empty) is not tested here as DeformableRep does not provide an implementation
+ /// This API will be tested in derived classes when the API will be provided
+ virtual const Matrix& computeD(const OdeState& state) override;
+
+ /// OdeEquation API (empty) is not tested here as DeformableRep does not provide an implementation
+ /// This API will be tested in derived classes when the API will be provided
+ virtual const Matrix& computeK(const OdeState& state) override;
+
+ /// OdeEquation API (empty) is not tested here as DeformableRep does not provide an implementation
+ /// This API will be tested in derived classes when the API will be provided
+ virtual void computeFMDK(const OdeState& state, Vector** f, Matrix** M, Matrix** D, Matrix** K) override;
+
+protected:
+ virtual void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform) override;
+
+ Vector m_F;
+ Matrix m_M, m_D, m_K;
+};
+
+class MockSpring : public SurgSim::Physics::Spring
+{
+public:
+ MockSpring();
+
+ void addNode(size_t nodeId);
+
+ virtual void addForce(const OdeState& state, Vector* F, double scale = 1.0) override;
+ virtual void addDamping(const OdeState& state, Matrix* D, double scale = 1.0) override;
+ virtual void addStiffness(const OdeState& state, Matrix* K, double scale = 1.0) override;
+ virtual void addFDK(const OdeState& state, Vector* f, Matrix* D, Matrix* K) override;
+ virtual void addMatVec(const OdeState& state, double alphaD, double alphaK, const Vector& x, Vector* F) override;
+
+private:
+ Vector m_F;
+ Matrix m_D, m_K;
+};
+
+class MockMassSpring : public SurgSim::Physics::MassSpringRepresentation
+{
+public:
+ MockMassSpring() : MassSpringRepresentation("MassSpring")
+ {
+ }
+
+ MockMassSpring(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& pose,
+ size_t numNodes, std::vector<size_t> nodeBoundaryConditions,
+ double totalMass,
+ double rayleighDampingMass, double rayleighDampingStiffness,
+ double springStiffness, double springDamping,
+ SurgSim::Math::IntegrationScheme integrationScheme);
+
+ virtual ~MockMassSpring();
+
+ const Vector3d& getGravityVector() const;
+
+ const SurgSim::Math::Vector& getExternalForce() const { return m_externalGeneralizedForce; }
+ const SurgSim::Math::Matrix& getExternalStiffness() const { return m_externalGeneralizedStiffness; }
+ const SurgSim::Math::Matrix& getExternalDamping() const { return m_externalGeneralizedDamping; }
+};
+
+class MockFemElement : public FemElement
+{
+public:
+ MockFemElement();
+
+ void addNode(size_t nodeId);
+
+ virtual double getVolume(const OdeState& state) const override;
+ virtual void addForce(const OdeState& state, Vector* F, double scale = 1.0) override;
+ virtual void addMass(const OdeState& state, Matrix* M, double scale = 1.0) override;
+ virtual void addDamping(const OdeState& state, Matrix* D, double scale = 1.0) override;
+ virtual void addStiffness(const OdeState& state, Matrix* K, double scale = 1.0) override;
+ virtual void addFMDK(const OdeState& state, Vector* f, Matrix* M, Matrix* D, Matrix* K) override;
+ virtual void addMatVec(
+ const OdeState& state, double alphaM, double alphaD, double alphaK, const Vector& x, Vector* F) override;
+ virtual Vector computeCartesianCoordinate(
+ const OdeState& state, const Vector &barycentricCoordinate) const override;
+ virtual Vector computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state, const Vector &globalCoordinate) const override;
+
+ virtual void initialize(const SurgSim::Math::OdeState& state) override;
+
+ bool isInitialized() const;
+
+private:
+ Vector m_F;
+ Matrix m_M, m_D, m_K;
+ bool m_isInitialized;
+};
+
+class InvalidMockFemElement : public MockFemElement
+{
+public:
+ virtual bool update(const SurgSim::Math::OdeState& state) override;
+};
+
+// Concrete class for testing
+class MockFemRepresentation : public FemRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name of the FemRepresentation
+ explicit MockFemRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~MockFemRepresentation();
+
+ virtual void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K,
+ const SurgSim::Math::Matrix& D) override;
+
+ virtual std::shared_ptr<FemPlyReaderDelegate> getDelegate() override;
+
+ /// Query the representation type
+ /// \return the RepresentationType for this representation
+ virtual RepresentationType getType() const override;
+
+ std::shared_ptr<OdeSolver> getOdeSolver() const;
+
+ const std::vector<double>& getMassPerNode() const;
+
+protected:
+ /// Transform a state using a given transformation
+ /// \param[in,out] state The state to be transformed
+ /// \param transform The transformation to apply
+ virtual void transformState(std::shared_ptr<OdeState> state, const RigidTransform3d& transform) override;
+};
+
+class MockFem1DRepresentation : public SurgSim::Physics::Fem1DRepresentation
+{
+public:
+ explicit MockFem1DRepresentation(const std::string& name);
+
+ const std::shared_ptr<OdeSolver> getOdeSolver() const;
+};
+
+class MockFixedConstraintBilateral3D : public ConstraintImplementation
+{
+public:
+ MockFixedConstraintBilateral3D();
+ virtual ~MockFixedConstraintBilateral3D();
+
+ virtual SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+
+ virtual RepresentationType getRepresentationType() const override;
+
+private:
+ virtual size_t doGetNumDof() const override;
+
+ virtual void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) override;
+};
+
+class MockRigidConstraintBilateral3D : public ConstraintImplementation
+{
+public:
+ MockRigidConstraintBilateral3D();
+ virtual ~MockRigidConstraintBilateral3D();
+
+ virtual SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+
+ virtual RepresentationType getRepresentationType() const override;
+
+private:
+ virtual size_t doGetNumDof() const override;
+
+ virtual void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) override;
+};
+
+template <class Base>
+class MockDescendent : public Base
+{
+public:
+ MockDescendent() : Base() {}
+ explicit MockDescendent(const std::string &name) : Base(name) {}
+};
+
+class MockLocalization : public Localization
+{
+public:
+ MockLocalization();
+
+ explicit MockLocalization(std::shared_ptr<Representation> representation);
+
+private:
+ /// Calculates the global position of this localization
+ /// \param time The time in [0..1] at which the position should be calculated
+ /// \return The global position of the localization at the requested time
+ /// \note time can useful when dealing with CCD
+ virtual SurgSim::Math::Vector3d doCalculatePosition(double time) override;
+};
+
+class MockConstraintImplementation : public ConstraintImplementation
+{
+public:
+ virtual SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+
+ virtual RepresentationType getRepresentationType() const override;
+
+private:
+ virtual size_t doGetNumDof() const override;
+
+ virtual void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign);
+};
+
+class MockVirtualToolCoupler : public VirtualToolCoupler
+{
+public:
+ MockVirtualToolCoupler();
+
+ const SurgSim::DataStructures::OptionalValue<double>& getOptionalLinearStiffness() const;
+ const SurgSim::DataStructures::OptionalValue<double>& getOptionalLinearDamping() const;
+ const SurgSim::DataStructures::OptionalValue<double>& getOptionalAngularStiffness() const;
+ const SurgSim::DataStructures::OptionalValue<double>& getOptionalAngularDamping() const;
+ const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& getOptionalAttachmentPoint() const;
+
+ void setOptionalLinearStiffness(const SurgSim::DataStructures::OptionalValue<double>& val);
+ void setOptionalLinearDamping(const SurgSim::DataStructures::OptionalValue<double>& val);
+ void setOptionalAngularStiffness(const SurgSim::DataStructures::OptionalValue<double>& val);
+ void setOptionalAngularDamping(const SurgSim::DataStructures::OptionalValue<double>& val);
+ void setOptionalAttachmentPoint(const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& val);
+
+ const SurgSim::DataStructures::DataGroup& getOutputData() const;
+};
+
+inline std::shared_ptr<Constraint> makeMockConstraint(std::shared_ptr<MockRepresentation> firstRepresentation,
+ std::shared_ptr<MockRepresentation> secondRepresentation)
+{
+ return std::make_shared<Constraint>(std::make_shared<ConstraintData>(),
+ std::make_shared<MockConstraintImplementation>(),
+ std::make_shared<MockLocalization>(firstRepresentation),
+ std::make_shared<MockConstraintImplementation>(),
+ std::make_shared<MockLocalization>(secondRepresentation));
+}
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_UNITTESTS_MOCKOBJECTS_H
diff --git a/SurgSim/Physics/UnitTests/PhysicsManagerStateTests.cpp b/SurgSim/Physics/UnitTests/PhysicsManagerStateTests.cpp
new file mode 100644
index 0000000..2ed270b
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/PhysicsManagerStateTests.cpp
@@ -0,0 +1,346 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 PhysicsManagerState class.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/MlcpMapping.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationContact.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+using SurgSim::Physics::Constraint;
+using SurgSim::Physics::ContactConstraintData;
+using SurgSim::Physics::Localization;
+using SurgSim::Physics::MlcpMapping;
+using SurgSim::Physics::PhysicsManagerState;
+using SurgSim::Physics::Representation;
+using SurgSim::Physics::RigidCollisionRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+using SurgSim::Physics::RigidRepresentationContact;
+using SurgSim::Physics::RigidRepresentationLocalization;
+
+TEST(PhysicsManagerStateTest, SetGetRigidRepresentations)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+ std::vector<std::shared_ptr<Representation>> expectedRepresentations;
+ std::vector<std::shared_ptr<Representation>> actualRepresentations;
+ std::unordered_map<std::shared_ptr<SurgSim::Collision::Representation>,
+ std::shared_ptr<Representation>> actualCollisionsToPhysicsMap;
+
+ // Add a representation.
+ auto rigid1 = std::make_shared<RigidRepresentation>("rigid1");
+ expectedRepresentations.push_back(rigid1);
+ physicsState->setRepresentations(expectedRepresentations);
+ actualRepresentations = physicsState->getRepresentations();
+ ASSERT_EQ(1, actualRepresentations.size());
+ EXPECT_EQ(rigid1, actualRepresentations.back());
+ actualCollisionsToPhysicsMap = physicsState->getCollisionToPhysicsMap();
+ EXPECT_EQ(0, actualCollisionsToPhysicsMap.size());
+
+ // Add a second representation. This one has a collision representation.
+ auto rigid2 = std::make_shared<RigidRepresentation>("rigid2");
+ auto collisionRepresentation = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("rigid2 collision");
+ rigid2->setCollisionRepresentation(collisionRepresentation);
+ expectedRepresentations.push_back(rigid2);
+ physicsState->setRepresentations(expectedRepresentations);
+ actualRepresentations = physicsState->getRepresentations();
+ ASSERT_EQ(2, actualRepresentations.size());
+ EXPECT_EQ(rigid2, actualRepresentations.back());
+
+ // check the collisionsToPhysicsMap
+ actualCollisionsToPhysicsMap = physicsState->getCollisionToPhysicsMap();
+ ASSERT_EQ(1, actualCollisionsToPhysicsMap.size());
+ EXPECT_EQ(rigid2, actualCollisionsToPhysicsMap[collisionRepresentation]);
+}
+
+TEST(PhysicsManagerStateTest, SetGetRepresentationsMapping)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+ MlcpMapping<Representation> expectedRepresentationsIndexMapping;
+ MlcpMapping<Representation> actualRepresentationsIndexMapping;
+
+ // Add a representation.
+ auto rigid1 = std::make_shared<RigidRepresentation>("rigid1");
+ expectedRepresentationsIndexMapping.setValue(rigid1.get(), 13);
+ physicsState->setRepresentationsMapping(expectedRepresentationsIndexMapping);
+ actualRepresentationsIndexMapping = physicsState->getRepresentationsMapping();
+ std::shared_ptr<Representation> rigid1AsRepresentation = rigid1;
+ EXPECT_EQ(13, actualRepresentationsIndexMapping.getValue(rigid1AsRepresentation.get()));
+
+ // Add a second representation. This one has a collision representation.
+ auto rigid2 = std::make_shared<RigidRepresentation>("rigid2");
+ auto collisionRepresentation = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("rigid2 collision");
+ rigid2->setCollisionRepresentation(collisionRepresentation);
+ expectedRepresentationsIndexMapping.setValue(rigid2.get(), 17);
+ physicsState->setRepresentationsMapping(expectedRepresentationsIndexMapping);
+ actualRepresentationsIndexMapping = physicsState->getRepresentationsMapping();
+ std::shared_ptr<Representation> rigid2AsRepresentation = rigid2;
+ EXPECT_EQ(17, actualRepresentationsIndexMapping.getValue(rigid2AsRepresentation.get()));
+}
+
+TEST(PhysicsManagerStateTest, SetGetActiveRepresentations)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+ std::vector<std::shared_ptr<Representation>> expectedRepresentations;
+ std::vector<std::shared_ptr<Representation>> actualRepresentations;
+
+ // Add a representation.
+ auto rigid1 = std::make_shared<RigidRepresentation>("rigid1");
+ expectedRepresentations.push_back(rigid1);
+ physicsState->setActiveRepresentations(expectedRepresentations);
+
+ // Filter the active representations and test.
+ actualRepresentations = physicsState->getActiveRepresentations();
+ ASSERT_EQ(1, actualRepresentations.size());
+ EXPECT_EQ(rigid1, actualRepresentations.back());
+
+ // Add a second representation. This one has a collision representation.
+ auto rigid2 = std::make_shared<RigidRepresentation>("rigid2");
+ auto collisionRepresentation = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("rigid2 collision");
+ rigid2->setCollisionRepresentation(collisionRepresentation);
+ expectedRepresentations.push_back(rigid2);
+ physicsState->setActiveRepresentations(expectedRepresentations);
+
+ // Filter the active representations and test.
+ actualRepresentations = physicsState->getActiveRepresentations();
+ ASSERT_EQ(2, actualRepresentations.size());
+ EXPECT_EQ(rigid2, actualRepresentations.back());
+}
+
+TEST(PhysicsManagerStateTest, SetGetCollisionRepresentations)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> expectedRepresentations;
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> actualRepresentations;
+
+ // Add a collision representation.
+ std::shared_ptr<SurgSim::Collision::Representation> collision1 =
+ std::make_shared<RigidCollisionRepresentation>("collision1");
+
+ expectedRepresentations.push_back(collision1);
+ physicsState->setCollisionRepresentations(expectedRepresentations);
+ actualRepresentations = physicsState->getCollisionRepresentations();
+ ASSERT_EQ(1, actualRepresentations.size());
+ EXPECT_EQ(collision1, actualRepresentations.back());
+
+ // Add a second collision representation.
+ std::shared_ptr<SurgSim::Collision::Representation> collision2 =
+ std::make_shared<RigidCollisionRepresentation>("collision2");
+ expectedRepresentations.push_back(collision2);
+ physicsState->setCollisionRepresentations(expectedRepresentations);
+ actualRepresentations = physicsState->getCollisionRepresentations();
+ ASSERT_EQ(2, actualRepresentations.size());
+ EXPECT_EQ(collision2, actualRepresentations.back());
+}
+
+TEST(PhysicsManagerStateTest, SetGetCollisionPairs)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+ std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>> expectedPairs;
+ std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>> actualPairs;
+
+ // Add a collision pair.
+ auto pair1 = std::make_shared<SurgSim::Collision::CollisionPair>();
+ expectedPairs.push_back(pair1);
+ physicsState->setCollisionPairs(expectedPairs);
+ actualPairs = physicsState->getCollisionPairs();
+ ASSERT_EQ(1, actualPairs.size());
+ EXPECT_EQ(pair1, actualPairs.back());
+
+ // Add a second collision representation.
+ auto pair2 = std::make_shared<SurgSim::Collision::CollisionPair>();
+ expectedPairs.push_back(pair2);
+ physicsState->setCollisionPairs(expectedPairs);
+ actualPairs = physicsState->getCollisionPairs();
+ ASSERT_EQ(2, actualPairs.size());
+ EXPECT_EQ(pair2, actualPairs.back());
+}
+
+TEST(PhysicsManagerStateTest, SetGetConstraintGroup)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+ std::vector<std::shared_ptr<Constraint>> expectedConstraints;
+ std::vector<std::shared_ptr<Constraint>> actualConstraints;
+
+ // We need a populated constraint to check the constraintsIndexMapping.
+ // Create first side of a constraint.
+ auto rigid1 = std::make_shared<RigidRepresentation>("rigid1");
+ std::shared_ptr<RigidRepresentationLocalization> rigid1LocalizationTyped =
+ std::make_shared<RigidRepresentationLocalization>();
+ rigid1LocalizationTyped->setRepresentation(rigid1);
+ std::shared_ptr<Localization> rigid1Localization = rigid1LocalizationTyped;
+ auto rigid1Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Create second side of a constraint.
+ auto rigid2 = std::make_shared<RigidRepresentation>("rigid2");
+ std::shared_ptr<RigidRepresentationLocalization> rigid2LocalizationTyped =
+ std::make_shared<RigidRepresentationLocalization>();
+ rigid2LocalizationTyped->setRepresentation(rigid2);
+ std::shared_ptr<Localization> rigid2Localization = rigid2LocalizationTyped;
+ auto rigid2Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Create the constraint specific data.
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+
+ // Create the constraint.
+ auto constraint1 = std::make_shared<Constraint>(data, rigid1Contact, rigid1Localization,
+ rigid2Contact, rigid2Localization);
+
+ // Check the constraintGroup.
+ expectedConstraints.push_back(constraint1);
+ physicsState->setConstraintGroup(SurgSim::Physics::CONSTRAINT_GROUP_TYPE_CONTACT, expectedConstraints);
+ actualConstraints = physicsState->getConstraintGroup(SurgSim::Physics::CONSTRAINT_GROUP_TYPE_CONTACT);
+ ASSERT_EQ(1, actualConstraints.size());
+ EXPECT_EQ(constraint1, actualConstraints.back());
+
+ // Create a second constraint.
+ auto constraint2 = std::make_shared<Constraint>(data, rigid1Contact, rigid1Localization,
+ rigid2Contact, rigid2Localization);
+
+ // Check the constraintGroup.
+ expectedConstraints.push_back(constraint2);
+ physicsState->setConstraintGroup(SurgSim::Physics::CONSTRAINT_GROUP_TYPE_CONTACT, expectedConstraints);
+ actualConstraints = physicsState->getConstraintGroup(SurgSim::Physics::CONSTRAINT_GROUP_TYPE_CONTACT);
+ ASSERT_EQ(2, actualConstraints.size());
+ EXPECT_EQ(constraint2, actualConstraints.back());
+}
+
+TEST(PhysicsManagerStateTest, SetGetConstraintsMapping)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+ MlcpMapping<Constraint> expectedConstraintsIndexMapping;
+ MlcpMapping<Constraint> actualConstraintsIndexMapping;
+
+ // We need a populated constraint to check the constraintsIndexMapping.
+ // Create first side of a constraint.
+ auto rigid1 = std::make_shared<RigidRepresentation>("rigid1");
+ std::shared_ptr<RigidRepresentationLocalization> rigid1LocalizationTyped =
+ std::make_shared<RigidRepresentationLocalization>();
+ rigid1LocalizationTyped->setRepresentation(rigid1);
+ std::shared_ptr<Localization> rigid1Localization = rigid1LocalizationTyped;
+ auto rigid1Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Create second side of a constraint.
+ auto rigid2 = std::make_shared<RigidRepresentation>("rigid2");
+ std::shared_ptr<RigidRepresentationLocalization> rigid2LocalizationTyped =
+ std::make_shared<RigidRepresentationLocalization>();
+ rigid2LocalizationTyped->setRepresentation(rigid2);
+ std::shared_ptr<Localization> rigid2Localization = rigid2LocalizationTyped;
+ auto rigid2Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Create the constraint specific data.
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+
+ // Create the constraint.
+ auto constraint1 = std::make_shared<Constraint>(data, rigid1Contact, rigid1Localization,
+ rigid2Contact, rigid2Localization);
+
+ // Check the constraintGroup.
+ expectedConstraintsIndexMapping.setValue(constraint1.get(), 5);
+ physicsState->setConstraintsMapping(expectedConstraintsIndexMapping);
+ actualConstraintsIndexMapping = physicsState->getConstraintsMapping();
+ std::shared_ptr<Constraint> constraint1AsConstraint = constraint1;
+ ASSERT_EQ(5, actualConstraintsIndexMapping.getValue(constraint1AsConstraint.get()));
+}
+
+TEST(PhysicsManagerStateTest, SetGetActiveConstraints)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+ std::vector<std::shared_ptr<Constraint>> expectedConstraints;
+ std::vector<std::shared_ptr<Constraint>> actualConstraints;
+
+ // Create first side of a constraint.
+ auto rigid1 = std::make_shared<RigidRepresentation>("rigid1");
+ std::shared_ptr<RigidRepresentationLocalization> rigid1LocalizationTyped =
+ std::make_shared<RigidRepresentationLocalization>();
+ rigid1LocalizationTyped->setRepresentation(rigid1);
+ std::shared_ptr<Localization> rigid1Localization = rigid1LocalizationTyped;
+ auto rigid1Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Create second side of a constraint.
+ auto rigid2 = std::make_shared<RigidRepresentation>("rigid2");
+ std::shared_ptr<RigidRepresentationLocalization> rigid2LocalizationTyped =
+ std::make_shared<RigidRepresentationLocalization>();
+ rigid2LocalizationTyped->setRepresentation(rigid2);
+ std::shared_ptr<Localization> rigid2Localization = rigid2LocalizationTyped;
+ auto rigid2Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Create the constraint specific data.
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+
+ // Create the constraint.
+ auto constraint1 = std::make_shared<Constraint>(data, rigid1Contact, rigid1Localization,
+ rigid2Contact, rigid2Localization);
+
+ // Check the active constraints.
+ expectedConstraints.push_back(constraint1);
+ physicsState->setActiveConstraints(expectedConstraints);
+ actualConstraints = physicsState->getActiveConstraints();
+ ASSERT_EQ(1, actualConstraints.size());
+ EXPECT_EQ(constraint1, actualConstraints.back());
+
+ // Create a second constraint.
+ auto constraint2 = std::make_shared<Constraint>(data, rigid1Contact, rigid1Localization,
+ rigid2Contact, rigid2Localization);
+
+ // Check the active constraints.
+ expectedConstraints.push_back(constraint2);
+ physicsState->setActiveConstraints(expectedConstraints);
+ actualConstraints = physicsState->getActiveConstraints();
+ ASSERT_EQ(2, actualConstraints.size());
+ EXPECT_EQ(constraint1, actualConstraints.front());
+ EXPECT_EQ(constraint2, actualConstraints.back());
+}
+
+TEST(PhysicsManagerStateTest, GetMlcpProblem)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+
+ // Check non-const getter.
+ EXPECT_NO_THROW(physicsState->getMlcpProblem().A.resize(21, 8));
+ EXPECT_EQ(21, physicsState->getMlcpProblem().A.rows());
+ EXPECT_EQ(8, physicsState->getMlcpProblem().A.cols());
+
+ // Check const getter.
+ std::shared_ptr<const PhysicsManagerState> constPhysicsState = std::make_shared<PhysicsManagerState>();
+ EXPECT_NO_THROW(constPhysicsState->getMlcpProblem());
+}
+
+TEST(PhysicsManagerStateTest, GetMlcpSolution)
+{
+ auto physicsState = std::make_shared<PhysicsManagerState>();
+
+ // Check non-const getter.
+ EXPECT_NO_THROW(physicsState->getMlcpSolution().x.resize(23));
+ EXPECT_EQ(23, physicsState->getMlcpSolution().x.size());
+
+ // Check const getter.
+ std::shared_ptr<const PhysicsManagerState> constPhysicsState = std::make_shared<PhysicsManagerState>();
+ EXPECT_NO_THROW(constPhysicsState->getMlcpSolution());
+}
diff --git a/SurgSim/Physics/UnitTests/PhysicsManagerTests.cpp b/SurgSim/Physics/UnitTests/PhysicsManagerTests.cpp
new file mode 100644
index 0000000..4d3520f
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/PhysicsManagerTests.cpp
@@ -0,0 +1,183 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 PhysicsManager class. Note that PhysicsManagerTest, the test fixture
+/// is declared as a friend class in PhysicsManager to make it easier to test the
+/// add and removal of components, for this to work correctly PhysicsManagerTest is required
+/// to be in the SurgSim::Physics namespace.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Physics/ConstraintComponent.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Framework::Runtime;
+using SurgSim::Framework::Component;
+using SurgSim::Physics::FixedRepresentation;
+using SurgSim::Physics::PhysicsManager;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class PhysicsManagerTest : public ::testing::Test
+{
+public:
+ virtual void SetUp()
+ {
+ physicsManager = std::make_shared<PhysicsManager>();
+ }
+
+ virtual void TearDown()
+ {
+ }
+
+
+ bool testDoAddComponent(const std::shared_ptr<Component>& component)
+ {
+ return physicsManager->executeAdditions(component);
+ }
+
+ bool testDoRemoveComponent(const std::shared_ptr<Component>& component)
+ {
+ return physicsManager->executeRemovals(component);
+ }
+
+ const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>* getExcludedCollisionPairs(
+ const SurgSim::Physics::PhysicsManager& physicsManager)
+ {
+ return &physicsManager.m_excludedCollisionPairs;
+ }
+
+ std::shared_ptr<PhysicsManager> physicsManager;
+};
+
+TEST_F(PhysicsManagerTest, InitTest)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ runtime->addManager(physicsManager);
+ EXPECT_NO_THROW(runtime->start());
+ EXPECT_NO_THROW(runtime->stop());
+}
+
+TEST_F(PhysicsManagerTest, AddRemoveRepresentation)
+{
+ std::shared_ptr<FixedRepresentation> representation1 = std::make_shared<FixedRepresentation>("Rep1");
+ std::shared_ptr<FixedRepresentation> representation2 = std::make_shared<FixedRepresentation>("Rep2");
+
+ EXPECT_TRUE(testDoAddComponent(representation1));
+ EXPECT_TRUE(testDoAddComponent(representation2));
+ EXPECT_FALSE(testDoAddComponent(representation1));
+
+ EXPECT_TRUE(testDoRemoveComponent(representation1));
+ EXPECT_FALSE(testDoRemoveComponent(representation1));
+ EXPECT_TRUE(testDoRemoveComponent(representation2));
+}
+
+TEST_F(PhysicsManagerTest, AddRemoveConstraintComponent)
+{
+ auto constraintComponent1 = std::make_shared<ConstraintComponent>("component1");
+ auto constraintComponent2 = std::make_shared<ConstraintComponent>("component2");
+
+ constraintComponent1->setConstraint(
+ makeMockConstraint(std::make_shared<MockRepresentation>(), std::make_shared<MockRepresentation>()));
+ constraintComponent2->setConstraint(
+ makeMockConstraint(std::make_shared<MockRepresentation>(), std::make_shared<MockRepresentation>()));
+
+ EXPECT_TRUE(testDoAddComponent(constraintComponent1));
+ EXPECT_TRUE(testDoAddComponent(constraintComponent2));
+ EXPECT_FALSE(testDoAddComponent(constraintComponent1));
+
+ EXPECT_TRUE(testDoRemoveComponent(constraintComponent1));
+ EXPECT_FALSE(testDoRemoveComponent(constraintComponent1));
+ EXPECT_TRUE(testDoRemoveComponent(constraintComponent2));
+}
+
+TEST_F(PhysicsManagerTest, AddRemoveExcludedCollisionPair)
+{
+ auto physicsManager = std::make_shared<PhysicsManager>();
+
+ auto rep1 = std::make_shared<SurgSim::Physics::DeformableCollisionRepresentation>("rep1");
+ auto rep2 = std::make_shared<SurgSim::Physics::DeformableCollisionRepresentation>("rep2");
+ auto rep3 = std::make_shared<SurgSim::Physics::DeformableCollisionRepresentation>("rep3");
+
+ auto collisionPairs = getExcludedCollisionPairs(*physicsManager);
+ EXPECT_EQ(0u, collisionPairs->size());
+
+ {
+ SCOPED_TRACE("Add once.");
+ EXPECT_NO_THROW(physicsManager->addExcludedCollisionPair(rep1, rep2));
+ EXPECT_EQ(1u, collisionPairs->size());
+ }
+
+ {
+ SCOPED_TRACE("Double add.");
+ EXPECT_NO_THROW(physicsManager->addExcludedCollisionPair(rep1, rep2));
+ EXPECT_EQ(1u, collisionPairs->size());
+ }
+
+ {
+ SCOPED_TRACE("Removal.");
+ EXPECT_NO_THROW(physicsManager->removeExcludedCollisionPair(rep1, rep2));
+ EXPECT_EQ(0u, collisionPairs->size());
+ }
+
+ {
+ SCOPED_TRACE("Double removal.");
+ EXPECT_NO_THROW(physicsManager->removeExcludedCollisionPair(rep1, rep2));
+ EXPECT_EQ(0u, collisionPairs->size());
+ }
+
+ {
+ SCOPED_TRACE("Adding multiple.");
+ EXPECT_NO_THROW(physicsManager->addExcludedCollisionPair(rep1, rep2));
+ EXPECT_NO_THROW(physicsManager->addExcludedCollisionPair(rep1, rep3));
+ EXPECT_NO_THROW(physicsManager->addExcludedCollisionPair(rep2, rep3));
+ EXPECT_EQ(3u, collisionPairs->size());
+ }
+
+ {
+ SCOPED_TRACE("Removing multiple.");
+ EXPECT_NO_THROW(physicsManager->removeExcludedCollisionPair(rep1, rep2));
+ EXPECT_NO_THROW(physicsManager->removeExcludedCollisionPair(rep1, rep3));
+ EXPECT_NO_THROW(physicsManager->removeExcludedCollisionPair(rep2, rep3));
+ EXPECT_EQ(0u, collisionPairs->size());
+ }
+
+ {
+ SCOPED_TRACE("Add and remove with swapped representations.");
+ EXPECT_NO_THROW(physicsManager->addExcludedCollisionPair(rep1, rep2));
+ EXPECT_EQ(1u, collisionPairs->size());
+
+ EXPECT_NO_THROW(physicsManager->removeExcludedCollisionPair(rep2, rep1));
+ EXPECT_EQ(0u, collisionPairs->size());
+ }
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
+
diff --git a/SurgSim/Physics/UnitTests/PostUpdateTests.cpp b/SurgSim/Physics/UnitTests/PostUpdateTests.cpp
new file mode 100644
index 0000000..ea1bc2b
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/PostUpdateTests.cpp
@@ -0,0 +1,66 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 PostUpdateTests.cpp
+/// Simple Test for PostUpdate calculation
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/PostUpdate.h"
+#include "SurgSim/Physics/Representation.h"
+
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+
+TEST(PostUpdateTest, UpdateCallTest)
+{
+ std::shared_ptr<MockRepresentation> mockRepresentation = std::make_shared<MockRepresentation>();
+ std::shared_ptr<PhysicsManagerState> physicsManagerState = std::make_shared<PhysicsManagerState>();
+ std::vector<std::shared_ptr<Representation>> allRepresentations;
+
+ allRepresentations.push_back(mockRepresentation);
+ physicsManagerState->setRepresentations(allRepresentations);
+
+ double dt = 1e-3;
+ std::shared_ptr<PostUpdate> postUpdateComputation = std::make_shared<PostUpdate>();
+ EXPECT_EQ(0, mockRepresentation->getPreUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getPostUpdateCount());
+ postUpdateComputation->update(dt, physicsManagerState);
+ EXPECT_EQ(0, mockRepresentation->getPreUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getUpdateCount());
+ EXPECT_EQ(1, mockRepresentation->getPostUpdateCount());
+ postUpdateComputation->update(dt, physicsManagerState);
+ EXPECT_EQ(0, mockRepresentation->getPreUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getUpdateCount());
+ EXPECT_EQ(2, mockRepresentation->getPostUpdateCount());
+ postUpdateComputation->update(dt, physicsManagerState);
+ EXPECT_EQ(0, mockRepresentation->getPreUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getUpdateCount());
+ EXPECT_EQ(3, mockRepresentation->getPostUpdateCount());
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
+
diff --git a/SurgSim/Physics/UnitTests/PreUpdateTests.cpp b/SurgSim/Physics/UnitTests/PreUpdateTests.cpp
new file mode 100644
index 0000000..6649cd8
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/PreUpdateTests.cpp
@@ -0,0 +1,66 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 PreUpdateTests.cpp
+/// Simple Test for PreUpdate calculation
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/PreUpdate.h"
+#include "SurgSim/Physics/Representation.h"
+
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+
+TEST(PreUpdateTest, UpdateCallTest)
+{
+ std::shared_ptr<MockRepresentation> mockRepresentation = std::make_shared<MockRepresentation>();
+ std::shared_ptr<PhysicsManagerState> physicsManagerState = std::make_shared<PhysicsManagerState>();
+ std::vector<std::shared_ptr<Representation>> allRepresentations;
+
+ allRepresentations.push_back(mockRepresentation);
+ physicsManagerState->setRepresentations(allRepresentations);
+
+ double dt = 1e-3;
+ std::shared_ptr<PreUpdate> preUpdateComputation = std::make_shared<PreUpdate>();
+ EXPECT_EQ(0, mockRepresentation->getPreUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getPostUpdateCount());
+ preUpdateComputation->update(dt, physicsManagerState);
+ EXPECT_EQ(1, mockRepresentation->getPreUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getPostUpdateCount());
+ preUpdateComputation->update(dt, physicsManagerState);
+ EXPECT_EQ(2, mockRepresentation->getPreUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getPostUpdateCount());
+ preUpdateComputation->update(dt, physicsManagerState);
+ EXPECT_EQ(3, mockRepresentation->getPreUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getUpdateCount());
+ EXPECT_EQ(0, mockRepresentation->getPostUpdateCount());
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
+
diff --git a/SurgSim/Physics/UnitTests/PushResultsTests.cpp b/SurgSim/Physics/UnitTests/PushResultsTests.cpp
new file mode 100644
index 0000000..081e57e
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/PushResultsTests.cpp
@@ -0,0 +1,492 @@
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 PushResultsTests.cpp
+/// Simple Test for PushResults calculation
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "SurgSim/Physics/UnitTests/CommonTests.h"
+#include "SurgSim/Physics/BuildMlcp.h"
+#include "SurgSim/Physics/PushResults.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class PushResultsTests : public CommonTests
+{
+public:
+ void SetUp()
+ {
+ CommonTests::SetUp();
+
+ // Create the BuildMlcp computation
+ m_pushResultsComputation = std::make_shared<PushResults>();
+ }
+
+protected:
+
+ /// The Push Results computation
+ std::shared_ptr<PushResults> m_pushResultsComputation;
+};
+
+void updateRepresentationsMapping(std::shared_ptr<PhysicsManagerState> state)
+{
+ // The BuildMlcp computation build the representations mapping. So it is called.
+ auto buildMlcpComputation = std::make_shared<BuildMlcp>();
+ buildMlcpComputation->update(0.0, state);
+}
+
+TEST_F(PushResultsTests, NoRepresentationNoConstraint)
+{
+ // Run the BuildMlcp computation...
+ ASSERT_NO_THROW(m_pushResultsComputation->update(dt, m_physicsManagerState));
+}
+
+TEST_F(PushResultsTests, OneRepresentationNoConstraintTest)
+{
+ // Prep the list of representations: use only 1 representation
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Run the BuildMlcp computation...
+ ASSERT_NO_THROW(m_pushResultsComputation->update(dt, m_physicsManagerState));
+}
+
+TEST_F(PushResultsTests, TwoRepresentationsNoConstraintTest)
+{
+ // Prep the list of representations: use 2 representations
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ m_usedRepresentations.push_back(m_allRepresentations[1]);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Run the BuildMlcp computation...
+ ASSERT_NO_THROW(m_pushResultsComputation->update(dt, m_physicsManagerState));
+}
+
+TEST_F(PushResultsTests, OneRepresentationOneConstraintTest)
+{
+ // Prep the list of representations: use only 1 rigid representation + 1 fixed
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ m_usedRepresentations.push_back(m_fixedWorldRepresentation);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Prep the list of constraints: use only 1 constraint
+ {
+ std::shared_ptr<Localization> rigidLocalization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Zero());
+ rigidLocalization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSideContact;
+ rigidSideContact = std::make_shared<RigidRepresentationContact>();
+
+ std::shared_ptr<Localization> fixedLocalization;
+ {
+ std::shared_ptr<FixedRepresentationLocalization> fixedLocalizationTyped;
+ fixedLocalizationTyped = std::make_shared<FixedRepresentationLocalization>();
+ fixedLocalizationTyped->setRepresentation(m_fixedWorldRepresentation);
+ fixedLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Zero());
+ fixedLocalization = fixedLocalizationTyped;
+ }
+ std::shared_ptr<FixedRepresentationContact> fixedSideContact;
+ fixedSideContact = std::make_shared<FixedRepresentationContact>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(SurgSim::Math::Vector3d(0.0, 1.0, 0.0), 0.0);
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSideContact, rigidLocalization, fixedSideContact, fixedLocalization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+
+ // Set the constraint list in the Physics Manager State
+ m_physicsManagerState->setConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT, m_usedConstraints);
+
+ // Update the Representations mapping.
+ updateRepresentationsMapping(m_physicsManagerState);
+
+ // Fill up the Mlcp problem and clear up the Mlcp solution
+ resetMlcpProblem(6, 1);
+ MlcpPhysicsProblem& mlcpProblem = m_physicsManagerState->getMlcpProblem();
+ MlcpPhysicsSolution& mlcpSolution = m_physicsManagerState->getMlcpSolution();
+ {
+ mlcpProblem.CHt(0, 0) = 0.0;
+ mlcpProblem.CHt(1, 0) = 1.0;
+ mlcpProblem.CHt(2, 0) = 2.0;
+ mlcpProblem.CHt(3, 0) = 3.0;
+ mlcpProblem.CHt(4, 0) = 4.0;
+ mlcpProblem.CHt(5, 0) = 5.0;
+
+ mlcpSolution.x(0) = 1.3;
+ }
+
+ // Run the BuildMlcp computation...
+ ASSERT_NO_THROW(m_pushResultsComputation->update(dt, m_physicsManagerState));
+
+ // Test that the Mlcp is as expected
+ EXPECT_EQ(1, mlcpSolution.x.rows());
+ EXPECT_NEAR(1.3, mlcpSolution.x(0), epsilon);
+ EXPECT_EQ(6, mlcpSolution.dofCorrection.rows());
+ EXPECT_NEAR(1.3 * 0.0, mlcpSolution.dofCorrection(0), epsilon);
+ EXPECT_NEAR(1.3 * 1.0, mlcpSolution.dofCorrection(1), epsilon);
+ EXPECT_NEAR(1.3 * 2.0, mlcpSolution.dofCorrection(2), epsilon);
+ EXPECT_NEAR(1.3 * 3.0, mlcpSolution.dofCorrection(3), epsilon);
+ EXPECT_NEAR(1.3 * 4.0, mlcpSolution.dofCorrection(4), epsilon);
+ EXPECT_NEAR(1.3 * 5.0, mlcpSolution.dofCorrection(5), epsilon);
+
+ std::shared_ptr<RigidRepresentation> rigid;
+ rigid = std::static_pointer_cast<RigidRepresentation>(m_usedRepresentations[0]);
+ const SurgSim::Math::Vector3d& linVel = rigid->getCurrentState().getLinearVelocity();
+ const SurgSim::Math::Vector3d& angVel = rigid->getCurrentState().getAngularVelocity();
+ EXPECT_NEAR(1.3 * 0.0, linVel[0], epsilon);
+ EXPECT_NEAR(1.3 * 1.0, linVel[1], epsilon);
+ EXPECT_NEAR(1.3 * 2.0, linVel[2], epsilon);
+ EXPECT_NEAR(1.3 * 3.0, angVel[0], epsilon);
+ EXPECT_NEAR(1.3 * 4.0, angVel[1], epsilon);
+ EXPECT_NEAR(1.3 * 5.0, angVel[2], epsilon);
+
+ const SurgSim::Math::RigidTransform3d& pose = rigid->getCurrentState().getPose();
+ EXPECT_NEAR(1.3 * 0.0 * dt, pose.translation()[0], epsilon);
+ EXPECT_NEAR(1.3 * 1.0 * dt, pose.translation()[1], epsilon);
+ EXPECT_NEAR(1.3 * 2.0 * dt, pose.translation()[2], epsilon);
+}
+
+TEST_F(PushResultsTests, OneRepresentationTwoConstraintsTest)
+{
+ // Prep the list of representations: use only 1 rigid representation + 1 fixed
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ m_usedRepresentations.push_back(m_fixedWorldRepresentation);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Prep the list of constraints: use 2 constraints
+ {
+ std::shared_ptr<Localization> rigidLocalization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Zero());
+ rigidLocalization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSideContact;
+ rigidSideContact = std::make_shared<RigidRepresentationContact>();
+
+ std::shared_ptr<Localization> fixedLocalization;
+ {
+ std::shared_ptr<FixedRepresentationLocalization> fixedLocalizationTyped;
+ fixedLocalizationTyped = std::make_shared<FixedRepresentationLocalization>();
+ fixedLocalizationTyped->setRepresentation(m_fixedWorldRepresentation);
+ fixedLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Zero());
+ fixedLocalization = fixedLocalizationTyped;
+ }
+ std::shared_ptr<FixedRepresentationContact> fixedSideContact;
+ fixedSideContact = std::make_shared<FixedRepresentationContact>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(SurgSim::Math::Vector3d(0.0, 1.0, 0.0), 0.0);
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSideContact, rigidLocalization, fixedSideContact, fixedLocalization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+ {
+ std::shared_ptr<Localization> rigidLocalization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Ones());
+ rigidLocalization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSideContact;
+ rigidSideContact = std::make_shared<RigidRepresentationContact>();
+
+ std::shared_ptr<Localization> fixedLocalization;
+ {
+ std::shared_ptr<FixedRepresentationLocalization> fixedLocalizationTyped;
+ fixedLocalizationTyped = std::make_shared<FixedRepresentationLocalization>();
+ fixedLocalizationTyped->setRepresentation(m_fixedWorldRepresentation);
+ fixedLocalizationTyped->setLocalPosition(SurgSim::Math::Vector3d::Ones());
+ fixedLocalization = fixedLocalizationTyped;
+ }
+ std::shared_ptr<FixedRepresentationContact> fixedSideContact;
+ fixedSideContact = std::make_shared<FixedRepresentationContact>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(SurgSim::Math::Vector3d(0.0, 1.0, 0.0), 0.0);
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSideContact, rigidLocalization, fixedSideContact, fixedLocalization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+
+ // Set the constraint list in the Physics Manager State
+ m_physicsManagerState->setConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT, m_usedConstraints);
+
+ // Update the Representations mapping.
+ updateRepresentationsMapping(m_physicsManagerState);
+
+ // Fill up the Mlcp problem and clear up the Mlcp solution
+ resetMlcpProblem(6, 2);
+ MlcpPhysicsProblem& mlcpProblem = m_physicsManagerState->getMlcpProblem();
+ MlcpPhysicsSolution& mlcpSolution = m_physicsManagerState->getMlcpSolution();
+ {
+ for (int dofId = 0; dofId < 6; dofId++)
+ {
+ mlcpProblem.CHt(dofId, 0) = static_cast<double>(dofId);
+ mlcpProblem.CHt(dofId, 1) = static_cast<double>(dofId + 1);
+ }
+
+ mlcpSolution.x(0) = 1.3;
+ mlcpSolution.x(1) =-0.9;
+ }
+
+ // Run the BuildMlcp computation...
+ ASSERT_NO_THROW(m_pushResultsComputation->update(dt, m_physicsManagerState));
+
+ // Test that the Mlcp is as expected
+ EXPECT_EQ(2, mlcpSolution.x.rows());
+ EXPECT_NEAR( 1.3, mlcpSolution.x(0), epsilon);
+ EXPECT_NEAR(-0.9, mlcpSolution.x(1), epsilon);
+ // dofCorrection = CHt .x = (0 1) . ( 1.3)
+ // (1 2) (-0.9)
+ // (2 3)
+ // (3 4)
+ // (4 5)
+ // (5 6)
+ EXPECT_EQ(6, mlcpSolution.dofCorrection.rows());
+ EXPECT_NEAR(1.3 * 0.0 - 0.9 * 1.0, mlcpSolution.dofCorrection(0), epsilon);
+ EXPECT_NEAR(1.3 * 1.0 - 0.9 * 2.0, mlcpSolution.dofCorrection(1), epsilon);
+ EXPECT_NEAR(1.3 * 2.0 - 0.9 * 3.0, mlcpSolution.dofCorrection(2), epsilon);
+ EXPECT_NEAR(1.3 * 3.0 - 0.9 * 4.0, mlcpSolution.dofCorrection(3), epsilon);
+ EXPECT_NEAR(1.3 * 4.0 - 0.9 * 5.0, mlcpSolution.dofCorrection(4), epsilon);
+ EXPECT_NEAR(1.3 * 5.0 - 0.9 * 6.0, mlcpSolution.dofCorrection(5), epsilon);
+
+ std::shared_ptr<RigidRepresentation> rigid;
+ rigid = std::static_pointer_cast<RigidRepresentation>(m_usedRepresentations[0]);
+ const SurgSim::Math::Vector3d& linVel = rigid->getCurrentState().getLinearVelocity();
+ const SurgSim::Math::Vector3d& angVel = rigid->getCurrentState().getAngularVelocity();
+ EXPECT_NEAR(1.3 * 0.0 - 0.9 * 1.0, linVel[0], epsilon);
+ EXPECT_NEAR(1.3 * 1.0 - 0.9 * 2.0, linVel[1], epsilon);
+ EXPECT_NEAR(1.3 * 2.0 - 0.9 * 3.0, linVel[2], epsilon);
+ EXPECT_NEAR(1.3 * 3.0 - 0.9 * 4.0, angVel[0], epsilon);
+ EXPECT_NEAR(1.3 * 4.0 - 0.9 * 5.0, angVel[1], epsilon);
+ EXPECT_NEAR(1.3 * 5.0 - 0.9 * 6.0, angVel[2], epsilon);
+
+ const SurgSim::Math::RigidTransform3d& pose = rigid->getCurrentState().getPose();
+ EXPECT_NEAR((1.3 * 0.0 - 0.9 * 1.0) * dt, pose.translation()[0], epsilon);
+ EXPECT_NEAR((1.3 * 1.0 - 0.9 * 2.0) * dt, pose.translation()[1], epsilon);
+ EXPECT_NEAR((1.3 * 2.0 - 0.9 * 3.0) * dt, pose.translation()[2], epsilon);
+}
+
+TEST_F(PushResultsTests, TwoRepresentationsTwoConstraintsTest)
+{
+ SurgSim::Math::Vector3d pointOrigin = SurgSim::Math::Vector3d::Zero();
+ SurgSim::Math::Vector3d planeDirection(0.0, 1.0, 0.0);
+ double planeDistance = 0.0;
+ SurgSim::Math::Vector3d pointOne = planeDirection * 1.0;
+
+ // Prep the list of representations: use only 1 rigid representation + 1 fixed
+ m_usedRepresentations.push_back(m_allRepresentations[0]);
+ m_usedRepresentations.push_back(m_allRepresentations[1]);
+ // Set the representation list in the Physics Manager State
+ m_physicsManagerState->setRepresentations(m_usedRepresentations);
+
+ // Prep the list of constraints: use 2 constraints
+ {
+ std::shared_ptr<Localization> rigid1Localization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(pointOrigin);
+ rigid1Localization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSide1Contact;
+ rigidSide1Contact = std::make_shared<RigidRepresentationContact>();
+
+ std::shared_ptr<Localization> rigid2Localization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[1]);
+ rigidLocalizationTyped->setLocalPosition(pointOrigin);
+ rigid2Localization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSide2Contact;
+ rigidSide2Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(planeDirection, planeDistance);
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSide1Contact, rigid1Localization, rigidSide2Contact, rigid2Localization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+ {
+ std::shared_ptr<Localization> rigid1Localization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[0]);
+ rigidLocalizationTyped->setLocalPosition(pointOrigin);
+ rigid1Localization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSide1Contact;
+ rigidSide1Contact = std::make_shared<RigidRepresentationContact>();
+
+ std::shared_ptr<Localization> rigid2Localization;
+ {
+ std::shared_ptr<RigidRepresentationLocalization> rigidLocalizationTyped;
+ rigidLocalizationTyped = std::make_shared<RigidRepresentationLocalization>();
+ rigidLocalizationTyped->setRepresentation(m_usedRepresentations[1]);
+ rigidLocalizationTyped->setLocalPosition(pointOne);
+ rigid2Localization = rigidLocalizationTyped;
+ }
+ std::shared_ptr<RigidRepresentationContact> rigidSide2Contact;
+ rigidSide2Contact = std::make_shared<RigidRepresentationContact>();
+
+ // Define the constraint specific data
+ std::shared_ptr<ContactConstraintData> data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(planeDirection, planeDistance);
+
+ // Set up the constraint
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
+ data, rigidSide1Contact, rigid1Localization, rigidSide2Contact, rigid2Localization);
+
+ // Register the constraint in the list of used constraints for this test
+ m_usedConstraints.push_back(constraint);
+ }
+
+ // Set the constraint list in the Physics Manager State
+ m_physicsManagerState->setConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT, m_usedConstraints);
+
+ // Update the Representations mapping.
+ updateRepresentationsMapping(m_physicsManagerState);
+
+ // Fill up the Mlcp problem and clear up the Mlcp solution
+ resetMlcpProblem(12, 2);
+ MlcpPhysicsProblem& mlcpProblem = m_physicsManagerState->getMlcpProblem();
+ MlcpPhysicsSolution& mlcpSolution = m_physicsManagerState->getMlcpSolution();
+ {
+ for (int dofId = 0; dofId < 12; dofId++)
+ {
+ mlcpProblem.CHt(dofId, 0) = static_cast<double>(dofId);
+ mlcpProblem.CHt(dofId, 1) = static_cast<double>(dofId + 1);
+ }
+
+ mlcpSolution.x(0) = 1.3;
+ mlcpSolution.x(1) =-0.9;
+ }
+
+ // Run the BuildMlcp computation...
+ ASSERT_NO_THROW(m_pushResultsComputation->update(dt, m_physicsManagerState));
+
+ // Test that the Mlcp is as expected
+ EXPECT_EQ(2, mlcpSolution.x.rows());
+ EXPECT_NEAR( 1.3, mlcpSolution.x(0), epsilon);
+ EXPECT_NEAR(-0.9, mlcpSolution.x(1), epsilon);
+ // dofCorrection = CHt .x = ( 0 1) . ( 1.3)
+ // ( 1 2) (-0.9)
+ // ( 2 3)
+ // ( 3 4)
+ // ( 4 5)
+ // ( 5 6)
+ // ( 6 7)
+ // ( 7 8)
+ // ( 8 9)
+ // ( 9 10)
+ // (10 11)
+ // (11 12)
+ EXPECT_EQ(12, mlcpSolution.dofCorrection.rows());
+ EXPECT_NEAR(1.3 * 0.0 - 0.9 * 1.0, mlcpSolution.dofCorrection(0), epsilon);
+ EXPECT_NEAR(1.3 * 1.0 - 0.9 * 2.0, mlcpSolution.dofCorrection(1), epsilon);
+ EXPECT_NEAR(1.3 * 2.0 - 0.9 * 3.0, mlcpSolution.dofCorrection(2), epsilon);
+ EXPECT_NEAR(1.3 * 3.0 - 0.9 * 4.0, mlcpSolution.dofCorrection(3), epsilon);
+ EXPECT_NEAR(1.3 * 4.0 - 0.9 * 5.0, mlcpSolution.dofCorrection(4), epsilon);
+ EXPECT_NEAR(1.3 * 5.0 - 0.9 * 6.0, mlcpSolution.dofCorrection(5), epsilon);
+
+ EXPECT_NEAR(1.3 * 6.0 - 0.9 * 7.0, mlcpSolution.dofCorrection(6), epsilon);
+ EXPECT_NEAR(1.3 * 7.0 - 0.9 * 8.0, mlcpSolution.dofCorrection(7), epsilon);
+ EXPECT_NEAR(1.3 * 8.0 - 0.9 * 9.0, mlcpSolution.dofCorrection(8), epsilon);
+ EXPECT_NEAR(1.3 * 9.0 - 0.9 * 10.0, mlcpSolution.dofCorrection(9), epsilon);
+ EXPECT_NEAR(1.3 * 10.0 - 0.9 * 11.0, mlcpSolution.dofCorrection(10), epsilon);
+ EXPECT_NEAR(1.3 * 11.0 - 0.9 * 12.0, mlcpSolution.dofCorrection(11), epsilon);
+
+ {
+ std::shared_ptr<RigidRepresentation> rigid;
+ rigid = std::static_pointer_cast<RigidRepresentation>(m_usedRepresentations[0]);
+ const SurgSim::Math::Vector3d& linVel = rigid->getCurrentState().getLinearVelocity();
+ const SurgSim::Math::Vector3d& angVel = rigid->getCurrentState().getAngularVelocity();
+ EXPECT_NEAR(1.3 * 0.0 - 0.9 * 1.0, linVel[0], epsilon);
+ EXPECT_NEAR(1.3 * 1.0 - 0.9 * 2.0, linVel[1], epsilon);
+ EXPECT_NEAR(1.3 * 2.0 - 0.9 * 3.0, linVel[2], epsilon);
+ EXPECT_NEAR(1.3 * 3.0 - 0.9 * 4.0, angVel[0], epsilon);
+ EXPECT_NEAR(1.3 * 4.0 - 0.9 * 5.0, angVel[1], epsilon);
+ EXPECT_NEAR(1.3 * 5.0 - 0.9 * 6.0, angVel[2], epsilon);
+ const SurgSim::Math::RigidTransform3d& pose = rigid->getCurrentState().getPose();
+ EXPECT_NEAR((1.3 * 0.0 - 0.9 * 1.0) * dt, pose.translation()[0], epsilon);
+ EXPECT_NEAR((1.3 * 1.0 - 0.9 * 2.0) * dt, pose.translation()[1], epsilon);
+ EXPECT_NEAR((1.3 * 2.0 - 0.9 * 3.0) * dt, pose.translation()[2], epsilon);
+ }
+
+ {
+ std::shared_ptr<RigidRepresentation> rigid;
+ rigid = std::static_pointer_cast<RigidRepresentation>(m_usedRepresentations[1]);
+ const SurgSim::Math::Vector3d& linVel = rigid->getCurrentState().getLinearVelocity();
+ const SurgSim::Math::Vector3d& angVel = rigid->getCurrentState().getAngularVelocity();
+ EXPECT_NEAR(1.3 * 6.0 - 0.9 * 7.0, linVel[0], epsilon);
+ EXPECT_NEAR(1.3 * 7.0 - 0.9 * 8.0, linVel[1], epsilon);
+ EXPECT_NEAR(1.3 * 8.0 - 0.9 * 9.0, linVel[2], epsilon);
+ EXPECT_NEAR(1.3 * 9.0 - 0.9 * 10.0, angVel[0], epsilon);
+ EXPECT_NEAR(1.3 * 10.0 - 0.9 * 11.0, angVel[1], epsilon);
+ EXPECT_NEAR(1.3 * 11.0 - 0.9 * 12.0, angVel[2], epsilon);
+ const SurgSim::Math::RigidTransform3d& pose = rigid->getCurrentState().getPose();
+ EXPECT_NEAR((1.3 * 6.0 - 0.9 * 7.0) * dt, pose.translation()[0], epsilon);
+ EXPECT_NEAR((1.3 * 7.0 - 0.9 * 8.0) * dt, pose.translation()[1], epsilon);
+ EXPECT_NEAR((1.3 * 8.0 - 0.9 * 9.0) * dt, pose.translation()[2], epsilon);
+ }
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/RepresentationTest.cpp b/SurgSim/Physics/UnitTests/RepresentationTest.cpp
new file mode 100644
index 0000000..38949dd
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RepresentationTest.cpp
@@ -0,0 +1,186 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::Physics::MockRepresentation;
+using SurgSim::Physics::Representation;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Physics::RigidCollisionRepresentation;
+using SurgSim::Physics::MockRepresentation;
+
+
+TEST(RepresentationTest, ConstructorTest)
+{
+ ASSERT_NO_THROW({MockRepresentation representation;});
+}
+
+TEST(RepresentationTest, SetGetAndDefaultValueTest)
+{
+ /// Create the representation
+ std::shared_ptr<Representation> representation = std::make_shared<MockRepresentation>();
+
+ /// Get/Set active flag [default = true]
+ EXPECT_TRUE(representation->isActive());
+ representation->setLocalActive(false);
+ ASSERT_FALSE(representation->isLocalActive());
+ ASSERT_FALSE(representation->isActive());
+ representation->setLocalActive(true);
+ ASSERT_TRUE(representation->isLocalActive());
+ ASSERT_TRUE(representation->isActive());
+
+ /// Get numDof = 0
+ ASSERT_EQ(0u, representation->getNumDof());
+
+ /// Set/Get isGravityEnabled [default = true]
+ EXPECT_TRUE(representation->isGravityEnabled());
+ representation->setIsGravityEnabled(false);
+ ASSERT_FALSE(representation->isGravityEnabled());
+ representation->setIsGravityEnabled(true);
+ ASSERT_TRUE(representation->isGravityEnabled());
+
+ /// Set/Get isDrivingSceneElementPose [default = true]
+ EXPECT_TRUE(representation->isDrivingSceneElementPose());
+ representation->setIsDrivingSceneElementPose(false);
+ ASSERT_FALSE(representation->isDrivingSceneElementPose());
+ representation->setIsDrivingSceneElementPose(true);
+ ASSERT_TRUE(representation->isDrivingSceneElementPose());
+}
+
+TEST(RepresentationTest, SetGetCollisionRepresentationTest)
+{
+ std::shared_ptr<Representation> physicsRepresentation = std::make_shared<MockRepresentation>("MockRepresentation");
+ auto collisionRepresentation = std::make_shared<RigidCollisionRepresentation>("CollisionRepresentatoin");
+
+ EXPECT_NO_THROW(physicsRepresentation->setCollisionRepresentation(collisionRepresentation));
+ EXPECT_EQ(collisionRepresentation, physicsRepresentation->getCollisionRepresentation());
+}
+
+TEST(RepresentationTest, SerializationTest)
+{
+ {
+ SCOPED_TRACE("Encode instance, decoded as shared_ptr<>");
+ std::shared_ptr<Representation> representation = std::make_shared<MockRepresentation>("MockRepresentation");
+ size_t numDof = 1;
+ representation->setValue("NumDof", numDof);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*representation));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Physics::MockRepresentation"];
+ EXPECT_EQ(7u, data.size());
+
+ std::shared_ptr<MockRepresentation> newRepresentation;
+ ASSERT_NO_THROW(newRepresentation =
+ std::dynamic_pointer_cast<MockRepresentation>(node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_EQ(representation->getName(), newRepresentation->getName());
+ EXPECT_EQ("SurgSim::Physics::MockRepresentation", newRepresentation->getClassName());
+ EXPECT_TRUE(newRepresentation->getValue<bool>("IsLocalActive"));
+ EXPECT_TRUE(newRepresentation->getValue<bool>("IsGravityEnabled"));
+ EXPECT_TRUE(newRepresentation->getValue<bool>("IsDrivingSceneElementPose"));
+ EXPECT_EQ(1u, newRepresentation->getValue<size_t>("NumDof"));
+ }
+
+ {
+ SCOPED_TRACE("Encode shared_ptr<>, decoded as shared_ptr<>");
+ std::shared_ptr<Representation> representation = std::make_shared<MockRepresentation>("MockRepresentation");
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::encode(representation));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Physics::MockRepresentation"];
+ EXPECT_EQ(2u, data.size());
+
+ std::shared_ptr<MockRepresentation> newRepresentation;
+ ASSERT_NO_THROW(newRepresentation =
+ std::dynamic_pointer_cast<MockRepresentation>(node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_NE(nullptr, newRepresentation);
+ }
+
+ {
+ SCOPED_TRACE("Test serialization for accesible boolean properties");
+ std::shared_ptr<Representation> representation1 = std::make_shared<MockRepresentation>("MockRepresentation1");
+ std::shared_ptr<Representation> representation2 = std::make_shared<MockRepresentation>("MockRepresentation2");
+ std::shared_ptr<Representation> representation3 = std::make_shared<MockRepresentation>("MockRepresentation3");
+ std::shared_ptr<Representation> representation4 = std::make_shared<MockRepresentation>("MockRepresentation4");
+
+ representation1->setValue("IsLocalActive", false);
+
+ representation2->setValue("IsGravityEnabled", false);
+
+ representation3->setValue("IsDrivingSceneElementPose", false);
+
+ representation4->setValue("IsLocalActive", false);
+ representation4->setValue("IsGravityEnabled", false);
+ representation4->setValue("IsDrivingSceneElementPose", false);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node.push_back(YAML::convert<SurgSim::Framework::Component>::encode(*representation1)));
+ ASSERT_NO_THROW(node.push_back(YAML::convert<SurgSim::Framework::Component>::encode(*representation2)));
+ ASSERT_NO_THROW(node.push_back(YAML::convert<SurgSim::Framework::Component>::encode(*representation3)));
+ ASSERT_NO_THROW(node.push_back(YAML::convert<SurgSim::Framework::Component>::encode(*representation4)));
+
+ std::shared_ptr<MockRepresentation> newRepresentation1;
+ ASSERT_NO_THROW(newRepresentation1 =
+ std::dynamic_pointer_cast<MockRepresentation>(node[0].as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ std::shared_ptr<MockRepresentation> newRepresentation2;
+ ASSERT_NO_THROW(newRepresentation2 =
+ std::dynamic_pointer_cast<MockRepresentation>(node[1].as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ std::shared_ptr<MockRepresentation> newRepresentation3;
+ ASSERT_NO_THROW(newRepresentation3 =
+ std::dynamic_pointer_cast<MockRepresentation>(node[2].as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ std::shared_ptr<MockRepresentation> newRepresentation4;
+ ASSERT_NO_THROW(newRepresentation4 =
+ std::dynamic_pointer_cast<MockRepresentation>(node[3].as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_EQ(representation1->getName(), newRepresentation1->getName());
+ EXPECT_EQ(representation2->getName(), newRepresentation2->getName());
+ EXPECT_EQ(representation3->getName(), newRepresentation3->getName());
+ EXPECT_EQ(representation4->getName(), newRepresentation4->getName());
+
+ EXPECT_FALSE(newRepresentation1->getValue<bool>("IsLocalActive"));
+ EXPECT_TRUE(newRepresentation1->getValue<bool>("IsGravityEnabled"));
+ EXPECT_TRUE(newRepresentation1->getValue<bool>("IsDrivingSceneElementPose"));
+
+ EXPECT_TRUE(newRepresentation2->getValue<bool>("IsLocalActive"));
+ EXPECT_FALSE(newRepresentation2->getValue<bool>("IsGravityEnabled"));
+ EXPECT_TRUE(newRepresentation2->getValue<bool>("IsDrivingSceneElementPose"));
+
+ EXPECT_TRUE(newRepresentation3->getValue<bool>("IsLocalActive"));
+ EXPECT_TRUE(newRepresentation3->getValue<bool>("IsGravityEnabled"));
+ EXPECT_FALSE(newRepresentation3->getValue<bool>("IsDrivingSceneElementPose"));
+
+ EXPECT_FALSE(newRepresentation4->getValue<bool>("IsLocalActive"));
+ EXPECT_FALSE(newRepresentation4->getValue<bool>("IsGravityEnabled"));
+ EXPECT_FALSE(newRepresentation4->getValue<bool>("IsDrivingSceneElementPose"));
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/RigidCollisionRepresentationTest.cpp b/SurgSim/Physics/UnitTests/RigidCollisionRepresentationTest.cpp
new file mode 100644
index 0000000..c7a36f2
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidCollisionRepresentationTest.cpp
@@ -0,0 +1,215 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+static const double dt = 0.001;
+const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+struct RigidCollisionRepresentationTest : public ::testing::Test
+{
+ void SetUp()
+ {
+ m_sphereShape = std::make_shared<SurgSim::Math::SphereShape>(1.0);
+ m_rigidRepresentation = std::make_shared<RigidRepresentation>("RigidRepresentation");
+ m_rigidRepresentation->setShape(m_sphereShape);
+ }
+
+ std::shared_ptr<SurgSim::Math::SphereShape> m_sphereShape;
+ std::shared_ptr<SurgSim::Physics::RigidCollisionRepresentation> m_rigidCollisionRepresentation;
+ std::shared_ptr<SurgSim::Physics::RigidRepresentation> m_rigidRepresentation;
+};
+
+TEST_F(RigidCollisionRepresentationTest, InitTest)
+{
+ EXPECT_NO_THROW(RigidCollisionRepresentation("TestRigidCollisionRepresentation"));
+
+ EXPECT_NO_THROW(m_rigidCollisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("RigidCollisionRepresentation")
+ );
+}
+
+TEST_F(RigidCollisionRepresentationTest, SetGetRigidRepresentationTest)
+{
+ m_rigidCollisionRepresentation = std::make_shared<RigidCollisionRepresentation>("RigidCollisionRepresentation");
+ ASSERT_NO_THROW(m_rigidCollisionRepresentation->setRigidRepresentation(m_rigidRepresentation));
+ EXPECT_EQ(m_rigidRepresentation, m_rigidCollisionRepresentation->getRigidRepresentation());
+}
+
+TEST_F(RigidCollisionRepresentationTest, ShapeTest)
+{
+ m_rigidCollisionRepresentation = std::make_shared<RigidCollisionRepresentation>("RigidCollisionRepresentation");
+ ASSERT_ANY_THROW(m_rigidCollisionRepresentation->getShape());
+
+ m_rigidCollisionRepresentation->setRigidRepresentation(m_rigidRepresentation);
+
+ EXPECT_EQ(m_sphereShape, m_rigidCollisionRepresentation->getShape());
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_SPHERE, m_rigidCollisionRepresentation->getShapeType());
+
+ std::shared_ptr<SurgSim::Math::BoxShape> boxShape = std::make_shared<SurgSim::Math::BoxShape>(1.0, 1.0, 1.0);
+ m_rigidCollisionRepresentation->setShape(boxShape);
+ EXPECT_EQ(boxShape, m_rigidCollisionRepresentation->getShape());
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_BOX, m_rigidCollisionRepresentation->getShapeType());
+}
+
+TEST_F(RigidCollisionRepresentationTest, PoseTest)
+{
+ m_rigidCollisionRepresentation = std::make_shared<RigidCollisionRepresentation>("RigidCollisionRepresentation");
+ m_rigidCollisionRepresentation->setRigidRepresentation(m_rigidRepresentation);
+
+ SurgSim::Math::Quaterniond rotation(1.0, 2.0, 3.0, 4.0);
+ SurgSim::Math::Vector3d translation(11.0, 12.0, 13.0);
+ SurgSim::Math::RigidTransform3d pose = SurgSim::Math::makeRigidTransform(rotation, translation);
+ m_rigidCollisionRepresentation->setLocalPose(pose);
+ EXPECT_TRUE(pose.isApprox(m_rigidCollisionRepresentation->getPose()));
+}
+
+TEST_F(RigidCollisionRepresentationTest, SerializationTest)
+{
+ {
+ SCOPED_TRACE("RigidCollisionRepresenation must have a shape.");
+ auto rigidCollisionRepresentation = std::make_shared<RigidCollisionRepresentation>("CollisionRepresentation");
+
+ YAML::Node node;
+ // Same as call YAML::convert<SurgSim::Framework::Component>::encode(rigidCollisionRepresentation);
+ // Encode RigidCollisionRepresentation as reference has no problem.
+ ASSERT_NO_THROW(node = rigidCollisionRepresentation);
+
+ // Encode RigidCollisionRepresentation as concrete object will throw.
+ // It's because RigidCollisionRepresentation::getShape() will throw
+ // if no shape is assigned and no PhysicsRepresentation is connected.
+ ASSERT_ANY_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*rigidCollisionRepresentation));
+ }
+ {
+ SCOPED_TRACE("RigidCollisionRepresenation uses a shape directly.");
+ auto rigidCollisionRepresentation = std::make_shared<RigidCollisionRepresentation>("CollisionRepresentation");
+ std::shared_ptr<SurgSim::Math::Shape> shape = std::make_shared<SurgSim::Math::BoxShape>(0.1, 0.1, 0.1);
+ rigidCollisionRepresentation->setValue("Shape", shape);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*rigidCollisionRepresentation));
+ YAML::Node data = node["SurgSim::Physics::RigidCollisionRepresentation"];
+ EXPECT_EQ(5u, data.size());
+
+ std::shared_ptr<SurgSim::Physics::RigidCollisionRepresentation> newRepresentation;
+ ASSERT_NO_THROW(newRepresentation =
+ std::dynamic_pointer_cast<SurgSim::Physics::RigidCollisionRepresentation>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>())
+ );
+ EXPECT_EQ("SurgSim::Physics::RigidCollisionRepresentation", newRepresentation->getClassName());
+ auto boxShape = std::dynamic_pointer_cast<SurgSim::Math::BoxShape>(shape);
+ auto newBoxShape = std::dynamic_pointer_cast<SurgSim::Math::BoxShape>(
+ newRepresentation->getValue<std::shared_ptr<SurgSim::Math::Shape>>("Shape"));
+ ASSERT_NE(nullptr, newBoxShape);
+
+ EXPECT_EQ(boxShape->getType(), newRepresentation->getShapeType());
+ EXPECT_TRUE(boxShape->getSize().isApprox(newBoxShape->getSize()));
+ EXPECT_TRUE(boxShape->getCenter().isApprox(newBoxShape->getCenter()));
+ EXPECT_TRUE(boxShape->getSecondMomentOfVolume().isApprox(newBoxShape->getSecondMomentOfVolume()));
+ EXPECT_DOUBLE_EQ(boxShape->getVolume(), newBoxShape->getVolume());
+ }
+ {
+ SCOPED_TRACE("RigidCollisionRepresenation uses the shape in PhysicsRepresentation.");
+ auto rigidCollisionRepresentation = std::make_shared<RigidCollisionRepresentation>("CollisionRepresentation");
+ m_rigidRepresentation->setCollisionRepresentation(rigidCollisionRepresentation);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*rigidCollisionRepresentation));
+ YAML::Node data = node["SurgSim::Physics::RigidCollisionRepresentation"];
+ EXPECT_EQ(5u, data.size());
+
+ std::shared_ptr<SurgSim::Physics::RigidCollisionRepresentation> newRepresentation;
+ ASSERT_NO_THROW(newRepresentation =
+ std::dynamic_pointer_cast<SurgSim::Physics::RigidCollisionRepresentation>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>())
+ );
+ EXPECT_EQ("SurgSim::Physics::RigidCollisionRepresentation", newRepresentation->getClassName());
+ EXPECT_EQ(m_sphereShape->getType(), newRepresentation->getShapeType());
+
+ auto newShpereShape = std::dynamic_pointer_cast<SurgSim::Math::SphereShape>(
+ newRepresentation->getValue<std::shared_ptr<SurgSim::Math::Shape>>("Shape"));
+ ASSERT_NE(nullptr, newShpereShape);
+
+ EXPECT_TRUE(m_sphereShape->getCenter().isApprox(newShpereShape->getCenter()));
+ EXPECT_TRUE(m_sphereShape->getSecondMomentOfVolume().isApprox(newShpereShape->getSecondMomentOfVolume()));
+ EXPECT_DOUBLE_EQ(m_sphereShape->getVolume(), newShpereShape->getVolume());
+ EXPECT_DOUBLE_EQ(m_sphereShape->getRadius(), newShpereShape->getRadius());
+ }
+}
+
+TEST_F(RigidCollisionRepresentationTest, MeshUpdateTest)
+{
+ SurgSim::Framework::ApplicationData applicationData("config.txt");
+ const std::string fileName = "MeshShapeData/staple_collision.ply";
+
+ auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+ EXPECT_NO_THROW(meshShape->load(fileName, applicationData));
+
+ auto collisionRepresentation = std::make_shared<RigidCollisionRepresentation>("Collision");
+ auto physicsRepresentation = std::make_shared<RigidRepresentation>("Physics");
+
+ physicsRepresentation->setDensity(8050); // Stainless steel (in Kg.m-3)
+ physicsRepresentation->setShape(meshShape);
+ physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
+ collisionRepresentation->update(dt);
+
+ auto originalMesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(*meshShape->getMesh());
+ auto expectedMesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(*meshShape->getMesh());
+ auto actualMesh
+ = std::static_pointer_cast<SurgSim::Math::MeshShape>(collisionRepresentation->getShape())->getMesh();
+
+ EXPECT_EQ(expectedMesh->getVertices(), actualMesh->getVertices());
+ EXPECT_EQ(expectedMesh->getTriangles(), actualMesh->getTriangles());
+
+ RigidTransform3d transform = SurgSim::Math::makeRigidTransform(Vector3d(4.3, 2.1, 6.5),
+ Vector3d(-1.5, 7.5, -2.5),
+ Vector3d(8.7, -4.7, -3.1));
+
+ //physicsRepresentation->setLocalPose(transform);
+ //collisionRepresentation->update(dt);
+
+ std::dynamic_pointer_cast<SurgSim::Math::MeshShape>(collisionRepresentation->getShape())->setPose(transform);
+ expectedMesh->copyWithTransform(transform, *originalMesh);
+
+ EXPECT_EQ(expectedMesh->getVertices(), actualMesh->getVertices());
+ EXPECT_EQ(expectedMesh->getTriangles(), actualMesh->getTriangles());
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/RigidRepresentationBilateral3DTests.cpp b/SurgSim/Physics/UnitTests/RigidRepresentationBilateral3DTests.cpp
new file mode 100644
index 0000000..0dece56
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidRepresentationBilateral3DTests.cpp
@@ -0,0 +1,163 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/MlcpConstraintType.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationBilateral3D.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::Math::makeSkewSymmetricMatrix;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+const double epsilon = 1e-10;
+const double dt = 1e-3;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST(RigidRepresentationBilateral3DTests, Constructor)
+{
+ ASSERT_NO_THROW(
+ { RigidRepresentationBilateral3D constraint; });
+}
+
+TEST(RigidRepresentationBilateral3DTests, Constants)
+{
+ RigidRepresentationBilateral3D constraint;
+
+ EXPECT_EQ(SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT, constraint.getMlcpConstraintType());
+ EXPECT_EQ(SurgSim::Physics::REPRESENTATION_TYPE_RIGID, constraint.getRepresentationType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(RigidRepresentationBilateral3DTests, BuildMlcp)
+{
+ // Whitebox test which validates ConstraintImplementation::build's output parameter, MlcpPhysicsProblem. It assumes
+ // CHt and HCHt can be correctly built given H, so it does not neccessarily construct the physical parameters
+ // neccessary to supply a realistic C. It only checks H and b.
+ RigidRepresentationBilateral3D constraint;
+
+ Vector3d centerOfMass = Vector3d(3.0, 2.42, 9.54);
+ Quaterniond objectRotation = Quaterniond(0.1, 0.35, 4.2, 5.0).normalized();
+
+ RigidTransform3d objectPose = makeRigidTransform(objectRotation, centerOfMass);
+ Vector3d constraintPoint = Vector3d(8.0, 6.4, 3.5);
+
+ // Setup parameters for RigidRepresentationBilateral3D::build
+ auto representation = std::make_shared<MockRigidRepresentation>();
+ representation->setShape(std::make_shared<SurgSim::Math::SphereShape>(1.0));
+ auto localization = std::make_shared<RigidRepresentationLocalization>(representation);
+ localization->setLocalPosition(objectPose.inverse() * constraintPoint);
+ representation->getCurrentState().setPose(objectPose);
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(6, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ // Compare results
+ Eigen::Matrix<double, 3, 1> violation = constraintPoint;
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ Eigen::Matrix<double, 3, 6> H = Eigen::Matrix<double, 3, 6>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::setSubMatrix(dt * identity,
+ 0, 0, 3, 3, &H);
+ SurgSim::Math::setSubMatrix(makeSkewSymmetricMatrix((dt * (constraintPoint - centerOfMass)).eval()),
+ 0, 1, 3, 3, &H);
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(RigidRepresentationBilateral3DTests, BuildMlcpTwoStep)
+{
+ // Whitebox test which validates ConstraintImplementation::build's output parameter, MlcpPhysicsProblem. It assumes
+ // CHt and HCHt can be correctly built given H, so it does not neccessarily construct the physical parameters
+ // neccessary to supply a realistic C. It only checks H and b.
+ RigidRepresentationBilateral3D constraint;
+
+ Vector3d centerOfMassLhs = Vector3d(3.0, 2.42, 9.54);
+ Vector3d centerOfMassRhs = Vector3d(1.0, 24.52, 8.00);
+
+ Quaterniond objectRotationLhs = Quaterniond(0.1, 0.35, 4.2, 5.0).normalized();
+ Quaterniond objectRotationRhs = Quaterniond(1.43, 6.21, 7.11, 0.55).normalized();
+
+ RigidTransform3d objectPoseLhs = makeRigidTransform(objectRotationLhs, centerOfMassLhs);
+ RigidTransform3d objectPoseRhs = makeRigidTransform(objectRotationRhs, centerOfMassRhs);
+
+ Vector3d constraintPointLhs = Vector3d(8.0, 6.4, 3.5);
+ Vector3d constraintPointRhs = Vector3d(3.0, 7.7, 0.0);
+
+ // Setup parameters for RigidRepresentationBilateral3D::build
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(12, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ auto representation = std::make_shared<MockRigidRepresentation>();
+ representation->setShape(std::make_shared<SurgSim::Math::SphereShape>(1.0));
+ auto localization = std::make_shared<RigidRepresentationLocalization>(representation);
+
+ localization->setLocalPosition(objectPoseLhs.inverse() * constraintPointLhs);
+ representation->getCurrentState().setPose(objectPoseLhs);
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ localization->setLocalPosition(objectPoseRhs.inverse() * constraintPointRhs);
+ representation->getCurrentState().setPose(objectPoseRhs);
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 6, 0, SurgSim::Physics::CONSTRAINT_NEGATIVE_SIDE));
+
+ // Compare results
+ Eigen::Matrix<double, 3, 1> violation = constraintPointLhs - constraintPointRhs;
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ Eigen::Matrix<double, 3, 12> H = Eigen::Matrix<double, 3, 12>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::setSubMatrix(dt * identity,
+ 0, 0, 3, 3, &H);
+ SurgSim::Math::setSubMatrix(makeSkewSymmetricMatrix((dt * (constraintPointLhs - centerOfMassLhs)).eval()),
+ 0, 1, 3, 3, &H);
+ SurgSim::Math::setSubMatrix(-dt * identity,
+ 0, 2, 3, 3, &H);
+ SurgSim::Math::setSubMatrix(makeSkewSymmetricMatrix((-dt * (constraintPointRhs - centerOfMassRhs)).eval()),
+ 0, 3, 3, 3, &H);
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/RigidRepresentationContactTests.cpp b/SurgSim/Physics/UnitTests/RigidRepresentationContactTests.cpp
new file mode 100644
index 0000000..73c65c6
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidRepresentationContactTests.cpp
@@ -0,0 +1,99 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationContact.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST (RigidRepresentationContactTests, SetGet_BuildMlcp_Test)
+{
+ Vector3d n(0.0, 1.0, 0.0);
+ double d = 0.0;
+ double radius = 0.01;
+ double violation = -radius;
+
+ Vector3d contactPosition = -n * (d - violation);
+ SurgSim::Math::RigidTransform3d poseRigid;
+ poseRigid.setIdentity();
+
+ std::shared_ptr<RigidRepresentation> rigid = std::make_shared<RigidRepresentation>("Rigid");
+ rigid->setLocalActive(true);
+ rigid->setIsGravityEnabled(false);
+ rigid->setLocalPose(poseRigid);
+ rigid->setDensity(1000.0);
+ rigid->setShape(std::make_shared<SphereShape>(radius));
+
+ std::shared_ptr<RigidRepresentationLocalization> loc = std::make_shared<RigidRepresentationLocalization>(rigid);
+ loc->setLocalPosition(contactPosition);
+ std::shared_ptr<RigidRepresentationContact> implementation = std::make_shared<RigidRepresentationContact>();
+
+ EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, implementation->getMlcpConstraintType());
+ EXPECT_EQ(1u, implementation->getNumDof());
+
+ ContactConstraintData constraintData;
+ constraintData.setPlaneEquation(n, d);
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(rigid->getNumDof(), 1, 1);
+
+ // Fill up the Mlcp
+ double dt = 1e-3;
+ implementation->build(dt, constraintData, loc,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ // Violation b should be exactly violation = -radius (the sphere center is on the plane)
+ EXPECT_NEAR(violation, mlcpPhysicsProblem.b[0], epsilon);
+
+ // Constraint H should be
+ // H = dt.[nx ny nz nz.GPy-ny.GPz nx.GPz-nz.GPx ny.GPx-nx.GPy]
+ Vector3d n_GP = n.cross(Vector3d(0.0, 0.0, 0.0));
+ EXPECT_NEAR(dt * n[0] , mlcpPhysicsProblem.H(0, 0), epsilon);
+ EXPECT_NEAR(dt * n[1] , mlcpPhysicsProblem.H(0, 1), epsilon);
+ EXPECT_NEAR(dt * n[2] , mlcpPhysicsProblem.H(0, 2), epsilon);
+ EXPECT_NEAR(dt * n_GP[0], mlcpPhysicsProblem.H(0, 3), epsilon);
+ EXPECT_NEAR(dt * n_GP[1], mlcpPhysicsProblem.H(0, 4), epsilon);
+ EXPECT_NEAR(dt * n_GP[2], mlcpPhysicsProblem.H(0, 5), epsilon);
+
+ // ConstraintTypes should contain 0 entry as it is setup by the constraint and not the ConstraintImplementation
+ // This way, the constraint can verify that both ConstraintImplementation are the same type
+ ASSERT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/RigidRepresentationLocalizationTest.cpp b/SurgSim/Physics/UnitTests/RigidRepresentationLocalizationTest.cpp
new file mode 100644
index 0000000..624a1ad
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidRepresentationLocalizationTest.cpp
@@ -0,0 +1,123 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::Physics::MockRigidRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+using SurgSim::Physics::RigidRepresentationLocalization;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+class RigidRepresentationLocalizationTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ SurgSim::Math::Quaterniond q;
+ SurgSim::Math::Vector3d t;
+
+ q.coeffs().setRandom();
+ q.normalize();
+ t.setRandom();
+ m_initialTransformation = SurgSim::Math::makeRigidTransform(q, t);
+
+ do
+ {
+ q.coeffs().setRandom();
+ q.normalize();
+ t.setRandom();
+ m_currentTransformation = SurgSim::Math::makeRigidTransform(q, t);
+ } while (m_initialTransformation.isApprox(m_currentTransformation));
+
+ m_identityTransformation.setIdentity();
+ }
+
+ void TearDown()
+ {
+ }
+
+ // Fixed representation initialization pose
+ SurgSim::Math::RigidTransform3d m_initialTransformation;
+
+ // Fixed representation current pose
+ SurgSim::Math::RigidTransform3d m_currentTransformation;
+
+ // Identity pose (no translation/rotation)
+ SurgSim::Math::RigidTransform3d m_identityTransformation;
+};
+
+TEST_F(RigidRepresentationLocalizationTest, ConstructorTest)
+{
+ ASSERT_NO_THROW( {RigidRepresentationLocalization rigidRepresentationLoc;});
+
+ ASSERT_NO_THROW(
+ {
+ std::shared_ptr<RigidRepresentation> rigid = std::make_shared<RigidRepresentation>("RigidRepresentation");
+ RigidRepresentationLocalization rigidRepresentationLoc(rigid);
+ });
+}
+
+TEST_F(RigidRepresentationLocalizationTest, SetGetRepresentation)
+{
+ RigidRepresentationLocalization rigidRepresentationLoc;
+ std::shared_ptr<RigidRepresentation> rigid = std::make_shared<RigidRepresentation>("RigidRepresentation");
+
+ EXPECT_EQ(nullptr, rigidRepresentationLoc.getRepresentation());
+
+ rigidRepresentationLoc.setRepresentation(rigid);
+ EXPECT_EQ(rigid, rigidRepresentationLoc.getRepresentation());
+
+ rigidRepresentationLoc.setRepresentation(nullptr);
+ EXPECT_EQ(nullptr, rigidRepresentationLoc.getRepresentation());
+}
+
+TEST_F(RigidRepresentationLocalizationTest, GetPositionTest)
+{
+ // Create the rigid body
+ std::shared_ptr<MockRigidRepresentation> rigidRepresentation =
+ std::make_shared<MockRigidRepresentation>();
+
+ // Activate the rigid body and setup its initial pose
+ rigidRepresentation->setLocalActive(true);
+ rigidRepresentation->getCurrentState().setPose(m_initialTransformation);
+
+ RigidRepresentationLocalization localization = RigidRepresentationLocalization(rigidRepresentation);
+ ASSERT_EQ(rigidRepresentation, localization.getRepresentation());
+
+ SurgSim::Math::Vector3d origin = m_initialTransformation.translation();
+ SurgSim::Math::Vector3d zero = SurgSim::Math::Vector3d::Zero();
+ localization.setLocalPosition(zero);
+ EXPECT_TRUE(localization.getLocalPosition().isZero(epsilon));
+ EXPECT_TRUE(localization.calculatePosition().isApprox(origin, epsilon));
+
+ SurgSim::Math::Vector3d position = SurgSim::Math::Vector3d::Random();
+ localization.setLocalPosition(position);
+ EXPECT_TRUE(localization.getLocalPosition().isApprox(position, epsilon));
+ EXPECT_FALSE(localization.calculatePosition().isApprox(origin, epsilon));
+}
diff --git a/SurgSim/Physics/UnitTests/RigidRepresentationStateTest.cpp b/SurgSim/Physics/UnitTests/RigidRepresentationStateTest.cpp
new file mode 100644
index 0000000..8921900
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidRepresentationStateTest.cpp
@@ -0,0 +1,134 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/PhysicsConvert.h"
+#include "SurgSim/Physics/RigidRepresentationState.h"
+
+struct RigidRepresentationStateTest : public ::testing::Test
+{
+ void SetUp()
+ {
+ quaterniond = SurgSim::Math::Quaterniond(0.5, 0.4, 0.3, 0.2);
+ quaterniond.normalize();
+ translation = SurgSim::Math::Vector3d(5.2, -6.13, 4.12356);
+ pose = SurgSim::Math::makeRigidTransform(quaterniond, translation);
+ linearVelocity = SurgSim::Math::Vector3d(2, -3.1, -2.75);
+ angularVelocity = SurgSim::Math::Vector3d (5, -10, 21.5);
+ id4x4 = SurgSim::Math::RigidTransform3d::Identity();
+ }
+
+ SurgSim::Math::Quaterniond quaterniond;
+ SurgSim::Math::Vector3d translation;
+ SurgSim::Math::RigidTransform3d pose;
+
+ SurgSim::Math::Vector3d linearVelocity;
+ SurgSim::Math::Vector3d angularVelocity;
+
+ SurgSim::Math::RigidTransform3d id4x4;
+};
+
+TEST_F(RigidRepresentationStateTest, ConstructorTest)
+{
+ ASSERT_NO_THROW(SurgSim::Physics::RigidRepresentationState rigidRepresentationState);
+}
+
+TEST_F(RigidRepresentationStateTest, DefaultValueTest)
+{
+ // Create the base rigid representation state
+ std::shared_ptr<SurgSim::Physics::RigidRepresentationState> rigidRepresentationState;
+ rigidRepresentationState = std::make_shared<SurgSim::Physics::RigidRepresentationState>();
+
+ // Linear velocity [default = (0 0 0)]
+ EXPECT_TRUE(rigidRepresentationState->getLinearVelocity().isZero());
+ // Angular velocity [default = (0 0 0)]
+ EXPECT_TRUE(rigidRepresentationState->getAngularVelocity().isZero());
+ // Pose [default = Identity]
+ EXPECT_TRUE(rigidRepresentationState->getPose().isApprox(id4x4));
+}
+
+TEST_F(RigidRepresentationStateTest, ResetTest)
+{
+ // Create the base rigid representation state
+ std::shared_ptr<SurgSim::Physics::RigidRepresentationState> rigidRepresentationState;
+ rigidRepresentationState = std::make_shared<SurgSim::Physics::RigidRepresentationState>();
+
+ rigidRepresentationState->setLinearVelocity(linearVelocity);
+ rigidRepresentationState->setAngularVelocity(angularVelocity);
+ rigidRepresentationState->setPose(pose);
+
+ // Reset the rigid representation state to default values
+ rigidRepresentationState->reset();
+
+ // Test Linear velocity has been reset to (0 0 0)
+ EXPECT_TRUE(rigidRepresentationState->getLinearVelocity().isZero());
+ // Test Angular velocity has been reset to (0 0 0)
+ EXPECT_TRUE(rigidRepresentationState->getAngularVelocity().isZero());
+ // Test pose has been reset to Identity
+ EXPECT_TRUE(rigidRepresentationState->getPose().isApprox(id4x4));
+}
+
+TEST_F(RigidRepresentationStateTest, SetGetTest)
+{
+ // Create the base rigid representation state
+ std::shared_ptr<SurgSim::Physics::RigidRepresentationState> rigidRepresentationState;
+ rigidRepresentationState = std::make_shared<SurgSim::Physics::RigidRepresentationState>();
+
+ // Get/Set linear velocity
+ rigidRepresentationState->setLinearVelocity(linearVelocity);
+ EXPECT_TRUE(linearVelocity.isApprox(rigidRepresentationState->getLinearVelocity()));
+ rigidRepresentationState->setLinearVelocity(SurgSim::Math::Vector3d::Zero());
+ EXPECT_TRUE(rigidRepresentationState->getLinearVelocity().isZero());
+
+ // Get/Set angular velocity
+ rigidRepresentationState->setAngularVelocity(angularVelocity);
+ EXPECT_TRUE(angularVelocity.isApprox(rigidRepresentationState->getAngularVelocity()));
+ rigidRepresentationState->setAngularVelocity(SurgSim::Math::Vector3d::Zero());
+ EXPECT_TRUE(rigidRepresentationState->getAngularVelocity().isZero());
+
+ // Get/Set pose
+ rigidRepresentationState->setPose(pose);
+ EXPECT_TRUE(rigidRepresentationState->getPose().isApprox(pose));
+ rigidRepresentationState->setPose(id4x4);
+ EXPECT_TRUE(rigidRepresentationState->getPose().isApprox(id4x4));
+}
+
+TEST_F(RigidRepresentationStateTest, SerializationTest)
+{
+ SurgSim::Physics::RigidRepresentationState rigidRepresentationState;
+
+ rigidRepresentationState.setValue("Pose", pose);
+ rigidRepresentationState.setValue("LinearVelocity", linearVelocity);
+ rigidRepresentationState.setValue("AngularVelocity", angularVelocity);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Physics::RigidRepresentationState>::encode(rigidRepresentationState));
+ EXPECT_EQ(1u, node.size());
+
+ SurgSim::Physics::RigidRepresentationState newRigidRepresentationState;
+ ASSERT_NO_THROW(newRigidRepresentationState = node.as<SurgSim::Physics::RigidRepresentationState>());
+
+ EXPECT_TRUE(pose.isApprox(newRigidRepresentationState.getValue<SurgSim::Math::RigidTransform3d>("Pose")));
+ EXPECT_TRUE(linearVelocity.isApprox(
+ newRigidRepresentationState.getValue<SurgSim::Math::Vector3d>("LinearVelocity")));
+ EXPECT_TRUE(angularVelocity.isApprox(
+ newRigidRepresentationState.getValue<SurgSim::Math::Vector3d>("AngularVelocity")));
+}
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/RigidRepresentationTest.cpp b/SurgSim/Physics/UnitTests/RigidRepresentationTest.cpp
new file mode 100644
index 0000000..957b3d1
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidRepresentationTest.cpp
@@ -0,0 +1,1131 @@
+//// This file is a part of the OpenSurgSim project.
+//// Copyright 2013, SimQuest Solutions Inc.
+////
+//// Licensed under the Apache License, Version 2.0 (the "License");
+//// you may not use this file except in compliance with the License.
+//// You may obtain a copy of the License at
+////
+//// http://www.apache.org/licenses/LICENSE-2.0
+////
+//// Unless required by applicable law or agreed to in writing, software
+//// distributed under the License is distributed on an "AS IS" BASIS,
+//// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//// See the License for the specific language governing permissions and
+//// limitations under the License.
+
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Valid.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Framework::Component;
+using SurgSim::Framework::Runtime;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Matrix66d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Shape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector6d;
+
+namespace
+{
+const double epsilon = 1e-10;
+}
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class RigidRepresentationTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ m_dt = 1e-3;
+
+ m_radius = 0.1;
+ m_density = 9000.0;
+ m_mass = 4.0 / 3.0 * M_PI * m_radius * m_radius * m_radius * m_density;
+ double coef = 2.0 / 5.0 * m_mass * m_radius * m_radius;
+ m_inertia << coef, 0.0, 0.0, 0.0, coef, 0.0, 0.0, 0.0, coef;
+ m_id33.setIdentity();
+ m_zero33.setZero();
+ m_invalidInertia.setRandom();
+ m_invalidInertia = m_invalidInertia + m_invalidInertia.transpose().eval(); // make symmetric
+ m_invalidInertia(0, 0) = -12.3; // Negative value on hte diagonal (invalid)
+ m_sphere = std::make_shared<SphereShape>(m_radius);
+
+ Quaterniond q(0.5, 0.4, 0.3, 0.2);
+ q.normalize();
+ Vector3d t(1.2, 2.1, 12.21);
+ m_state.setAngularVelocity(Vector3d(1, 2, 3));
+ m_state.setLinearVelocity(Vector3d(3, 2, 1));
+ m_state.setPose(SurgSim::Math::makeRigidTransform(q, t));
+
+ m_maxNumSimulationStepTest = 100;
+ }
+
+ void TearDown()
+ {
+ }
+
+ // Time step
+ double m_dt;
+
+ // Sphere radius (in m)
+ double m_radius;
+
+ // Sphere density (in Kg.m-3)
+ double m_density;
+
+ // Sphere mass (in Kg)
+ double m_mass;
+
+ // Sphere inertia matrix
+ SurgSim::Math::Matrix33d m_inertia;
+
+ // Identity matrix 3x3 (for convenience)
+ SurgSim::Math::Matrix33d m_id33;
+
+ // Zero matrix 3x3 (for convenience)
+ SurgSim::Math::Matrix33d m_zero33;
+
+ // Invalid inertia matrix
+ SurgSim::Math::Matrix33d m_invalidInertia;
+
+ // SphereShape
+ std::shared_ptr<SphereShape> m_sphere;
+
+ // Rigid representation state
+ RigidRepresentationState m_state;
+
+ // Rigid representation default state
+ RigidRepresentationState m_defaultState;
+
+ // Max number of simulation step for testing
+ int m_maxNumSimulationStepTest;
+};
+
+TEST_F(RigidRepresentationTest, ConstructorTest)
+{
+ ASSERT_NO_THROW(RigidRepresentation rigidBody("Rigid"));
+}
+
+TEST_F(RigidRepresentationTest, ResetTest)
+{
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+
+ rigidBody->setInitialState(m_state);
+ rigidBody->setLocalActive(false);
+ rigidBody->setIsGravityEnabled(false);
+ rigidBody->setLocalPose(RigidTransform3d::Identity());
+
+ // reset the representation state
+ rigidBody->resetState();
+
+ // isActive unchanged
+ EXPECT_FALSE(rigidBody->isActive());
+ // isGravityEnable flag unchanged
+ EXPECT_FALSE(rigidBody->isGravityEnabled());
+ // current state = initial state
+ EXPECT_TRUE(rigidBody->getInitialState() == rigidBody->getCurrentState());
+ // previous state = initial state
+ EXPECT_TRUE(rigidBody->getInitialState() == rigidBody->getPreviousState());
+}
+
+TEST_F(RigidRepresentationTest, SetGetAndDefaultValueTest)
+{
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+
+ // Get state (current, initial)
+ EXPECT_TRUE(m_defaultState == rigidBody->getCurrentState());
+ EXPECT_TRUE(m_defaultState == rigidBody->getPreviousState());
+ EXPECT_TRUE(m_defaultState == rigidBody->getInitialState());
+ rigidBody->setInitialState(m_state);
+ EXPECT_TRUE(m_state == rigidBody->getInitialState());
+ EXPECT_TRUE(m_state == rigidBody->getCurrentState());
+ EXPECT_TRUE(m_state == rigidBody->getPreviousState());
+
+ // Mass density [default = 0]
+ EXPECT_NEAR(0.0, rigidBody->getDensity(), epsilon);
+ // Mass [default = qNaA]
+ EXPECT_FALSE(SurgSim::Math::isValid(rigidBody->getMass()));
+ // Inertia 3x3 symmetric matrix [default = qNaN values]
+ EXPECT_FALSE(SurgSim::Math::isValid(rigidBody->getLocalInertia()));
+ // Linear damping [default = 0]
+ EXPECT_NEAR(0.0, rigidBody->getLinearDamping(), epsilon);
+ // Angular damping [default = 0]
+ EXPECT_NEAR(0.0, rigidBody->getAngularDamping(), epsilon);
+ // Shape [default = nullptr]
+ EXPECT_EQ(nullptr, rigidBody->getShape());
+
+ // Mass density
+ rigidBody->setDensity(m_density);
+ EXPECT_NEAR(m_density, rigidBody->getDensity(), epsilon);
+ rigidBody->setDensity(0.0);
+ EXPECT_NEAR(0.0, rigidBody->getDensity(), epsilon);
+
+ // Linear damping
+ rigidBody->setLinearDamping(5.5);
+ EXPECT_NEAR(5.5, rigidBody->getLinearDamping(), epsilon);
+ rigidBody->setLinearDamping(0.0);
+ EXPECT_NEAR(0.0, rigidBody->getLinearDamping(), epsilon);
+
+ // Angular damping
+ rigidBody->setAngularDamping(5.5);
+ EXPECT_NEAR(5.5, rigidBody->getAngularDamping(), epsilon);
+ rigidBody->setAngularDamping(0.0);
+ EXPECT_NEAR(0.0, rigidBody->getAngularDamping(), epsilon);
+
+ // Shape
+ rigidBody->setShape(m_sphere);
+ EXPECT_EQ(m_sphere, rigidBody->getShape());
+ rigidBody->setShape(nullptr);
+ EXPECT_EQ(nullptr, rigidBody->getShape());
+
+ // Get/Set active flag [default = true]
+ EXPECT_TRUE(rigidBody->isActive());
+ EXPECT_TRUE(rigidBody->isLocalActive());
+ rigidBody->setLocalActive(false);
+ ASSERT_FALSE(rigidBody->isActive());
+ ASSERT_FALSE(rigidBody->isLocalActive());
+ rigidBody->setLocalActive(true);
+ ASSERT_TRUE(rigidBody->isActive());
+ ASSERT_TRUE(rigidBody->isLocalActive());
+
+ // Get numDof = 6
+ ASSERT_EQ(6u, rigidBody->getNumDof());
+
+ // Set/Get isGravityEnabled [default = true]
+ EXPECT_TRUE(rigidBody->isGravityEnabled());
+ rigidBody->setIsGravityEnabled(false);
+ ASSERT_FALSE(rigidBody->isGravityEnabled());
+ rigidBody->setIsGravityEnabled(true);
+ ASSERT_TRUE(rigidBody->isGravityEnabled());
+}
+
+TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceOnMassCenterTest)
+{
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setShape(m_sphere);
+
+ Vector6d F = 6.4 * Vector6d::Ones();
+ Matrix66d K = 0.4 * Matrix66d::Ones() + 8.3 * Matrix66d::Identity();
+ Matrix66d D = -0.6 * Matrix66d::Ones() + 4.1 * Matrix66d::Identity();
+
+ rigidBody->addExternalGeneralizedForce(F, K, D);
+ EXPECT_TRUE(rigidBody->getExternalGeneralizedForce().isApprox(F));
+ EXPECT_TRUE(rigidBody->getExternalGeneralizedStiffness().isApprox(K));
+ EXPECT_TRUE(rigidBody->getExternalGeneralizedDamping().isApprox(D));
+}
+
+namespace
+{
+// Extra force/torque that should be added
+Vector6d computeExtraTorque(const Vector6d& inputForce, const Vector3d& anchorLocalPoint,
+ const Vector6d& dofPosition, const Vector6d& dofVelocity)
+{
+ Vector6d f = inputForce;
+
+ auto C = dofPosition.segment<3>(0); // Mass center
+ auto rotVector = dofPosition.segment<3>(3); // Rotation vector
+
+ double angle;
+ Vector3d axis = rotVector;
+ angle = axis.norm();
+ if (std::abs(angle) < 1e-8)
+ {
+ axis.setZero();
+ }
+ else
+ {
+ axis.normalize();
+ }
+ Matrix33d R = SurgSim::Math::makeRotationMatrix(angle, axis);
+
+ Vector3d anchorPoint = C + R * anchorLocalPoint;
+ Vector3d lever = anchorPoint - C;
+
+ f.segment<3>(3) += lever.cross(f.segment<3>(0));
+
+ return f;
+}
+
+Matrix66d computeExtraStiffness(const Vector6d& inputForce, const Vector3d& anchorLocalPoint,
+ const Vector6d& dofPosition, const Vector6d& dofVelocity)
+{
+ Matrix66d K = Matrix66d::Zero();
+ const double epsilon = 1e-8;
+
+ for (size_t column = 0; column < 6; ++column)
+ {
+ Vector6d dofX = dofPosition;
+ dofX[column] += 2.0 * epsilon;
+ Vector6d fXPlus2H = computeExtraTorque(inputForce, anchorLocalPoint, dofX, dofVelocity);
+
+ dofX = dofPosition;
+ dofX[column] += epsilon;
+ Vector6d fXPlusH = computeExtraTorque(inputForce, anchorLocalPoint, dofX, dofVelocity);
+
+ dofX = dofPosition;
+ dofX[column] -= epsilon;
+ Vector6d fXMinusH = computeExtraTorque(inputForce, anchorLocalPoint, dofX, dofVelocity);
+
+ dofX = dofPosition;
+ dofX[column] -= 2.0 * epsilon;
+ Vector6d fXMinus2H = computeExtraTorque(inputForce, anchorLocalPoint, dofX, dofVelocity);
+
+ K.col(column) = - (-fXPlus2H + 8.0 * fXPlusH - 8.0 * fXMinusH + fXMinus2H) / (12.0 * epsilon);
+ }
+
+ return K;
+}
+
+Matrix66d computeExtraDamping(const Vector6d& inputForce, const Vector3d& anchorLocalPoint,
+ const Vector6d& dofPosition, const Vector6d& dofVelocity)
+{
+ Matrix66d D = Matrix66d::Zero();
+ const double epsilon = 1e-8;
+
+ for (size_t column = 0; column < 6; ++column)
+ {
+ Vector6d dofV = dofVelocity;
+ dofV[column] += 2.0 * epsilon;
+ Vector6d fXPlus2H = computeExtraTorque(inputForce, anchorLocalPoint, dofPosition, dofV);
+
+ dofV = dofVelocity;
+ dofV[column] += epsilon;
+ Vector6d fXPlusH = computeExtraTorque(inputForce, anchorLocalPoint, dofPosition, dofV);
+
+ dofV = dofVelocity;
+ dofV[column] -= epsilon;
+ Vector6d fXMinusH = computeExtraTorque(inputForce, anchorLocalPoint, dofPosition, dofV);
+
+ dofV = dofVelocity;
+ dofV[column] -= 2.0 * epsilon;
+ Vector6d fXMinus2H = computeExtraTorque(inputForce, anchorLocalPoint, dofPosition, dofV);
+
+ D.col(column) = - (-fXPlus2H + 8.0 * fXPlusH - 8.0 * fXMinusH + fXMinus2H) / (12.0 * epsilon);
+ }
+
+ return D;
+}
+}; // namespace anonymous
+
+TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceExtraTermsTest)
+{
+ {
+ SCOPED_TRACE("Non identity pose");
+
+ Eigen::AngleAxisd angleAxis(0.34512, Vector3d(1.1, -1.4, 3.23).normalized());
+ Vector3d t(1.1, 2.2, 3.3);
+ RigidTransform3d transform = makeRigidTransform(Quaterniond(angleAxis), t);
+
+ Vector6d inputForce = Vector6d::LinSpaced(1.1, 6.6);
+ Vector6d dofX = Vector6d::Zero();
+ dofX.segment<3>(0) = transform.translation();
+ Eigen::AngleAxisd angleAxis2(transform.rotation());
+ dofX.segment<3>(3) = angleAxis2.axis() * angleAxis2.angle();
+ Vector6d dofV = Vector6d::Zero();
+ Vector3d anchorLocalPoint = Vector3d::Ones();
+
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setShape(m_sphere);
+ SurgSim::Physics::RigidRepresentationState initialState;
+ initialState.setPose(transform);
+ rigidBody->setInitialState(initialState);
+ SurgSim::DataStructures::Location location(anchorLocalPoint);
+
+ Vector6d Fnumeric = computeExtraTorque(inputForce, anchorLocalPoint, dofX, dofV);
+ Matrix66d Knumeric = computeExtraStiffness(inputForce, anchorLocalPoint, dofX, dofV);
+ Matrix66d Dnumeric = computeExtraDamping(inputForce, anchorLocalPoint, dofX, dofV);
+
+ Vector6d F = inputForce;
+ Matrix66d K = Matrix66d::Zero(), D = Matrix66d::Zero();
+ rigidBody->addExternalGeneralizedForce(location, F, K, D);
+ F = rigidBody->getExternalGeneralizedForce();
+ K = rigidBody->getExternalGeneralizedStiffness();
+ D = rigidBody->getExternalGeneralizedDamping();
+
+ EXPECT_LE((F - Fnumeric).cwiseAbs().maxCoeff(), 2e-7);
+ EXPECT_LE((K - Knumeric).cwiseAbs().maxCoeff(), 2e-7);
+ EXPECT_LE((D - Dnumeric).cwiseAbs().maxCoeff(), 2e-7);
+ }
+
+ {
+ SCOPED_TRACE("Exactly Identity pose");
+
+ Vector6d inputForce = Vector6d::LinSpaced(1.1, 6.6);
+ Vector6d dofX = Vector6d::Zero();
+ Vector6d dofV = Vector6d::Zero();
+ Vector3d anchorLocalPoint = Vector3d::Ones();
+
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setShape(m_sphere);
+ SurgSim::DataStructures::Location location(anchorLocalPoint);
+
+ Vector6d Fnumeric = computeExtraTorque(inputForce, anchorLocalPoint, dofX, dofV);
+ Matrix66d Knumeric = computeExtraStiffness(inputForce, anchorLocalPoint, dofX, dofV);
+ Matrix66d Dnumeric = computeExtraDamping(inputForce, anchorLocalPoint, dofX, dofV);
+
+ Vector6d F = inputForce;
+ Matrix66d K = Matrix66d::Zero(), D = Matrix66d::Zero();
+ rigidBody->addExternalGeneralizedForce(location, F, K, D);
+ F = rigidBody->getExternalGeneralizedForce();
+ K = rigidBody->getExternalGeneralizedStiffness();
+ D = rigidBody->getExternalGeneralizedDamping();
+
+ EXPECT_LE((F - Fnumeric).cwiseAbs().maxCoeff(), 2e-7);
+ EXPECT_LE((K - Knumeric).cwiseAbs().maxCoeff(), 2e-7);
+ EXPECT_LE((D - Dnumeric).cwiseAbs().maxCoeff(), 2e-7);
+ }
+
+ {
+ SCOPED_TRACE("Almost Identity pose, limitted development not used yet");
+
+ Eigen::AngleAxisd angleAxis(5e-8, Vector3d(1.1, -1.4, 3.23).normalized());
+ Vector3d t(1.1, 2.2, 3.3);
+ RigidTransform3d transform = makeRigidTransform(Quaterniond(angleAxis), t);
+
+ Vector6d inputForce = Vector6d::LinSpaced(1.1, 6.6);
+ Vector6d dofX = Vector6d::Zero();
+ dofX.segment<3>(0) = transform.translation();
+ Eigen::AngleAxisd angleAxis2(transform.rotation());
+ dofX.segment<3>(3) = angleAxis2.angle() * angleAxis2.axis();
+ Vector6d dofV = Vector6d::Zero();
+ Vector3d anchorLocalPoint = Vector3d::Ones();
+
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setShape(m_sphere);
+ SurgSim::Physics::RigidRepresentationState initialState;
+ initialState.setPose(transform);
+ rigidBody->setInitialState(initialState);
+ SurgSim::DataStructures::Location location(anchorLocalPoint);
+
+ Vector6d Fnumeric = computeExtraTorque(inputForce, anchorLocalPoint, dofX, dofV);
+ Matrix66d Knumeric = computeExtraStiffness(inputForce, anchorLocalPoint, dofX, dofV);
+ Matrix66d Dnumeric = computeExtraDamping(inputForce, anchorLocalPoint, dofX, dofV);
+
+ Vector6d F = inputForce;
+ Matrix66d K = Matrix66d::Zero(), D = Matrix66d::Zero();
+ rigidBody->addExternalGeneralizedForce(location, F, K, D);
+ F = rigidBody->getExternalGeneralizedForce();
+ K = rigidBody->getExternalGeneralizedStiffness();
+ D = rigidBody->getExternalGeneralizedDamping();
+
+ EXPECT_LE((F - Fnumeric).cwiseAbs().maxCoeff(), 2e-7);
+ EXPECT_LE((K - Knumeric).cwiseAbs().maxCoeff(), 2e-7);
+ EXPECT_LE((D - Dnumeric).cwiseAbs().maxCoeff(), 2e-7);
+ }
+
+ {
+ SCOPED_TRACE("Almost Identity pose, limitted development in use");
+
+ Eigen::AngleAxisd angleAxis(0.2e-8, Vector3d(1.1, -1.4, 3.23).normalized());
+ Vector3d t(1.1, 2.2, 3.3);
+ RigidTransform3d transform = makeRigidTransform(Quaterniond(angleAxis), t);
+
+ Vector6d inputForce = Vector6d::LinSpaced(1.1, 6.6);
+ Vector6d dofX = Vector6d::Zero();
+ dofX.segment<3>(0) = transform.translation();
+ Eigen::AngleAxisd angleAxis2(transform.rotation());
+ dofX.segment<3>(3) = angleAxis2.angle() * angleAxis2.axis();
+ Vector6d dofV = Vector6d::Zero();
+ Vector3d anchorLocalPoint = Vector3d::Ones();
+
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setShape(m_sphere);
+ SurgSim::Physics::RigidRepresentationState initialState;
+ initialState.setPose(transform);
+ rigidBody->setInitialState(initialState);
+ SurgSim::DataStructures::Location location(anchorLocalPoint);
+
+ Vector6d Fnumeric = computeExtraTorque(inputForce, anchorLocalPoint, dofX, dofV);
+ Matrix66d Knumeric = computeExtraStiffness(inputForce, anchorLocalPoint, dofX, dofV);
+ Matrix66d Dnumeric = computeExtraDamping(inputForce, anchorLocalPoint, dofX, dofV);
+
+ Vector6d F = inputForce;
+ Matrix66d K = Matrix66d::Zero(), D = Matrix66d::Zero();
+ rigidBody->addExternalGeneralizedForce(location, F, K, D);
+ F = rigidBody->getExternalGeneralizedForce();
+ K = rigidBody->getExternalGeneralizedStiffness();
+ D = rigidBody->getExternalGeneralizedDamping();
+
+ EXPECT_LE((F - Fnumeric).cwiseAbs().maxCoeff(), 2e-7);
+ EXPECT_LE((K - Knumeric).cwiseAbs().maxCoeff(), 2e-7);
+ EXPECT_LE((D - Dnumeric).cwiseAbs().maxCoeff(), 2e-7);
+ }
+}
+
+namespace
+{
+Vector6d computeSpringForce(double linearStiffness, double linearDamping,
+ double angularStiffness, double angularDamping,
+ Vector3d targetPosition, Vector3d targetLinearVelocity,
+ Vector3d targetRotationVector, Vector3d targetAngularVelocity,
+ Vector6d dofX, Vector6d dofV,
+ bool doComputeExtraTorque = false, Vector3d localApplicationPoint = Vector3d::Zero())
+{
+ Vector6d f = Vector6d::Zero();
+
+ f.segment<3>(0) += linearStiffness * (targetPosition - dofX.segment<3>(0));
+ f.segment<3>(0) += linearDamping * (targetLinearVelocity - dofV.segment<3>(0));
+
+ f.segment<3>(3) += angularStiffness * (targetRotationVector - dofX.segment<3>(3));
+ f.segment<3>(3) += angularDamping * (targetAngularVelocity - dofV.segment<3>(3));
+ if (doComputeExtraTorque)
+ {
+ Vector3d rotationVector = dofX.segment<3>(3);
+ Matrix33d R;
+ double angle = rotationVector.norm();
+ if (std::abs(angle) < 1e-8)
+ {
+ R.setIdentity();
+ }
+ else
+ {
+ Vector3d axis = rotationVector / angle;
+ R = cos(angle) * Matrix33d::Identity() +
+ sin(angle) * SurgSim::Math::makeSkewSymmetricMatrix(axis) +
+ (1.0 - cos(angle)) * axis * axis.transpose();
+ }
+ Vector3d applicationPoint = dofX.segment<3>(0) + R * localApplicationPoint;
+ Vector3d lever = applicationPoint - dofX.segment<3>(0);
+ f.segment<3>(3) += lever.cross(f.segment<3>(0));
+ }
+
+ return f;
+}
+
+Matrix66d computeSpringStiffness(double linearStiffness, double linearDamping,
+ double angularStiffness, double angularDamping,
+ Vector3d targetPosition, Vector3d targetLinearVelocity,
+ Vector3d targetRotationVector, Vector3d targetAngularVelocity,
+ Vector6d dofPosition, Vector6d dofVelocity,
+ bool doComputeExtraTorque = false, Vector3d localApplicationPoint = Vector3d::Zero())
+{
+ Matrix66d K = Matrix66d::Zero();
+ const double epsilon = 1e-8;
+
+ for (size_t column = 0; column < 6; ++column)
+ {
+ Vector6d dofX = dofPosition;
+ dofX[column] += 2.0 * epsilon;
+ Vector6d fXPlus2H = computeSpringForce(linearStiffness, linearDamping, angularStiffness, angularDamping,
+ targetPosition, targetLinearVelocity, targetRotationVector, targetAngularVelocity, dofX, dofVelocity,
+ doComputeExtraTorque, localApplicationPoint);
+
+ dofX = dofPosition;
+ dofX[column] += epsilon;
+ Vector6d fXPlusH = computeSpringForce(linearStiffness, linearDamping, angularStiffness, angularDamping,
+ targetPosition, targetLinearVelocity, targetRotationVector, targetAngularVelocity, dofX, dofVelocity,
+ doComputeExtraTorque, localApplicationPoint);
+
+ dofX = dofPosition;
+ dofX[column] -= epsilon;
+ Vector6d fXMinusH = computeSpringForce(linearStiffness, linearDamping, angularStiffness, angularDamping,
+ targetPosition, targetLinearVelocity, targetRotationVector, targetAngularVelocity, dofX, dofVelocity,
+ doComputeExtraTorque, localApplicationPoint);
+
+ dofX = dofPosition;
+ dofX[column] -= 2.0 * epsilon;
+ Vector6d fXMinus2H = computeSpringForce(linearStiffness, linearDamping, angularStiffness, angularDamping,
+ targetPosition, targetLinearVelocity, targetRotationVector, targetAngularVelocity, dofX, dofVelocity,
+ doComputeExtraTorque, localApplicationPoint);
+
+ K.col(column) = - (-fXPlus2H + 8.0 * fXPlusH - 8.0 * fXMinusH + fXMinus2H) / (12.0 * epsilon);
+ }
+
+ return K;
+}
+
+Matrix66d computeSpringDamping(double linearStiffness, double linearDamping,
+ double angularStiffness, double angularDamping,
+ Vector3d targetPosition, Vector3d targetLinearVelocity,
+ Vector3d targetRotationVector, Vector3d targetAngularVelocity,
+ Vector6d dofPosition, Vector6d dofVelocity,
+ bool doComputeExtraTorque = false, Vector3d localApplicationPoint = Vector3d::Zero())
+{
+ Matrix66d D = Matrix66d::Zero();
+ const double epsilon = 1e-8;
+
+ for (size_t column = 0; column < 6; ++column)
+ {
+ Vector6d dofV = dofVelocity;
+ dofV[column] += 2.0 * epsilon;
+ Vector6d fXPlus2H = computeSpringForce(linearStiffness, linearDamping, angularStiffness, angularDamping,
+ targetPosition, targetLinearVelocity, targetRotationVector, targetAngularVelocity, dofPosition, dofV,
+ doComputeExtraTorque, localApplicationPoint);
+
+ dofV = dofVelocity;
+ dofV[column] += epsilon;
+ Vector6d fXPlusH = computeSpringForce(linearStiffness, linearDamping, angularStiffness, angularDamping,
+ targetPosition, targetLinearVelocity, targetRotationVector, targetAngularVelocity, dofPosition, dofV,
+ doComputeExtraTorque, localApplicationPoint);
+
+ dofV = dofVelocity;
+ dofV[column] -= epsilon;
+ Vector6d fXMinusH = computeSpringForce(linearStiffness, linearDamping, angularStiffness, angularDamping,
+ targetPosition, targetLinearVelocity, targetRotationVector, targetAngularVelocity, dofPosition, dofV,
+ doComputeExtraTorque, localApplicationPoint);
+
+ dofV = dofVelocity;
+ dofV[column] -= 2.0 * epsilon;
+ Vector6d fXMinus2H = computeSpringForce(linearStiffness, linearDamping, angularStiffness, angularDamping,
+ targetPosition, targetLinearVelocity, targetRotationVector, targetAngularVelocity, dofPosition, dofV,
+ doComputeExtraTorque, localApplicationPoint);
+
+ D.col(column) = - (-fXPlus2H + 8.0 * fXPlusH - 8.0 * fXMinusH + fXMinus2H) / (12.0 * epsilon);
+ }
+
+ return D;
+}
+}; // namespace anonymous
+
+TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceTest)
+{
+ double linearStiffness = 1.1;
+ double linearDamping = 1.2;
+ double angularStiffness = 1.3;
+ double angularDamping = 1.4;
+ Vector3d targetPosition = Vector3d::Ones();
+ Vector3d targetLinearVelocity = Vector3d::Ones() / 100.0;
+ double angle = 1.2354;
+ Vector3d axis = Vector3d(1.2, 0.4, 5.2).normalized();
+ Vector3d targetRotationVector = axis * angle;
+ Vector3d targetAngularVelocity = Vector3d::Ones() / 200.0;
+ Vector3d localAnchorPoint = Vector3d::Ones();
+
+ {
+ SCOPED_TRACE("With stiffness, with damping");
+
+ Eigen::AngleAxisd angleAxis(0.34512, Vector3d(1.1, -1.4, 3.23).normalized());
+ Vector3d t(1.1, 0.9, 1.02);
+ RigidTransform3d transform = makeRigidTransform(Quaterniond(angleAxis), t);
+
+ Vector6d dofX = Vector6d::Zero();
+ dofX.segment<3>(0) = transform.translation();
+ Eigen::AngleAxisd angleAxis2(transform.rotation());
+ dofX.segment<3>(3) = angleAxis2.axis() * angleAxis2.angle();
+ Vector6d dofV = Vector6d::Zero();
+
+ Vector6d FWithoutExtraTorque = computeSpringForce(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
+ Matrix66d KWithoutExtraTorque = computeSpringStiffness(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
+ Matrix66d DWithoutExtraTorque = computeSpringDamping(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
+
+ Vector6d FWithExtraTorque = computeSpringForce(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV, true, localAnchorPoint);
+ Matrix66d KWithExtraTorque = computeSpringStiffness(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV, true, localAnchorPoint);
+ Matrix66d DWithExtraTorque = computeSpringDamping(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV, true, localAnchorPoint);
+
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setShape(m_sphere);
+ SurgSim::Physics::RigidRepresentationState initialState;
+ initialState.setPose(transform);
+ rigidBody->setInitialState(initialState);
+ SurgSim::DataStructures::Location location(localAnchorPoint);
+
+ Vector6d F = FWithoutExtraTorque;
+ Matrix66d K = KWithoutExtraTorque, D = DWithoutExtraTorque;
+ rigidBody->addExternalGeneralizedForce(location, F, K, D);
+ F = rigidBody->getExternalGeneralizedForce();
+ K = rigidBody->getExternalGeneralizedStiffness();
+ D = rigidBody->getExternalGeneralizedDamping();
+
+ EXPECT_LE((F - FWithExtraTorque).cwiseAbs().maxCoeff(), 1e-7);
+ EXPECT_LE((K - KWithExtraTorque).cwiseAbs().maxCoeff(), 1e-7);
+ EXPECT_LE((D - DWithExtraTorque).cwiseAbs().maxCoeff(), 1e-7);
+ }
+
+ {
+ SCOPED_TRACE("With stiffness, without damping");
+
+ Eigen::AngleAxisd angleAxis(0.34512, Vector3d(1.1, -1.4, 3.23).normalized());
+ Vector3d t(1.1, 0.9, 1.02);
+ RigidTransform3d transform = makeRigidTransform(Quaterniond(angleAxis), t);
+
+ Vector6d dofX = Vector6d::Zero();
+ dofX.segment<3>(0) = transform.translation();
+ Eigen::AngleAxisd angleAxis2(transform.rotation());
+ dofX.segment<3>(3) = angleAxis2.axis() * angleAxis2.angle();
+ Vector6d dofV = Vector6d::Zero();
+
+ Vector6d FWithoutExtraTorque = computeSpringForce(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
+ Matrix66d KWithoutExtraTorque = computeSpringStiffness(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
+
+ Vector6d FWithExtraTorque = computeSpringForce(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV, true, localAnchorPoint);
+ Matrix66d KWithExtraTorque = computeSpringStiffness(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV, true, localAnchorPoint);
+
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setShape(m_sphere);
+ SurgSim::Physics::RigidRepresentationState initialState;
+ initialState.setPose(transform);
+ rigidBody->setInitialState(initialState);
+ SurgSim::DataStructures::Location location(localAnchorPoint);
+
+ Vector6d F = FWithoutExtraTorque;
+ Matrix66d K = KWithoutExtraTorque;
+ rigidBody->addExternalGeneralizedForce(location, F, K);
+ F = rigidBody->getExternalGeneralizedForce();
+ K = rigidBody->getExternalGeneralizedStiffness();
+
+ EXPECT_LE((F - FWithExtraTorque).cwiseAbs().maxCoeff(), 1e-7);
+ EXPECT_LE((K - KWithExtraTorque).cwiseAbs().maxCoeff(), 1e-7);
+ }
+
+ {
+ SCOPED_TRACE("Without stiffness, without damping");
+
+ Eigen::AngleAxisd angleAxis(0.34512, Vector3d(1.1, -1.4, 3.23).normalized());
+ Vector3d t(1.1, 0.9, 1.02);
+ RigidTransform3d transform = makeRigidTransform(Quaterniond(angleAxis), t);
+
+ Vector6d dofX = Vector6d::Zero();
+ dofX.segment<3>(0) = transform.translation();
+ Eigen::AngleAxisd angleAxis2(transform.rotation());
+ dofX.segment<3>(3) = angleAxis2.axis() * angleAxis2.angle();
+ Vector6d dofV = Vector6d::Zero();
+
+ Vector6d FWithoutExtraTorque = computeSpringForce(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
+
+ Vector6d FWithExtraTorque = computeSpringForce(linearStiffness, linearDamping,
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV, true, localAnchorPoint);
+
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setShape(m_sphere);
+ SurgSim::Physics::RigidRepresentationState initialState;
+ initialState.setPose(transform);
+ rigidBody->setInitialState(initialState);
+ SurgSim::DataStructures::Location location(localAnchorPoint);
+
+ Vector6d F = FWithoutExtraTorque;
+ rigidBody->addExternalGeneralizedForce(location, F);
+ F = rigidBody->getExternalGeneralizedForce();
+
+ EXPECT_LE((F - FWithExtraTorque).cwiseAbs().maxCoeff(), 1e-7);
+ }
+}
+
+TEST_F(RigidRepresentationTest, NoForceTorqueTest)
+{
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+
+ // Setup phase
+ rigidBody->setLocalActive(true);
+ rigidBody->setIsGravityEnabled(false);
+ rigidBody->setDensity(m_density);
+ rigidBody->setShape(m_sphere);
+
+ // Run few time steps
+ for (int timeStep = 0; timeStep < m_maxNumSimulationStepTest; timeStep++)
+ {
+ rigidBody->beforeUpdate(m_dt);
+ rigidBody->update(m_dt);
+ rigidBody->afterUpdate(m_dt);
+ }
+
+ const Vector3d G = rigidBody->getCurrentState().getPose().translation();
+ const Matrix33d& R = rigidBody->getCurrentState().getPose().linear();
+ const Quaterniond q = Quaterniond(R);
+ const Vector3d v = rigidBody->getCurrentState().getLinearVelocity();
+ const Vector3d w = rigidBody->getCurrentState().getAngularVelocity();
+ ASSERT_EQ(Vector3d::Zero(), G);
+ ASSERT_TRUE(q.isApprox(Quaterniond::Identity()));
+ ASSERT_EQ(Vector3d::Zero(), v);
+ ASSERT_EQ(Vector3d::Zero(), w);
+}
+
+TEST_F(RigidRepresentationTest, GravityTest)
+{
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ Vector3d gravity(0.0, -9.81, 0.0);
+
+ // Setup phase
+ rigidBody->setLocalActive(true);
+ rigidBody->setIsGravityEnabled(true);
+ rigidBody->setDensity(m_density);
+ rigidBody->setShape(m_sphere);
+
+ // Run few time steps
+ for (int timeStep = 0; timeStep < m_maxNumSimulationStepTest; timeStep++)
+ {
+ rigidBody->beforeUpdate(m_dt);
+ rigidBody->update(m_dt);
+ rigidBody->afterUpdate(m_dt);
+
+ const Vector3d G = rigidBody->getCurrentState().getPose().translation();
+ const Matrix33d& R = rigidBody->getCurrentState().getPose().linear();
+ const Quaterniond q = Quaterniond(R);
+ const Vector3d v = rigidBody->getCurrentState().getLinearVelocity();
+ const Vector3d w = rigidBody->getCurrentState().getAngularVelocity();
+
+ const Vector3d Gprev = rigidBody->getPreviousState().getPose().translation();
+ const Vector3d vprev = rigidBody->getPreviousState().getLinearVelocity();
+
+ // Implicit numerical integration gives v(t+dt) = v(t) + dt.a(t+dt) = v(t) + dt.g
+ // p(t+dt) = p(t) + dt.v(t+dt) = p(t) + dt.v(t) + dt^2.g
+ Vector3d tmpV = vprev + gravity * m_dt;
+ double diffV = (v - tmpV).norm();
+ Vector3d tmpG = Gprev + tmpV * m_dt;
+ double diffG = (G - tmpG).norm();
+
+ ASSERT_NEAR(0.0, diffG, epsilon);
+ ASSERT_TRUE(q.isApprox(Quaterniond::Identity()));
+ ASSERT_NEAR(0.0, diffV, epsilon);
+ ASSERT_EQ(Vector3d::Zero(), w);
+ }
+}
+
+TEST_F(RigidRepresentationTest, PreviousStateDifferentFromCurrentTest)
+{
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ Vector3d gravity(0.0, -9.81, 0.0);
+
+ // Setup phase
+ rigidBody->setLocalActive(true);
+ rigidBody->setIsGravityEnabled(true);
+ rigidBody->setDensity(m_density);
+ rigidBody->setShape(m_sphere);
+
+ // Run few time steps
+ for (int timeStep = 0; timeStep < m_maxNumSimulationStepTest; timeStep++)
+ {
+ rigidBody->beforeUpdate(m_dt);
+ rigidBody->update(m_dt);
+ rigidBody->afterUpdate(m_dt);
+
+ ASSERT_NE(rigidBody->getPreviousState(), rigidBody->getCurrentState());
+ }
+}
+
+void disableWhenDivergeTest(std::shared_ptr<RigidRepresentation> rigidBody,
+ const RigidRepresentationState& state, double dt)
+{
+ // Setup phase
+ rigidBody->setLocalActive(true);
+ rigidBody->setIsGravityEnabled(true);
+ rigidBody->setInitialState(state);
+
+ // Run 1 time step and make sure that the rigid body has been disabled
+ // The rotation explode under the angular velocity too strong !
+ {
+ ASSERT_TRUE(rigidBody->isActive());
+
+ rigidBody->beforeUpdate(dt);
+ rigidBody->update(dt);
+ rigidBody->afterUpdate(dt);
+
+ ASSERT_FALSE(rigidBody->isActive());
+ }
+}
+
+TEST_F(RigidRepresentationTest, DisableWhenDivergeTest)
+{
+ Quaterniond q = Quaterniond::Identity();
+ Vector3d vZero = Vector3d::Zero();
+ Vector3d vMax = Vector3d::Constant(std::numeric_limits<double>::max());
+
+ // Test linear failure (with Signaling Nan)
+ {
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setDensity(m_density);
+ rigidBody->setShape(m_sphere);
+
+ // Sets the state
+ m_state.reset();
+ m_state.setPose(SurgSim::Math::makeRigidTransform(q, vMax));
+ m_state.setLinearVelocity(Vector3d::Constant(std::numeric_limits<double>::signaling_NaN()));
+
+ SCOPED_TRACE("Testing linear with Signaling Nan");
+ disableWhenDivergeTest(rigidBody, m_state, m_dt);
+ }
+
+ // Test linear failure (with quiet Nan)
+ {
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setDensity(m_density);
+ rigidBody->setShape(m_sphere);
+
+ // Sets the state
+ m_state.reset();
+ m_state.setPose(makeRigidTransform(q, vMax));
+ m_state.setLinearVelocity(Vector3d::Constant(std::numeric_limits<double>::quiet_NaN()));
+
+ SCOPED_TRACE("Testing linear with Quiet Nan");
+ disableWhenDivergeTest(rigidBody, m_state, m_dt);
+ }
+
+ // Test linear failure (with numerical maximum value)
+ {
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setDensity(m_density);
+ rigidBody->setShape(m_sphere);
+
+ // Sets the state
+ m_state.reset();
+ m_state.setPose(makeRigidTransform(q, vMax));
+ m_state.setLinearVelocity(Vector3d::Constant(std::numeric_limits<double>::max()));
+
+ SCOPED_TRACE("Testing linear with double max");
+ disableWhenDivergeTest(rigidBody, m_state, m_dt);
+ }
+
+ // Test angular failure (with Signaling Nan)
+ {
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setDensity(m_density);
+ rigidBody->setShape(m_sphere);
+
+ // Sets the state
+ m_state.reset();
+ m_state.setPose(makeRigidTransform(q, vZero));
+ m_state.setAngularVelocity(Vector3d::Constant(std::numeric_limits<double>::signaling_NaN()));
+
+ SCOPED_TRACE("Testing angular with Signaling Nan");
+ disableWhenDivergeTest(rigidBody, m_state, m_dt);
+ }
+
+ // Test angular failure (with quiet Nan)
+ {
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setDensity(m_density);
+ rigidBody->setShape(m_sphere);
+
+ // Sets the state
+ m_state.reset();
+ m_state.setPose(makeRigidTransform(q, vZero));
+ m_state.setAngularVelocity(Vector3d::Constant(std::numeric_limits<double>::quiet_NaN()));
+
+ SCOPED_TRACE("Testing angular with Quiet Nan");
+ disableWhenDivergeTest(rigidBody, m_state, m_dt);
+ }
+
+ // Test angular failure (with numerical maximum value)
+ {
+ // Create the rigid body
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setDensity(m_density);
+ rigidBody->setShape(m_sphere);
+
+ // Sets the state
+ m_state.reset();
+ m_state.setPose(makeRigidTransform(q, vZero));
+ m_state.setAngularVelocity(Vector3d::Constant(std::numeric_limits<double>::max()));
+
+ SCOPED_TRACE("Testing angular with double max");
+ disableWhenDivergeTest(rigidBody, m_state, m_dt);
+ }
+}
+
+TEST_F(RigidRepresentationTest, LocalizationCreation)
+{
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ Location loc(Vector3d(3.0, 2.0, 1.0));
+
+ std::shared_ptr<Localization> localization = rigidBody->createLocalization(loc);
+ localization->setRepresentation(rigidBody);
+
+ Vector3d globalPos = rigidBody->getCurrentState().getPose() * loc.rigidLocalPosition.getValue();
+
+ EXPECT_TRUE(globalPos.isApprox(localization->calculatePosition(0.0)));
+ EXPECT_TRUE(globalPos.isApprox(localization->calculatePosition(1.0)));
+
+}
+
+TEST_F(RigidRepresentationTest, InvalidShapes)
+{
+ std::shared_ptr<Shape> shapeWithNoVolume = std::make_shared<PlaneShape>();
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+
+ {
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setShape(shapeWithNoVolume);
+
+ std::shared_ptr<Component> component = rigidBody;
+ EXPECT_THROW(component->initialize(runtime), SurgSim::Framework::AssertionFailure);
+ }
+}
+
+TEST_F(RigidRepresentationTest, WithMeshShape)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>("config.txt");
+
+ std::shared_ptr<SurgSim::Math::MeshShape> shape = std::make_shared<SurgSim::Math::MeshShape>();
+ shape->load("MeshShapeData/staple_collision.ply");
+ EXPECT_TRUE(shape->isValid());
+
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+ rigidBody->setShape(shape);
+
+ EXPECT_NO_THROW(rigidBody->initialize(runtime));
+ EXPECT_TRUE(shape->isValid());
+
+ std::shared_ptr<RigidRepresentation> rigidBody2 = std::make_shared<RigidRepresentation>("Rigid2");
+ rigidBody2->setShape(shape);
+ EXPECT_NO_THROW(rigidBody2->initialize(runtime));
+}
+
+TEST_F(RigidRepresentationTest, CollisionRepresentationTest)
+{
+ auto rigidBody(std::make_shared<RigidRepresentation>("Rigid"));
+
+ auto collision1(std::make_shared<RigidCollisionRepresentation>("collision1"));
+ auto collision2(std::make_shared<RigidCollisionRepresentation>("collision2"));
+
+ EXPECT_EQ(nullptr, rigidBody->getCollisionRepresentation());
+ EXPECT_NO_THROW(rigidBody->setCollisionRepresentation(collision1));
+ EXPECT_EQ(collision1, rigidBody->getCollisionRepresentation());
+ EXPECT_EQ(rigidBody, collision1->getRigidRepresentation());
+
+ // Change the collision object
+ EXPECT_NO_THROW(rigidBody->setCollisionRepresentation(collision2));
+ EXPECT_EQ(collision2, rigidBody->getCollisionRepresentation());
+ EXPECT_EQ(rigidBody, collision2->getRigidRepresentation());
+ EXPECT_EQ(nullptr, collision1->getRigidRepresentation());
+
+ // Clear the collision representation
+ EXPECT_NO_THROW(rigidBody->setCollisionRepresentation(nullptr));
+ EXPECT_EQ(nullptr, rigidBody->getCollisionRepresentation());
+ EXPECT_EQ(nullptr, collision1->getRigidRepresentation());
+ EXPECT_EQ(nullptr, collision2->getRigidRepresentation());
+}
+
+TEST_F(RigidRepresentationTest, DensityWithSphereShapeTest)
+{
+ std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
+
+ // Mass density
+ rigidBody->setDensity(m_density);
+
+ // Shape
+ rigidBody->setShape(m_sphere);
+
+ // Test inertia
+ EXPECT_TRUE(rigidBody->getLocalInertia().isApprox(m_inertia));
+
+ // Test mass
+ EXPECT_EQ(m_mass, rigidBody->getMass());
+
+ // Test mass center
+ EXPECT_TRUE(rigidBody->getMassCenter().isZero());
+}
+
+
+TEST_F(RigidRepresentationTest, SerializationTest)
+{
+ {
+ SCOPED_TRACE("Encode/Decode as shared_ptr<>, should be OK");
+ auto rigidRepresentation = std::make_shared<RigidRepresentation>("TestRigidRepresentation");
+ YAML::Node node;
+ ASSERT_NO_THROW(node =
+ YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::encode(rigidRepresentation));
+
+ std::shared_ptr<RigidRepresentation> newRepresentation;
+ EXPECT_NO_THROW(newRepresentation =
+ std::dynamic_pointer_cast<RigidRepresentation>(node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ }
+
+ {
+ SCOPED_TRACE("Encode a RigidRepresentation object without a shape, should throw.");
+ auto rigidRepresentation = std::make_shared<RigidRepresentation>("TestRigidRepresentation");
+ auto rigidCollisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("RigidCollisionRepresentation");
+
+ rigidRepresentation->setCollisionRepresentation(rigidCollisionRepresentation);
+
+ EXPECT_ANY_THROW(YAML::convert<SurgSim::Framework::Component>::encode(*rigidRepresentation));
+ }
+
+ {
+ SCOPED_TRACE("Encode a RigidRepresentation object with valid RigidCollisionRepresentation and shape, no throw");
+ auto rigidRepresentation = std::make_shared<RigidRepresentation>("TestRigidRepresentation");
+ auto rigidCollisionRepresentation =
+ std::make_shared<RigidCollisionRepresentation>("RigidCollisionRepresentation");
+
+ rigidRepresentation->setCollisionRepresentation(rigidCollisionRepresentation);
+ rigidRepresentation->setDensity(0.1);
+ rigidRepresentation->setLinearDamping(0.2);
+ rigidRepresentation->setAngularDamping(0.3);
+ rigidRepresentation->setShape(m_sphere);
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*rigidRepresentation));
+
+ std::shared_ptr<RigidRepresentation> newRepresentation;
+ newRepresentation =
+ std::dynamic_pointer_cast<RigidRepresentation>(node.as<std::shared_ptr<SurgSim::Framework::Component>>());
+ EXPECT_NE(nullptr, newRepresentation->getCollisionRepresentation());
+ EXPECT_NEAR(0.1, newRepresentation->getValue<double>("Density"), epsilon);
+ EXPECT_NEAR(0.2, newRepresentation->getValue<double>("LinearDamping"), epsilon);
+ EXPECT_NEAR(0.3, newRepresentation->getValue<double>("AngularDamping"), epsilon);
+
+ // Shape is encoded/decoded as concrete object instead of reference/shared_ptr<>.
+ EXPECT_NE(m_sphere, newRepresentation->getValue<std::shared_ptr<SurgSim::Math::Shape>>("Shape"));
+
+ auto decodedShape = newRepresentation->getValue<std::shared_ptr<SurgSim::Math::Shape>>("Shape");
+ EXPECT_EQ(m_sphere->getClassName(), decodedShape->getClassName());
+ EXPECT_NEAR(m_sphere->getVolume(), decodedShape->getVolume(), epsilon);
+
+ auto newCollisionRepresentation =
+ std::dynamic_pointer_cast<RigidCollisionRepresentation>(newRepresentation->getCollisionRepresentation());
+ EXPECT_EQ(newRepresentation, newCollisionRepresentation->getRigidRepresentation());
+ }
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/SolveMlcpTests.cpp b/SurgSim/Physics/UnitTests/SolveMlcpTests.cpp
new file mode 100644
index 0000000..d6d43ce
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/SolveMlcpTests.cpp
@@ -0,0 +1,91 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 SolveMlcpTests.cpp
+/// Simple Test for SolveMlcp computation
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/SolveMlcp.h"
+
+#include "SurgSim/Testing/MlcpIO/MlcpTestData.h"
+#include "SurgSim/Testing/MlcpIO/ReadText.h"
+
+using SurgSim::Physics::PhysicsManagerState;
+using SurgSim::Physics::SolveMlcp;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+TEST(SolveMlcpTest, CanConstruct)
+{
+ ASSERT_NO_THROW({std::shared_ptr<SolveMlcp> solveMlcpComputation = std::make_shared<SolveMlcp>();});
+}
+
+static void testMlcp(std::string filename, double contactTolerance, double solverPrecision, int maxIteration)
+{
+ std::shared_ptr<MlcpTestData> data = loadTestData(filename);
+ ASSERT_NE(nullptr, data) << "Could not load data file 'mlcpOriginalTest.txt'";
+
+ std::shared_ptr<PhysicsManagerState> state = std::make_shared<PhysicsManagerState>();
+ // default parameter explicitly marked just to be clear, we are working in 1 instance of the state, no copy.
+ std::shared_ptr<SolveMlcp> solveMlcpComputation = std::make_shared<SolveMlcp>(false);
+ double dt = 1e-3;
+
+ solveMlcpComputation->setContactTolerance(contactTolerance);
+ solveMlcpComputation->setSolverPrecision(solverPrecision);
+ solveMlcpComputation->setMaxIterations(maxIteration);
+
+ // Copy the MlcpProblem data over into the input state
+ state->getMlcpProblem().A = data->problem.A;
+ state->getMlcpProblem().b = data->problem.b;
+ state->getMlcpProblem().constraintTypes = data->problem.constraintTypes;
+ state->getMlcpProblem().mu = data->problem.mu;
+
+ // Allocate the MlcpSolution to store the output
+ state->getMlcpSolution().x.resize(state->getMlcpProblem().b.size());
+ state->getMlcpSolution().x.setZero();
+
+ // Solve the mlcp problem and store the solution in the output state
+ state = solveMlcpComputation->update(dt, state);
+
+ // Compare the mlcp solution with the expected one
+ EXPECT_TRUE(state->getMlcpSolution().x.isApprox(data->expectedLambda, epsilon)) <<
+ "lambda:" << std::endl << state->getMlcpSolution().x.transpose() << std::endl <<
+ "expected:" << std::endl << data->expectedLambda.transpose() << std::endl;
+}
+
+TEST(SolveMlcpTest, TestOriginalMlcp)
+{
+ testMlcp("mlcpOriginalTest.txt", 2e-4, 1e-4, 30);
+}
+
+TEST(SolveMlcpTest, TestSequenceMlcps)
+{
+ for (int i = 0; i <= 9; ++i)
+ {
+ std::ostringstream scopeName;
+ scopeName << "Testing Mlcp " << i;
+ SCOPED_TRACE(scopeName.str());
+
+ testMlcp(getTestFileName("mlcpTest", i, ".txt"), 1e-9, 1e-9, 100);
+ }
+}
diff --git a/SurgSim/Physics/UnitTests/VirtualToolCouplerTest.cpp b/SurgSim/Physics/UnitTests/VirtualToolCouplerTest.cpp
new file mode 100644
index 0000000..2d99ab3
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/VirtualToolCouplerTest.cpp
@@ -0,0 +1,566 @@
+//// This file is a part of the OpenSurgSim project.
+//// Copyright 2013, SimQuest Solutions Inc.
+////
+//// Licensed under the Apache License, Version 2.0 (the "License");
+//// you may not use this file except in compliance with the License.
+//// You may obtain a copy of the License at
+////
+//// http://www.apache.org/licenses/LICENSE-2.0
+////
+//// Unless required by applicable law or agreed to in writing, software
+//// distributed under the License is distributed on an "AS IS" BASIS,
+//// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//// See the License for the specific language governing permissions and
+//// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <yaml-cpp/yaml.h>
+
+#include <math.h>
+
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationState.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+#include "SurgSim/Physics/VirtualToolCoupler.h"
+
+using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Input::InputComponent;
+using SurgSim::Framework::Runtime;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+const double epsilon = 1e-4;
+
+struct VirtualToolCouplerTest : public ::testing::Test
+{
+ virtual void SetUp()
+ {
+ representationName = "Rigid Representation";
+ inputDeviceName = "Device";
+
+ rigidBody = std::make_shared<RigidRepresentation>(representationName);
+ rigidBody->setDensity(700.0);
+ rigidBody->setAngularDamping(0.0);
+ rigidBody->setLinearDamping(0.0);
+ rigidBody->setShape(std::make_shared<SphereShape>(0.1));
+
+ RigidRepresentationState state;
+ state.setAngularVelocity(Vector3d::Zero());
+ state.setLinearVelocity(Vector3d::Zero());
+ state.setPose(RigidTransform3d::Identity());
+ rigidBody->setInitialState(state);
+
+ input = std::make_shared<InputComponent>("Input");
+ input->setDeviceName(inputDeviceName);
+ device = std::make_shared<IdentityPoseDevice>(inputDeviceName);
+ input->connectDevice(device);
+
+ virtualToolCoupler = std::make_shared<MockVirtualToolCoupler>();
+ virtualToolCoupler->setInput(input);
+ virtualToolCoupler->setRepresentation(rigidBody);
+ }
+
+ virtual void TearDown()
+ {
+ }
+
+ std::shared_ptr<RigidRepresentation> rigidBody;
+ std::shared_ptr<MockVirtualToolCoupler> virtualToolCoupler;
+ std::shared_ptr<InputComponent> input;
+ std::shared_ptr<IdentityPoseDevice> device;
+
+protected:
+ void runSystem(const int numSteps, const double dt = 0.001)
+ {
+ for(int step=0; step<numSteps; step++)
+ {
+ virtualToolCoupler->update(dt);
+ rigidBody->beforeUpdate(dt);
+ rigidBody->update(dt);
+ rigidBody->afterUpdate(dt);
+ }
+ }
+
+ void checkLinearIsCriticallyDamped()
+ {
+ RigidTransform3d initialPose = RigidTransform3d::Identity();
+ initialPose.translation() = Vector3d(0.1, 0.0, 0.0);
+ rigidBody->setLocalPose(initialPose);
+ rigidBody->setIsGravityEnabled(false);
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ virtualToolCoupler->initialize(runtime);
+ rigidBody->initialize(runtime);
+ virtualToolCoupler->wakeUp();
+ rigidBody->wakeUp();
+
+ // Critically damped mass-damper systems settle to within 5% of their
+ // equilibrium when:
+ // t = 4.744 * naturalFrequency;
+ // we will run the system to this point, and then check that the
+ // final position is 5.1% of its initial position
+ double mass = rigidBody->getMass();
+ double stiffness = virtualToolCoupler->getLinearStiffness();
+ double naturalFrequency = sqrt(stiffness / mass);
+ double expectedSettlingTime = 4.744 / naturalFrequency;
+ const int NUM_STEPS = 1000;
+ double dt = expectedSettlingTime / NUM_STEPS;
+ Vector3d previousPosition = initialPose.translation();
+ Vector3d currentPosition;
+ for(int step=0; step<NUM_STEPS; step++)
+ {
+ virtualToolCoupler->update(dt);
+ rigidBody->beforeUpdate(dt);
+ rigidBody->update(dt);
+ rigidBody->afterUpdate(dt);
+ currentPosition = rigidBody->getCurrentState().getPose().translation();
+ // Check for exponential behavior. The position should monotonically decrease and not oscillate.
+ ASSERT_TRUE((previousPosition.array() >= currentPosition.array()).all());
+ previousPosition = currentPosition;
+ }
+ Vector3d expectedPosition = 0.051 * initialPose.translation();
+ EXPECT_TRUE((expectedPosition.array() >= currentPosition.array()).all());
+ }
+
+ void checkAngularIsCriticallyDamped()
+ {
+ double initialAngle = M_PI_4;
+ RigidTransform3d initialPose = RigidTransform3d::Identity();
+ initialPose.linear() = Matrix33d(Eigen::AngleAxisd(initialAngle, Vector3d::UnitY()));
+ rigidBody->setLocalPose(initialPose);
+ rigidBody->setIsGravityEnabled(false);
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ virtualToolCoupler->initialize(runtime);
+ rigidBody->initialize(runtime);
+ virtualToolCoupler->wakeUp();
+ rigidBody->wakeUp();
+
+ // Critically damped mass-damper systems settle to within 5% of their
+ // equilibrium when:
+ // t = 4.744 * naturalFrequency;
+ // we will run the system to this point, and then check that the
+ // final position is 5.1% of its initial position
+ double inertia = rigidBody->getLocalInertia()(1,1);
+ double stiffness = virtualToolCoupler->getAngularStiffness();
+ double naturalFrequency = sqrt(stiffness / inertia);
+ double expectedSettlingTime = 4.744 / naturalFrequency;
+ const int NUM_STEPS = 500;
+ double dt = expectedSettlingTime / NUM_STEPS;
+ double previousAngle = initialAngle;
+ double currentAngle;
+ for(int step=0; step<NUM_STEPS; step++)
+ {
+ virtualToolCoupler->update(dt);
+ rigidBody->beforeUpdate(dt);
+ rigidBody->update(dt);
+ rigidBody->afterUpdate(dt);
+ RigidTransform3d currentPose = rigidBody->getCurrentState().getPose();
+ currentAngle = atan2(-currentPose(2, 0), currentPose(0, 0));
+ // Check for exponential behavior. The angle should monotonically decrease and not oscillate.
+ ASSERT_GT(previousAngle, currentAngle);
+ previousAngle = currentAngle;
+ }
+ EXPECT_GT(0.051 * initialAngle, currentAngle);
+ }
+
+ std::string representationName;
+ std::string inputDeviceName;
+};
+
+TEST_F(VirtualToolCouplerTest, LinearDisplacement)
+{
+ const double mass = rigidBody->getMass();
+ virtualToolCoupler->overrideAngularDamping(mass * 1.0);
+ virtualToolCoupler->overrideAngularStiffness(mass * 200);
+ virtualToolCoupler->overrideLinearDamping(mass * 50);
+ virtualToolCoupler->overrideLinearStiffness(mass * 200);
+
+ RigidTransform3d initialPose = RigidTransform3d::Identity();
+ initialPose.translation() = Vector3d(0.1, 0.0, 0.0);
+ rigidBody->setLocalPose(initialPose);
+ rigidBody->setIsGravityEnabled(false);
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ virtualToolCoupler->initialize(runtime);
+ rigidBody->initialize(runtime);
+ virtualToolCoupler->wakeUp();
+ rigidBody->wakeUp();
+
+ EXPECT_FALSE(rigidBody->getCurrentState().getPose().translation().isZero(epsilon));
+
+ EXPECT_TRUE(rigidBody->isActive());
+ runSystem(2);
+ EXPECT_TRUE(rigidBody->isActive());
+
+ EXPECT_FALSE(rigidBody->getCurrentState().getLinearVelocity().isZero(epsilon));
+ EXPECT_TRUE(rigidBody->getCurrentState().getAngularVelocity().isZero(epsilon));
+
+ runSystem(2000);
+ EXPECT_TRUE(rigidBody->isActive());
+
+ RigidRepresentationState state = rigidBody->getCurrentState();
+ EXPECT_TRUE(state.getLinearVelocity().isZero(epsilon));
+ EXPECT_TRUE(state.getAngularVelocity().isZero(epsilon));
+ EXPECT_TRUE(state.getPose().translation().isZero(epsilon));
+
+ Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(state.getPose().linear());
+ EXPECT_NEAR(0.0, angleAxis.angle(), epsilon);
+}
+
+TEST_F(VirtualToolCouplerTest, LinearDisplacementWithInertialTorques)
+{
+ const double mass = rigidBody->getMass();
+ virtualToolCoupler->overrideAngularDamping(mass * 2);
+ virtualToolCoupler->overrideAngularStiffness(mass * 20);
+ virtualToolCoupler->overrideLinearDamping(mass * 5);
+ virtualToolCoupler->overrideLinearStiffness(mass * 20);
+
+ const Vector3d attachmentPoint = Vector3d::UnitY();
+ virtualToolCoupler->overrideAttachmentPoint(attachmentPoint);
+ virtualToolCoupler->setCalculateInertialTorques(true);
+
+ RigidTransform3d expectedFinalPose = RigidTransform3d::Identity();
+ expectedFinalPose.translation() = -attachmentPoint;
+ RigidTransform3d initialPose = RigidTransform3d::Identity();
+ initialPose.translation() = Vector3d(0.01, 0.0, 0.0) + expectedFinalPose.translation();
+ rigidBody->setLocalPose(initialPose);
+ rigidBody->setIsGravityEnabled(false);
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ virtualToolCoupler->initialize(runtime);
+ rigidBody->initialize(runtime);
+ virtualToolCoupler->wakeUp();
+ rigidBody->wakeUp();
+
+ RigidRepresentationState state = rigidBody->getCurrentState();
+ EXPECT_TRUE(state.getAngularVelocity().isZero(epsilon));
+ Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(state.getPose().linear());
+ EXPECT_NEAR(0.0, angleAxis.angle(), epsilon);
+
+ EXPECT_TRUE(rigidBody->isActive());
+ runSystem(2);
+ EXPECT_TRUE(rigidBody->isActive());
+ EXPECT_FALSE(rigidBody->getCurrentState().getAngularVelocity().isZero(epsilon));
+
+ runSystem(6000);
+ EXPECT_TRUE(rigidBody->isActive());
+
+ state = rigidBody->getCurrentState();
+ EXPECT_TRUE(state.getLinearVelocity().isZero(epsilon));
+ EXPECT_TRUE(state.getAngularVelocity().isZero(epsilon));
+ EXPECT_TRUE(state.getPose().isApprox(expectedFinalPose, epsilon));
+}
+
+TEST_F(VirtualToolCouplerTest, AngularDisplacement)
+{
+ const double mass = rigidBody->getMass();
+ virtualToolCoupler->overrideAngularDamping(mass * 1.0);
+ virtualToolCoupler->overrideAngularStiffness(mass * 200);
+ virtualToolCoupler->overrideLinearDamping(mass * 50);
+ virtualToolCoupler->overrideLinearStiffness(mass * 200);
+
+ RigidTransform3d initialPose = RigidTransform3d::Identity();
+ initialPose.linear() = Matrix33d(Eigen::AngleAxisd(M_PI/4.0, Vector3d::UnitY()));
+ rigidBody->setLocalPose(initialPose);
+ rigidBody->setIsGravityEnabled(false);
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ virtualToolCoupler->initialize(runtime);
+ rigidBody->initialize(runtime);
+ virtualToolCoupler->wakeUp();
+ rigidBody->wakeUp();
+
+ Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(rigidBody->getCurrentState().getPose().linear());
+ EXPECT_NEAR(M_PI_4, angleAxis.angle(), epsilon);
+
+ EXPECT_TRUE(rigidBody->isActive());
+ runSystem(2);
+ EXPECT_TRUE(rigidBody->isActive());
+ EXPECT_FALSE(rigidBody->getCurrentState().getAngularVelocity().isZero(epsilon));
+
+ runSystem(2000);
+ EXPECT_TRUE(rigidBody->isActive());
+
+ RigidRepresentationState state = rigidBody->getCurrentState();
+ EXPECT_TRUE(state.getLinearVelocity().isZero(epsilon));
+ EXPECT_TRUE(state.getAngularVelocity().isZero(epsilon));
+ EXPECT_TRUE(state.getPose().translation().isZero(epsilon));
+
+ angleAxis = Eigen::AngleAxisd(state.getPose().linear());
+ EXPECT_NEAR(0.0, angleAxis.angle(), epsilon);
+}
+
+TEST_F(VirtualToolCouplerTest, WithGravity)
+{
+ const double mass = rigidBody->getMass();
+ virtualToolCoupler->overrideAngularDamping(mass * 1.0 );
+ virtualToolCoupler->overrideAngularStiffness(mass * 200);
+ virtualToolCoupler->overrideLinearDamping(mass * 50);
+ virtualToolCoupler->overrideLinearStiffness(mass * 200);
+
+ RigidTransform3d initialPose = RigidTransform3d::Identity();
+ initialPose.translation() = Vector3d(0.0, 0.0, 0.0);
+ rigidBody->setLocalPose(initialPose);
+ rigidBody->setIsGravityEnabled(true);
+
+ const double stiffness = 1000;
+ virtualToolCoupler->overrideLinearStiffness(stiffness);
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ virtualToolCoupler->initialize(runtime);
+ rigidBody->initialize(runtime);
+ virtualToolCoupler->wakeUp();
+ rigidBody->wakeUp();
+
+ EXPECT_TRUE(rigidBody->getCurrentState().getPose().translation().isApprox(Vector3d::Zero(), epsilon));
+
+ EXPECT_TRUE(rigidBody->isActive());
+ runSystem(2000);
+ EXPECT_TRUE(rigidBody->isActive());
+
+ RigidRepresentationState state = rigidBody->getCurrentState();
+ EXPECT_TRUE(state.getLinearVelocity().isZero(epsilon));
+ EXPECT_TRUE(state.getAngularVelocity().isZero(epsilon));
+
+ Vector3d g = Vector3d(0.0, -9.81, 0.0);
+ Vector3d expectedPosition = rigidBody->getMass() * g / stiffness;
+ EXPECT_TRUE(state.getPose().translation().isApprox(expectedPosition, epsilon));
+
+ Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(state.getPose().linear());
+ EXPECT_NEAR(0.0, angleAxis.angle(), epsilon);
+}
+
+TEST_F(VirtualToolCouplerTest, DefaultLinearParameters)
+{
+ SCOPED_TRACE("Default Linear Parameters");
+ checkLinearIsCriticallyDamped();
+}
+
+TEST_F(VirtualToolCouplerTest, SetLinearStiffness)
+{
+ SCOPED_TRACE("Set Linear Stiffness");
+ virtualToolCoupler->overrideLinearStiffness(1.234);
+ checkLinearIsCriticallyDamped();
+}
+
+TEST_F(VirtualToolCouplerTest, SetLinearDamping)
+{
+ SCOPED_TRACE("Set Linear Damping");
+ virtualToolCoupler->overrideLinearDamping(500.2);
+ checkLinearIsCriticallyDamped();
+}
+
+TEST_F(VirtualToolCouplerTest, DefaultAngularParameters)
+{
+ SCOPED_TRACE("Default Linear Parameters");
+ checkAngularIsCriticallyDamped();
+}
+
+TEST_F(VirtualToolCouplerTest, SetAngularStiffness)
+{
+ SCOPED_TRACE("Set Angular Stiffness");
+ virtualToolCoupler->overrideAngularStiffness(1234.6);
+ checkAngularIsCriticallyDamped();
+}
+
+TEST_F(VirtualToolCouplerTest, SetAngularDamping)
+{
+ SCOPED_TRACE("Set Angular Damping");
+ virtualToolCoupler->overrideAngularDamping(0.1235);
+ checkAngularIsCriticallyDamped();
+}
+
+TEST_F(VirtualToolCouplerTest, GetInput)
+{
+ EXPECT_EQ(input, virtualToolCoupler->getInput());
+}
+
+TEST_F(VirtualToolCouplerTest, GetPoseName)
+{
+ EXPECT_EQ(SurgSim::DataStructures::Names::POSE, virtualToolCoupler->getPoseName());
+}
+
+TEST_F(VirtualToolCouplerTest, GetRigid)
+{
+ EXPECT_EQ(rigidBody, virtualToolCoupler->getRepresentation());
+}
+
+typedef SurgSim::DataStructures::OptionalValue<double> OptionalValued;
+typedef SurgSim::DataStructures::OptionalValue<Vector3d> OptionalValueVec3;
+
+TEST_F(VirtualToolCouplerTest, OptionalParams)
+{
+ double num = 2.56527676;
+ Vector3d vec(1.0, 2.0, 3.0);
+ {
+ SCOPED_TRACE("Getters");
+ EXPECT_FALSE(virtualToolCoupler->getOptionalLinearStiffness().hasValue());
+ virtualToolCoupler->overrideLinearStiffness(num);
+ const OptionalValued& linearStiffness = virtualToolCoupler->getOptionalLinearStiffness();
+ EXPECT_TRUE(linearStiffness.hasValue());
+ EXPECT_EQ(num, linearStiffness.getValue());
+
+ EXPECT_FALSE(virtualToolCoupler->getOptionalLinearDamping().hasValue());
+ virtualToolCoupler->overrideLinearDamping(num);
+ const OptionalValued& linearDamping = virtualToolCoupler->getOptionalLinearDamping();
+ EXPECT_TRUE(linearDamping.hasValue());
+ EXPECT_EQ(num, linearDamping.getValue());
+
+ EXPECT_FALSE(virtualToolCoupler->getOptionalAngularStiffness().hasValue());
+ virtualToolCoupler->overrideAngularStiffness(num);
+ const OptionalValued& angularStiffness = virtualToolCoupler->getOptionalAngularStiffness();
+ EXPECT_TRUE(angularStiffness.hasValue());
+ EXPECT_EQ(num, angularStiffness.getValue());
+
+ EXPECT_FALSE(virtualToolCoupler->getOptionalAngularDamping().hasValue());
+ virtualToolCoupler->overrideAngularDamping(num);
+ const OptionalValued& angularDamping = virtualToolCoupler->getOptionalAngularDamping();
+ EXPECT_TRUE(angularDamping.hasValue());
+ EXPECT_EQ(num, angularDamping.getValue());
+
+ EXPECT_FALSE(virtualToolCoupler->getOptionalAttachmentPoint().hasValue());
+ virtualToolCoupler->overrideAttachmentPoint(vec);
+ const OptionalValueVec3& attachmentPoint = virtualToolCoupler->getOptionalAttachmentPoint();
+ EXPECT_TRUE(attachmentPoint.hasValue());
+ EXPECT_TRUE(vec.isApprox(attachmentPoint.getValue()));
+ }
+ {
+ SCOPED_TRACE("Setters");
+ OptionalValued optionalNum;
+ optionalNum.setValue(num);
+
+ OptionalValueVec3 optionalVec;
+ optionalVec.setValue(vec);
+
+ virtualToolCoupler->overrideLinearStiffness(0.0);
+ EXPECT_NEAR(0.0, virtualToolCoupler->getOptionalLinearStiffness().getValue(), 1e-9);
+ virtualToolCoupler->setOptionalLinearStiffness(optionalNum);
+ const OptionalValued& linearStiffness = virtualToolCoupler->getOptionalLinearStiffness();
+ EXPECT_EQ(num, linearStiffness.getValue());
+
+ virtualToolCoupler->overrideLinearDamping(0.0);
+ EXPECT_NEAR(0.0, virtualToolCoupler->getOptionalLinearDamping().getValue(), 1e-9);
+ virtualToolCoupler->setOptionalLinearDamping(optionalNum);
+ const OptionalValued& linearDamping = virtualToolCoupler->getOptionalLinearDamping();
+ EXPECT_TRUE(linearDamping.hasValue());
+ EXPECT_EQ(num, linearDamping.getValue());
+
+ virtualToolCoupler->overrideAngularStiffness(0.0);
+ EXPECT_NEAR(0.0, virtualToolCoupler->getOptionalAngularStiffness().getValue(), 1e-9);
+ virtualToolCoupler->setOptionalAngularStiffness(optionalNum);
+ const OptionalValued& angularStiffness = virtualToolCoupler->getOptionalAngularStiffness();
+ EXPECT_TRUE(angularStiffness.hasValue());
+ EXPECT_EQ(num, angularStiffness.getValue());
+
+ virtualToolCoupler->overrideAngularDamping(0.0);
+ EXPECT_NEAR(0.0, virtualToolCoupler->getOptionalAngularDamping().getValue(), 1e-9);
+ virtualToolCoupler->setOptionalAngularDamping(optionalNum);
+ const OptionalValued& angularDamping = virtualToolCoupler->getOptionalAngularDamping();
+ EXPECT_TRUE(angularDamping.hasValue());
+ EXPECT_EQ(num, angularDamping.getValue());
+
+ virtualToolCoupler->overrideAttachmentPoint(Vector3d::Zero());
+ EXPECT_TRUE(virtualToolCoupler->getOptionalAttachmentPoint().getValue().isApprox(Vector3d::Zero()));
+ virtualToolCoupler->setOptionalAttachmentPoint(optionalVec);
+ const OptionalValueVec3& attachmentPoint = virtualToolCoupler->getOptionalAttachmentPoint();
+ EXPECT_TRUE(attachmentPoint.hasValue());
+ EXPECT_TRUE(vec.isApprox(attachmentPoint.getValue()));
+ }
+}
+
+TEST_F(VirtualToolCouplerTest, Serialization)
+{
+ double num = 3.6415;
+ Vector3d vec(28.4, -37.2, 91.8);
+
+ OptionalValued optionalNum;
+ optionalNum.setValue(num);
+ OptionalValueVec3 optionalVec;
+ optionalVec.setValue(vec);
+ virtualToolCoupler->setOptionalLinearStiffness(optionalNum);
+ virtualToolCoupler->setOptionalLinearDamping(optionalNum);
+ virtualToolCoupler->setOptionalAngularStiffness(optionalNum);
+ virtualToolCoupler->setOptionalAngularDamping(optionalNum);
+ virtualToolCoupler->setOptionalAttachmentPoint(optionalVec);
+ virtualToolCoupler->setCalculateInertialTorques(true);
+
+ // Encode
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*virtualToolCoupler););
+
+ // Decode
+ std::shared_ptr<SurgSim::Physics::VirtualToolCoupler> newVirtualToolCoupler;
+ EXPECT_NO_THROW(newVirtualToolCoupler = std::dynamic_pointer_cast<VirtualToolCoupler>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ // Verify
+ newVirtualToolCoupler->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ newVirtualToolCoupler->wakeUp();
+
+ EXPECT_EQ(num, newVirtualToolCoupler->getLinearStiffness());
+ EXPECT_EQ(num, newVirtualToolCoupler->getLinearDamping());
+ EXPECT_EQ(num, newVirtualToolCoupler->getAngularStiffness());
+ EXPECT_EQ(num, newVirtualToolCoupler->getAngularDamping());
+ EXPECT_TRUE(vec.isApprox(newVirtualToolCoupler->getAttachmentPoint()));
+ EXPECT_TRUE(newVirtualToolCoupler->getCalculateInertialTorques());
+
+ EXPECT_NE(nullptr, newVirtualToolCoupler->getInput());
+ EXPECT_NE(nullptr, newVirtualToolCoupler->getRepresentation());
+ EXPECT_EQ(nullptr, newVirtualToolCoupler->getOutput());
+
+ YAML::Node inputNode;
+ EXPECT_NO_THROW(inputNode = YAML::convert<SurgSim::Framework::Component>::encode(*input););
+
+ YAML::Node representationNode;
+ EXPECT_NO_THROW(representationNode = YAML::convert<SurgSim::Framework::Component>::encode(*rigidBody););
+
+ EXPECT_EQ(inputNode[input->getClassName()]["Id"].as<std::string>(),
+ node[virtualToolCoupler->getClassName()][input->getName()][input->getClassName()]["Id"].as<std::string>());
+}
+
+TEST_F(VirtualToolCouplerTest, OutputDataEntries)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ ASSERT_TRUE(virtualToolCoupler->initialize(runtime));
+ auto data = virtualToolCoupler->getOutputData();
+
+ EXPECT_EQ(1, data.poses().getNumEntries());
+ EXPECT_TRUE(data.poses().hasEntry(SurgSim::DataStructures::Names::INPUT_POSE));
+
+ EXPECT_EQ(4, data.vectors().getNumEntries());
+ EXPECT_TRUE(data.vectors().hasEntry(SurgSim::DataStructures::Names::FORCE));
+ EXPECT_TRUE(data.vectors().hasEntry(SurgSim::DataStructures::Names::TORQUE));
+ EXPECT_TRUE(data.vectors().hasEntry(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY));
+ EXPECT_TRUE(data.vectors().hasEntry(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY));
+
+ EXPECT_EQ(2, data.matrices().getNumEntries());
+ EXPECT_TRUE(data.matrices().hasEntry(SurgSim::DataStructures::Names::SPRING_JACOBIAN));
+ EXPECT_TRUE(data.matrices().hasEntry(SurgSim::DataStructures::Names::DAMPER_JACOBIAN));
+
+ EXPECT_EQ(0, data.scalars().getNumEntries());
+ EXPECT_EQ(0, data.integers().getNumEntries());
+ EXPECT_EQ(0, data.booleans().getNumEntries());
+ EXPECT_EQ(0, data.strings().getNumEntries());
+ EXPECT_EQ(0, data.customData().getNumEntries());
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/config.txt.in b/SurgSim/Physics/UnitTests/config.txt.in
new file mode 100644
index 0000000..22fbf60
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/config.txt.in
@@ -0,0 +1,2 @@
+${CMAKE_CURRENT_BINARY_DIR}/Data
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data
\ No newline at end of file
diff --git a/SurgSim/Physics/UpdateCollisionRepresentations.cpp b/SurgSim/Physics/UpdateCollisionRepresentations.cpp
new file mode 100644
index 0000000..f215e22
--- /dev/null
+++ b/SurgSim/Physics/UpdateCollisionRepresentations.cpp
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Physics/UpdateCollisionRepresentations.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Collision/Representation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+UpdateCollisionRepresentations::UpdateCollisionRepresentations(bool doCopyState) :
+ Computation(doCopyState)
+{
+
+}
+
+UpdateCollisionRepresentations::~UpdateCollisionRepresentations()
+{
+
+}
+
+std::shared_ptr<PhysicsManagerState> SurgSim::Physics::UpdateCollisionRepresentations::doUpdate(const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> representations =
+ result->getCollisionRepresentations();
+
+ std::for_each(representations.begin(), representations.end(),
+ [&dt](std::shared_ptr<SurgSim::Collision::Representation> representation)
+ {
+ representation->update(dt);
+ });
+
+ return result;
+}
+
+}
+}
+
diff --git a/SurgSim/Physics/UpdateCollisionRepresentations.h b/SurgSim/Physics/UpdateCollisionRepresentations.h
new file mode 100644
index 0000000..ac56444
--- /dev/null
+++ b/SurgSim/Physics/UpdateCollisionRepresentations.h
@@ -0,0 +1,45 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_UPDATECOLLISIONREPRESENTATIONS_H
+#define SURGSIM_PHYSICS_UPDATECOLLISIONREPRESENTATIONS_H
+
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+/// Computation that calls the CollisionRepresentations update() function
+class UpdateCollisionRepresentations : public Computation
+{
+public:
+ /// Constructor
+ /// \param doCopyState whether to copy the PhysicsManagerState on update
+ explicit UpdateCollisionRepresentations(bool doCopyState);
+
+ /// Destructor
+ virtual ~UpdateCollisionRepresentations();
+
+ virtual std::shared_ptr<PhysicsManagerState> doUpdate(
+ const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Physics/VirtualToolCoupler.cpp b/SurgSim/Physics/VirtualToolCoupler.cpp
new file mode 100644
index 0000000..bf77005
--- /dev/null
+++ b/SurgSim/Physics/VirtualToolCoupler.cpp
@@ -0,0 +1,481 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <Eigen/Eigenvalues>
+
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/LogMacros.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Input/OutputComponent.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+#include "SurgSim/Physics/VirtualToolCoupler.h"
+
+using SurgSim::Math::makeSkewSymmetricMatrix;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector6d;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Matrix66d;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Quaterniond;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::VirtualToolCoupler, VirtualToolCoupler);
+
+VirtualToolCoupler::VirtualToolCoupler(const std::string& name) :
+ SurgSim::Framework::Behavior(name),
+ m_poseName(SurgSim::DataStructures::Names::POSE),
+ m_linearStiffness(std::numeric_limits<double>::quiet_NaN()),
+ m_linearDamping(std::numeric_limits<double>::quiet_NaN()),
+ m_angularStiffness(std::numeric_limits<double>::quiet_NaN()),
+ m_angularDamping(std::numeric_limits<double>::quiet_NaN()),
+ m_localAttachmentPoint(Vector3d(std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
+ std::numeric_limits<double>::quiet_NaN())),
+ m_calculateInertialTorques(false),
+ m_poseIndex(-1),
+ m_linearVelocityIndex(-1),
+ m_angularVelocityIndex(-1),
+ m_forceIndex(-1),
+ m_torqueIndex(-1),
+ m_inputLinearVelocityIndex(-1),
+ m_inputAngularVelocityIndex(-1),
+ m_inputPoseIndex(-1),
+ m_springJacobianIndex(-1),
+ m_damperJacobianIndex(-1)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
+ LinearStiffness, getOptionalLinearStiffness, setOptionalLinearStiffness);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
+ LinearDamping, getOptionalLinearDamping, setOptionalLinearDamping);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
+ AngularStiffness, getOptionalAngularStiffness, setOptionalAngularStiffness);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
+ AngularDamping, getOptionalAngularDamping, setOptionalAngularDamping);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler,
+ SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>, AttachmentPoint,
+ getOptionalAttachmentPoint, setOptionalAttachmentPoint);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, bool, CalculateInertialTorques,
+ getCalculateInertialTorques, setCalculateInertialTorques);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, std::shared_ptr<SurgSim::Framework::Component>,
+ Input, getInput, setInput);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, std::shared_ptr<SurgSim::Framework::Component>,
+ Output, getOutput, setOutput);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, std::shared_ptr<SurgSim::Framework::Component>,
+ Representation, getRepresentation, setRepresentation);
+}
+
+VirtualToolCoupler::~VirtualToolCoupler()
+{
+}
+
+const std::shared_ptr<SurgSim::Framework::Component> VirtualToolCoupler::getInput()
+{
+ return m_input;
+}
+
+void VirtualToolCoupler::setInput(const std::shared_ptr<SurgSim::Framework::Component> input)
+{
+ m_input = std::dynamic_pointer_cast<SurgSim::Input::InputComponent>(input);
+}
+
+const std::shared_ptr<SurgSim::Framework::Component> VirtualToolCoupler::getOutput()
+{
+ return m_output;
+}
+
+void VirtualToolCoupler::setOutput(const std::shared_ptr<SurgSim::Framework::Component> output)
+{
+ m_output = std::dynamic_pointer_cast<SurgSim::Input::OutputComponent>(output);
+}
+
+const std::shared_ptr<SurgSim::Framework::Component> VirtualToolCoupler::getRepresentation()
+{
+ return m_rigid;
+}
+
+void VirtualToolCoupler::setRepresentation(const std::shared_ptr<SurgSim::Framework::Component> rigid)
+{
+ m_rigid = std::dynamic_pointer_cast<SurgSim::Physics::RigidRepresentation>(rigid);
+}
+
+const std::string& VirtualToolCoupler::getPoseName()
+{
+ return m_poseName;
+}
+
+void VirtualToolCoupler::setPoseName(const std::string& poseName)
+{
+ m_poseName = poseName;
+}
+
+void VirtualToolCoupler::update(double dt)
+{
+ SurgSim::DataStructures::DataGroup inputData;
+ m_input->getData(&inputData);
+ RigidTransform3d inputPose;
+ inputData.poses().cacheIndex(m_poseName, &m_poseIndex);
+ if (inputData.poses().get(m_poseIndex, &inputPose))
+ {
+ // TODO(ryanbeasley): If the RigidRepresentation is not colliding, we should turn off the VTC forces and set the
+ // RigidRepresentation's state to the input state.
+ Vector3d inputLinearVelocity, inputAngularVelocity;
+ inputLinearVelocity.setZero();
+ inputData.vectors().cacheIndex(SurgSim::DataStructures::Names::LINEAR_VELOCITY, &m_linearVelocityIndex);
+ inputData.vectors().get(m_linearVelocityIndex, &inputLinearVelocity);
+ inputAngularVelocity.setZero();
+ inputData.vectors().cacheIndex(SurgSim::DataStructures::Names::ANGULAR_VELOCITY, &m_angularVelocityIndex);
+ inputData.vectors().get(m_angularVelocityIndex, &inputAngularVelocity);
+
+ RigidRepresentationState objectState(m_rigid->getCurrentState());
+ RigidTransform3d objectPose(objectState.getPose());
+ Vector3d objectPosition = objectPose * m_rigid->getMassCenter();
+ Vector3d attachmentPoint = objectPose * m_localAttachmentPoint;
+ Vector3d leverArm = attachmentPoint - objectPosition;
+ Vector3d attachmentPointVelocity = objectState.getLinearVelocity();
+ attachmentPointVelocity += objectState.getAngularVelocity().cross(leverArm);
+
+ Vector3d force = m_linearStiffness * (inputPose.translation() - attachmentPoint);
+ force += m_linearDamping * (inputLinearVelocity - attachmentPointVelocity);
+
+ Vector3d rotationVector;
+ SurgSim::Math::computeRotationVector(inputPose, objectPose, &rotationVector);
+ Vector3d torque = m_angularStiffness * rotationVector;
+ torque += m_angularDamping * (inputAngularVelocity - objectState.getAngularVelocity());
+
+ const Matrix33d identity3x3 = Matrix33d::Identity();
+ const Matrix33d zero3x3 = Matrix33d::Zero();
+ const Matrix33d linearStiffnessMatrix = m_linearStiffness * identity3x3;
+ const Matrix33d linearDampingMatrix = m_linearDamping * identity3x3;
+ const Matrix33d angularStiffnessMatrix = m_angularStiffness * identity3x3;
+ const Matrix33d angularDampingMatrix = m_angularDamping * identity3x3;
+
+ Vector6d generalizedForce;
+ Matrix66d generalizedStiffness, generalizedDamping;
+ generalizedForce << force, torque;
+ generalizedStiffness << linearStiffnessMatrix, zero3x3,
+ zero3x3, angularStiffnessMatrix;
+ generalizedDamping << linearDampingMatrix, zero3x3,
+ zero3x3, angularDampingMatrix;
+
+ if (m_calculateInertialTorques)
+ {
+ SurgSim::DataStructures::Location location;
+ location.rigidLocalPosition.setValue(m_localAttachmentPoint);
+ m_rigid->addExternalGeneralizedForce(location, generalizedForce, generalizedStiffness, generalizedDamping);
+ }
+ else
+ {
+ m_rigid->addExternalGeneralizedForce(generalizedForce, generalizedStiffness, generalizedDamping);
+ }
+
+ if (m_output != nullptr)
+ {
+ m_outputData.vectors().set(m_forceIndex, -generalizedForce.segment<3>(0));
+ m_outputData.vectors().set(m_torqueIndex, -generalizedForce.segment<3>(3));
+ m_outputData.vectors().set(m_inputLinearVelocityIndex, inputLinearVelocity);
+ m_outputData.vectors().set(m_inputAngularVelocityIndex, inputAngularVelocity);
+
+ m_outputData.poses().set(m_inputPoseIndex, inputPose);
+
+ m_outputData.matrices().set(m_springJacobianIndex, -generalizedStiffness);
+ m_outputData.matrices().set(m_damperJacobianIndex, -generalizedDamping);
+
+ m_output->setData(m_outputData);
+ }
+ }
+}
+
+bool VirtualToolCoupler::doInitialize()
+{
+ m_outputData = buildOutputData();
+
+ m_forceIndex = m_outputData.vectors().getIndex(SurgSim::DataStructures::Names::FORCE);
+ m_torqueIndex = m_outputData.vectors().getIndex(SurgSim::DataStructures::Names::TORQUE);
+ m_inputLinearVelocityIndex = m_outputData.vectors().getIndex(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY);
+ m_inputAngularVelocityIndex =
+ m_outputData.vectors().getIndex(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY);
+ m_inputPoseIndex = m_outputData.poses().getIndex(SurgSim::DataStructures::Names::INPUT_POSE);
+ m_springJacobianIndex = m_outputData.matrices().getIndex(SurgSim::DataStructures::Names::SPRING_JACOBIAN);
+ m_damperJacobianIndex = m_outputData.matrices().getIndex(SurgSim::DataStructures::Names::DAMPER_JACOBIAN);
+
+ return true;
+}
+
+SurgSim::DataStructures::DataGroup VirtualToolCoupler::buildOutputData()
+{
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::FORCE);
+ builder.addVector(SurgSim::DataStructures::Names::TORQUE);
+ builder.addMatrix(SurgSim::DataStructures::Names::SPRING_JACOBIAN);
+ builder.addPose(SurgSim::DataStructures::Names::INPUT_POSE);
+ builder.addMatrix(SurgSim::DataStructures::Names::DAMPER_JACOBIAN);
+ builder.addVector(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY);
+ builder.addVector(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY);
+ return builder.createData();
+}
+
+bool VirtualToolCoupler::doWakeUp()
+{
+ if (m_input == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << "VirtualToolCoupler named " <<
+ getName() << " does not have an Input Component.";
+ return false;
+ }
+ if (m_rigid == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << "VirtualToolCoupler named " <<
+ getName() << " does not have a Representation.";
+ return false;
+ }
+
+ if (m_optionalAttachmentPoint.hasValue())
+ {
+ m_localAttachmentPoint = m_optionalAttachmentPoint.getValue();
+ }
+ else
+ {
+ m_localAttachmentPoint = m_rigid->getMassCenter();
+ }
+
+ // Provide sensible defaults based on the rigid representation.
+ // If one or both of the stiffness and damping are not provided, they are
+ // calculated to provide a critically damped system (dampingRatio-1.0).
+ // For a mass-spring system, the damping ratio is defined as:
+ //
+ // dampingRatio = (damping) / (2 * sqrt(mass * stiffness))
+ //
+ double dampingRatio = 1.0;
+ double mass = m_rigid->getMass();
+ if (!m_optionalLinearDamping.hasValue())
+ {
+ if (m_optionalLinearStiffness.hasValue())
+ {
+ m_linearStiffness = m_optionalLinearStiffness.getValue();
+ }
+ else
+ {
+ m_linearStiffness = mass * 100.0;
+ }
+ m_linearDamping = 2.0 * dampingRatio * sqrt(mass * m_linearStiffness);
+ }
+ else
+ {
+ m_linearDamping = m_optionalLinearDamping.getValue();
+ if (m_optionalLinearStiffness.hasValue())
+ {
+ m_linearStiffness = m_optionalLinearStiffness.getValue();
+ }
+ else
+ {
+ m_linearStiffness = pow(m_linearDamping / dampingRatio, 2) / (4.0 * mass);
+ }
+ }
+
+ Matrix33d leverArmMatrix = makeSkewSymmetricMatrix((m_localAttachmentPoint - m_rigid->getMassCenter()).eval());
+ const Matrix33d& inertia = m_rigid->getLocalInertia() + mass * leverArmMatrix * leverArmMatrix;
+ double maxInertia = inertia.eigenvalues().real().maxCoeff();
+ if (!m_optionalAngularDamping.hasValue())
+ {
+ if (m_optionalAngularStiffness.hasValue())
+ {
+ m_angularStiffness = m_optionalAngularStiffness.getValue();
+ }
+ else
+ {
+ m_angularStiffness = maxInertia * 1000.0;
+ }
+ m_angularDamping = 2.0 * dampingRatio * sqrt(maxInertia * m_angularStiffness);
+ }
+ else
+ {
+ m_angularDamping = m_optionalAngularDamping.getValue();
+ if (m_optionalAngularStiffness.hasValue())
+ {
+ m_angularStiffness = m_optionalAngularStiffness.getValue();
+ }
+ else
+ {
+ m_angularStiffness = pow(m_angularDamping / dampingRatio, 2) / (4.0 * maxInertia);
+ }
+ }
+
+
+ return true;
+}
+
+int VirtualToolCoupler::getTargetManagerType() const
+{
+ return SurgSim::Framework::MANAGER_TYPE_PHYSICS;
+}
+
+void VirtualToolCoupler::overrideLinearStiffness(double linearStiffness)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot override vtc parameter after it has initialized";
+
+ m_optionalLinearStiffness.setValue(linearStiffness);
+ m_linearStiffness = linearStiffness;
+}
+
+double VirtualToolCoupler::getLinearStiffness()
+{
+ SURGSIM_ASSERT(isAwake() || m_optionalLinearStiffness.hasValue())
+ << "Vtc parameter has not been initialized";
+
+ return m_linearStiffness;
+}
+
+void VirtualToolCoupler::overrideLinearDamping(double linearDamping)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot override vtc parameter after it has initialized";
+
+ m_optionalLinearDamping.setValue(linearDamping);
+ m_linearDamping = linearDamping;
+}
+
+double VirtualToolCoupler::getLinearDamping()
+{
+ SURGSIM_ASSERT(isAwake() || m_optionalLinearDamping.hasValue())
+ << "Vtc parameter has not been initialized";
+
+ return m_linearDamping;
+}
+
+void VirtualToolCoupler::overrideAngularStiffness(double angularStiffness)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot override vtc parameter after it has initialized";
+
+ m_optionalAngularStiffness.setValue(angularStiffness);
+ m_angularStiffness = angularStiffness;
+}
+
+double VirtualToolCoupler::getAngularStiffness()
+{
+ SURGSIM_ASSERT(isAwake() || m_optionalAngularStiffness.hasValue())
+ << "Vtc parameter has not been initialized";
+
+ return m_angularStiffness;
+}
+
+void VirtualToolCoupler::overrideAngularDamping(double angularDamping)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot override vtc parameter after it has initialized";
+
+ m_optionalAngularDamping.setValue(angularDamping);
+ m_angularDamping = angularDamping;
+}
+
+double VirtualToolCoupler::getAngularDamping()
+{
+ SURGSIM_ASSERT(isAwake() || m_optionalAngularDamping.hasValue())
+ << "Vtc parameter has not been initialized";
+
+ return m_angularDamping;
+}
+
+void VirtualToolCoupler::overrideAttachmentPoint(const SurgSim::Math::Vector3d& attachment)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot override vtc parameter after it has initialized";
+
+ m_optionalAttachmentPoint.setValue(attachment);
+ m_localAttachmentPoint = attachment;
+}
+
+const SurgSim::Math::Vector3d& VirtualToolCoupler::getAttachmentPoint()
+{
+ SURGSIM_ASSERT(isAwake() || m_optionalAttachmentPoint.hasValue())
+ << "Vtc parameter has not been initialized";
+ return m_localAttachmentPoint;
+}
+
+void VirtualToolCoupler::setOptionalLinearStiffness(
+ const SurgSim::DataStructures::OptionalValue<double>& linearStiffness)
+{
+ m_optionalLinearStiffness = linearStiffness;
+}
+
+const SurgSim::DataStructures::OptionalValue<double>& VirtualToolCoupler::getOptionalLinearStiffness() const
+{
+ return m_optionalLinearStiffness;
+}
+
+void VirtualToolCoupler::setOptionalLinearDamping(const SurgSim::DataStructures::OptionalValue<double>& linearDamping)
+{
+ m_optionalLinearDamping = linearDamping;
+}
+
+const SurgSim::DataStructures::OptionalValue<double>& VirtualToolCoupler::getOptionalLinearDamping() const
+{
+ return m_optionalLinearDamping;
+}
+
+void VirtualToolCoupler::setOptionalAngularStiffness(
+ const SurgSim::DataStructures::OptionalValue<double>& angularStiffness)
+{
+ m_optionalAngularStiffness = angularStiffness;
+}
+
+const SurgSim::DataStructures::OptionalValue<double>& VirtualToolCoupler::getOptionalAngularStiffness() const
+{
+ return m_optionalAngularStiffness;
+}
+
+void VirtualToolCoupler::setOptionalAngularDamping(
+ const SurgSim::DataStructures::OptionalValue<double>& angularDamping)
+{
+ m_optionalAngularDamping = angularDamping;
+}
+
+const SurgSim::DataStructures::OptionalValue<double>& VirtualToolCoupler::getOptionalAngularDamping() const
+{
+ return m_optionalAngularDamping;
+}
+
+void VirtualToolCoupler::setOptionalAttachmentPoint(
+ const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& attachmentPoint)
+{
+ m_optionalAttachmentPoint = attachmentPoint;
+}
+
+const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>&
+VirtualToolCoupler::getOptionalAttachmentPoint() const
+{
+ return m_optionalAttachmentPoint;
+}
+
+void VirtualToolCoupler::setCalculateInertialTorques(bool calculateInertialTorques)
+{
+ m_calculateInertialTorques = calculateInertialTorques;
+}
+
+bool VirtualToolCoupler::getCalculateInertialTorques() const
+{
+ return m_calculateInertialTorques;
+}
+
+}; /// Physics
+}; /// SurgSim
diff --git a/SurgSim/Physics/VirtualToolCoupler.h b/SurgSim/Physics/VirtualToolCoupler.h
new file mode 100644
index 0000000..79de9d9
--- /dev/null
+++ b/SurgSim/Physics/VirtualToolCoupler.h
@@ -0,0 +1,269 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PHYSICS_VIRTUALTOOLCOUPLER_H
+#define SURGSIM_PHYSICS_VIRTUALTOOLCOUPLER_H
+
+#include <memory>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+namespace SurgSim
+{
+
+namespace Input
+{
+class InputComponent;
+class OutputComponent;
+}
+
+namespace Physics
+{
+
+class RigidRepresentation;
+
+SURGSIM_STATIC_REGISTRATION(VirtualToolCoupler);
+
+/// The VirtualToolCoupler couples a rigid object to an input/output device through a spring and damper. If the device
+/// will output forces and/or torques, we pass it a force (and/or torque) as well as the derivatives (Jacobians) of
+/// the force with respect to position and velocity, so that the device can recalculate its forces at its update rate.
+class VirtualToolCoupler : public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit VirtualToolCoupler(const std::string& name);
+
+ ~VirtualToolCoupler();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::VirtualToolCoupler);
+
+ /// \return Input Component to get the pose from
+ const std::shared_ptr<SurgSim::Framework::Component> getInput();
+
+ /// Set the Input Component
+ /// \param input Input Component to get the pose from
+ void setInput(const std::shared_ptr<SurgSim::Framework::Component> input);
+
+ /// \return Output Component to send forces and torques
+ const std::shared_ptr<SurgSim::Framework::Component> getOutput();
+
+ /// Set the Output Component (if any)
+ /// \param output Output Component to send forces and torques
+ void setOutput(const std::shared_ptr<SurgSim::Framework::Component> output);
+
+ /// \return Rigid Representation that provides state and receives external forces and torques
+ const std::shared_ptr<SurgSim::Framework::Component> getRepresentation();
+
+ /// Set the Physics Representation which follows the input
+ /// \param rigid Rigid Representation that provides state and receives external forces and torques
+ void setRepresentation(const std::shared_ptr<SurgSim::Framework::Component> rigid);
+
+ /// \return Name of the pose data in the input to transfer
+ const std::string& getPoseName();
+
+ /// Set the name of the pose entry in the input DataGroup
+ /// \param poseName Name of the pose data in the input to transfer
+ void setPoseName(const std::string& poseName = SurgSim::DataStructures::Names::POSE);
+
+ virtual void update(double dt) override;
+
+ /// Override the linear stiffness connecting the input device and the physics representation
+ /// If this value is not provided, the stiffness will be automatically tuned using
+ /// the properties of the Representation
+ /// \param linearStiffness The stiffness of the vtc in linear mode (in N·m-1)
+ void overrideLinearStiffness(double linearStiffness);
+
+ /// \return The stiffness of the vtc in linear mode (in N·m-1)
+ double getLinearStiffness();
+
+ /// Override the linear damping connecting the input device and the physics representation
+ /// If this value is not provided, the damping will be automatically tuned using
+ /// the properties of the Representation
+ /// \param linearDamping The damping of the vtc in linear mode (in N·s·m-1 or Kg·s-1)
+ void overrideLinearDamping(double linearDamping);
+
+ /// \return The damping of the vtc in linear mode (in N·s·m-1 or Kg·s-1)
+ double getLinearDamping();
+
+ /// Override the angular stiffness connecting the input device and the physics representation
+ /// If this value is not provided, the stiffness will be automatically tuned using
+ /// the properties of the Representation
+ /// \param angularStiffness The stiffness of the vtc in angular mode (in N·m rad-1)
+ void overrideAngularStiffness(double angularStiffness);
+
+ /// \return The stiffness of the vtc in angular mode (in N·m rad-1)
+ double getAngularStiffness();
+
+ /// Override the angular damping connecting the input device and the physics representation
+ /// If this value is not provided, the damping will be automatically tuned using
+ /// the properties of the Representation
+ /// \param angularDamping The damping of the vtc in angular mode (in N·m·s·rad-1)
+ void overrideAngularDamping(double angularDamping);
+
+ /// \return The damping of the vtc in angular mode (in N·m·s·rad-1)
+ double getAngularDamping();
+
+ /// Override the point of attachment to the Representation
+ /// If this value is not provided, the point of attachment will be automatically
+ /// set to the Representation's center of mass.
+ /// \param attachment The attachment point in the Representations local coordinate frame
+ void overrideAttachmentPoint(const SurgSim::Math::Vector3d& attachment);
+
+ /// Get the point of attachment on the Representation
+ /// \return The attachment point in the Representations local coordinate frame
+ const SurgSim::Math::Vector3d& getAttachmentPoint();
+
+ /// Enable/disable torques that simulate inertia. This setting only has an effect if the attachment point is not
+ /// the mass center.
+ /// \sa overrideAttachmentPoint
+ /// \param calculateInertialTorques true to simulate inertia.
+ void setCalculateInertialTorques(bool calculateInertialTorques);
+
+ /// Get whether the calculated torques will simulate inertia. This setting only has an effect if the attachment
+ /// point is not the mass center.
+ /// \sa overrideAttachmentPoint
+ /// \return true if inertia is being simulated.
+ bool getCalculateInertialTorques() const;
+
+protected:
+ virtual bool doInitialize() override;
+ virtual bool doWakeUp() override;
+ virtual int getTargetManagerType() const override;
+
+ /// \return The DataGroup to be sent to the device via the OutputComponent.
+ virtual SurgSim::DataStructures::DataGroup buildOutputData();
+
+ /// Used for Serialization.
+ /// \param linearStiffness The OptionalValue object containing the stiffness of the vtc in linear mode (in N·m-1)
+ void setOptionalLinearStiffness(const SurgSim::DataStructures::OptionalValue<double>& linearStiffness);
+
+ /// Used for Serialization.
+ /// \return The OptionalValue object containing the stiffness of the vtc in linear mode (in N·m-1)
+ const SurgSim::DataStructures::OptionalValue<double>& getOptionalLinearStiffness() const;
+
+ /// Used for Serialization.
+ /// \param linearDamping The OptionalValue object containing the damping of the vtc in linear
+ /// mode (in N·s·m-1 or Kg·s-1)
+ void setOptionalLinearDamping(const SurgSim::DataStructures::OptionalValue<double>& linearDamping);
+
+ /// Used for Serialization.
+ /// \return The OptionalValue object containing the damping of the vtc in linear mode (in N·s·m-1 or Kg·s-1)
+ const SurgSim::DataStructures::OptionalValue<double>& getOptionalLinearDamping() const;
+
+ /// Used for Serialization.
+ /// \param angularStiffness The OptionalValue object containing the stiffness of the vtc in angular
+ /// mode (in N·m rad-1)
+ void setOptionalAngularStiffness(const SurgSim::DataStructures::OptionalValue<double>& angularStiffness);
+
+ /// Used for Serialization.
+ /// \return The OptionalValue object containing the stiffness of the vtc in angular mode (in N·m rad-1)
+ const SurgSim::DataStructures::OptionalValue<double>& getOptionalAngularStiffness() const;
+
+ /// Used for Serialization.
+ /// \param angularDamping The OptionalValue object containing the damping of the vtc in angular
+ /// mode (in N·m·s·rad-1)
+ void setOptionalAngularDamping(const SurgSim::DataStructures::OptionalValue<double>& angularDamping);
+
+ /// Used for Serialization.
+ /// \return The OptionalValue object containing the damping of the vtc in angular mode (in N·m·s·rad-1)
+ const SurgSim::DataStructures::OptionalValue<double>& getOptionalAngularDamping() const;
+
+ /// Used for Serialization.
+ /// \param attachmentPoint The OptionalValue object containing the attachment point.
+ void setOptionalAttachmentPoint(
+ const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& attachmentPoint);
+
+ /// Used for Serialization.
+ /// \return The OptionalValue object containing the attachment point.
+ const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& getOptionalAttachmentPoint() const;
+
+ /// User supplied Vtc stiffness parameter in linear mode (in N·m-1)
+ SurgSim::DataStructures::OptionalValue<double> m_optionalLinearStiffness;
+
+ /// User supplied Vtc damping parameter in linear mode (in N·s·m-1 or Kg·s-1)
+ SurgSim::DataStructures::OptionalValue<double> m_optionalLinearDamping;
+
+ /// User supplied Vtc stiffness parameter in angular mode (in N·m rad-1)
+ SurgSim::DataStructures::OptionalValue<double> m_optionalAngularStiffness;
+
+ /// User supplied Vtc damping parameter in angular mode (in N·m·s·rad-1)
+ SurgSim::DataStructures::OptionalValue<double> m_optionalAngularDamping;
+
+ /// User supplied attachment point
+ SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d> m_optionalAttachmentPoint;
+
+ /// The DataGroup to output
+ SurgSim::DataStructures::DataGroup m_outputData;
+
+ /// The input component.
+ std::shared_ptr<SurgSim::Input::InputComponent> m_input;
+
+ /// The output component.
+ std::shared_ptr<SurgSim::Input::OutputComponent> m_output;
+
+private:
+ std::shared_ptr<SurgSim::Physics::RigidRepresentation> m_rigid;
+ std::string m_poseName;
+
+ /// Used Vtc stiffness parameter in linear mode (in N·m-1)
+ double m_linearStiffness;
+
+ /// Used Vtc damping parameter in linear mode (in N·s·m-1 or Kg·s-1)
+ double m_linearDamping;
+
+ /// Used Vtc stiffness parameter in angular mode (in N·m rad-1)
+ double m_angularStiffness;
+
+ /// Used Vtc damping parameter in angular mode (in N·m·s·rad-1)
+ double m_angularDamping;
+
+ /// Scaling factor for the forces sent to the OutputComponent
+ double m_outputForceScaling;
+
+ /// Scaling factor for the torques sent to the OutputComponent
+ double m_outputTorqueScaling;
+
+ /// The input's point of attachment in the local frame, i.e., the same frame in which the mass center is defined.
+ SurgSim::Math::Vector3d m_localAttachmentPoint;
+
+ /// Whether or not the calculated torques will simulate inertia. This setting only has an effect if the device
+ /// input point is not the mass center.
+ bool m_calculateInertialTorques;
+
+ ///@{
+ /// Cached DataGroup indices.
+ int m_poseIndex;
+ int m_linearVelocityIndex;
+ int m_angularVelocityIndex;
+ int m_forceIndex;
+ int m_torqueIndex;
+ int m_inputLinearVelocityIndex;
+ int m_inputAngularVelocityIndex;
+ int m_inputPoseIndex;
+ int m_springJacobianIndex;
+ int m_damperJacobianIndex;
+ ///@}
+};
+
+}; // Physics
+
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_VIRTUALTOOLCOUPLER_H
diff --git a/SurgSim/Serialize/CMakeLists.txt b/SurgSim/Serialize/CMakeLists.txt
new file mode 100644
index 0000000..2868c11
--- /dev/null
+++ b/SurgSim/Serialize/CMakeLists.txt
@@ -0,0 +1,51 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${OSG_INCLUDE_DIR}
+)
+
+set(SURGSIM_SERIALIZE_SOURCES
+ GraphicsConvert.cpp
+ ShapesFactory.cpp
+)
+
+set(SURGSIM_SERIALIZE_HEADERS
+ GraphicsConvert.h
+ ShapesFactory.h
+ ShapesFactory-inl.h
+)
+
+surgsim_add_library(
+ SurgSimSerialize
+ "${SURGSIM_SERIALIZE_SOURCES}"
+ "${SURGSIM_SERIALIZE_HEADERS}"
+ "SurgSim/Serialize"
+)
+
+set(LIBS
+
+)
+
+target_link_libraries(SurgSimSerialize ${LIBS}
+)
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+endif()
+
+# Put SurgSimSerialize into folder "Serialize"
+set_target_properties(SurgSimSerialize PROPERTIES FOLDER "Serialize")
diff --git a/SurgSim/Serialize/GraphicsConvert.cpp b/SurgSim/Serialize/GraphicsConvert.cpp
new file mode 100644
index 0000000..aead1c7
--- /dev/null
+++ b/SurgSim/Serialize/GraphicsConvert.cpp
@@ -0,0 +1,67 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Serialize/GraphicsConvert.h"
+
+namespace YAML
+{
+ /// Specialize of YAML::convert<> template Presensation class.
+ Node convert<SurgSim::Graphics::Representation>::encode(const SurgSim::Graphics::Representation& rhs)
+ {
+ Node node;
+
+ node = convert<SurgSim::Framework::Component>::encode(rhs);
+ node["initialPose"] = rhs.getInitialPose();
+ node["pose"] = rhs.getPose();
+
+ return node;
+ }
+
+ bool convert<SurgSim::Graphics::Representation>::decode(const Node& node,
+ std::shared_ptr<SurgSim::Graphics::Representation> rhs)
+ {
+ if (! node.IsMap())
+ {
+ return false;
+ }
+ convert<SurgSim::Framework::Component>::decode(node, rhs);
+ rhs->setInitialPose(node["initialPose"].as<SurgSim::Math::RigidTransform3d>());
+ rhs->setPose(node["pose"].as<SurgSim::Math::RigidTransform3d>());
+ return true;
+ }
+
+ /// Specialize of YAML::convert<> template SpherePresensation class.
+ Node convert<SurgSim::Graphics::SphereRepresentation>::encode(const SurgSim::Graphics::SphereRepresentation& rhs)
+ {
+ Node node;
+
+ node = convert<SurgSim::Graphics::Representation>::encode(rhs);
+ node["class"] = "SphereRepresentation";
+ node["radius"] = rhs.getRadius();
+ return node;
+ }
+
+ bool convert<SurgSim::Graphics::SphereRepresentation>::decode(const Node& node,
+ std::shared_ptr<SurgSim::Graphics::SphereRepresentation> rhs)
+ {
+ if (! node.IsMap())
+ {
+ return false;
+ }
+ convert<SurgSim::Graphics::Representation>::decode(node, rhs);
+ rhs->setRadius(node["radius"].as<double>());
+ return true;
+ }
+}
\ No newline at end of file
diff --git a/SurgSim/Serialize/GraphicsConvert.h b/SurgSim/Serialize/GraphicsConvert.h
new file mode 100644
index 0000000..b4e1a3c
--- /dev/null
+++ b/SurgSim/Serialize/GraphicsConvert.h
@@ -0,0 +1,46 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_SERIALIZE_GRAPHICSCONVERT_H
+#define SURGSIM_SERIALIZE_GRAPHICSCONVERT_H
+
+#include "SurgSim/Serialize/Convert.h"
+#include "SurgSim/Serialize/FrameworkConvert.h"
+#include "SurgSim/Serialize/MathConvert.h"
+
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Graphics/SphereRepresentation.h"
+
+namespace YAML
+{
+ /// Specialize of YAML::convert<> template Represensation class.
+ template <>
+ struct convert <SurgSim::Graphics::Representation>
+ {
+ static Node encode(const SurgSim::Graphics::Representation& rhs);
+ static bool decode(const Node& node, std::shared_ptr<SurgSim::Graphics::Representation> rhs);
+
+ };
+
+ /// Specialize of YAML::convert<> template SphereRepresensation class.
+ template <>
+ struct convert <SurgSim::Graphics::SphereRepresentation>
+ {
+ static Node encode(const SurgSim::Graphics::SphereRepresentation& rhs);
+ static bool decode(const Node& node, std::shared_ptr<SurgSim::Graphics::SphereRepresentation> rhs);
+ };
+};
+
+#endif // SURGSIM_SERIALIZE_GRAPHICSCONVERT_H
\ No newline at end of file
diff --git a/SurgSim/Serialize/ShapesFactory-inl.h b/SurgSim/Serialize/ShapesFactory-inl.h
new file mode 100644
index 0000000..b8031e4
--- /dev/null
+++ b/SurgSim/Serialize/ShapesFactory-inl.h
@@ -0,0 +1,37 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef SURGSIM_SERIALIZE_SHAPESFACTORY_INL_H
+#define SURGSIM_SERIALIZE_SHAPESFACTORY_INL_H
+
+
+namespace SurgSim
+{
+namespace Serialize
+{
+
+template <class Derived>
+void SurgSim::Serialize::ShapesFactory::registerShape(const std::string& className)
+{
+ if (m_registerDirectory.find(className) == m_registerDirectory.end())
+ {
+ m_registerDirectory[className] = boost::factory<std::shared_ptr<Derived>>();
+ };
+};
+
+};
+};
+#endif // SURGSIM_SERIALIZE_SHAPESFACTORY_INL_H
\ No newline at end of file
diff --git a/SurgSim/Serialize/ShapesFactory.cpp b/SurgSim/Serialize/ShapesFactory.cpp
new file mode 100644
index 0000000..53b0293
--- /dev/null
+++ b/SurgSim/Serialize/ShapesFactory.cpp
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Serialize/ShapesFactory.h"
+
+namespace SurgSim
+{
+namespace Serialize
+{
+
+ /// Constructor
+ SurgSim::Serialize::ShapesFactory::ShapesFactory()
+ {
+ }
+
+ /// Destructor
+ SurgSim::Serialize::ShapesFactory::~ShapesFactory()
+ {
+ }
+
+ std::shared_ptr<SurgSim::Math::Shape> SurgSim::Serialize::ShapesFactory::
+ createShape(const std::string& className)
+ {
+ auto it = m_registerDirectory.find(className);
+
+ /// Return a nullptr if the class name has not been registered before.
+ if (it == m_registerDirectory.end())
+ {
+ return nullptr;
+ }
+
+ return (it->second)();
+ };
+
+};
+};
\ No newline at end of file
diff --git a/SurgSim/Serialize/ShapesFactory.h b/SurgSim/Serialize/ShapesFactory.h
new file mode 100644
index 0000000..185da1b
--- /dev/null
+++ b/SurgSim/Serialize/ShapesFactory.h
@@ -0,0 +1,64 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_SERIALIZE_SHAPESFACTORY_H
+#define SURGSIM_SERIALIZE_SHAPESFACTORY_H
+
+#include "SurgSim/Math/Shape.h"
+#include <boost/function.hpp>
+#include <boost/functional/factory.hpp>
+
+namespace SurgSim
+{
+namespace Serialize
+{
+
+/// A factory implementation for shapes
+class ShapesFactory
+{
+public:
+ /// Constructor
+ ShapesFactory();
+
+ /// Destructor
+ virtual ~ShapesFactory();
+
+ /// Template version to register a shape into the internal directory.
+ /// \tparam T The specific type of the shape to be registered.
+ /// \param className The identifier name to be used.
+ template <class Derived>
+ void registerShape(const std::string& className);
+
+ /// Create an instance of derived rigid shape based on the specific class name.
+ /// \param className The identifier name to be used.
+ /// \return a pointer to the object of derived rigid shape, or nullptr otherwise.
+ std::shared_ptr<SurgSim::Math::Shape> createShape(const std::string& className);
+
+private:
+ /// A wrapper of function object
+ typedef boost::function<std::shared_ptr<SurgSim::Math::Shape> ()> ShapesFactoryFunction;
+
+ /// Look up table for shapes factory.
+ std::map<std::string, ShapesFactoryFunction> m_registerDirectory;
+
+};
+
+
+}
+}
+
+#include "SurgSim/Serialize/ShapesFactory-inl.h"
+
+#endif // SURGSIM_SERIALIZE_SHAPESFACTORY_H
\ No newline at end of file
diff --git a/SurgSim/Serialize/UnitTests/CMakeLists.txt b/SurgSim/Serialize/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..27d5794
--- /dev/null
+++ b/SurgSim/Serialize/UnitTests/CMakeLists.txt
@@ -0,0 +1,45 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ GraphicsConvertTest.cpp
+ ShapesConvertTest.cpp
+ ShapesFactoryTest.cpp
+)
+
+set(UNIT_TEST_HEADERS
+)
+
+set(LIBS
+ SurgSimSerialize
+ SurgSimPhysics
+ SurgSimMath
+ SurgSimFramework
+ SurgSimGraphics
+ gmock
+ gtest
+ ${YAML_CPP_LIBRARIES}}
+ ${Boost_LIBRARIES}
+)
+
+surgsim_add_unit_tests(SurgSimSerializeTest)
+
+# Put SurgSimSerializeTest into folder "Serialize"
+set_target_properties(SurgSimSerializeTest PROPERTIES FOLDER "Serialize")
diff --git a/SurgSim/Serialize/UnitTests/GraphicsConvertTest.cpp b/SurgSim/Serialize/UnitTests/GraphicsConvertTest.cpp
new file mode 100644
index 0000000..738b2ff
--- /dev/null
+++ b/SurgSim/Serialize/UnitTests/GraphicsConvertTest.cpp
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <fstream>
+#include <gtest/gtest.h>
+#include "SurgSim/Serialize/GraphicsConvert.h"
+#include "SurgSim/Graphics/SphereRepresentation.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+
+class GraphicsConvertTest : public ::testing::Test
+{
+protected:
+
+ // Setup out stream file
+ void SetUp()
+ {
+ datafile = "convertertest.yaml";
+ fout.open(datafile);
+ }
+
+ // Remove the config testing file
+ void TearDown()
+ {
+ remove(datafile.c_str());
+ }
+
+ // Stream file
+ std::ofstream fout;
+
+ // Datafile name
+ std::string datafile;
+};
+
+
+TEST_F(GraphicsConvertTest, ConvertSphereRepresentationTest)
+{
+ YAML::Node node;
+ YAML::Emitter outnode(fout);
+
+ std::shared_ptr<SurgSim::Graphics::SphereRepresentation> sphereRepresentation =
+ std::make_shared<SurgSim::Graphics::OsgSphereRepresentation>("Sphere_Obj");
+
+ double sphereRadius = 5.0;
+ sphereRepresentation->setRadius(sphereRadius);
+
+ SurgSim::Math::RigidTransform3d spherePose = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::Quaterniond(SurgSim::Math::Vector4d::Identity()).normalized(),
+ SurgSim::Math::Vector3d::Identity());
+ sphereRepresentation->setPose(spherePose);
+
+ /// Encoding sphere representation
+ node = YAML::convert<SurgSim::Graphics::SphereRepresentation>::encode(*sphereRepresentation);
+ outnode << node;
+ fout.close();
+
+ /// Decoding sphere representation
+ YAML::Node innode = YAML::LoadFile(datafile);
+ std::shared_ptr<SurgSim::Graphics::SphereRepresentation> actualSphere =
+ std::make_shared<SurgSim::Graphics::OsgSphereRepresentation>("ImageSphere");
+ YAML::convert<SurgSim::Graphics::SphereRepresentation>::decode(innode, actualSphere);
+
+ EXPECT_EQ(sphereRepresentation->getRadius(), actualSphere->getRadius());
+ EXPECT_TRUE(actualSphere->getInitialPose().matrix().isApprox(sphereRepresentation->getInitialPose().matrix()));
+ EXPECT_TRUE(actualSphere->getPose().matrix().isApprox(sphereRepresentation->getPose().matrix()));
+}
\ No newline at end of file
diff --git a/SurgSim/Serialize/UnitTests/ShapesConvertTest.cpp b/SurgSim/Serialize/UnitTests/ShapesConvertTest.cpp
new file mode 100644
index 0000000..ea453f1
--- /dev/null
+++ b/SurgSim/Serialize/UnitTests/ShapesConvertTest.cpp
@@ -0,0 +1,259 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <fstream>
+#include <gtest/gtest.h>
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/CapsuleShape.h"
+#include "SurgSim/Math/CylinderShape.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Shape.h"
+
+#include "SurgSim/Framework/Convert.h"
+
+#include "SurgSim/Serialize/ShapesFactory.h"
+
+
+class ShapesConvertTest : public ::testing::Test
+{
+protected:
+
+ // Setup out stream file
+ void SetUp()
+ {
+ datafile = "ShapesConvertTest.yaml";
+ fout.open(datafile);
+ }
+
+ // Remove the config testing file
+ void TearDown()
+ {
+ remove(datafile.c_str());
+ }
+
+ // Stream file
+ std::ofstream fout;
+
+ // Datafile name
+ std::string datafile;
+};
+
+TEST_F(ShapesConvertTest, BoxConvertTest)
+{
+ YAML::Node node;
+ YAML::Emitter emitter(fout);
+
+ std::shared_ptr<SurgSim::Math::BoxShape> shape =
+ std::make_shared<SurgSim::Math::BoxShape>(1.0, 2.0, 3.0);
+
+ /// Registering classes
+ std::shared_ptr<SurgSim::Serialize::ShapesFactory> rigidShapesFactory=
+ std::make_shared<SurgSim::Serialize::ShapesFactory>();
+
+ rigidShapesFactory->registerShape<SurgSim::Math::BoxShape>(shape->getClassName());
+
+
+ /// Encoding box shape
+ node = shape->encode();
+ emitter << node;
+ fout.close();
+
+ /// Decoding box shape
+ YAML::Node innode = YAML::LoadFile(datafile);
+ std::string className = innode["ClassName"].as<std::string>();
+
+ /// Instantiate derived objected from class name.
+ std::shared_ptr<SurgSim::Math::Shape> actualShape =
+ rigidShapesFactory->createShape(className);
+
+ actualShape->decode(innode);
+
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_BOX, actualShape->getType());
+ EXPECT_EQ(shape->getSizeX(), std::static_pointer_cast<SurgSim::Math::BoxShape>(actualShape)->getSizeX());
+ EXPECT_EQ(shape->getSizeY(), std::static_pointer_cast<SurgSim::Math::BoxShape>(actualShape)->getSizeY());
+ EXPECT_EQ(shape->getSizeZ(), std::static_pointer_cast<SurgSim::Math::BoxShape>(actualShape)->getSizeZ());
+}
+
+TEST_F(ShapesConvertTest, CapsuleConvertTest)
+{
+ YAML::Node node;
+ YAML::Emitter emitter(fout);
+
+ std::shared_ptr<SurgSim::Math::CapsuleShape> shape =
+ std::make_shared<SurgSim::Math::CapsuleShape>(1.0, 0.5);
+
+ /// Registering classes
+ std::shared_ptr<SurgSim::Serialize::ShapesFactory> rigidShapesFactory=
+ std::make_shared<SurgSim::Serialize::ShapesFactory>();
+
+ rigidShapesFactory->registerShape<SurgSim::Math::CapsuleShape>(shape->getClassName());
+
+
+ /// Encoding box shape
+ node = shape->encode();
+ emitter << node;
+ fout.close();
+
+ /// Decoding box shape
+ YAML::Node innode = YAML::LoadFile(datafile);
+ std::string className = innode["ClassName"].as<std::string>();
+
+ /// Instantiate derived objected from class name.
+ std::shared_ptr<SurgSim::Math::Shape> actualShape =
+ rigidShapesFactory->createShape(className);
+
+ actualShape->decode(innode);
+
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_CAPSULE, actualShape->getType());
+ EXPECT_EQ(shape->getLength(), std::static_pointer_cast<SurgSim::Math::CapsuleShape>(actualShape)->getLength());
+ EXPECT_EQ(shape->getRadius(), std::static_pointer_cast<SurgSim::Math::CapsuleShape>(actualShape)->getRadius());
+}
+
+TEST_F(ShapesConvertTest, CylinderConvertTest)
+{
+ YAML::Node node;
+ YAML::Emitter emitter(fout);
+
+ std::shared_ptr<SurgSim::Math::CylinderShape> shape =
+ std::make_shared<SurgSim::Math::CylinderShape>(1.0, 0.5);
+
+ /// Registering classes
+ std::shared_ptr<SurgSim::Serialize::ShapesFactory> rigidShapesFactory=
+ std::make_shared<SurgSim::Serialize::ShapesFactory>();
+
+ rigidShapesFactory->registerShape<SurgSim::Math::CylinderShape>(shape->getClassName());
+
+
+ /// Encoding box shape
+ node = shape->encode();
+ emitter << node;
+ fout.close();
+
+ /// Decoding box shape
+ YAML::Node innode = YAML::LoadFile(datafile);
+ std::string className = innode["ClassName"].as<std::string>();
+
+ /// Instantiate derived objected from class name.
+ std::shared_ptr<SurgSim::Math::Shape> actualShape =
+ rigidShapesFactory->createShape(className);
+
+ actualShape->decode(innode);
+
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_CYLINDER, actualShape->getType());
+ EXPECT_EQ(shape->getLength(), std::static_pointer_cast<SurgSim::Math::CylinderShape>(actualShape)->getLength());
+ EXPECT_EQ(shape->getRadius(), std::static_pointer_cast<SurgSim::Math::CylinderShape>(actualShape)->getRadius());
+}
+
+TEST_F(ShapesConvertTest, SphereConvertTest)
+{
+ YAML::Node node;
+ YAML::Emitter emitter(fout);
+
+ std::shared_ptr<SurgSim::Math::SphereShape> shape =
+ std::make_shared<SurgSim::Math::SphereShape>(2.0);
+
+ /// Registering classes
+ std::shared_ptr<SurgSim::Serialize::ShapesFactory> rigidShapesFactory=
+ std::make_shared<SurgSim::Serialize::ShapesFactory>();
+
+ rigidShapesFactory->registerShape<SurgSim::Math::SphereShape>(shape->getClassName());
+
+
+ /// Encoding box shape
+ node = shape->encode();
+ emitter << node;
+ fout.close();
+
+ /// Decoding box shape
+ YAML::Node innode = YAML::LoadFile(datafile);
+ std::string className = innode["ClassName"].as<std::string>();
+
+ /// Instantiate derived objected from class name.
+ std::shared_ptr<SurgSim::Math::Shape> actualShape =
+ rigidShapesFactory->createShape(className);
+
+ actualShape->decode(innode);
+
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_SPHERE, actualShape->getType());
+ EXPECT_EQ(shape->getRadius(), std::static_pointer_cast<SurgSim::Math::SphereShape>(actualShape)->getRadius());
+}
+
+TEST_F(ShapesConvertTest, DoubleSidedPlaneShapeTest)
+{
+ YAML::Node node;
+ YAML::Emitter emitter(fout);
+
+ std::shared_ptr<SurgSim::Math::DoubleSidedPlaneShape> shape =
+ std::make_shared<SurgSim::Math::DoubleSidedPlaneShape>();
+
+ /// Registering classes
+ std::shared_ptr<SurgSim::Serialize::ShapesFactory> rigidShapesFactory=
+ std::make_shared<SurgSim::Serialize::ShapesFactory>();
+
+ rigidShapesFactory->registerShape<SurgSim::Math::DoubleSidedPlaneShape>(shape->getClassName());
+
+
+ /// Encoding box shape
+ node = shape->encode();
+ emitter << node;
+ fout.close();
+
+ /// Decoding box shape
+ YAML::Node innode = YAML::LoadFile(datafile);
+ std::string className = innode["ClassName"].as<std::string>();
+
+ /// Instantiate derived objected from class name.
+ std::shared_ptr<SurgSim::Math::Shape> actualShape =
+ rigidShapesFactory->createShape(className);
+
+ actualShape->decode(innode);
+
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_DOUBLESIDEDPLANE, actualShape->getType());
+}
+
+TEST_F(ShapesConvertTest, PlaneConvertTest)
+{
+ YAML::Node node;
+ YAML::Emitter emitter(fout);
+
+ std::shared_ptr<SurgSim::Math::PlaneShape> shape =
+ std::make_shared<SurgSim::Math::PlaneShape>();
+
+ /// Registering classes
+ std::shared_ptr<SurgSim::Serialize::ShapesFactory> rigidShapesFactory=
+ std::make_shared<SurgSim::Serialize::ShapesFactory>();
+
+ rigidShapesFactory->registerShape<SurgSim::Math::PlaneShape>(shape->getClassName());
+
+
+ /// Encoding box shape
+ node = shape->encode();
+ emitter << node;
+ fout.close();
+
+ /// Decoding box shape
+ YAML::Node innode = YAML::LoadFile(datafile);
+ std::string className = innode["ClassName"].as<std::string>();
+
+ /// Instantiate derived objected from class name.
+ std::shared_ptr<SurgSim::Math::Shape> actualShape =
+ rigidShapesFactory->createShape(className);
+
+ actualShape->decode(innode);
+
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_PLANE, actualShape->getType());
+}
\ No newline at end of file
diff --git a/SurgSim/Serialize/UnitTests/ShapesFactoryTest.cpp b/SurgSim/Serialize/UnitTests/ShapesFactoryTest.cpp
new file mode 100644
index 0000000..3ea9357
--- /dev/null
+++ b/SurgSim/Serialize/UnitTests/ShapesFactoryTest.cpp
@@ -0,0 +1,138 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/CapsuleShape.h"
+#include "SurgSim/Math/CylinderShape.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Shape.h"
+
+#include "SurgSim/Serialize/ShapesFactory.h"
+
+
+TEST(ShapesFactoryTest, InitTest)
+{
+ ASSERT_NO_THROW({std::shared_ptr<SurgSim::Serialize::ShapesFactory> rigidShapesFactory =
+ std::make_shared<SurgSim::Serialize::ShapesFactory>();});
+}
+
+TEST(ShapesFactoryTest, CreateShapeInstancesTest)
+{
+ std::shared_ptr<SurgSim::Serialize::ShapesFactory> rigidShapesFactory =
+ std::make_shared<SurgSim::Serialize::ShapesFactory>();
+
+ std::string boxName = "SurgSim::Physics::BoxShape";
+
+ /// Register derived object classes
+ rigidShapesFactory->registerShape<SurgSim::Math::BoxShape>(boxName);
+
+
+ /// Instantiate derived objects from class name.
+ std::shared_ptr<SurgSim::Math::Shape> boxShape1 = rigidShapesFactory->createShape(boxName);
+ std::shared_ptr<SurgSim::Math::Shape> boxShape2 = rigidShapesFactory->createShape(boxName);
+
+ // Expect all derived objects from the same class are not the same!
+ EXPECT_NE(boxShape1, boxShape2);
+}
+
+TEST(ShapesFactoryTest, CreateShapesTest)
+{
+ std::shared_ptr<SurgSim::Serialize::ShapesFactory> rigidShapesFactory =
+ std::make_shared<SurgSim::Serialize::ShapesFactory>();
+
+ std::string boxName = "SurgSim::Physics::BoxShape";
+ std::string capsuleName = "SurgSim::Physics::CapsuleShape";
+ std::string cylinderName = "SurgSim::Physics::CylinderShape";
+ std::string doublesideplaneName = "SurgSim::Physics::DoubleSidedPlaneShape";
+ std::string planeName = "SurgSim::Physics::PlaneShape";
+ std::string sphereName = "SurgSim::Physics::SphereShape";
+
+ /// Register derived object classes
+ rigidShapesFactory->registerShape<SurgSim::Math::BoxShape>(boxName);
+
+ rigidShapesFactory->registerShape<SurgSim::Math::CapsuleShape>(capsuleName);
+ rigidShapesFactory->registerShape<SurgSim::Math::CylinderShape>(cylinderName);
+ rigidShapesFactory->registerShape<SurgSim::Math::DoubleSidedPlaneShape>(doublesideplaneName);
+ rigidShapesFactory->registerShape<SurgSim::Math::PlaneShape>(planeName);
+ rigidShapesFactory->registerShape<SurgSim::Math::SphereShape>(sphereName);
+
+ /// Instantiate derived objected from class name.
+ std::shared_ptr<SurgSim::Math::Shape> boxShape = rigidShapesFactory->createShape(boxName);
+ std::shared_ptr<SurgSim::Math::Shape> capsuleShape = rigidShapesFactory->createShape(capsuleName);
+ std::shared_ptr<SurgSim::Math::Shape> cylinderShape = rigidShapesFactory->createShape(cylinderName);
+ std::shared_ptr<SurgSim::Math::Shape> doublesidedplaneShape =
+ rigidShapesFactory->createShape(doublesideplaneName);
+ std::shared_ptr<SurgSim::Math::Shape> planeShape = rigidShapesFactory->createShape(planeName);
+ std::shared_ptr<SurgSim::Math::Shape> sphereShape = rigidShapesFactory->createShape(sphereName);
+
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_BOX, boxShape->getType());
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_CAPSULE, capsuleShape->getType());
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_CYLINDER, cylinderShape->getType());
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_DOUBLESIDEDPLANE, doublesidedplaneShape->getType());
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_PLANE, planeShape->getType());
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_SPHERE, sphereShape->getType());
+
+}
+
+TEST(ShapesFactoryTest, NonRegisterTest)
+{
+ std::shared_ptr<SurgSim::Serialize::ShapesFactory> rigidShapesFactory =
+ std::make_shared<SurgSim::Serialize::ShapesFactory>();
+
+ std::string boxName = "SurgSim::Physics::BoxShape";
+ std::string capsuleName = "SurgSim::Physics::CapsuleShape";
+ std::string cylinderName = "SurgSim::Physics::CylinderShape";
+ std::string doublesideplaneName = "SurgSim::Physics::DoubleSidedPlaneShape";
+ std::string planeName = "SurgSim::Physics::PlaneShape";
+ std::string sphereName = "SurgSim::Physics::SphereShape";
+
+ /// Not register any derived object classes
+
+
+ /// Instantiate derived objected from class name.
+ std::shared_ptr<SurgSim::Math::Shape> boxShape = rigidShapesFactory->createShape(boxName);
+ std::shared_ptr<SurgSim::Math::Shape> capsuleShape = rigidShapesFactory->createShape(capsuleName);
+ std::shared_ptr<SurgSim::Math::Shape> cylinderShape = rigidShapesFactory->createShape(cylinderName);
+ std::shared_ptr<SurgSim::Math::Shape> doublesidedplaneShape =
+ rigidShapesFactory->createShape(doublesideplaneName);
+ std::shared_ptr<SurgSim::Math::Shape> planeShape = rigidShapesFactory->createShape(planeName);
+ std::shared_ptr<SurgSim::Math::Shape> sphereShape = rigidShapesFactory->createShape(sphereName);
+
+ /// Expect a null pointer form the Shape objects
+ EXPECT_EQ(nullptr, boxShape);
+ EXPECT_EQ(nullptr, capsuleShape);
+ EXPECT_EQ(nullptr, cylinderShape);
+ EXPECT_EQ(nullptr, doublesidedplaneShape);
+ EXPECT_EQ(nullptr, planeShape);
+ EXPECT_EQ(nullptr, sphereShape);
+
+}
diff --git a/SurgSim/Testing/CMakeLists.txt b/SurgSim/Testing/CMakeLists.txt
new file mode 100644
index 0000000..06c54e9
--- /dev/null
+++ b/SurgSim/Testing/CMakeLists.txt
@@ -0,0 +1,62 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ ${gmock_SOURCE_DIR}/include
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(SURGSIM_TESTING_SOURCES
+ MathUtilities.cpp
+ MockInputOutput.cpp
+ SerializationMockComponent.cpp
+ TestCube.cpp
+ TestingMain.cpp
+)
+
+set(SURGSIM_TESTING_HEADERS
+ MathUtilities.h
+ MockInputOutput.h
+ MockPhysicsManager.h
+ SerializationMockComponent.h
+ TestCube.h
+)
+
+surgsim_add_library(
+ SurgSimTesting
+ "${SURGSIM_TESTING_SOURCES}"
+ "${SURGSIM_TESTING_HEADERS}"
+ "SurgSim/Testing"
+)
+add_dependencies(SurgSimTesting yaml-cpp)
+
+set(LIBS
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ SurgSimMath
+ SurgSimPhysics
+)
+target_link_libraries(SurgSimTesting ${LIBS})
+
+add_subdirectory(MlcpIO)
+
+if(GLUT_FOUND)
+ add_subdirectory(VisualTestCommon)
+endif(GLUT_FOUND)
+
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
+set_target_properties(SurgSimTesting PROPERTIES FOLDER "Testing")
diff --git a/SurgSim/Testing/Data/Geometry/arm_collision.ply b/SurgSim/Testing/Data/Geometry/arm_collision.ply
new file mode 100644
index 0000000..bc55c96
--- /dev/null
+++ b/SurgSim/Testing/Data/Geometry/arm_collision.ply
@@ -0,0 +1,2125 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 1582
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+property float s
+property float t
+element face 528
+property list uchar uint vertex_indices
+end_header
+-0.034406 -0.085027 -0.138728 0.463145 -0.829541 -0.312024 0.135340 0.509730
+-0.095526 -0.117518 -0.143070 0.463145 -0.829541 -0.312024 0.282049 0.406200
+-0.073474 -0.097069 -0.164703 0.463145 -0.829541 -0.312024 0.259420 0.472470
+-0.095526 -0.117518 -0.143070 -0.477746 -0.729957 0.488795 0.282049 0.406200
+-0.089139 -0.098688 -0.108707 -0.477746 -0.729957 0.488795 0.188060 0.358130
+-0.129650 -0.099186 -0.149046 -0.477746 -0.729957 0.488795 0.350790 0.366940
+-0.129650 -0.099186 -0.149046 -0.496761 -0.707815 0.502222 0.350790 0.366940
+-0.133343 -0.079854 -0.125453 -0.496761 -0.707815 0.502222 0.293109 0.318900
+-0.158610 -0.087944 -0.161847 -0.496761 -0.707815 0.502222 0.443599 0.357970
+-0.133343 -0.079854 -0.125453 -0.732850 -0.346174 0.585742 0.293109 0.318900
+-0.147506 -0.066233 -0.135123 -0.732850 -0.346174 0.585742 0.342009 0.305170
+-0.158610 -0.087944 -0.161847 -0.732850 -0.346174 0.585742 0.443599 0.357970
+-0.140091 -0.047694 -0.134212 -0.313873 0.817137 0.483498 0.319526 0.270466
+-0.112058 -0.031450 -0.143467 -0.313873 0.817137 0.483498 0.278074 0.212619
+-0.137006 -0.024205 -0.171907 -0.313873 0.817137 0.483498 0.365560 0.214520
+-0.147506 -0.066233 -0.135123 -0.903873 0.349389 0.246860 0.342009 0.305170
+-0.127485 -0.041471 -0.096863 -0.903873 0.349389 0.246860 0.207157 0.259402
+-0.140091 -0.047694 -0.134212 -0.903873 0.349389 0.246860 0.319526 0.270466
+-0.127485 -0.041471 -0.096863 -0.715701 0.686737 0.127140 0.207157 0.259402
+-0.107972 -0.020968 -0.097765 -0.715701 0.686737 0.127140 0.188810 0.220410
+-0.140091 -0.047694 -0.134212 -0.715701 0.686737 0.127140 0.319526 0.270466
+-0.140091 -0.047694 -0.134212 -0.531301 0.834859 -0.143978 0.319526 0.270466
+-0.107972 -0.020968 -0.097765 -0.531301 0.834859 -0.143978 0.188810 0.220410
+-0.112058 -0.031450 -0.143467 -0.531301 0.834859 -0.143978 0.278074 0.212619
+-0.127485 -0.041471 -0.096863 -0.718029 0.687994 0.105349 0.207157 0.259402
+-0.100711 -0.017457 -0.071205 -0.718029 0.687994 0.105349 0.153949 0.220629
+-0.107972 -0.020968 -0.097765 -0.718029 0.687994 0.105349 0.188810 0.220410
+-0.100711 -0.017457 -0.071205 -0.795001 0.438312 0.419352 0.153949 0.220629
+-0.127485 -0.041471 -0.096863 -0.795001 0.438312 0.419352 0.207157 0.259402
+-0.094869 -0.027848 -0.049269 -0.795001 0.438312 0.419352 0.131870 0.234050
+-0.094869 -0.027848 -0.049269 -0.834640 0.121010 0.537339 0.131870 0.234050
+-0.127485 -0.041471 -0.096863 -0.834640 0.121010 0.537339 0.207157 0.259402
+-0.117000 -0.054747 -0.077587 -0.834640 0.121010 0.537339 0.163450 0.270300
+-0.117000 -0.054747 -0.077587 -0.650931 -0.557859 0.514861 0.163450 0.270300
+-0.133343 -0.079854 -0.125453 -0.650931 -0.557859 0.514861 0.293109 0.318900
+-0.095276 -0.068078 -0.064566 -0.650931 -0.557859 0.514861 0.130968 0.286588
+-0.127485 -0.041471 -0.096863 -0.882285 -0.013640 0.470517 0.207157 0.259402
+-0.147506 -0.066233 -0.135123 -0.882285 -0.013640 0.470517 0.342009 0.305170
+-0.117000 -0.054747 -0.077587 -0.882285 -0.013640 0.470517 0.163450 0.270300
+-0.147506 -0.066233 -0.135123 -0.754814 -0.438572 0.487761 0.342009 0.305170
+-0.133343 -0.079854 -0.125453 -0.754814 -0.438572 0.487761 0.293109 0.318900
+-0.117000 -0.054747 -0.077587 -0.754814 -0.438572 0.487761 0.163450 0.270300
+-0.113704 -0.014641 -0.177831 0.106434 0.895194 0.432782 0.354810 0.871980
+-0.112058 -0.031450 -0.143467 0.106434 0.895194 0.432782 0.296340 0.934129
+-0.089934 -0.029127 -0.153713 0.106434 0.895194 0.432782 0.266570 0.800189
+-0.089934 -0.029127 -0.153713 -0.118744 0.992428 -0.031396 0.266570 0.800189
+-0.112058 -0.031450 -0.143467 -0.118744 0.992428 -0.031396 0.296340 0.934129
+-0.079289 -0.027650 -0.147286 -0.118744 0.992428 -0.031396 0.236930 0.764110
+-0.137006 -0.024205 -0.171907 -0.244844 0.866271 0.435461 0.366090 0.965530
+-0.112058 -0.031450 -0.143467 -0.244844 0.866271 0.435461 0.296340 0.934129
+-0.113704 -0.014641 -0.177831 -0.244844 0.866271 0.435461 0.354810 0.871980
+-0.079289 -0.027650 -0.147286 0.201966 0.942022 -0.267964 0.236930 0.764110
+-0.082414 -0.008469 -0.082211 0.201966 0.942022 -0.267964 0.147348 0.905596
+-0.051717 -0.030921 -0.138004 0.201966 0.942022 -0.267964 0.172420 0.668253
+-0.082414 -0.008469 -0.082211 0.000804 0.927855 -0.372941 0.147348 0.905596
+-0.025528 -0.015002 -0.098342 0.000804 0.927855 -0.372941 0.106645 0.656621
+-0.051717 -0.030921 -0.138004 0.000804 0.927855 -0.372941 0.172420 0.668253
+-0.112058 -0.031450 -0.143467 -0.136721 0.968133 -0.209823 0.296340 0.934129
+-0.107972 -0.020968 -0.097765 -0.136721 0.968133 -0.209823 0.209280 0.974420
+-0.079289 -0.027650 -0.147286 -0.136721 0.968133 -0.209823 0.236930 0.764110
+-0.107972 -0.020968 -0.097765 -0.276259 0.918193 -0.283906 0.209280 0.974420
+-0.082414 -0.008469 -0.082211 -0.276259 0.918193 -0.283906 0.147348 0.905596
+-0.079289 -0.027650 -0.147286 -0.276259 0.918193 -0.283906 0.236930 0.764110
+-0.142783 -0.123768 -0.254342 0.665454 -0.706442 -0.241061 0.656300 0.541330
+-0.101305 -0.091627 -0.234032 0.665454 -0.706442 -0.241061 0.553391 0.588060
+-0.102843 -0.103309 -0.204043 0.665454 -0.706442 -0.241061 0.405861 0.510860
+-0.102843 -0.103309 -0.204043 0.696247 -0.680205 -0.229262 0.405861 0.510860
+-0.101305 -0.091627 -0.234032 0.696247 -0.680205 -0.229262 0.553391 0.588060
+-0.072018 -0.074296 -0.196510 0.696247 -0.680205 -0.229262 0.356783 0.579623
+-0.142783 -0.123768 -0.254342 0.627018 -0.777378 -0.050307 0.656300 0.541330
+-0.142754 -0.121434 -0.290047 0.627018 -0.777378 -0.050307 0.750953 0.583825
+-0.101305 -0.091627 -0.234032 0.627018 -0.777378 -0.050307 0.553391 0.588060
+-0.118145 -0.075826 -0.291408 0.815307 -0.450766 -0.363433 0.807090 0.695700
+-0.101305 -0.091627 -0.234032 0.815307 -0.450766 -0.363433 0.553391 0.588060
+-0.142754 -0.121434 -0.290047 0.815307 -0.450766 -0.363433 0.750953 0.583825
+-0.094587 -0.070190 -0.237734 0.811379 -0.336776 -0.477751 0.571026 0.654340
+-0.072018 -0.074296 -0.196510 0.811379 -0.336776 -0.477751 0.356783 0.579623
+-0.101305 -0.091627 -0.234032 0.811379 -0.336776 -0.477751 0.553391 0.588060
+-0.101305 -0.091627 -0.234032 0.875332 -0.334594 -0.349057 0.553391 0.588060
+-0.118145 -0.075826 -0.291408 0.875332 -0.334594 -0.349057 0.807090 0.695700
+-0.094587 -0.070190 -0.237734 0.875332 -0.334594 -0.349057 0.571026 0.654340
+-0.118145 -0.075826 -0.291408 0.910991 0.067262 -0.406905 0.807090 0.695700
+-0.107834 -0.057088 -0.265226 0.910991 0.067262 -0.406905 0.726160 0.731651
+-0.094587 -0.070190 -0.237734 0.910991 0.067262 -0.406905 0.571026 0.654340
+-0.100382 -0.018451 -0.205285 0.630584 0.461830 -0.623760 0.411672 0.797526
+-0.071727 -0.047559 -0.197868 0.630584 0.461830 -0.623760 0.379059 0.666993
+-0.094587 -0.070190 -0.237734 0.630584 0.461830 -0.623760 0.571026 0.654340
+-0.107834 -0.057088 -0.265226 0.908900 0.289840 -0.299822 0.726160 0.731651
+-0.100382 -0.018451 -0.205285 0.908900 0.289840 -0.299822 0.411672 0.797526
+-0.094587 -0.070190 -0.237734 0.908900 0.289840 -0.299822 0.571026 0.654340
+-0.100382 -0.018451 -0.205285 0.872117 0.354694 -0.337053 0.411672 0.797526
+-0.107834 -0.057088 -0.265226 0.872117 0.354694 -0.337053 0.726160 0.731651
+-0.117261 -0.018451 -0.248959 0.872117 0.354694 -0.337053 0.595351 0.822090
+-0.100382 -0.018451 -0.205285 0.712515 0.701656 0.000902 0.411672 0.797526
+-0.089934 -0.029127 -0.153713 0.712515 0.701656 0.000902 0.266570 0.800189
+-0.071727 -0.047559 -0.197868 0.712515 0.701656 0.000902 0.379059 0.666993
+-0.089934 -0.029127 -0.153713 0.084482 0.931430 -0.353979 0.266570 0.800189
+-0.079289 -0.027650 -0.147286 0.084482 0.931430 -0.353979 0.236930 0.764110
+-0.071727 -0.047559 -0.197868 0.084482 0.931430 -0.353979 0.379059 0.666993
+-0.137698 -0.007571 -0.242054 0.278313 0.960489 0.001757 0.534770 0.882390
+-0.113704 -0.014641 -0.177831 0.278313 0.960489 0.001757 0.354810 0.871980
+-0.100382 -0.018451 -0.205285 0.278313 0.960489 0.001757 0.411672 0.797526
+-0.113704 -0.014641 -0.177831 0.447186 0.889536 0.093549 0.354810 0.871980
+-0.089934 -0.029127 -0.153713 0.447186 0.889536 0.093549 0.266570 0.800189
+-0.100382 -0.018451 -0.205285 0.447186 0.889536 0.093549 0.411672 0.797526
+-0.137698 -0.007571 -0.242054 0.420355 0.892698 -0.162457 0.534770 0.882390
+-0.100382 -0.018451 -0.205285 0.420355 0.892698 -0.162457 0.411672 0.797526
+-0.117261 -0.018451 -0.248959 0.420355 0.892698 -0.162457 0.595351 0.822090
+-0.169827 -0.013664 -0.222033 -0.096689 0.984765 0.144531 0.462741 0.950520
+-0.113704 -0.014641 -0.177831 -0.096689 0.984765 0.144531 0.354810 0.871980
+-0.137698 -0.007571 -0.242054 -0.096689 0.984765 0.144531 0.534770 0.882390
+-0.072018 -0.074296 -0.196510 0.276017 -0.787433 -0.551147 0.356783 0.579623
+-0.054913 -0.089177 -0.166683 0.276017 -0.787433 -0.551147 0.228820 0.514750
+-0.073474 -0.097069 -0.164703 0.276017 -0.787433 -0.551147 0.255890 0.486160
+-0.072018 -0.074296 -0.196510 0.708039 -0.379637 -0.595447 0.356783 0.579623
+-0.053999 -0.068243 -0.178943 0.708039 -0.379637 -0.595447 0.256240 0.570673
+-0.054913 -0.089177 -0.166683 0.708039 -0.379637 -0.595447 0.228820 0.514750
+-0.094587 -0.070190 -0.237734 0.875211 -0.034034 -0.482543 0.571026 0.654340
+-0.071727 -0.047559 -0.197868 0.875211 -0.034034 -0.482543 0.379059 0.666993
+-0.072018 -0.074296 -0.196510 0.875211 -0.034034 -0.482543 0.356783 0.579623
+-0.071727 -0.047559 -0.197868 0.769087 -0.040767 -0.637842 0.379059 0.666993
+-0.056141 -0.053097 -0.178721 0.769087 -0.040767 -0.637842 0.263661 0.612951
+-0.072018 -0.074296 -0.196510 0.769087 -0.040767 -0.637842 0.356783 0.579623
+-0.072018 -0.074296 -0.196510 0.675592 0.106238 -0.729581 0.356783 0.579623
+-0.056141 -0.053097 -0.178721 0.675592 0.106238 -0.729581 0.263661 0.612951
+-0.053999 -0.068243 -0.178943 0.675592 0.106238 -0.729581 0.256240 0.570673
+-0.056141 -0.053097 -0.178721 0.744189 0.114890 -0.658015 0.263661 0.612951
+-0.040966 -0.049422 -0.160917 0.744189 0.114890 -0.658015 0.191410 0.590020
+-0.053999 -0.068243 -0.178943 0.744189 0.114890 -0.658015 0.256240 0.570673
+-0.056141 -0.053097 -0.178721 0.318008 0.837714 -0.443966 0.263661 0.612951
+-0.079289 -0.027650 -0.147286 0.318008 0.837714 -0.443966 0.236930 0.764110
+-0.040966 -0.049422 -0.160917 0.318008 0.837714 -0.443966 0.191410 0.590020
+-0.040966 -0.049422 -0.160917 0.272144 0.807082 -0.523982 0.191410 0.590020
+-0.079289 -0.027650 -0.147286 0.272144 0.807082 -0.523982 0.236930 0.764110
+-0.051717 -0.030921 -0.138004 0.272144 0.807082 -0.523982 0.172420 0.668253
+-0.051717 -0.030921 -0.138004 0.476749 0.660592 -0.579939 0.172420 0.668253
+-0.025528 -0.015002 -0.098342 0.476749 0.660592 -0.579939 0.106645 0.656621
+-0.024684 -0.048566 -0.135880 0.476749 0.660592 -0.579939 0.122524 0.566469
+-0.071727 -0.047559 -0.197868 0.562916 0.794306 -0.228482 0.379059 0.666993
+-0.079289 -0.027650 -0.147286 0.562916 0.794306 -0.228482 0.236930 0.764110
+-0.056141 -0.053097 -0.178721 0.562916 0.794306 -0.228482 0.263661 0.612951
+-0.053999 -0.068243 -0.178943 0.785684 0.046741 -0.616860 0.256240 0.570673
+-0.040966 -0.049422 -0.160917 0.785684 0.046741 -0.616860 0.191410 0.590020
+-0.032947 -0.077119 -0.152802 0.785684 0.046741 -0.616860 0.169058 0.522833
+-0.040966 -0.049422 -0.160917 0.834235 0.081761 -0.545313 0.191410 0.590020
+-0.024684 -0.048566 -0.135880 0.834235 0.081761 -0.545313 0.122524 0.566469
+-0.032947 -0.077119 -0.152802 0.834235 0.081761 -0.545313 0.169058 0.522833
+-0.040966 -0.049422 -0.160917 0.528082 0.764562 -0.369561 0.191410 0.590020
+-0.051717 -0.030921 -0.138004 0.528082 0.764562 -0.369561 0.172420 0.668253
+-0.024684 -0.048566 -0.135880 0.528082 0.764562 -0.369561 0.122524 0.566469
+-0.034406 -0.085027 -0.138728 0.608441 -0.717136 -0.339875 0.133570 0.514180
+-0.054913 -0.089177 -0.166683 0.608441 -0.717136 -0.339875 0.228820 0.514750
+-0.032947 -0.077119 -0.152802 0.608441 -0.717136 -0.339875 0.169058 0.522833
+-0.054913 -0.089177 -0.166683 0.637355 -0.409948 -0.652473 0.228820 0.514750
+-0.053999 -0.068243 -0.178943 0.637355 -0.409948 -0.652473 0.256240 0.570673
+-0.032947 -0.077119 -0.152802 0.637355 -0.409948 -0.652473 0.169058 0.522833
+-0.034406 -0.085027 -0.138728 0.374905 -0.916591 -0.138950 0.133570 0.514180
+-0.073474 -0.097069 -0.164703 0.374905 -0.916591 -0.138950 0.255890 0.486160
+-0.054913 -0.089177 -0.166683 0.374905 -0.916591 -0.138950 0.228820 0.514750
+-0.095526 -0.117518 -0.143070 0.512856 -0.820429 -0.252735 0.282049 0.406200
+-0.102843 -0.103309 -0.204043 0.512856 -0.820429 -0.252735 0.415881 0.497320
+-0.073474 -0.097069 -0.164703 0.512856 -0.820429 -0.252735 0.259420 0.472470
+-0.073474 -0.097069 -0.164703 0.676644 -0.612992 -0.407913 0.259420 0.472470
+-0.102843 -0.103309 -0.204043 0.676644 -0.612992 -0.407913 0.415881 0.497320
+-0.072018 -0.074296 -0.196510 0.676644 -0.612992 -0.407913 0.361480 0.499110
+-0.095526 -0.117518 -0.143070 0.236883 -0.939518 -0.247370 0.282049 0.406200
+-0.124310 -0.115529 -0.178188 0.236883 -0.939518 -0.247370 0.421920 0.440600
+-0.102843 -0.103309 -0.204043 0.236883 -0.939518 -0.247370 0.415881 0.497320
+-0.102843 -0.103309 -0.204043 0.424966 -0.904694 0.030537 0.415881 0.497320
+-0.151491 -0.126917 -0.226451 0.424966 -0.904694 0.030537 0.608100 0.477680
+-0.142783 -0.123768 -0.254342 0.424966 -0.904694 0.030537 0.645660 0.519570
+-0.168455 -0.131316 -0.250414 0.272706 -0.960063 -0.062537 0.689931 0.491150
+-0.142754 -0.121434 -0.290047 0.272706 -0.960063 -0.062537 0.754360 0.554610
+-0.142783 -0.123768 -0.254342 0.272706 -0.960063 -0.062537 0.645660 0.519570
+-0.162641 -0.121335 -0.214932 -0.169057 -0.941215 0.292463 0.600580 0.452870
+-0.168455 -0.131316 -0.250414 -0.169057 -0.941215 0.292463 0.689931 0.491150
+-0.151491 -0.126917 -0.226451 -0.169057 -0.941215 0.292463 0.608100 0.477680
+-0.168455 -0.131316 -0.250414 -0.250108 -0.920587 0.299941 0.689931 0.491150
+-0.162641 -0.121335 -0.214932 -0.250108 -0.920587 0.299941 0.600580 0.452870
+-0.192548 -0.119802 -0.235165 -0.250108 -0.920587 0.299941 0.712841 0.454750
+-0.192548 -0.119802 -0.235165 -0.575113 -0.594797 0.561660 0.712841 0.454750
+-0.175871 -0.092860 -0.189557 -0.575113 -0.594797 0.561660 0.559241 0.386500
+-0.205504 -0.102593 -0.230207 -0.575113 -0.594797 0.561660 0.730051 0.431070
+-0.205504 -0.102593 -0.230207 -0.813566 -0.099523 0.572892 0.730051 0.431070
+-0.180305 -0.073818 -0.189423 -0.813566 -0.099523 0.572892 0.576059 0.350370
+-0.204769 -0.059822 -0.221733 -0.813566 -0.099523 0.572892 0.717807 0.342718
+-0.162641 -0.121335 -0.214932 -0.386754 -0.765893 0.513643 0.600580 0.452870
+-0.161023 -0.105518 -0.190129 -0.386754 -0.765893 0.513643 0.528620 0.411331
+-0.192548 -0.119802 -0.235165 -0.386754 -0.765893 0.513643 0.712841 0.454750
+-0.161023 -0.105518 -0.190129 -0.521526 -0.637389 0.567226 0.528620 0.411331
+-0.175871 -0.092860 -0.189557 -0.521526 -0.637389 0.567226 0.559241 0.386500
+-0.192548 -0.119802 -0.235165 -0.521526 -0.637389 0.567226 0.712841 0.454750
+-0.175871 -0.092860 -0.189557 -0.772734 -0.184209 0.607413 0.559241 0.386500
+-0.180305 -0.073818 -0.189423 -0.772734 -0.184209 0.607413 0.576059 0.350370
+-0.205504 -0.102593 -0.230207 -0.772734 -0.184209 0.607413 0.730051 0.431070
+-0.180305 -0.073818 -0.189423 -0.797283 -0.000158 0.603606 0.576059 0.350370
+-0.164097 -0.053186 -0.168009 -0.797283 -0.000158 0.603606 0.473341 0.291050
+-0.204769 -0.059822 -0.221733 -0.797283 -0.000158 0.603606 0.717807 0.342718
+-0.161023 -0.105518 -0.190129 -0.561833 -0.680306 0.470667 0.528620 0.411331
+-0.158610 -0.087944 -0.161847 -0.561833 -0.680306 0.470667 0.443599 0.357970
+-0.175871 -0.092860 -0.189557 -0.561833 -0.680306 0.470667 0.559241 0.386500
+-0.175871 -0.092860 -0.189557 -0.816845 -0.194029 0.543248 0.559241 0.386500
+-0.158610 -0.087944 -0.161847 -0.816845 -0.194029 0.543248 0.443599 0.357970
+-0.180305 -0.073818 -0.189423 -0.816845 -0.194029 0.543248 0.576059 0.350370
+-0.161023 -0.105518 -0.190129 -0.494924 -0.718494 0.488688 0.528620 0.411331
+-0.129650 -0.099186 -0.149046 -0.494924 -0.718494 0.488688 0.350790 0.366940
+-0.158610 -0.087944 -0.161847 -0.494924 -0.718494 0.488688 0.443599 0.357970
+-0.158610 -0.087944 -0.161847 -0.818624 -0.229412 0.526522 0.443599 0.357970
+-0.147506 -0.066233 -0.135123 -0.818624 -0.229412 0.526522 0.342009 0.305170
+-0.180305 -0.073818 -0.189423 -0.818624 -0.229412 0.526522 0.576059 0.350370
+-0.180305 -0.073818 -0.189423 -0.854780 0.158605 0.494161 0.576059 0.350370
+-0.147506 -0.066233 -0.135123 -0.854780 0.158605 0.494161 0.342009 0.305170
+-0.164097 -0.053186 -0.168009 -0.854780 0.158605 0.494161 0.473341 0.291050
+-0.147506 -0.066233 -0.135123 -0.801007 0.294772 0.521054 0.342009 0.305170
+-0.140091 -0.047694 -0.134212 -0.801007 0.294772 0.521054 0.319526 0.270466
+-0.164097 -0.053186 -0.168009 -0.801007 0.294772 0.521054 0.473341 0.291050
+-0.140091 -0.047694 -0.134212 -0.624110 0.709123 0.328073 0.319526 0.270466
+-0.147443 -0.045298 -0.153377 -0.624110 0.709123 0.328073 0.381480 0.262520
+-0.164097 -0.053186 -0.168009 -0.624110 0.709123 0.328073 0.473341 0.291050
+-0.175397 -0.029894 -0.208187 -0.687990 0.526958 0.498984 0.596889 0.253480
+-0.164097 -0.053186 -0.168009 -0.687990 0.526958 0.498984 0.473341 0.291050
+-0.147443 -0.045298 -0.153377 -0.687990 0.526958 0.498984 0.381480 0.262520
+-0.164097 -0.053186 -0.168009 -0.723318 0.489391 0.487142 0.473341 0.291050
+-0.175397 -0.029894 -0.208187 -0.723318 0.489391 0.487142 0.596889 0.253480
+-0.204769 -0.059822 -0.221733 -0.723318 0.489391 0.487142 0.717807 0.342718
+-0.175397 -0.029894 -0.208187 -0.741902 0.563983 0.362636 0.596889 0.253480
+-0.169827 -0.013664 -0.222033 -0.741902 0.563983 0.362636 0.587758 0.225680
+-0.204769 -0.059822 -0.221733 -0.741902 0.563983 0.362636 0.717807 0.342718
+-0.102843 -0.103309 -0.204043 0.519801 -0.853827 0.028033 0.415881 0.497320
+-0.124310 -0.115529 -0.178188 0.519801 -0.853827 0.028033 0.421920 0.440600
+-0.123395 -0.115491 -0.193997 0.519801 -0.853827 0.028033 0.457371 0.460770
+-0.102843 -0.103309 -0.204043 0.471050 -0.876510 -0.099206 0.415881 0.497320
+-0.123395 -0.115491 -0.193997 0.471050 -0.876510 -0.099206 0.457371 0.460770
+-0.151491 -0.126917 -0.226451 0.471050 -0.876510 -0.099206 0.608100 0.477680
+-0.124310 -0.115529 -0.178188 -0.059584 -0.998206 -0.005848 0.421920 0.440600
+-0.139576 -0.114521 -0.194705 -0.059584 -0.998206 -0.005848 0.499065 0.446365
+-0.123395 -0.115491 -0.193997 -0.059584 -0.998206 -0.005848 0.457371 0.460770
+-0.123395 -0.115491 -0.193997 -0.072024 -0.919631 0.386125 0.457371 0.460770
+-0.139576 -0.114521 -0.194705 -0.072024 -0.919631 0.386125 0.499065 0.446365
+-0.151491 -0.126917 -0.226451 -0.072024 -0.919631 0.386125 0.608100 0.477680
+-0.139576 -0.114521 -0.194705 -0.064537 -0.921118 0.383896 0.499065 0.446365
+-0.162641 -0.121335 -0.214932 -0.064537 -0.921118 0.383896 0.600580 0.452870
+-0.151491 -0.126917 -0.226451 -0.064537 -0.921118 0.383896 0.608100 0.477680
+-0.139576 -0.114521 -0.194705 -0.227822 -0.814171 0.534063 0.499065 0.446365
+-0.161023 -0.105518 -0.190129 -0.227822 -0.814171 0.534063 0.528620 0.411331
+-0.162641 -0.121335 -0.214932 -0.227822 -0.814171 0.534063 0.600580 0.452870
+-0.129650 -0.099186 -0.149046 -0.294368 -0.884819 0.361169 0.350790 0.366940
+-0.161023 -0.105518 -0.190129 -0.294368 -0.884819 0.361169 0.528620 0.411331
+-0.139576 -0.114521 -0.194705 -0.294368 -0.884819 0.361169 0.499065 0.446365
+-0.095526 -0.117518 -0.143070 -0.489842 -0.795621 0.356430 0.282049 0.406200
+-0.129650 -0.099186 -0.149046 -0.489842 -0.795621 0.356430 0.350790 0.366940
+-0.124310 -0.115529 -0.178188 -0.489842 -0.795621 0.356430 0.421920 0.440600
+-0.168455 -0.131316 -0.250414 0.279015 -0.960051 -0.021281 0.689931 0.491150
+-0.142783 -0.123768 -0.254342 0.279015 -0.960051 -0.021281 0.645660 0.519570
+-0.151491 -0.126917 -0.226451 0.279015 -0.960051 -0.021281 0.608100 0.477680
+-0.175397 -0.029894 -0.208187 -0.578082 0.635273 0.512102 0.596889 0.253480
+-0.137006 -0.024205 -0.171907 -0.578082 0.635273 0.512102 0.365560 0.214520
+-0.169827 -0.013664 -0.222033 -0.578082 0.635273 0.512102 0.587758 0.225680
+-0.169827 -0.013664 -0.222033 -0.272387 0.890056 0.365521 0.587758 0.225680
+-0.137006 -0.024205 -0.171907 -0.272387 0.890056 0.365521 0.365560 0.214520
+-0.113704 -0.014641 -0.177831 -0.272387 0.890056 0.365521 0.380571 0.208017
+-0.175397 -0.029894 -0.208187 -0.549633 0.688114 0.473712 0.596889 0.253480
+-0.147443 -0.045298 -0.153377 -0.549633 0.688114 0.473712 0.381480 0.262520
+-0.137006 -0.024205 -0.171907 -0.549633 0.688114 0.473712 0.365560 0.214520
+-0.147443 -0.045298 -0.153377 -0.686218 0.641229 0.343410 0.381480 0.262520
+-0.140091 -0.047694 -0.134212 -0.686218 0.641229 0.343410 0.319526 0.270466
+-0.137006 -0.024205 -0.171907 -0.686218 0.641229 0.343410 0.365560 0.214520
+-0.095276 -0.068078 -0.064566 -0.488904 -0.747201 0.450181 0.130968 0.286588
+-0.133343 -0.079854 -0.125453 -0.488904 -0.747201 0.450181 0.293109 0.318900
+-0.089139 -0.098688 -0.108707 -0.488904 -0.747201 0.450181 0.188060 0.358130
+-0.133343 -0.079854 -0.125453 -0.493067 -0.709186 0.503924 0.293109 0.318900
+-0.129650 -0.099186 -0.149046 -0.493067 -0.709186 0.503924 0.350790 0.366940
+-0.089139 -0.098688 -0.108707 -0.493067 -0.709186 0.503924 0.188060 0.358130
+-0.029661 -0.072947 -0.026017 -0.402720 -0.693022 0.597944 0.040560 0.331810
+-0.016507 -0.053189 0.005742 -0.402720 -0.693022 0.597944 0.082710 0.266979
+-0.095276 -0.068078 -0.064566 -0.402720 -0.693022 0.597944 0.130968 0.286588
+-0.095276 -0.068078 -0.064566 -0.529318 -0.485446 0.695819 0.130968 0.286588
+-0.016507 -0.053189 0.005742 -0.529318 -0.485446 0.695819 0.082710 0.266979
+-0.077178 -0.040515 -0.031569 -0.529318 -0.485446 0.695819 0.101726 0.254639
+-0.095276 -0.068078 -0.064566 -0.636433 -0.383377 0.669309 0.130968 0.286588
+-0.077178 -0.040515 -0.031569 -0.636433 -0.383377 0.669309 0.101726 0.254639
+-0.117000 -0.054747 -0.077587 -0.636433 -0.383377 0.669309 0.163450 0.270300
+-0.117000 -0.054747 -0.077587 -0.737679 -0.096543 0.668213 0.163450 0.270300
+-0.077178 -0.040515 -0.031569 -0.737679 -0.096543 0.668213 0.101726 0.254639
+-0.094869 -0.027848 -0.049269 -0.737679 -0.096543 0.668213 0.131870 0.234050
+-0.029661 -0.072947 -0.026017 0.012096 -0.951483 0.307463 0.040560 0.331810
+-0.069241 -0.093206 -0.087154 0.012096 -0.951483 0.307463 0.131630 0.354850
+-0.012845 -0.075192 -0.033626 0.012096 -0.951483 0.307463 0.064239 0.351802
+-0.069241 -0.093206 -0.087154 -0.000226 -0.917027 0.398825 0.131630 0.354850
+-0.095526 -0.117518 -0.143070 -0.000226 -0.917027 0.398825 0.282049 0.406200
+-0.047403 -0.100711 -0.104398 -0.000226 -0.917027 0.398825 0.159656 0.410770
+-0.095276 -0.068078 -0.064566 -0.329351 -0.796728 0.506708 0.130968 0.286588
+-0.089139 -0.098688 -0.108707 -0.329351 -0.796728 0.506708 0.188060 0.358130
+-0.069241 -0.093206 -0.087154 -0.329351 -0.796728 0.506708 0.131630 0.354850
+-0.069241 -0.093206 -0.087154 -0.320327 -0.804401 0.500329 0.131630 0.354850
+-0.089139 -0.098688 -0.108707 -0.320327 -0.804401 0.500329 0.188060 0.358130
+-0.095526 -0.117518 -0.143070 -0.320327 -0.804401 0.500329 0.282049 0.406200
+-0.029661 -0.072947 -0.026017 -0.347068 -0.799819 0.489728 0.040560 0.331810
+-0.095276 -0.068078 -0.064566 -0.347068 -0.799819 0.489728 0.130968 0.286588
+-0.069241 -0.093206 -0.087154 -0.347068 -0.799819 0.489728 0.131630 0.354850
+-0.129650 -0.099186 -0.149046 -0.454677 -0.809791 0.370820 0.350790 0.366940
+-0.139576 -0.114521 -0.194705 -0.454677 -0.809791 0.370820 0.499065 0.446365
+-0.124310 -0.115529 -0.178188 -0.454677 -0.809791 0.370820 0.421920 0.440600
+-0.100711 -0.017457 -0.071205 -0.440118 0.897938 0.001620 0.143802 0.982094
+-0.082414 -0.008469 -0.082211 -0.440118 0.897938 0.001620 0.147348 0.905596
+-0.107972 -0.020968 -0.097765 -0.440118 0.897938 0.001620 0.209280 0.974420
+-0.045772 -0.006289 -0.019566 -0.515516 0.712087 0.476629 0.000000 0.000000
+-0.070686 -0.013774 -0.035330 -0.515516 0.712087 0.476629 1.000000 1.000000
+-0.060966 -0.016531 -0.020698 -0.515516 0.712087 0.476629 1.000000 0.000000
+-0.045772 -0.006289 -0.019566 -0.515529 0.712052 0.476667 0.000000 0.000000
+-0.080407 -0.011017 -0.049962 -0.515529 0.712052 0.476667 1.000000 0.000000
+-0.070686 -0.013774 -0.035330 -0.515529 0.712052 0.476667 1.000000 1.000000
+-0.042958 -0.003989 -0.023216 -0.365347 0.888354 0.278117 0.000000 0.000000
+-0.080407 -0.011017 -0.049962 -0.365347 0.888354 0.278117 1.000000 1.000000
+-0.045772 -0.006289 -0.019566 -0.365347 0.888354 0.278117 1.000000 0.000000
+-0.042958 -0.003989 -0.023216 -0.264898 0.956846 0.119474 0.000000 0.000000
+-0.064116 -0.005870 -0.055063 -0.264898 0.956846 0.119474 1.000000 1.000000
+-0.080407 -0.011017 -0.049962 -0.264898 0.956846 0.119474 1.000000 0.000000
+-0.045772 -0.006289 -0.019566 -0.536799 0.754638 0.377317 0.000000 0.000000
+-0.060966 -0.016531 -0.020698 -0.536799 0.754638 0.377317 1.000000 1.000000
+-0.041213 -0.007368 -0.010922 -0.536799 0.754638 0.377317 1.000000 0.000000
+-0.041213 -0.007368 -0.010922 -0.561355 0.562765 0.606775 0.000000 0.000000
+-0.060966 -0.016531 -0.020698 -0.561355 0.562765 0.606775 1.000000 0.000000
+-0.045510 -0.018062 -0.004979 -0.561355 0.562765 0.606775 1.000000 1.000000
+-0.038985 -0.007852 -0.007374 -0.673320 0.546990 0.497435 0.000000 0.000000
+-0.041213 -0.007368 -0.010922 -0.673320 0.546990 0.497435 1.000000 0.000000
+-0.045510 -0.018062 -0.004979 -0.673320 0.546990 0.497435 1.000000 1.000000
+-0.038985 -0.007852 -0.007374 -0.640237 0.537824 0.548491 0.000000 0.000000
+-0.045510 -0.018062 -0.004979 -0.640237 0.537824 0.548491 1.000000 1.000000
+-0.031328 -0.009383 0.003065 -0.640237 0.537824 0.548491 1.000000 0.000000
+-0.031328 -0.009383 0.003065 -0.627779 0.416148 0.657810 0.000000 0.000000
+-0.045510 -0.018062 -0.004979 -0.627779 0.416148 0.657810 1.000000 1.000000
+-0.030055 -0.019593 0.010739 -0.627779 0.416148 0.657810 1.000000 0.000000
+-0.031328 -0.009383 0.003065 -0.541170 0.461086 0.703232 0.000000 0.000000
+-0.030055 -0.019593 0.010739 -0.541170 0.461086 0.703232 1.000000 1.000000
+-0.024969 -0.006524 0.006084 -0.541170 0.461086 0.703232 1.000000 0.000000
+0.005457 0.003922 0.021488 -0.480042 0.796582 0.367447 0.000000 0.000000
+-0.003050 0.001094 0.016505 -0.480042 0.796582 0.367447 1.000000 0.000000
+-0.003552 -0.007667 0.034842 -0.480042 0.796582 0.367447 1.000000 1.000000
+0.005457 0.003922 0.021488 -0.615744 0.751523 0.236795 0.000000 0.000000
+-0.003552 -0.007667 0.034842 -0.615744 0.751523 0.236795 1.000000 0.000000
+0.012243 0.001958 0.045367 -0.615744 0.751523 0.236795 1.000000 1.000000
+0.005457 0.003922 0.021488 -0.318409 0.933086 0.167231 0.000000 0.000000
+0.012243 0.001958 0.045367 -0.318409 0.933086 0.167231 1.000000 1.000000
+0.017288 0.006507 0.029591 -0.318409 0.933086 0.167231 1.000000 0.000000
+0.025400 0.008882 0.035581 -0.374675 0.915863 0.144271 0.000000 0.000000
+0.017288 0.006507 0.029591 -0.374675 0.915863 0.144271 1.000000 0.000000
+0.012243 0.001958 0.045367 -0.374675 0.915863 0.144271 1.000000 1.000000
+0.025400 0.008882 0.035581 -0.494629 0.867597 -0.051154 0.000000 0.000000
+0.012243 0.001958 0.045367 -0.494629 0.867597 -0.051154 1.000000 1.000000
+0.028037 0.011583 0.055893 -0.494629 0.867597 -0.051154 1.000000 0.000000
+0.025400 0.008882 0.035581 -0.265878 0.959503 -0.093073 0.000000 0.000000
+0.028037 0.011583 0.055893 -0.265878 0.959503 -0.093073 1.000000 1.000000
+0.028559 0.009976 0.037835 -0.265878 0.959503 -0.093073 1.000000 0.000000
+0.028559 0.009976 0.037835 -0.116648 0.988960 -0.091381 0.000000 0.000000
+0.028037 0.011583 0.055893 -0.116648 0.988960 -0.091381 1.000000 0.000000
+0.036877 0.011536 0.044100 -0.116648 0.988960 -0.091381 1.000000 1.000000
+0.028559 0.009976 0.037835 -0.087429 0.987671 -0.129854 0.000000 0.000000
+0.036877 0.011536 0.044100 -0.087429 0.987671 -0.129854 1.000000 1.000000
+0.031086 0.009975 0.036126 -0.087429 0.987671 -0.129854 1.000000 0.000000
+0.037350 0.010384 0.030379 -0.110824 0.990030 -0.086942 0.000000 0.000000
+0.036877 0.011536 0.044100 -0.110824 0.990030 -0.086942 1.000000 1.000000
+0.045717 0.011490 0.032308 -0.110824 0.990030 -0.086942 1.000000 0.000000
+0.031086 0.009975 0.036126 -0.144849 0.985556 -0.087740 0.000000 0.000000
+0.036877 0.011536 0.044100 -0.144849 0.985556 -0.087740 1.000000 1.000000
+0.037350 0.010384 0.030379 -0.144849 0.985556 -0.087740 1.000000 0.000000
+0.040411 0.009454 0.028108 -0.026287 0.912167 -0.408975 0.000000 0.000000
+0.037350 0.010384 0.030379 -0.026287 0.912167 -0.408975 1.000000 0.000000
+0.045717 0.011490 0.032308 -0.026287 0.912167 -0.408975 1.000000 1.000000
+0.048486 0.003959 0.022397 0.143885 0.806883 -0.572920 0.000000 0.000000
+0.040411 0.009454 0.028108 0.143885 0.806883 -0.572920 1.000000 0.000000
+0.045717 0.011490 0.032308 0.143885 0.806883 -0.572920 1.000000 1.000000
+0.055057 -0.002370 0.018595 0.420243 0.711610 -0.563034 0.000000 0.000000
+0.052980 -0.000209 0.019776 0.420243 0.711610 -0.563034 1.000000 0.000000
+0.063030 -0.001161 0.026074 0.420243 0.711610 -0.563034 1.000000 1.000000
+0.055057 -0.002370 0.018595 0.368501 0.772161 -0.517663 0.000000 0.000000
+0.063030 -0.001161 0.026074 0.368501 0.772161 -0.517663 1.000000 0.000000
+0.050502 -0.009120 0.005284 0.368501 0.772161 -0.517663 1.000000 1.000000
+0.055057 -0.002370 0.018595 0.709834 0.499780 -0.496342 0.000000 0.000000
+0.050502 -0.009120 0.005284 0.709834 0.499780 -0.496342 1.000000 1.000000
+0.050963 -0.002376 0.012734 0.709834 0.499780 -0.496342 1.000000 0.000000
+0.050963 -0.002376 0.012734 0.698510 0.508482 -0.503519 0.000000 0.000000
+0.050502 -0.009120 0.005284 0.698510 0.508482 -0.503519 1.000000 1.000000
+0.042826 -0.002162 0.001662 0.698510 0.508482 -0.503519 1.000000 0.000000
+0.042826 -0.002162 0.001662 0.700957 0.518210 -0.490018 0.000000 0.000000
+0.050502 -0.009120 0.005284 0.700957 0.518210 -0.490018 1.000000 0.000000
+0.040124 -0.009825 -0.010307 0.700957 0.518210 -0.490018 1.000000 1.000000
+0.037686 -0.002161 -0.005301 0.676497 0.541338 -0.499304 0.000000 0.000000
+0.042826 -0.002162 0.001662 0.676497 0.541338 -0.499304 1.000000 0.000000
+0.040124 -0.009825 -0.010307 0.676497 0.541338 -0.499304 1.000000 1.000000
+0.037686 -0.002161 -0.005301 0.691229 0.536271 -0.484372 0.000000 0.000000
+0.040124 -0.009825 -0.010307 0.691229 0.536271 -0.484372 1.000000 1.000000
+0.029745 -0.010530 -0.025899 0.691229 0.536271 -0.484372 1.000000 0.000000
+0.037686 -0.002161 -0.005301 0.544411 0.682763 -0.487290 0.000000 0.000000
+0.029745 -0.010530 -0.025899 0.544411 0.682763 -0.487290 1.000000 1.000000
+0.026633 -0.001955 -0.017361 0.544411 0.682763 -0.487290 1.000000 0.000000
+0.026633 -0.001955 -0.017361 0.612689 0.658097 -0.437631 0.000000 0.000000
+0.029745 -0.010530 -0.025899 0.612689 0.658097 -0.437631 1.000000 0.000000
+0.019367 -0.011236 -0.041490 0.612689 0.658097 -0.437631 1.000000 1.000000
+0.026633 -0.001955 -0.017361 0.419944 0.797484 -0.433204 0.000000 0.000000
+0.019367 -0.011236 -0.041490 0.419944 0.797484 -0.433204 1.000000 1.000000
+0.018301 -0.002420 -0.026294 0.419944 0.797484 -0.433204 1.000000 0.000000
+0.018301 -0.002420 -0.026294 0.310508 0.831512 -0.460622 0.000000 0.000000
+0.019367 -0.011236 -0.041490 0.310508 0.831512 -0.460622 1.000000 1.000000
+0.010551 -0.002935 -0.032448 0.310508 0.831512 -0.460622 1.000000 0.000000
+0.010551 -0.002935 -0.032448 0.442964 0.832609 -0.332483 0.000000 0.000000
+0.019367 -0.011236 -0.041490 0.442964 0.832609 -0.332483 1.000000 1.000000
+0.008989 -0.011941 -0.057082 0.442964 0.832609 -0.332483 1.000000 0.000000
+0.010551 -0.002935 -0.032448 0.230950 0.908997 -0.346967 0.000000 0.000000
+0.008989 -0.011941 -0.057082 0.230950 0.908997 -0.346967 1.000000 1.000000
+0.002911 -0.002986 -0.037667 0.230950 0.908997 -0.346967 1.000000 0.000000
+0.002911 -0.002986 -0.037667 0.251867 0.906368 -0.339205 0.000000 0.000000
+0.008989 -0.011941 -0.057082 0.251867 0.906368 -0.339205 1.000000 1.000000
+-0.004764 -0.002641 -0.042444 0.251867 0.906368 -0.339205 1.000000 0.000000
+-0.004764 -0.002641 -0.042444 0.286681 0.907444 -0.307180 0.000000 0.000000
+0.008989 -0.011941 -0.057082 0.286681 0.907444 -0.307180 1.000000 0.000000
+-0.008270 -0.013472 -0.077712 0.286681 0.907444 -0.307180 1.000000 1.000000
+-0.014017 -0.001829 -0.047775 0.257267 0.916287 -0.306972 0.000000 0.000000
+-0.004764 -0.002641 -0.042444 0.257267 0.916287 -0.306972 1.000000 0.000000
+-0.008270 -0.013472 -0.077712 0.257267 0.916287 -0.306972 1.000000 1.000000
+-0.014017 -0.001829 -0.047775 0.279052 0.911860 -0.301068 0.000000 0.000000
+-0.008270 -0.013472 -0.077712 0.279052 0.911860 -0.301068 1.000000 1.000000
+-0.025528 -0.015002 -0.098342 0.279052 0.911860 -0.301068 1.000000 0.000000
+-0.014017 -0.001829 -0.047775 0.106410 0.959588 -0.260514 0.000000 0.000000
+-0.036677 -0.007862 -0.079253 0.106410 0.959588 -0.260514 1.000000 1.000000
+-0.020227 0.000071 -0.043313 0.106410 0.959588 -0.260514 1.000000 0.000000
+-0.014017 -0.001829 -0.047775 0.133057 0.951290 -0.278106 0.000000 0.000000
+-0.025528 -0.015002 -0.098342 0.133057 0.951290 -0.278106 1.000000 0.000000
+-0.036677 -0.007862 -0.079253 0.133057 0.951290 -0.278106 1.000000 1.000000
+-0.020227 0.000071 -0.043313 0.140005 0.951457 -0.274096 0.000000 0.000000
+-0.036677 -0.007862 -0.079253 0.140005 0.951457 -0.274096 1.000000 1.000000
+-0.047826 -0.000722 -0.060163 0.140005 0.951457 -0.274096 1.000000 0.000000
+-0.026591 0.001059 -0.038391 0.051904 0.989943 -0.131603 0.000000 0.000000
+-0.020227 0.000071 -0.043313 0.051904 0.989943 -0.131603 1.000000 0.000000
+-0.047826 -0.000722 -0.060163 0.051904 0.989943 -0.131603 1.000000 1.000000
+-0.026591 0.001059 -0.038391 -0.038404 0.998284 -0.044205 0.000000 0.000000
+-0.047826 -0.000722 -0.060163 -0.038404 0.998284 -0.044205 1.000000 1.000000
+-0.030314 0.001072 -0.034863 -0.038404 0.998284 -0.044205 1.000000 0.000000
+-0.034613 -0.000049 -0.031344 -0.212069 0.975881 0.051798 0.000000 0.000000
+-0.030314 0.001072 -0.034863 -0.212069 0.975881 0.051798 1.000000 0.000000
+-0.055971 -0.003296 -0.057613 -0.212069 0.975881 0.051798 1.000000 1.000000
+-0.030314 0.001072 -0.034863 -0.266010 0.956932 0.116270 0.000000 0.000000
+-0.047826 -0.000722 -0.060163 -0.266010 0.956932 0.116270 1.000000 0.000000
+-0.055971 -0.003296 -0.057613 -0.266010 0.956932 0.116270 1.000000 1.000000
+-0.038250 -0.001447 -0.027644 -0.267589 0.958424 0.099096 0.000000 0.000000
+-0.034613 -0.000049 -0.031344 -0.267589 0.958424 0.099096 1.000000 0.000000
+-0.055971 -0.003296 -0.057613 -0.267589 0.958424 0.099096 1.000000 1.000000
+-0.038250 -0.001447 -0.027644 -0.270871 0.957291 0.101107 0.000000 0.000000
+-0.055971 -0.003296 -0.057613 -0.270871 0.957291 0.101107 1.000000 1.000000
+-0.064116 -0.005870 -0.055063 -0.270871 0.957291 0.101107 1.000000 0.000000
+-0.038250 -0.001447 -0.027644 -0.339058 0.925165 0.170615 0.000000 0.000000
+-0.064116 -0.005870 -0.055063 -0.339058 0.925165 0.170615 1.000000 1.000000
+-0.042958 -0.003989 -0.023216 -0.339058 0.925165 0.170615 1.000000 0.000000
+-0.024969 -0.006524 0.006084 -0.700964 0.465309 0.540497 0.000000 0.000000
+-0.030055 -0.019593 0.010739 -0.700964 0.465309 0.540497 1.000000 0.000000
+-0.023429 -0.016611 0.016765 -0.700964 0.465309 0.540497 1.000000 1.000000
+-0.019299 -0.004036 0.008727 -0.539148 0.572132 0.618049 0.000000 0.000000
+-0.024969 -0.006524 0.006084 -0.539148 0.572132 0.618049 1.000000 0.000000
+-0.023429 -0.016611 0.016765 -0.539148 0.572132 0.618049 1.000000 1.000000
+-0.012031 -0.001713 0.012042 -0.484270 0.578566 0.656311 0.000000 0.000000
+-0.019299 -0.004036 0.008727 -0.484270 0.578566 0.656311 1.000000 0.000000
+-0.023429 -0.016611 0.016765 -0.484270 0.578566 0.656311 1.000000 1.000000
+-0.012031 -0.001713 0.012042 -0.657173 0.632668 0.409701 0.000000 0.000000
+-0.023429 -0.016611 0.016765 -0.657173 0.632668 0.409701 1.000000 1.000000
+-0.016803 -0.013630 0.022790 -0.657173 0.632668 0.409701 1.000000 0.000000
+-0.006243 0.000018 0.014676 -0.460670 0.689048 0.559460 0.000000 0.000000
+-0.012031 -0.001713 0.012042 -0.460670 0.689048 0.559460 1.000000 0.000000
+-0.016803 -0.013630 0.022790 -0.460670 0.689048 0.559460 1.000000 1.000000
+-0.006243 0.000018 0.014676 -0.629542 0.694363 0.348620 0.000000 0.000000
+-0.016803 -0.013630 0.022790 -0.629542 0.694363 0.348620 1.000000 1.000000
+-0.003552 -0.007667 0.034842 -0.629542 0.694363 0.348620 1.000000 0.000000
+-0.006243 0.000018 0.014676 -0.479183 0.796996 0.367668 0.000000 0.000000
+-0.003552 -0.007667 0.034842 -0.479183 0.796996 0.367668 1.000000 1.000000
+-0.003050 0.001094 0.016505 -0.479183 0.796996 0.367668 1.000000 0.000000
+0.052980 -0.000209 0.019776 0.444707 0.768639 -0.459815 0.000000 0.000000
+0.048486 0.003959 0.022397 0.444707 0.768639 -0.459815 1.000000 0.000000
+0.045717 0.011490 0.032308 0.444707 0.768639 -0.459815 1.000000 1.000000
+0.052980 -0.000209 0.019776 0.386545 0.775200 -0.499648 0.000000 0.000000
+0.045717 0.011490 0.032308 0.386545 0.775200 -0.499648 1.000000 0.000000
+0.063030 -0.001161 0.026074 0.386545 0.775200 -0.499648 1.000000 1.000000
+0.053360 0.088072 0.187479 -0.745121 -0.286945 0.602044 0.322451 0.094662
+0.048824 0.075798 0.176015 -0.745121 -0.286945 0.602044 0.338641 0.110258
+0.060302 0.081476 0.192927 -0.745121 -0.286945 0.602044 0.310308 0.088773
+0.048824 0.075798 0.176015 -0.916966 0.394495 -0.059550 0.338641 0.110258
+0.053360 0.088072 0.187479 -0.916966 0.394495 -0.059550 0.322451 0.094662
+0.053192 0.085858 0.175399 -0.916966 0.394495 -0.059550 0.335220 0.101700
+-0.100711 -0.017457 -0.071205 -0.665292 0.590463 0.456881 0.966166 0.212554
+-0.094869 -0.027848 -0.049269 -0.665292 0.590463 0.456881 0.952723 0.231841
+-0.080407 -0.011017 -0.049962 -0.665292 0.590463 0.456881 0.934786 0.219472
+-0.080407 -0.011017 -0.049962 -0.629837 0.563209 0.534884 0.934786 0.219472
+-0.094869 -0.027848 -0.049269 -0.629837 0.563209 0.534884 0.952723 0.231841
+-0.071377 -0.019477 -0.030421 -0.629837 0.563209 0.534884 0.889548 0.226645
+-0.094869 -0.027848 -0.049269 -0.649287 0.138224 0.747877 0.952723 0.231841
+-0.077178 -0.040515 -0.031569 -0.649287 0.138224 0.747877 0.899208 0.251069
+-0.071377 -0.019477 -0.030421 -0.649287 0.138224 0.747877 0.889548 0.226645
+-0.071377 -0.019477 -0.030421 -0.697333 0.154085 0.699989 0.889548 0.226645
+-0.077178 -0.040515 -0.031569 -0.697333 0.154085 0.699989 0.899208 0.251069
+-0.060966 -0.016531 -0.020698 -0.697333 0.154085 0.699989 0.847640 0.236770
+-0.077178 -0.040515 -0.031569 -0.673515 0.125054 0.728518 0.899208 0.251069
+-0.030500 -0.033141 0.010319 -0.673515 0.125054 0.728518 0.740681 0.264800
+-0.060966 -0.016531 -0.020698 -0.673515 0.125054 0.728518 0.847640 0.236770
+-0.030500 -0.033141 0.010319 -0.613359 -0.556141 0.560800 0.740681 0.264800
+-0.016507 -0.053189 0.005742 -0.613359 -0.556141 0.560800 0.718071 0.309760
+-0.001950 -0.040273 0.034472 -0.613359 -0.556141 0.560800 0.643033 0.298300
+-0.077178 -0.040515 -0.031569 -0.523697 -0.519646 0.675062 0.899208 0.251069
+-0.016507 -0.053189 0.005742 -0.523697 -0.519646 0.675062 0.718071 0.309760
+-0.030500 -0.033141 0.010319 -0.523697 -0.519646 0.675062 0.740681 0.264800
+0.009464 -0.061640 0.000422 -0.181614 -0.859215 0.478294 0.670646 0.372138
+-0.001950 -0.040273 0.034472 -0.181614 -0.859215 0.478294 0.643033 0.298300
+-0.016507 -0.053189 0.005742 -0.181614 -0.859215 0.478294 0.718071 0.309760
+0.009464 -0.061640 0.000422 -0.227359 -0.857354 0.461792 0.670646 0.372138
+0.031230 -0.028204 0.073215 -0.227359 -0.857354 0.461792 0.513823 0.299301
+-0.001950 -0.040273 0.034472 -0.227359 -0.857354 0.461792 0.643033 0.298300
+-0.001950 -0.040273 0.034472 -0.691545 -0.419585 0.587975 0.643033 0.298300
+-0.001327 -0.021202 0.048814 -0.691545 -0.419585 0.587975 0.618682 0.273535
+-0.023862 -0.025303 0.019383 -0.691545 -0.419585 0.587975 0.712310 0.264110
+-0.001327 -0.021202 0.048814 -0.750619 0.410812 0.517497 0.618682 0.273535
+-0.003552 -0.007667 0.034842 -0.750619 0.410812 0.517497 0.647650 0.255020
+-0.023862 -0.025303 0.019383 -0.750619 0.410812 0.517497 0.712310 0.264110
+-0.001327 -0.021202 0.048814 -0.839834 0.316908 0.440737 0.618682 0.273535
+0.016955 -0.004583 0.071701 -0.839834 0.316908 0.440737 0.548534 0.263306
+-0.003552 -0.007667 0.034842 -0.839834 0.316908 0.440737 0.647650 0.255020
+0.016955 -0.004583 0.071701 -0.726514 0.460848 0.509702 0.548534 0.263306
+0.029876 -0.001883 0.087677 -0.726514 0.460848 0.509702 0.498770 0.268030
+0.034158 0.013303 0.080050 -0.726514 0.460848 0.509702 0.513733 0.250058
+0.043933 -0.002318 0.113530 -0.668360 -0.655072 0.352384 0.426570 0.255980
+0.034355 0.014456 0.126546 -0.668360 -0.655072 0.352384 0.427820 0.215940
+0.029876 -0.001883 0.087677 -0.668360 -0.655072 0.352384 0.498770 0.268030
+0.029876 -0.001883 0.087677 -0.838913 -0.460432 0.290218 0.498770 0.268030
+0.034355 0.014456 0.126546 -0.838913 -0.460432 0.290218 0.427820 0.215940
+0.027702 0.029101 0.130549 -0.838913 -0.460432 0.290218 0.434330 0.198350
+0.027702 0.029101 0.130549 -0.991845 -0.023218 -0.125317 0.434330 0.198350
+0.027156 0.044410 0.132034 -0.991845 -0.023218 -0.125317 0.437699 0.188555
+0.030087 0.039882 0.109675 -0.991845 -0.023218 -0.125317 0.470850 0.222740
+0.034355 0.014456 0.126546 -0.785511 -0.467636 0.405326 0.427820 0.215940
+0.033561 0.031491 0.144661 -0.785511 -0.467636 0.405326 0.402106 0.187773
+0.027702 0.029101 0.130549 -0.785511 -0.467636 0.405326 0.434330 0.198350
+0.033561 0.031491 0.144661 -0.916944 -0.070794 0.392685 0.402106 0.187773
+0.027156 0.044410 0.132034 -0.916944 -0.070794 0.392685 0.437699 0.188555
+0.027702 0.029101 0.130549 -0.916944 -0.070794 0.392685 0.434330 0.198350
+0.033561 0.031491 0.144661 -0.940232 -0.189813 0.282727 0.402106 0.187773
+0.031436 0.049052 0.149384 -0.940232 -0.189813 0.282727 0.399565 0.167196
+0.027156 0.044410 0.132034 -0.940232 -0.189813 0.282727 0.437699 0.188555
+0.027156 0.044410 0.132034 -0.864752 0.495672 0.080705 0.437699 0.188555
+0.031436 0.049052 0.149384 -0.864752 0.495672 0.080705 0.399565 0.167196
+0.042691 0.068252 0.152059 -0.864752 0.495672 0.080705 0.431340 0.169670
+0.031436 0.049052 0.149384 -0.828294 0.515973 -0.218405 0.399565 0.167196
+0.036733 0.065820 0.168909 -0.828294 0.515973 -0.218405 0.369148 0.127941
+0.042691 0.068252 0.152059 -0.828294 0.515973 -0.218405 0.395810 0.143320
+0.031436 0.049052 0.149384 -0.720002 -0.417785 0.554124 0.399565 0.167196
+0.041507 0.041081 0.156460 -0.720002 -0.417785 0.554124 0.365580 0.162280
+0.036733 0.065820 0.168909 -0.720002 -0.417785 0.554124 0.369148 0.127941
+0.036733 0.065820 0.168909 -0.470592 -0.467407 0.748380 0.369148 0.127941
+0.041507 0.041081 0.156460 -0.470592 -0.467407 0.748380 0.365580 0.162280
+0.053331 0.064539 0.178546 -0.470592 -0.467407 0.748380 0.324280 0.113570
+0.054500 0.046985 0.157414 0.334482 -0.813087 0.476457 0.327890 0.162200
+0.057856 0.057044 0.172224 0.334482 -0.813087 0.476457 0.313070 0.128910
+0.041507 0.041081 0.156460 0.334482 -0.813087 0.476457 0.365580 0.162280
+0.041507 0.041081 0.156460 -0.065725 -0.666251 0.742826 0.365580 0.162280
+0.057856 0.057044 0.172224 -0.065725 -0.666251 0.742826 0.313070 0.128910
+0.053331 0.064539 0.178546 -0.065725 -0.666251 0.742826 0.324280 0.113570
+0.057856 0.057044 0.172224 -0.004204 -0.646233 0.763129 0.313070 0.128910
+0.060302 0.081476 0.192927 -0.004204 -0.646233 0.763129 0.299460 0.087742
+0.053331 0.064539 0.178546 -0.004204 -0.646233 0.763129 0.309980 0.099810
+0.057856 0.057044 0.172224 0.038133 -0.648233 0.760487 0.313070 0.128910
+0.072808 0.072316 0.184492 0.038133 -0.648233 0.760487 0.284679 0.109697
+0.060302 0.081476 0.192927 0.038133 -0.648233 0.760487 0.299460 0.087742
+0.053331 0.064539 0.178546 -0.502495 -0.006800 0.864553 0.324280 0.113570
+0.048824 0.075798 0.176015 -0.502495 -0.006800 0.864553 0.344190 0.112360
+0.036733 0.065820 0.168909 -0.502495 -0.006800 0.864553 0.369148 0.127941
+0.048824 0.075798 0.176015 -0.598940 0.794892 -0.097051 0.378837 0.128303
+0.042691 0.068252 0.152059 -0.598940 0.794892 -0.097051 0.395810 0.143320
+0.036733 0.065820 0.168909 -0.598940 0.794892 -0.097051 0.369148 0.127941
+0.042691 0.068252 0.152059 -0.909661 0.401486 0.106418 0.384310 0.126990
+0.048824 0.075798 0.176015 -0.909661 0.401486 0.106418 0.344190 0.112360
+0.053192 0.085858 0.175399 -0.909661 0.401486 0.106418 0.347560 0.104090
+-0.001950 -0.040273 0.034472 -0.660818 -0.256176 0.705474 0.643033 0.298300
+-0.023862 -0.025303 0.019383 -0.660818 -0.256176 0.705474 0.712310 0.264110
+-0.030500 -0.033141 0.010319 -0.660818 -0.256176 0.705474 0.740681 0.264800
+-0.023862 -0.025303 0.019383 -0.810222 0.008444 0.586062 0.712310 0.264110
+-0.030055 -0.019593 0.010739 -0.810222 0.008444 0.586062 0.733560 0.250650
+-0.030500 -0.033141 0.010319 -0.810222 0.008444 0.586062 0.740681 0.264800
+0.053331 0.064539 0.178546 -0.784552 -0.180721 0.593142 0.325550 0.108270
+0.060302 0.081476 0.192927 -0.784552 -0.180721 0.593142 0.310308 0.088773
+0.048824 0.075798 0.176015 -0.784552 -0.180721 0.593142 0.338641 0.110258
+0.057856 0.057044 0.172224 0.804863 -0.559570 0.197677 0.313070 0.128910
+0.054500 0.046985 0.157414 0.804863 -0.559570 0.197677 0.327890 0.162200
+0.062351 0.052413 0.140813 0.804863 -0.559570 0.197677 0.316680 0.182090
+0.030087 0.039882 0.109675 -0.956082 0.140357 -0.257308 0.470850 0.222740
+0.034158 0.013303 0.080050 -0.956082 0.140357 -0.257308 0.513733 0.250058
+0.029876 -0.001883 0.087677 -0.956082 0.140357 -0.257308 0.498770 0.268030
+0.030087 0.039882 0.109675 -0.994873 0.051019 -0.087321 0.470850 0.222740
+0.029876 -0.001883 0.087677 -0.994873 0.051019 -0.087321 0.498770 0.268030
+0.027702 0.029101 0.130549 -0.994873 0.051019 -0.087321 0.434330 0.198350
+0.054500 0.046985 0.157414 0.801033 -0.566444 0.193618 0.327890 0.162200
+0.044423 0.031511 0.153834 0.801033 -0.566444 0.193618 0.364836 0.180676
+0.062351 0.052413 0.140813 0.801033 -0.566444 0.193618 0.316680 0.182090
+0.044423 0.031511 0.153834 -0.492545 -0.644665 0.584642 0.364836 0.180676
+0.033561 0.031491 0.144661 -0.492545 -0.644665 0.584642 0.402106 0.187773
+0.034355 0.014456 0.126546 -0.492545 -0.644665 0.584642 0.427820 0.215940
+0.041507 0.041081 0.156460 -0.597367 -0.376343 0.708179 0.365580 0.162280
+0.033561 0.031491 0.144661 -0.597367 -0.376343 0.708179 0.402106 0.187773
+0.044423 0.031511 0.153834 -0.597367 -0.376343 0.708179 0.364836 0.180676
+0.041507 0.041081 0.156460 -0.685984 -0.265235 0.677552 0.365580 0.162280
+0.031436 0.049052 0.149384 -0.685984 -0.265235 0.677552 0.399565 0.167196
+0.033561 0.031491 0.144661 -0.685984 -0.265235 0.677552 0.402106 0.187773
+0.044423 0.031511 0.153834 0.043531 -0.252010 0.966745 0.364836 0.180676
+0.054500 0.046985 0.157414 0.043531 -0.252010 0.966745 0.327890 0.162200
+0.041507 0.041081 0.156460 0.043531 -0.252010 0.966745 0.365580 0.162280
+0.044423 0.031511 0.153834 -0.616245 -0.362089 0.699380 0.364836 0.180676
+0.077645 0.019022 0.176641 -0.616245 -0.362089 0.699380 0.260121 0.223980
+0.062326 0.031884 0.169802 -0.616245 -0.362089 0.699380 0.301326 0.204647
+0.077645 0.019022 0.176641 -0.093862 -0.853624 0.512363 0.260121 0.223980
+0.089982 -0.007425 0.134839 -0.093862 -0.853624 0.512363 0.295650 0.315700
+0.090978 0.012292 0.167871 -0.093862 -0.853624 0.512363 0.243450 0.264540
+0.089982 -0.007425 0.134839 -0.198069 -0.839011 0.506783 0.295650 0.315700
+0.104416 0.007223 0.164731 -0.198069 -0.839011 0.506783 0.220810 0.296310
+0.090978 0.012292 0.167871 -0.198069 -0.839011 0.506783 0.243450 0.264540
+0.089982 -0.007425 0.134839 0.157287 -0.945881 0.283847 0.295650 0.315700
+0.071473 -0.024618 0.087802 0.157287 -0.945881 0.283847 0.405190 0.354640
+0.099112 -0.012738 0.112075 0.157287 -0.945881 0.283847 0.310110 0.370450
+0.089982 -0.007425 0.134839 -0.003403 -0.938786 0.344485 0.295650 0.315700
+0.056200 -0.017740 0.106395 -0.003403 -0.938786 0.344485 0.410687 0.294059
+0.071473 -0.024618 0.087802 -0.003403 -0.938786 0.344485 0.405190 0.354640
+0.071473 -0.024618 0.087802 -0.048886 -0.930285 0.363565 0.405190 0.354640
+0.031230 -0.028204 0.073215 -0.048886 -0.930285 0.363565 0.513823 0.299301
+0.048578 -0.040054 0.045226 -0.048886 -0.930285 0.363565 0.518630 0.377680
+0.031230 -0.028204 0.073215 -0.842235 -0.292242 0.453029 0.513823 0.299301
+0.043933 -0.002318 0.113530 -0.842235 -0.292242 0.453029 0.426570 0.255980
+0.029876 -0.001883 0.087677 -0.842235 -0.292242 0.453029 0.498770 0.268030
+0.029876 -0.001883 0.087677 -0.688296 -0.376206 0.620257 0.498770 0.268030
+0.016955 -0.004583 0.071701 -0.688296 -0.376206 0.620257 0.548534 0.263306
+0.031230 -0.028204 0.073215 -0.688296 -0.376206 0.620257 0.513823 0.299301
+0.031230 -0.028204 0.073215 -0.611092 -0.322985 0.722667 0.513823 0.299301
+0.016955 -0.004583 0.071701 -0.611092 -0.322985 0.722667 0.548534 0.263306
+-0.001327 -0.021202 0.048814 -0.611092 -0.322985 0.722667 0.618682 0.273535
+0.056200 -0.017740 0.106395 -0.539644 -0.666945 0.513779 0.410687 0.294059
+0.077645 0.019022 0.176641 -0.539644 -0.666945 0.513779 0.260121 0.223980
+0.043933 -0.002318 0.113530 -0.539644 -0.666945 0.513779 0.426570 0.255980
+0.043933 -0.002318 0.113530 -0.489524 -0.655452 0.575108 0.426570 0.255980
+0.031230 -0.028204 0.073215 -0.489524 -0.655452 0.575108 0.513823 0.299301
+0.056200 -0.017740 0.106395 -0.489524 -0.655452 0.575108 0.410687 0.294059
+0.056200 -0.017740 0.106395 -0.032782 -0.945844 0.322962 0.410687 0.294059
+0.031230 -0.028204 0.073215 -0.032782 -0.945844 0.322962 0.513823 0.299301
+0.071473 -0.024618 0.087802 -0.032782 -0.945844 0.322962 0.405190 0.354640
+-0.001327 -0.021202 0.048814 -0.591435 -0.472219 0.653616 0.618682 0.273535
+-0.001950 -0.040273 0.034472 -0.591435 -0.472219 0.653616 0.643033 0.298300
+0.031230 -0.028204 0.073215 -0.591435 -0.472219 0.653616 0.513823 0.299301
+0.048578 -0.040054 0.045226 0.522461 -0.851548 0.043590 0.518630 0.377680
+0.038577 -0.048319 0.003636 0.522461 -0.851548 0.043590 0.601120 0.425850
+0.069661 -0.026788 0.051686 0.522461 -0.851548 0.043590 0.462860 0.409610
+0.031230 -0.028204 0.073215 0.035688 -0.912147 0.408305 0.513823 0.299301
+0.009464 -0.061640 0.000422 0.035688 -0.912147 0.408305 0.670646 0.372138
+0.048578 -0.040054 0.045226 0.035688 -0.912147 0.408305 0.518630 0.377680
+0.048578 -0.040054 0.045226 0.407063 -0.909632 0.082882 0.518630 0.377680
+0.009464 -0.061640 0.000422 0.407063 -0.909632 0.082882 0.670646 0.372138
+0.038577 -0.048319 0.003636 0.407063 -0.909632 0.082882 0.601120 0.425850
+0.009464 -0.061640 0.000422 0.405337 -0.909086 0.096254 0.670646 0.372138
+-0.012845 -0.075192 -0.033626 0.405337 -0.909086 0.096254 0.768420 0.382030
+0.038577 -0.048319 0.003636 0.405337 -0.909086 0.096254 0.601120 0.425850
+0.038577 -0.048319 0.003636 0.511382 -0.854700 -0.089312 0.601120 0.425850
+-0.012845 -0.075192 -0.033626 0.511382 -0.854700 -0.089312 0.768420 0.382030
+0.022281 -0.052876 -0.046062 0.511382 -0.854700 -0.089312 0.714108 0.468367
+-0.012845 -0.075192 -0.033626 0.567700 -0.809188 0.151432 0.768420 0.382030
+-0.012438 -0.080839 -0.065327 0.567700 -0.809188 0.151432 0.815400 0.434250
+0.022281 -0.052876 -0.046062 0.567700 -0.809188 0.151432 0.714108 0.468367
+-0.012438 -0.080839 -0.065327 0.645768 -0.735881 -0.203625 0.815400 0.434250
+-0.047403 -0.100711 -0.104398 0.645768 -0.735881 -0.203625 0.951641 0.425704
+0.007882 -0.060295 -0.075129 0.645768 -0.735881 -0.203625 0.786970 0.489360
+-0.095526 -0.117518 -0.143070 0.470368 -0.856341 -0.213151 0.981023 0.456908
+-0.034406 -0.085027 -0.138728 0.470368 -0.856341 -0.213151 0.961750 0.509730
+-0.047403 -0.100711 -0.104398 0.470368 -0.856341 -0.213151 0.951641 0.425704
+-0.047403 -0.100711 -0.104398 0.625410 -0.771661 -0.115767 0.951641 0.425704
+-0.034406 -0.085027 -0.138728 0.625410 -0.771661 -0.115767 0.961750 0.509730
+0.007882 -0.060295 -0.075129 0.625410 -0.771661 -0.115767 0.786970 0.489360
+0.044423 0.031511 0.153834 -0.601876 -0.558302 0.571003 0.364836 0.180676
+0.034355 0.014456 0.126546 -0.601876 -0.558302 0.571003 0.427820 0.215940
+0.077645 0.019022 0.176641 -0.601876 -0.558302 0.571003 0.260121 0.223980
+0.034355 0.014456 0.126546 -0.515910 -0.689282 0.508653 0.427820 0.215940
+0.043933 -0.002318 0.113530 -0.515910 -0.689282 0.508653 0.426570 0.255980
+0.077645 0.019022 0.176641 -0.515910 -0.689282 0.508653 0.260121 0.223980
+0.089982 -0.007425 0.134839 -0.155682 -0.854882 0.494914 0.295650 0.315700
+0.077645 0.019022 0.176641 -0.155682 -0.854882 0.494914 0.260121 0.223980
+0.056200 -0.017740 0.106395 -0.155682 -0.854882 0.494914 0.410687 0.294059
+0.086777 -0.007718 0.051774 0.768201 -0.481981 -0.421381 0.428198 0.444416
+0.099099 0.007341 0.057013 0.768201 -0.481981 -0.421381 0.395210 0.454600
+0.116055 -0.004211 0.101138 0.768201 -0.481981 -0.421381 0.289090 0.422220
+0.099099 0.007341 0.057013 0.499735 -0.771389 -0.393985 0.395210 0.454600
+0.117596 0.011385 0.072557 0.499735 -0.771389 -0.393985 0.326990 0.473156
+0.116055 -0.004211 0.101138 0.499735 -0.771389 -0.393985 0.289090 0.422220
+0.117596 0.011385 0.072557 0.726269 -0.619128 -0.298686 0.326990 0.473156
+0.135593 0.023447 0.091315 0.726269 -0.619128 -0.298686 0.260220 0.478520
+0.116055 -0.004211 0.101138 0.726269 -0.619128 -0.298686 0.289090 0.422220
+0.153810 0.036576 0.117798 0.769282 -0.594302 -0.234544 0.196867 0.467708
+0.140320 0.014932 0.128395 0.769282 -0.594302 -0.234544 0.195960 0.427720
+0.135593 0.023447 0.091315 0.769282 -0.594302 -0.234544 0.260220 0.478520
+0.140320 0.014932 0.128395 0.872224 -0.425780 0.240700 0.195960 0.427720
+0.153810 0.036576 0.117798 0.872224 -0.425780 0.240700 0.196867 0.467708
+0.142930 0.026574 0.139531 0.872224 -0.425780 0.240700 0.174691 0.414974
+0.135350 0.021139 0.160543 0.247142 -0.851050 0.463287 0.158440 0.365650
+0.115943 0.014365 0.158452 0.247142 -0.851050 0.463287 0.203120 0.329410
+0.111779 0.000945 0.136021 0.247142 -0.851050 0.463287 0.245470 0.357710
+0.119415 0.029098 0.161260 -0.044159 -0.176988 0.983222 0.188520 0.331390
+0.115943 0.014365 0.158452 -0.044159 -0.176988 0.983222 0.203120 0.329410
+0.135350 0.021139 0.160543 -0.044159 -0.176988 0.983222 0.158440 0.365650
+0.119415 0.029098 0.161260 0.969037 -0.201043 -0.143350 0.188520 0.331390
+0.120143 0.026107 0.170376 0.969037 -0.201043 -0.143350 0.178018 0.316824
+0.115943 0.014365 0.158452 0.969037 -0.201043 -0.143350 0.203120 0.329410
+0.104416 0.007223 0.164731 0.618089 -0.719833 0.315921 0.220810 0.296310
+0.111779 0.000945 0.136021 0.618089 -0.719833 0.315921 0.245470 0.357710
+0.115943 0.014365 0.158452 0.618089 -0.719833 0.315921 0.203120 0.329410
+0.089982 -0.007425 0.134839 0.321758 -0.885320 0.335677 0.295650 0.315700
+0.099112 -0.012738 0.112075 0.321758 -0.885320 0.335677 0.310110 0.370450
+0.111779 0.000945 0.136021 0.321758 -0.885320 0.335677 0.245470 0.357710
+0.099112 -0.012738 0.112075 0.535414 -0.823546 0.187359 0.310110 0.370450
+0.116055 -0.004211 0.101138 0.535414 -0.823546 0.187359 0.289090 0.422220
+0.111779 0.000945 0.136021 0.535414 -0.823546 0.187359 0.245470 0.357710
+0.099112 -0.012738 0.112075 0.445046 -0.895465 -0.008706 0.310110 0.370450
+0.069661 -0.026788 0.051686 0.445046 -0.895465 -0.008706 0.462860 0.409610
+0.116055 -0.004211 0.101138 0.445046 -0.895465 -0.008706 0.289090 0.422220
+0.135593 0.023447 0.091315 0.751786 -0.615293 -0.237134 0.260220 0.478520
+0.140320 0.014932 0.128395 0.751786 -0.615293 -0.237134 0.195960 0.427720
+0.116055 -0.004211 0.101138 0.751786 -0.615293 -0.237134 0.289090 0.422220
+0.071473 -0.024618 0.087802 0.526997 -0.849511 0.024602 0.405190 0.354640
+0.048578 -0.040054 0.045226 0.526997 -0.849511 0.024602 0.518630 0.377680
+0.069661 -0.026788 0.051686 0.526997 -0.849511 0.024602 0.462860 0.409610
+0.143992 0.050780 0.185383 -0.796384 0.373403 0.475754 0.102570 0.343770
+0.124638 0.045934 0.156789 -0.796384 0.373403 0.475754 0.172400 0.336930
+0.119415 0.029098 0.161260 -0.796384 0.373403 0.475754 0.188520 0.331390
+0.143992 0.050780 0.185383 -0.892768 0.217083 0.394766 0.102570 0.343770
+0.149787 0.066389 0.189905 -0.892768 0.217083 0.394766 0.078462 0.341776
+0.142136 0.060030 0.176099 -0.892768 0.217083 0.394766 0.112540 0.336640
+0.158948 0.073733 0.201971 0.079730 -0.617483 0.782533 0.082315 0.372172
+0.143992 0.050780 0.185383 0.079730 -0.617483 0.782533 0.102570 0.343770
+0.156843 0.044757 0.179321 0.079730 -0.617483 0.782533 0.082094 0.377823
+0.143992 0.050780 0.185383 0.052633 -0.650386 0.757779 0.102570 0.343770
+0.135350 0.021139 0.160543 0.052633 -0.650386 0.757779 0.158440 0.365650
+0.156843 0.044757 0.179321 0.052633 -0.650386 0.757779 0.087780 0.372400
+0.111779 0.000945 0.136021 0.519729 -0.833631 0.186927 0.245470 0.357710
+0.116055 -0.004211 0.101138 0.519729 -0.833631 0.186927 0.289090 0.422220
+0.135350 0.021139 0.160543 0.519729 -0.833631 0.186927 0.158440 0.365650
+0.116055 -0.004211 0.101138 0.425900 -0.873837 0.234560 0.289090 0.422220
+0.140320 0.014932 0.128395 0.425900 -0.873837 0.234560 0.195960 0.427720
+0.135350 0.021139 0.160543 0.425900 -0.873837 0.234560 0.158440 0.365650
+0.143992 0.050780 0.185383 -0.829621 0.152396 0.537126 0.102570 0.343770
+0.158948 0.073733 0.201971 -0.829621 0.152396 0.537126 0.045070 0.346890
+0.149787 0.066389 0.189905 -0.829621 0.152396 0.537126 0.078462 0.341776
+0.164352 0.057553 0.175030 0.869006 -0.491847 0.053994 0.073840 0.398740
+0.174433 0.077625 0.195623 0.869006 -0.491847 0.053994 0.020480 0.387770
+0.156843 0.044757 0.179321 0.869006 -0.491847 0.053994 0.078100 0.382990
+0.156843 0.044757 0.179321 0.429872 -0.575227 0.695934 0.078100 0.382990
+0.174433 0.077625 0.195623 0.429872 -0.575227 0.695934 0.020480 0.387770
+0.158948 0.073733 0.201971 0.429872 -0.575227 0.695934 0.045070 0.346890
+0.164352 0.057553 0.175030 0.654188 -0.676188 0.338833 0.073840 0.398740
+0.173943 0.072128 0.185599 0.654188 -0.676188 0.338833 0.037193 0.399844
+0.174433 0.077625 0.195623 0.654188 -0.676188 0.338833 0.020480 0.387770
+0.069661 -0.026788 0.051686 0.709077 -0.635008 -0.306553 0.462860 0.409610
+0.068895 -0.018779 0.033324 0.709077 -0.635008 -0.306553 0.491740 0.437240
+0.086777 -0.007718 0.051774 0.709077 -0.635008 -0.306553 0.428198 0.444416
+0.069661 -0.026788 0.051686 0.366670 -0.929597 0.037458 0.462860 0.409610
+0.099112 -0.012738 0.112075 0.366670 -0.929597 0.037458 0.310110 0.370450
+0.071473 -0.024618 0.087802 0.366670 -0.929597 0.037458 0.405190 0.354640
+0.038577 -0.048319 0.003636 0.800629 -0.529312 -0.280752 0.601120 0.425850
+0.053625 -0.028163 0.008548 0.800629 -0.529312 -0.280752 0.561610 0.440210
+0.069661 -0.026788 0.051686 0.800629 -0.529312 -0.280752 0.462860 0.409610
+0.069661 -0.026788 0.051686 0.785880 -0.554125 -0.274478 0.462860 0.409610
+0.053625 -0.028163 0.008548 0.785880 -0.554125 -0.274478 0.561610 0.440210
+0.068895 -0.018779 0.033324 0.785880 -0.554125 -0.274478 0.491740 0.437240
+0.053625 -0.028163 0.008548 0.807024 -0.550314 -0.214163 0.561610 0.440210
+0.038577 -0.048319 0.003636 0.807024 -0.550314 -0.214163 0.601120 0.425850
+0.022281 -0.052876 -0.046062 0.807024 -0.550314 -0.214163 0.714108 0.468367
+0.022281 -0.052876 -0.046062 0.668114 -0.729858 -0.144678 0.714108 0.468367
+-0.012438 -0.080839 -0.065327 0.668114 -0.729858 -0.144678 0.815400 0.434250
+0.007882 -0.060295 -0.075129 0.668114 -0.729858 -0.144678 0.786970 0.489360
+0.069661 -0.026788 0.051686 0.693113 -0.620400 -0.367013 0.462860 0.409610
+0.086777 -0.007718 0.051774 0.693113 -0.620400 -0.367013 0.428198 0.444416
+0.116055 -0.004211 0.101138 0.693113 -0.620400 -0.367013 0.289090 0.422220
+0.143992 0.050780 0.185383 -0.253221 -0.576939 0.776544 0.102570 0.343770
+0.119415 0.029098 0.161260 -0.253221 -0.576939 0.776544 0.188520 0.331390
+0.135350 0.021139 0.160543 -0.253221 -0.576939 0.776544 0.158440 0.365650
+0.077645 0.019022 0.176641 0.042084 -0.760703 0.647735 0.260121 0.223980
+0.090978 0.012292 0.167871 0.042084 -0.760703 0.647735 0.243450 0.264540
+0.085054 0.021891 0.179529 0.042084 -0.760703 0.647735 0.240270 0.232350
+0.085054 0.021891 0.179529 -0.531019 -0.632109 0.564320 0.240270 0.232350
+0.087669 0.033440 0.194926 -0.531019 -0.632109 0.564320 0.211122 0.213362
+0.075763 0.037689 0.188482 -0.531019 -0.632109 0.564320 0.246170 0.197800
+0.075763 0.037689 0.188482 -0.879984 0.084000 0.467517 0.246170 0.197800
+0.086585 0.059950 0.204852 -0.879984 0.084000 0.467517 0.187156 0.190790
+0.071425 0.047528 0.178549 -0.879984 0.084000 0.467517 0.267450 0.197950
+0.075763 0.037689 0.188482 -0.948637 -0.124740 0.290736 0.246170 0.197800
+0.071425 0.047528 0.178549 -0.948637 -0.124740 0.290736 0.267450 0.197950
+0.073688 0.029712 0.178289 -0.948637 -0.124740 0.290736 0.265743 0.210273
+0.071425 0.047528 0.178549 -0.606906 -0.088616 0.789818 0.267450 0.197950
+0.062326 0.031884 0.169802 -0.606906 -0.088616 0.789818 0.301326 0.204647
+0.073688 0.029712 0.178289 -0.606906 -0.088616 0.789818 0.265743 0.210273
+0.073688 0.029712 0.178289 -0.604123 -0.335083 0.723017 0.265743 0.210273
+0.062326 0.031884 0.169802 -0.604123 -0.335083 0.723017 0.301326 0.204647
+0.077645 0.019022 0.176641 -0.604123 -0.335083 0.723017 0.260121 0.223980
+0.085054 0.021891 0.179529 -0.269053 -0.243256 0.931899 0.240270 0.232350
+0.073688 0.029712 0.178289 -0.269053 -0.243256 0.931899 0.265743 0.210273
+0.077645 0.019022 0.176641 -0.269053 -0.243256 0.931899 0.260121 0.223980
+0.085054 0.021891 0.179529 -0.498692 -0.630416 0.594880 0.240270 0.232350
+0.075763 0.037689 0.188482 -0.498692 -0.630416 0.594880 0.246170 0.197800
+0.073688 0.029712 0.178289 -0.498692 -0.630416 0.594880 0.265743 0.210273
+0.062326 0.031884 0.169802 -0.849376 0.227831 0.476081 0.301326 0.204647
+0.071425 0.047528 0.178549 -0.849376 0.227831 0.476081 0.267450 0.197950
+0.061518 0.042216 0.163416 -0.849376 0.227831 0.476081 0.310410 0.197450
+0.062326 0.031884 0.169802 -0.621038 0.376365 0.687504 0.301326 0.204647
+0.061518 0.042216 0.163416 -0.621038 0.376365 0.687504 0.310410 0.197450
+0.044423 0.031511 0.153834 -0.621038 0.376365 0.687504 0.364836 0.180676
+0.090978 0.012292 0.167871 0.033077 -0.461478 0.886535 0.243450 0.264540
+0.104416 0.007223 0.164731 0.033077 -0.461478 0.886535 0.220810 0.296310
+0.095701 0.027930 0.175835 0.033077 -0.461478 0.886535 0.220030 0.259690
+0.095701 0.027930 0.175835 -0.373817 -0.555589 0.742685 0.220030 0.259690
+0.104416 0.007223 0.164731 -0.373817 -0.555589 0.742685 0.220810 0.296310
+0.109910 0.024071 0.180100 -0.373817 -0.555589 0.742685 0.186271 0.283171
+0.104416 0.007223 0.164731 0.592484 -0.639773 0.489543 0.220810 0.296310
+0.120143 0.026107 0.170376 0.592484 -0.639773 0.489543 0.178018 0.316824
+0.109910 0.024071 0.180100 0.592484 -0.639773 0.489543 0.186271 0.283171
+0.109910 0.024071 0.180100 0.472250 -0.819184 0.325450 0.186271 0.283171
+0.120143 0.026107 0.170376 0.472250 -0.819184 0.325450 0.178018 0.316824
+0.141667 0.052178 0.204766 0.472250 -0.819184 0.325450 0.098020 0.295780
+0.120143 0.026107 0.170376 0.568638 -0.786656 0.240464 0.178018 0.316824
+0.141560 0.048534 0.193098 0.568638 -0.786656 0.240464 0.098520 0.321330
+0.141667 0.052178 0.204766 0.568638 -0.786656 0.240464 0.098020 0.295780
+0.123196 0.044287 0.202612 -0.491871 -0.233698 0.838718 0.092590 0.280640
+0.141494 0.076623 0.222353 -0.491871 -0.233698 0.838718 0.059790 0.277920
+0.123289 0.059764 0.206979 -0.491871 -0.233698 0.838718 0.112692 0.264495
+0.141667 0.052178 0.204766 0.999135 -0.041404 0.003769 0.081990 0.304590
+0.141560 0.048534 0.193098 0.999135 -0.041404 0.003769 0.098520 0.321330
+0.142189 0.063498 0.190741 0.999135 -0.041404 0.003769 0.087940 0.327060
+0.141560 0.048534 0.193098 0.828272 -0.120991 -0.547108 0.098520 0.321330
+0.135284 0.055424 0.182073 0.828272 -0.120991 -0.547108 0.107730 0.328780
+0.142189 0.063498 0.190741 0.828272 -0.120991 -0.547108 0.087940 0.327060
+0.141560 0.048534 0.193098 0.799768 -0.185219 -0.571021 0.098520 0.321330
+0.120143 0.026107 0.170376 0.799768 -0.185219 -0.571021 0.178018 0.316824
+0.135284 0.055424 0.182073 0.799768 -0.185219 -0.571021 0.107730 0.328780
+0.135284 0.055424 0.182073 0.867247 -0.285024 -0.408221 0.107730 0.328780
+0.120143 0.026107 0.170376 0.867247 -0.285024 -0.408221 0.178018 0.316824
+0.119302 0.038419 0.159993 0.867247 -0.285024 -0.408221 0.180790 0.332860
+0.120143 0.026107 0.170376 0.996860 0.001323 -0.079174 0.178018 0.316824
+0.119415 0.029098 0.161260 0.996860 0.001323 -0.079174 0.188520 0.331390
+0.119302 0.038419 0.159993 0.996860 0.001323 -0.079174 0.180790 0.332860
+0.090978 0.012292 0.167871 0.524756 -0.506802 0.683946 0.243450 0.264540
+0.095701 0.027930 0.175835 0.524756 -0.506802 0.683946 0.220030 0.259690
+0.085054 0.021891 0.179529 0.524756 -0.506802 0.683946 0.240270 0.232350
+0.100002 0.036926 0.190618 0.964968 -0.215553 -0.149578 0.190200 0.243860
+0.095701 0.027930 0.175835 0.964968 -0.215553 -0.149578 0.220030 0.259690
+0.097660 0.038978 0.172552 0.964968 -0.215553 -0.149578 0.211300 0.262970
+0.100002 0.036926 0.190618 0.548370 -0.775661 0.312474 0.190200 0.243860
+0.085054 0.021891 0.179529 0.548370 -0.775661 0.312474 0.240270 0.232350
+0.095701 0.027930 0.175835 0.548370 -0.775661 0.312474 0.220030 0.259690
+0.107389 0.046282 0.210566 0.668609 -0.737112 0.098125 0.147990 0.226080
+0.085054 0.021891 0.179529 0.668609 -0.737112 0.098125 0.240270 0.232350
+0.100002 0.036926 0.190618 0.668609 -0.737112 0.098125 0.190200 0.243860
+0.107389 0.046282 0.210566 0.052277 -0.803117 0.593524 0.147990 0.226080
+0.087669 0.033440 0.194926 0.052277 -0.803117 0.593524 0.211122 0.213362
+0.085054 0.021891 0.179529 0.052277 -0.803117 0.593524 0.240270 0.232350
+0.108825 0.043741 0.199336 0.924372 -0.026921 -0.380540 0.159984 0.245722
+0.097660 0.038978 0.172552 0.924372 -0.026921 -0.380540 0.211300 0.262970
+0.108636 0.055260 0.198062 0.924372 -0.026921 -0.380540 0.155020 0.252040
+0.108636 0.055260 0.198062 0.727876 0.391428 -0.563011 0.155020 0.252040
+0.097660 0.038978 0.172552 0.727876 0.391428 -0.563011 0.211300 0.262970
+0.088707 0.052685 0.170507 0.727876 0.391428 -0.563011 0.200790 0.261530
+0.131489 0.078973 0.229080 0.754704 -0.639453 0.146699 0.069787 0.240614
+0.118713 0.062923 0.224846 0.754704 -0.639453 0.146699 0.104782 0.225075
+0.107389 0.046282 0.210566 0.754704 -0.639453 0.146699 0.147990 0.226080
+0.118713 0.062923 0.224846 0.059877 -0.673205 0.737028 0.104782 0.225075
+0.107296 0.067456 0.229914 0.059877 -0.673205 0.737028 0.116859 0.196548
+0.107389 0.046282 0.210566 0.059877 -0.673205 0.737028 0.147990 0.226080
+0.107389 0.046282 0.210566 -0.085449 -0.672296 0.735334 0.147990 0.226080
+0.107296 0.067456 0.229914 -0.085449 -0.672296 0.735334 0.116859 0.196548
+0.093507 0.047680 0.210231 -0.085449 -0.672296 0.735334 0.175260 0.199050
+0.107296 0.067456 0.229914 -0.744129 -0.132778 0.654708 0.116859 0.196548
+0.086585 0.059950 0.204852 -0.744129 -0.132778 0.654708 0.187156 0.190790
+0.093507 0.047680 0.210231 -0.744129 -0.132778 0.654708 0.175260 0.199050
+0.108825 0.043741 0.199336 0.821736 -0.049308 -0.567731 0.159984 0.245722
+0.108636 0.055260 0.198062 0.821736 -0.049308 -0.567731 0.155020 0.252040
+0.131489 0.078973 0.229080 0.821736 -0.049308 -0.567731 0.069787 0.240614
+0.142987 0.029221 0.148360 0.853671 -0.470722 0.222860 0.161963 0.397901
+0.135350 0.021139 0.160543 0.853671 -0.470722 0.222860 0.158440 0.365650
+0.140320 0.014932 0.128395 0.853671 -0.470722 0.222860 0.195960 0.427720
+0.150255 0.033474 0.161588 0.605924 -0.753726 0.254467 0.126154 0.391636
+0.156843 0.044757 0.179321 0.605924 -0.753726 0.254467 0.089596 0.378162
+0.135350 0.021139 0.160543 0.605924 -0.753726 0.254467 0.158440 0.365650
+0.142987 0.029221 0.148360 0.885699 -0.170473 -0.431829 0.161963 0.397901
+0.150595 0.051764 0.155065 0.885699 -0.170473 -0.431829 0.121890 0.403040
+0.150255 0.033474 0.161588 0.885699 -0.170473 -0.431829 0.126154 0.391636
+0.150595 0.051764 0.155065 0.832961 -0.199547 -0.516097 0.121890 0.403040
+0.164352 0.057553 0.175030 0.832961 -0.199547 -0.516097 0.073840 0.398740
+0.150255 0.033474 0.161588 0.832961 -0.199547 -0.516097 0.126154 0.391636
+0.150255 0.033474 0.161588 0.862659 -0.505784 0.001328 0.126154 0.391636
+0.164352 0.057553 0.175030 0.862659 -0.505784 0.001328 0.073840 0.398740
+0.156843 0.044757 0.179321 0.862659 -0.505784 0.001328 0.089596 0.378162
+0.100002 0.036926 0.190618 0.424000 -0.870155 0.251107 0.190200 0.243860
+0.108825 0.043741 0.199336 0.424000 -0.870155 0.251107 0.159984 0.245722
+0.107389 0.046282 0.210566 0.424000 -0.870155 0.251107 0.147990 0.226080
+0.108825 0.043741 0.199336 0.701902 -0.691795 -0.169568 0.159984 0.245722
+0.100002 0.036926 0.190618 0.701902 -0.691795 -0.169568 0.190200 0.243860
+0.097660 0.038978 0.172552 0.701902 -0.691795 -0.169568 0.211300 0.262970
+0.089982 -0.007425 0.134839 0.330601 -0.900733 0.281749 0.295650 0.315700
+0.111779 0.000945 0.136021 0.330601 -0.900733 0.281749 0.245470 0.357710
+0.104416 0.007223 0.164731 0.330601 -0.900733 0.281749 0.220810 0.296310
+0.104416 0.007223 0.164731 0.632117 -0.651732 0.419134 0.220810 0.296310
+0.115943 0.014365 0.158452 0.632117 -0.651732 0.419134 0.203120 0.329410
+0.120143 0.026107 0.170376 0.632117 -0.651732 0.419134 0.178018 0.316824
+-0.047403 -0.100711 -0.104398 0.030618 -0.901724 0.431227 0.951641 0.425704
+-0.012438 -0.080839 -0.065327 0.030618 -0.901724 0.431227 0.815400 0.434250
+-0.069241 -0.093206 -0.087154 0.030618 -0.901724 0.431227 0.914152 0.365852
+-0.069241 -0.093206 -0.087154 0.144648 -0.973826 0.175328 0.914152 0.365852
+-0.012438 -0.080839 -0.065327 0.144648 -0.973826 0.175328 0.815400 0.434250
+-0.012845 -0.075192 -0.033626 0.144648 -0.973826 0.175328 0.768420 0.382030
+-0.012845 -0.075192 -0.033626 0.033640 -0.935960 0.350495 0.768420 0.382030
+0.009464 -0.061640 0.000422 0.033640 -0.935960 0.350495 0.670646 0.372138
+-0.029661 -0.072947 -0.026017 0.033640 -0.935960 0.350495 0.793950 0.336740
+0.009464 -0.061640 0.000422 -0.148008 -0.811083 0.565896 0.670646 0.372138
+-0.016507 -0.053189 0.005742 -0.148008 -0.811083 0.565896 0.718071 0.309760
+-0.029661 -0.072947 -0.026017 -0.148008 -0.811083 0.565896 0.790105 0.328337
+0.174312 0.080673 0.176682 -0.036027 -0.475138 0.879174 0.042770 0.427560
+0.168610 0.067224 0.169180 -0.036027 -0.475138 0.879174 0.058830 0.426820
+0.182614 0.078900 0.176064 -0.036027 -0.475138 0.879174 0.042790 0.429381
+0.168610 0.067224 0.169180 0.299846 -0.725121 0.619913 0.062340 0.427740
+0.179213 0.068282 0.165289 0.299846 -0.725121 0.619913 0.055790 0.445457
+0.182614 0.078900 0.176064 0.299846 -0.725121 0.619913 0.042790 0.429381
+-0.034406 -0.085027 -0.138728 0.750296 -0.606529 -0.263020 0.959968 0.514181
+-0.032947 -0.077119 -0.152802 0.750296 -0.606529 -0.263020 0.958829 0.529430
+0.007882 -0.060295 -0.075129 0.750296 -0.606529 -0.263020 0.932195 0.522238
+-0.032947 -0.077119 -0.152802 0.888469 -0.143330 -0.435981 0.958829 0.529430
+0.003153 -0.046601 -0.089268 0.888469 -0.143330 -0.435981 0.824771 0.564740
+0.007882 -0.060295 -0.075129 0.888469 -0.143330 -0.435981 0.787970 0.551140
+0.007882 -0.060295 -0.075129 0.901465 -0.118530 -0.416308 0.787970 0.551140
+0.003153 -0.046601 -0.089268 0.901465 -0.118530 -0.416308 0.824771 0.564740
+0.022281 -0.052876 -0.046062 0.901465 -0.118530 -0.416308 0.707390 0.565900
+0.003153 -0.046601 -0.089268 0.889847 -0.178533 -0.419879 0.824771 0.564740
+0.029309 -0.029773 -0.040991 0.889847 -0.178533 -0.419879 0.694998 0.593245
+0.022281 -0.052876 -0.046062 0.889847 -0.178533 -0.419879 0.707390 0.565900
+0.003153 -0.046601 -0.089268 0.831577 0.197122 -0.519251 0.824771 0.564740
+-0.004285 -0.027983 -0.094112 0.831577 0.197122 -0.519251 0.848720 0.590070
+0.029309 -0.029773 -0.040991 0.831577 0.197122 -0.519251 0.694998 0.593245
+-0.004285 -0.027983 -0.094112 0.762594 0.447416 -0.467192 0.848720 0.590070
+0.008989 -0.011941 -0.057082 0.762594 0.447416 -0.467192 0.757109 0.626270
+0.029309 -0.029773 -0.040991 0.762594 0.447416 -0.467192 0.694998 0.593245
+-0.025528 -0.015002 -0.098342 0.508655 0.647474 -0.567492 0.894100 0.654540
+-0.004285 -0.027983 -0.094112 0.508655 0.647474 -0.567492 0.848720 0.590070
+-0.024684 -0.048566 -0.135880 0.508655 0.647474 -0.567492 0.936076 0.565704
+-0.004285 -0.027983 -0.094112 0.837053 0.202129 -0.508415 0.848720 0.590070
+0.003153 -0.046601 -0.089268 0.837053 0.202129 -0.508415 0.824771 0.564740
+-0.024684 -0.048566 -0.135880 0.837053 0.202129 -0.508415 0.936076 0.565704
+0.087669 0.033440 0.194926 -0.521755 -0.206783 0.827655 0.211122 0.213362
+0.107389 0.046282 0.210566 -0.521755 -0.206783 0.827655 0.147990 0.226080
+0.075763 0.037689 0.188482 -0.521755 -0.206783 0.827655 0.246170 0.197800
+0.075763 0.037689 0.188482 -0.099325 -0.870994 0.481149 0.246170 0.197800
+0.107389 0.046282 0.210566 -0.099325 -0.870994 0.481149 0.147990 0.226080
+0.093507 0.047680 0.210231 -0.099325 -0.870994 0.481149 0.175260 0.199050
+-0.082414 -0.008469 -0.082211 -0.391394 0.915132 0.096663 0.963945 0.892963
+-0.100711 -0.017457 -0.071205 -0.391394 0.915132 0.096663 0.970203 0.982094
+-0.080407 -0.011017 -0.049962 -0.391394 0.915132 0.096663 0.928039 0.969549
+0.029309 -0.029773 -0.040991 0.894507 0.071045 -0.441373 0.694998 0.593245
+0.050502 -0.009120 0.005284 0.894507 0.071045 -0.441373 0.573141 0.624110
+0.053625 -0.028163 0.008548 0.894507 0.071045 -0.441373 0.563770 0.587680
+0.086777 -0.007718 0.051774 0.681626 0.542006 -0.491543 0.435511 0.601870
+0.063030 -0.001161 0.026074 0.681626 0.542006 -0.491543 0.516921 0.625450
+0.073622 0.006154 0.048828 0.681626 0.542006 -0.491543 0.455920 0.622257
+0.063030 -0.001161 0.026074 0.451598 0.766511 -0.456639 0.516921 0.625450
+0.062588 0.018440 0.058539 0.451598 0.766511 -0.456639 0.462180 0.663670
+0.073622 0.006154 0.048828 0.451598 0.766511 -0.456639 0.455920 0.622257
+0.063030 -0.001161 0.026074 0.405018 0.785142 -0.468522 0.516921 0.625450
+0.045717 0.011490 0.032308 0.405018 0.785142 -0.468522 0.534848 0.690702
+0.062588 0.018440 0.058539 0.405018 0.785142 -0.468522 0.462180 0.663670
+0.062588 0.018440 0.058539 -0.173223 0.959148 -0.223672 0.462180 0.663670
+0.028037 0.011583 0.055893 -0.173223 0.959148 -0.223672 0.537360 0.785940
+0.046779 0.017207 0.065495 -0.173223 0.959148 -0.223672 0.484460 0.711190
+0.063030 -0.001161 0.026074 0.844207 0.061935 -0.532427 0.516921 0.625450
+0.068895 -0.018779 0.033324 0.844207 0.061935 -0.532427 0.497300 0.593550
+0.050502 -0.009120 0.005284 0.844207 0.061935 -0.532427 0.573141 0.624110
+0.068895 -0.018779 0.033324 0.842489 0.046169 -0.536731 0.497300 0.593550
+0.053625 -0.028163 0.008548 0.842489 0.046169 -0.536731 0.563770 0.587680
+0.050502 -0.009120 0.005284 0.842489 0.046169 -0.536731 0.573141 0.624110
+0.068895 -0.018779 0.033324 0.728993 -0.038560 -0.683434 0.497300 0.593550
+0.063030 -0.001161 0.026074 0.728993 -0.038560 -0.683434 0.516921 0.625450
+0.086777 -0.007718 0.051774 0.728993 -0.038560 -0.683434 0.435511 0.601870
+0.046779 0.017207 0.065495 -0.289907 0.957040 0.005316 0.484460 0.711190
+0.028037 0.011583 0.055893 -0.289907 0.957040 0.005316 0.537360 0.785940
+0.034158 0.013303 0.080050 -0.289907 0.957040 0.005316 0.491340 0.772430
+0.028037 0.011583 0.055893 -0.745791 0.650730 0.142639 0.537360 0.785940
+0.016955 -0.004583 0.071701 -0.745791 0.650730 0.142639 0.536530 0.818012
+0.034158 0.013303 0.080050 -0.745791 0.650730 0.142639 0.491340 0.772430
+0.030087 0.039882 0.109675 -0.467734 0.624982 -0.624997 0.442490 0.810560
+0.052997 0.025909 0.078557 -0.467734 0.624982 -0.624997 0.450460 0.714650
+0.034158 0.013303 0.080050 -0.467734 0.624982 -0.624997 0.491340 0.772430
+0.052997 0.025909 0.078557 -0.335140 0.742402 -0.580103 0.450460 0.714650
+0.030087 0.039882 0.109675 -0.335140 0.742402 -0.580103 0.442490 0.810560
+0.037569 0.053938 0.123341 -0.335140 0.742402 -0.580103 0.410010 0.816830
+0.037569 0.053938 0.123341 0.007768 0.894407 -0.447187 0.410010 0.816830
+0.042691 0.068252 0.152059 0.007768 0.894407 -0.447187 0.349380 0.855820
+0.047565 0.058833 0.133305 0.007768 0.894407 -0.447187 0.373890 0.809190
+0.047565 0.058833 0.133305 0.498963 0.819491 -0.281905 0.373890 0.809190
+0.042691 0.068252 0.152059 0.498963 0.819491 -0.281905 0.349380 0.855820
+0.062351 0.052413 0.140813 0.498963 0.819491 -0.281905 0.331610 0.786970
+0.058327 0.063550 0.158191 0.977882 0.175297 0.114093 0.307721 0.837801
+0.057856 0.057044 0.172224 0.977882 0.175297 0.114093 0.297906 0.831523
+0.062351 0.052413 0.140813 0.977882 0.175297 0.114093 0.331610 0.786970
+0.042691 0.068252 0.152059 0.409099 0.808356 -0.423319 0.349380 0.855820
+0.058327 0.063550 0.158191 0.409099 0.808356 -0.423319 0.307721 0.837801
+0.062351 0.052413 0.140813 0.409099 0.808356 -0.423319 0.331610 0.786970
+0.042691 0.068252 0.152059 0.405169 0.177834 -0.896780 0.319714 0.849451
+0.056886 0.077181 0.160243 0.405169 0.177834 -0.896780 0.302102 0.842047
+0.058327 0.063550 0.158191 0.405169 0.177834 -0.896780 0.307721 0.837801
+0.058327 0.063550 0.158191 0.782993 -0.573945 -0.239813 0.286889 0.843306
+0.072808 0.072316 0.184492 0.782993 -0.573945 -0.239813 0.257003 0.877169
+0.057856 0.057044 0.172224 0.782993 -0.573945 -0.239813 0.297906 0.831523
+0.058327 0.063550 0.158191 0.839232 0.166641 -0.517611 0.286889 0.843306
+0.056886 0.077181 0.160243 0.839232 0.166641 -0.517611 0.305477 0.856494
+0.072808 0.072316 0.184492 0.839232 0.166641 -0.517611 0.257003 0.877169
+0.056886 0.077181 0.160243 0.808889 0.370093 -0.456869 0.305477 0.856494
+0.063223 0.090410 0.182179 0.808889 0.370093 -0.456869 0.270710 0.887400
+0.072808 0.072316 0.184492 0.808889 0.370093 -0.456869 0.257003 0.877169
+0.056886 0.077181 0.160243 -0.049432 0.861534 -0.505288 0.305477 0.856494
+0.053192 0.085858 0.175399 -0.049432 0.861534 -0.505288 0.298639 0.885778
+0.063223 0.090410 0.182179 -0.049432 0.861534 -0.505288 0.270710 0.887400
+0.042691 0.068252 0.152059 -0.215916 0.823748 -0.524232 0.336759 0.863565
+0.053192 0.085858 0.175399 -0.215916 0.823748 -0.524232 0.298639 0.885778
+0.056886 0.077181 0.160243 -0.215916 0.823748 -0.524232 0.305477 0.856494
+0.042691 0.068252 0.152059 -0.743626 0.641830 -0.187280 0.349380 0.855820
+0.037569 0.053938 0.123341 -0.743626 0.641830 -0.187280 0.410010 0.816830
+0.027156 0.044410 0.132034 -0.743626 0.641830 -0.187280 0.415793 0.851809
+0.053360 0.088072 0.187479 0.179671 0.731949 0.657243 0.282632 0.904939
+0.060302 0.081476 0.192927 0.179671 0.731949 0.657243 0.268112 0.906565
+0.063223 0.090410 0.182179 0.179671 0.731949 0.657243 0.270710 0.887400
+0.063223 0.090410 0.182179 0.701219 0.442880 0.558703 0.270710 0.887400
+0.060302 0.081476 0.192927 0.701219 0.442880 0.558703 0.268112 0.906565
+0.072808 0.072316 0.184492 0.701219 0.442880 0.558703 0.257003 0.877169
+0.047565 0.058833 0.133305 0.347701 0.930974 0.111313 0.373890 0.809190
+0.062351 0.052413 0.140813 0.347701 0.930974 0.111313 0.329190 0.784830
+0.068440 0.051968 0.125515 0.347701 0.930974 0.111313 0.344030 0.750640
+0.062351 0.052413 0.140813 0.438217 0.886495 0.148635 0.329190 0.784830
+0.084341 0.041774 0.139434 0.438217 0.886495 0.148635 0.288930 0.734470
+0.068440 0.051968 0.125515 0.438217 0.886495 0.148635 0.344030 0.750640
+0.062351 0.052413 0.140813 0.421293 0.820772 0.385805 0.329190 0.784830
+0.061518 0.042216 0.163416 0.421293 0.820772 0.385805 0.295840 0.820350
+0.084341 0.041774 0.139434 0.421293 0.820772 0.385805 0.288930 0.734470
+0.061518 0.042216 0.163416 -0.485820 0.873986 0.011261 0.295840 0.820350
+0.071425 0.047528 0.178549 -0.485820 0.873986 0.011261 0.250100 0.818029
+0.075682 0.050078 0.164293 -0.485820 0.873986 0.011261 0.266450 0.791830
+0.075682 0.050078 0.164293 -0.421508 0.805782 -0.415988 0.273470 0.776000
+0.084341 0.041774 0.139434 -0.421508 0.805782 -0.415988 0.288930 0.734470
+0.061518 0.042216 0.163416 -0.421508 0.805782 -0.415988 0.295840 0.820350
+0.084341 0.041774 0.139434 0.594443 0.798600 -0.094210 0.288930 0.734470
+0.071762 0.046670 0.101566 0.594443 0.798600 -0.094210 0.374767 0.709924
+0.068440 0.051968 0.125515 0.594443 0.798600 -0.094210 0.344030 0.750640
+0.084341 0.041774 0.139434 0.428511 0.903175 -0.025570 0.288930 0.734470
+0.092476 0.037228 0.115191 0.428511 0.903175 -0.025570 0.311310 0.679780
+0.071762 0.046670 0.101566 0.428511 0.903175 -0.025570 0.374767 0.709924
+0.092476 0.037228 0.115191 0.585187 0.704593 -0.401380 0.311310 0.679780
+0.081658 0.033262 0.092457 0.585187 0.704593 -0.401380 0.369880 0.669890
+0.071762 0.046670 0.101566 0.585187 0.704593 -0.401380 0.374767 0.709924
+0.081658 0.033262 0.092457 -0.056815 0.926175 -0.372790 0.369880 0.669890
+0.062588 0.018440 0.058539 -0.056815 0.926175 -0.372790 0.462180 0.663670
+0.052997 0.025909 0.078557 -0.056815 0.926175 -0.372790 0.450460 0.714650
+0.111719 0.044626 0.093231 -0.336433 0.907171 -0.252691 0.306530 0.603540
+0.101997 0.035315 0.072748 -0.336433 0.907171 -0.252691 0.357290 0.597774
+0.081658 0.033262 0.092457 -0.336433 0.907171 -0.252691 0.369880 0.669890
+0.081658 0.033262 0.092457 -0.313049 0.922211 -0.226993 0.369880 0.669890
+0.101997 0.035315 0.072748 -0.313049 0.922211 -0.226993 0.357290 0.597774
+0.062588 0.018440 0.058539 -0.313049 0.922211 -0.226993 0.462180 0.663670
+0.062588 0.018440 0.058539 -0.070025 0.733132 -0.676471 0.462180 0.663670
+0.101997 0.035315 0.072748 -0.070025 0.733132 -0.676471 0.357290 0.597774
+0.082305 0.017903 0.055916 -0.070025 0.733132 -0.676471 0.424935 0.616345
+0.101997 0.035315 0.072748 0.329360 0.436749 -0.837121 0.357290 0.597774
+0.099099 0.007341 0.057013 0.329360 0.436749 -0.837121 0.392743 0.582936
+0.082305 0.017903 0.055916 0.329360 0.436749 -0.837121 0.424935 0.616345
+0.111719 0.044626 0.093231 0.077988 0.893121 -0.443004 0.306530 0.603540
+0.121015 0.040796 0.087146 0.077988 0.893121 -0.443004 0.297580 0.574370
+0.101997 0.035315 0.072748 0.077988 0.893121 -0.443004 0.357290 0.597774
+0.121015 0.040796 0.087146 0.425958 0.501006 -0.753361 0.297580 0.574370
+0.123985 0.028751 0.080815 0.425958 0.501006 -0.753361 0.301840 0.556970
+0.101997 0.035315 0.072748 0.425958 0.501006 -0.753361 0.357290 0.597774
+0.123985 0.028751 0.080815 0.419674 0.411575 -0.809000 0.301840 0.556970
+0.099099 0.007341 0.057013 0.419674 0.411575 -0.809000 0.392743 0.582936
+0.101997 0.035315 0.072748 0.419674 0.411575 -0.809000 0.357290 0.597774
+0.123985 0.028751 0.080815 0.618935 0.139825 -0.772896 0.301840 0.556970
+0.117596 0.011385 0.072557 0.618935 0.139825 -0.772896 0.329168 0.559598
+0.099099 0.007341 0.057013 0.618935 0.139825 -0.772896 0.392743 0.582936
+0.117596 0.011385 0.072557 0.690001 0.087795 -0.718464 0.329168 0.559598
+0.123985 0.028751 0.080815 0.690001 0.087795 -0.718464 0.301840 0.556970
+0.135593 0.023447 0.091315 0.690001 0.087795 -0.718464 0.261680 0.547960
+0.123985 0.028751 0.080815 0.711784 0.322511 -0.623980 0.301840 0.556970
+0.138591 0.044894 0.105820 0.711784 0.322511 -0.623980 0.231540 0.563650
+0.135593 0.023447 0.091315 0.711784 0.322511 -0.623980 0.261680 0.547960
+0.123985 0.028751 0.080815 0.580459 0.486386 -0.653066 0.301840 0.556970
+0.121015 0.040796 0.087146 0.580459 0.486386 -0.653066 0.297580 0.574370
+0.138591 0.044894 0.105820 0.580459 0.486386 -0.653066 0.231540 0.563650
+0.111719 0.044626 0.093231 0.183978 0.933850 -0.306719 0.306530 0.603540
+0.127937 0.047717 0.112370 0.183978 0.933850 -0.306719 0.243160 0.595480
+0.121015 0.040796 0.087146 0.183978 0.933850 -0.306719 0.297580 0.574370
+0.092476 0.037228 0.115191 -0.353719 0.935337 0.005146 0.311310 0.679780
+0.111719 0.044626 0.093231 -0.353719 0.935337 0.005146 0.306530 0.603540
+0.081658 0.033262 0.092457 -0.353719 0.935337 0.005146 0.369880 0.669890
+0.037569 0.053938 0.123341 -0.056134 0.917262 -0.394308 0.410010 0.816830
+0.047565 0.058833 0.133305 -0.056134 0.917262 -0.394308 0.373890 0.809190
+0.071762 0.046670 0.101566 -0.056134 0.917262 -0.394308 0.374767 0.709924
+0.047565 0.058833 0.133305 0.247467 0.952698 -0.176429 0.373890 0.809190
+0.068440 0.051968 0.125515 0.247467 0.952698 -0.176429 0.344030 0.750640
+0.071762 0.046670 0.101566 0.247467 0.952698 -0.176429 0.374767 0.709924
+0.052997 0.025909 0.078557 -0.550643 0.791501 -0.265177 0.450460 0.714650
+0.046779 0.017207 0.065495 -0.550643 0.791501 -0.265177 0.484460 0.711190
+0.034158 0.013303 0.080050 -0.550643 0.791501 -0.265177 0.491340 0.772430
+0.052997 0.025909 0.078557 -0.262672 0.856016 -0.445242 0.450460 0.714650
+0.062588 0.018440 0.058539 -0.262672 0.856016 -0.445242 0.462180 0.663670
+0.046779 0.017207 0.065495 -0.262672 0.856016 -0.445242 0.484460 0.711190
+0.062588 0.018440 0.058539 -0.093763 0.564177 -0.820313 0.462180 0.663670
+0.082305 0.017903 0.055916 -0.093763 0.564177 -0.820313 0.424935 0.616345
+0.073622 0.006154 0.048828 -0.093763 0.564177 -0.820313 0.455920 0.622257
+0.082305 0.017903 0.055916 0.424912 0.216272 -0.879020 0.424935 0.616345
+0.086777 -0.007718 0.051774 0.424912 0.216272 -0.879020 0.435511 0.601870
+0.073622 0.006154 0.048828 0.424912 0.216272 -0.879020 0.455920 0.622257
+0.082305 0.017903 0.055916 0.181094 0.187678 -0.965392 0.424935 0.616345
+0.099099 0.007341 0.057013 0.181094 0.187678 -0.965392 0.392743 0.582936
+0.086777 -0.007718 0.051774 0.181094 0.187678 -0.965392 0.435511 0.601870
+0.105390 0.048647 0.164927 0.052283 0.952515 -0.299970 0.259600 0.732880
+0.084341 0.041774 0.139434 0.052283 0.952515 -0.299970 0.288930 0.734470
+0.075682 0.050078 0.164293 0.052283 0.952515 -0.299970 0.266450 0.791830
+0.037569 0.053938 0.123341 -0.753064 0.618642 -0.224001 0.410010 0.816830
+0.030087 0.039882 0.109675 -0.753064 0.618642 -0.224001 0.442490 0.810560
+0.027156 0.044410 0.132034 -0.753064 0.618642 -0.224001 0.415793 0.851809
+0.037569 0.053938 0.123341 -0.190908 0.801157 -0.567188 0.410010 0.816830
+0.071762 0.046670 0.101566 -0.190908 0.801157 -0.567188 0.374767 0.709924
+0.052997 0.025909 0.078557 -0.190908 0.801157 -0.567188 0.450460 0.714650
+0.062351 0.052413 0.140813 -0.621950 0.722121 0.302853 0.318930 0.812548
+0.044423 0.031511 0.153834 -0.621950 0.722121 0.302853 0.301720 0.824680
+0.061518 0.042216 0.163416 -0.621950 0.722121 0.302853 0.316160 0.818176
+0.135593 0.023447 0.091315 0.689132 0.337391 -0.641299 0.261680 0.547960
+0.138591 0.044894 0.105820 0.689132 0.337391 -0.641299 0.231540 0.563650
+0.153810 0.036576 0.117798 0.689132 0.337391 -0.641299 0.204756 0.548310
+0.121015 0.040796 0.087146 0.078730 0.955641 -0.283816 0.297580 0.574370
+0.127937 0.047717 0.112370 0.078730 0.955641 -0.283816 0.243160 0.595480
+0.138591 0.044894 0.105820 0.078730 0.955641 -0.283816 0.231540 0.563650
+0.127937 0.047717 0.112370 -0.079966 0.992494 -0.092529 0.243160 0.595480
+0.111719 0.044626 0.093231 -0.079966 0.992494 -0.092529 0.306530 0.603540
+0.124840 0.048888 0.127607 -0.079966 0.992494 -0.092529 0.225220 0.625490
+0.111719 0.044626 0.093231 -0.343962 0.938866 0.014885 0.306530 0.603540
+0.092476 0.037228 0.115191 -0.343962 0.938866 0.014885 0.262670 0.665250
+0.124840 0.048888 0.127607 -0.343962 0.938866 0.014885 0.225220 0.625490
+0.124840 0.048888 0.127607 -0.287104 0.947413 -0.141349 0.225220 0.625490
+0.092476 0.037228 0.115191 -0.287104 0.947413 -0.141349 0.262670 0.665250
+0.117201 0.050385 0.153157 -0.287104 0.947413 -0.141349 0.200900 0.681330
+0.153810 0.036576 0.117798 0.682750 0.273587 -0.677498 0.204756 0.548310
+0.138591 0.044894 0.105820 0.682750 0.273587 -0.677498 0.231540 0.563650
+0.161981 0.058897 0.135046 0.682750 0.273587 -0.677498 0.205510 0.554570
+0.138591 0.044894 0.105820 -0.026246 0.901799 -0.431359 0.231540 0.563650
+0.127937 0.047717 0.112370 -0.026246 0.901799 -0.431359 0.243160 0.595480
+0.143106 0.052573 0.121599 -0.026246 0.901799 -0.431359 0.196590 0.574980
+0.127937 0.047717 0.112370 -0.234628 0.964423 -0.121808 0.243160 0.595480
+0.124840 0.048888 0.127607 -0.234628 0.964423 -0.121808 0.225220 0.625490
+0.143106 0.052573 0.121599 -0.234628 0.964423 -0.121808 0.196590 0.574980
+0.124840 0.048888 0.127607 0.072311 0.996704 -0.036778 0.225220 0.625490
+0.117201 0.050385 0.153157 0.072311 0.996704 -0.036778 0.200900 0.681330
+0.130922 0.049236 0.148996 0.072311 0.996704 -0.036778 0.179280 0.644260
+0.161981 0.058897 0.135046 0.013148 0.897611 -0.440593 0.205510 0.554570
+0.138591 0.044894 0.105820 0.013148 0.897611 -0.440593 0.231540 0.563650
+0.143106 0.052573 0.121599 0.013148 0.897611 -0.440593 0.196590 0.574980
+0.124840 0.048888 0.127607 0.163404 0.984578 -0.062483 0.225220 0.625490
+0.130922 0.049236 0.148996 0.163404 0.984578 -0.062483 0.179280 0.644260
+0.136199 0.047431 0.134354 0.163404 0.984578 -0.062483 0.190281 0.610355
+0.143992 0.050780 0.185383 -0.806753 0.330047 0.490120 0.151250 0.671000
+0.142136 0.060030 0.176099 -0.806753 0.330047 0.490120 0.111570 0.658070
+0.124638 0.045934 0.156789 -0.806753 0.330047 0.490120 0.178405 0.669185
+0.124638 0.045934 0.156789 -0.555997 0.825311 -0.098641 0.178405 0.669185
+0.142136 0.060030 0.176099 -0.555997 0.825311 -0.098641 0.111570 0.658070
+0.130922 0.049236 0.148996 -0.555997 0.825311 -0.098641 0.179280 0.644260
+0.142136 0.060030 0.176099 -0.445050 0.879929 -0.166298 0.111570 0.658070
+0.158775 0.066912 0.167984 -0.445050 0.879929 -0.166298 0.089401 0.614255
+0.130922 0.049236 0.148996 -0.445050 0.879929 -0.166298 0.179280 0.644260
+0.158775 0.066912 0.167984 -0.538539 0.565618 -0.624542 0.089401 0.614255
+0.142136 0.060030 0.176099 -0.538539 0.565618 -0.624542 0.111570 0.658070
+0.159528 0.087068 0.185589 -0.538539 0.565618 -0.624542 0.063021 0.631922
+0.142136 0.060030 0.176099 -0.850902 0.455459 0.261769 0.111570 0.658070
+0.149787 0.066389 0.189905 -0.850902 0.455459 0.261769 0.077689 0.659309
+0.159528 0.087068 0.185589 -0.850902 0.455459 0.261769 0.063021 0.631922
+0.149787 0.066389 0.189905 -0.903421 0.428521 0.014169 0.077689 0.659309
+0.159469 0.086296 0.205175 -0.903421 0.428521 0.014169 0.032164 0.653374
+0.159528 0.087068 0.185589 -0.903421 0.428521 0.014169 0.063021 0.631922
+0.169176 0.083665 0.181754 0.897506 0.249696 -0.363503 0.047225 0.608980
+0.176435 0.092959 0.206061 0.897506 0.249696 -0.363503 0.007966 0.628355
+0.173943 0.072128 0.185599 0.897506 0.249696 -0.363503 0.042490 0.600000
+0.169176 0.083665 0.181754 0.953574 0.036913 -0.298888 0.047225 0.608980
+0.172181 0.094044 0.192623 0.953574 0.036913 -0.298888 0.024866 0.619390
+0.176435 0.092959 0.206061 0.953574 0.036913 -0.298888 0.007966 0.628355
+0.159528 0.087068 0.185589 -0.017556 0.725530 -0.687967 0.058333 0.632730
+0.172181 0.094044 0.192623 -0.017556 0.725530 -0.687967 0.024866 0.619390
+0.169176 0.083665 0.181754 -0.017556 0.725530 -0.687967 0.047225 0.608980
+0.172181 0.094044 0.192623 0.283248 0.958969 -0.012238 0.024866 0.619390
+0.165030 0.096221 0.197703 0.283248 0.958969 -0.012238 0.028538 0.636841
+0.176435 0.092959 0.206061 0.283248 0.958969 -0.012238 0.007966 0.628355
+0.159528 0.087068 0.185589 -0.144013 0.819906 -0.554089 0.058333 0.632730
+0.165030 0.096221 0.197703 -0.144013 0.819906 -0.554089 0.028538 0.636841
+0.172181 0.094044 0.192623 -0.144013 0.819906 -0.554089 0.024866 0.619390
+0.165030 0.096221 0.197703 -0.866743 0.498464 0.017036 0.028538 0.636841
+0.159528 0.087068 0.185589 -0.866743 0.498464 0.017036 0.063021 0.631922
+0.159469 0.086296 0.205175 -0.866743 0.498464 0.017036 0.032164 0.653374
+0.159469 0.086296 0.205175 -0.300410 0.675374 0.673516 0.032164 0.653374
+0.176435 0.092959 0.206061 -0.300410 0.675374 0.673516 0.007966 0.628355
+0.165030 0.096221 0.197703 -0.300410 0.675374 0.673516 0.028538 0.636841
+0.130922 0.049236 0.148996 0.229247 0.816022 0.530616 0.179280 0.644260
+0.117201 0.050385 0.153157 0.229247 0.816022 0.530616 0.200900 0.681330
+0.124638 0.045934 0.156789 0.229247 0.816022 0.530616 0.181247 0.669140
+0.124638 0.045934 0.156789 -0.142384 0.472025 0.870011 0.178405 0.669185
+0.117201 0.050385 0.153157 -0.142384 0.472025 0.870011 0.200900 0.681330
+0.119302 0.038419 0.159993 -0.142384 0.472025 0.870011 0.182900 0.687000
+0.117201 0.050385 0.153157 0.791086 0.402199 0.460890 0.200900 0.681330
+0.113399 0.048360 0.161450 0.791086 0.402199 0.460890 0.194766 0.703056
+0.119302 0.038419 0.159993 0.791086 0.402199 0.460890 0.182900 0.687000
+0.105390 0.048647 0.164927 0.052436 0.876901 -0.477802 0.205070 0.725670
+0.075682 0.050078 0.164293 0.052436 0.876901 -0.477802 0.266450 0.791830
+0.088707 0.052685 0.170507 0.052436 0.876901 -0.477802 0.230999 0.772807
+0.071425 0.047528 0.178549 -0.241205 0.965243 0.100629 0.250100 0.818029
+0.088707 0.052685 0.170507 -0.241205 0.965243 0.100629 0.230999 0.772807
+0.075682 0.050078 0.164293 -0.241205 0.965243 0.100629 0.266450 0.791830
+0.105390 0.048647 0.164927 -0.221711 0.971910 -0.078968 0.205070 0.725670
+0.117201 0.050385 0.153157 -0.221711 0.971910 -0.078968 0.200900 0.681330
+0.084341 0.041774 0.139434 -0.221711 0.971910 -0.078968 0.260430 0.698770
+0.117201 0.050385 0.153157 -0.154762 0.960323 -0.232010 0.200900 0.681330
+0.092476 0.037228 0.115191 -0.154762 0.960323 -0.232010 0.262670 0.665250
+0.084341 0.041774 0.139434 -0.154762 0.960323 -0.232010 0.260430 0.698770
+0.088707 0.052685 0.170507 0.637007 0.574152 -0.514366 0.230999 0.772807
+0.098148 0.065280 0.196258 0.637007 0.574152 -0.514366 0.169760 0.791210
+0.108636 0.055260 0.198062 0.637007 0.574152 -0.514366 0.148075 0.768006
+0.088707 0.052685 0.170507 -0.532034 0.821139 -0.206567 0.230999 0.772807
+0.086585 0.059950 0.204852 -0.532034 0.821139 -0.206567 0.176753 0.823046
+0.098148 0.065280 0.196258 -0.532034 0.821139 -0.206567 0.169760 0.791210
+0.098148 0.065280 0.196258 0.484165 0.363483 -0.795905 0.169760 0.791210
+0.119937 0.077291 0.214998 0.484165 0.363483 -0.795905 0.094384 0.772592
+0.108636 0.055260 0.198062 0.484165 0.363483 -0.795905 0.148075 0.768006
+0.086585 0.059950 0.204852 -0.569170 0.770049 -0.288219 0.172969 0.822403
+0.104757 0.078397 0.218252 -0.569170 0.770049 -0.288219 0.119475 0.806425
+0.098148 0.065280 0.196258 -0.569170 0.770049 -0.288219 0.167530 0.791000
+0.104757 0.078397 0.218252 -0.044623 0.863844 -0.501779 0.119475 0.806425
+0.119937 0.077291 0.214998 -0.044623 0.863844 -0.501779 0.094384 0.772592
+0.098148 0.065280 0.196258 -0.044623 0.863844 -0.501779 0.167530 0.791000
+0.118628 0.093343 0.233243 0.366299 0.919435 0.143053 0.068000 0.797370
+0.128282 0.088243 0.241302 0.366299 0.919435 0.143053 0.043946 0.784224
+0.128568 0.090583 0.225530 0.366299 0.919435 0.143053 0.062010 0.771769
+0.128568 0.090583 0.225530 0.964069 0.259680 0.056009 0.062010 0.771769
+0.128282 0.088243 0.241302 0.964069 0.259680 0.056009 0.043946 0.784224
+0.131489 0.078973 0.229080 0.964069 0.259680 0.056009 0.057647 0.763215
+0.128282 0.088243 0.241302 -0.172738 0.725585 0.666099 0.043946 0.784224
+0.118628 0.093343 0.233243 -0.172738 0.725585 0.666099 0.068000 0.797370
+0.113825 0.086887 0.239030 -0.172738 0.725585 0.666099 0.071740 0.814150
+0.086585 0.059950 0.204852 -0.369460 0.904245 -0.214102 0.176753 0.823046
+0.088707 0.052685 0.170507 -0.369460 0.904245 -0.214102 0.230999 0.772807
+0.071425 0.047528 0.178549 -0.369460 0.904245 -0.214102 0.250100 0.818029
+0.104757 0.078397 0.218252 -0.767031 0.637287 0.074351 0.113396 0.806480
+0.113825 0.086887 0.239030 -0.767031 0.637287 0.074351 0.071740 0.814150
+0.118628 0.093343 0.233243 -0.767031 0.637287 0.074351 0.068000 0.797370
+0.104757 0.078397 0.218252 -0.724534 0.688353 0.034938 0.113396 0.806480
+0.086585 0.059950 0.204852 -0.724534 0.688353 0.034938 0.172969 0.822403
+0.113825 0.086887 0.239030 -0.724534 0.688353 0.034938 0.071740 0.814150
+0.108636 0.055260 0.198062 0.763522 0.098921 -0.638161 0.148075 0.768006
+0.119937 0.077291 0.214998 0.763522 0.098921 -0.638161 0.094384 0.772592
+0.131489 0.078973 0.229080 0.763522 0.098921 -0.638161 0.057647 0.763215
+0.128568 0.090583 0.225530 0.773107 0.000565 -0.634276 0.062010 0.771769
+0.131489 0.078973 0.229080 0.773107 0.000565 -0.634276 0.057647 0.763215
+0.119937 0.077291 0.214998 0.773107 0.000565 -0.634276 0.094384 0.772592
+0.117201 0.050385 0.153157 0.166062 0.937727 0.305108 0.200900 0.681330
+0.105390 0.048647 0.164927 0.166062 0.937727 0.305108 0.205070 0.725670
+0.113399 0.048360 0.161450 0.166062 0.937727 0.305108 0.194766 0.703056
+0.105390 0.048647 0.164927 -0.154431 0.889921 -0.429176 0.205070 0.725670
+0.126783 0.063450 0.187924 -0.154431 0.889921 -0.429176 0.123640 0.711560
+0.113399 0.048360 0.161450 -0.154431 0.889921 -0.429176 0.194766 0.703056
+0.097660 0.038978 0.172552 -0.748673 0.658556 0.076109 0.207600 0.751920
+0.114482 0.056148 0.189459 -0.748673 0.658556 0.076109 0.147190 0.741800
+0.105390 0.048647 0.164927 -0.748673 0.658556 0.076109 0.205070 0.725670
+0.105390 0.048647 0.164927 -0.515642 0.853940 -0.069998 0.205070 0.725670
+0.114482 0.056148 0.189459 -0.515642 0.853940 -0.069998 0.147190 0.741800
+0.126783 0.063450 0.187924 -0.515642 0.853940 -0.069998 0.123640 0.711560
+0.097660 0.038978 0.172552 -0.819569 0.385936 0.423509 0.207600 0.751920
+0.107971 0.041337 0.190356 -0.819569 0.385936 0.423509 0.161290 0.754120
+0.114482 0.056148 0.189459 -0.819569 0.385936 0.423509 0.147190 0.741800
+0.107971 0.041337 0.190356 -0.850928 0.395041 0.346213 0.161290 0.754120
+0.123289 0.059764 0.206979 -0.850928 0.395041 0.346213 0.100160 0.745330
+0.114482 0.056148 0.189459 -0.850928 0.395041 0.346213 0.147190 0.741800
+0.114482 0.056148 0.189459 -0.843830 0.416628 0.338189 0.147190 0.741800
+0.123289 0.059764 0.206979 -0.843830 0.416628 0.338189 0.100160 0.745330
+0.127109 0.070575 0.203192 -0.843830 0.416628 0.338189 0.099430 0.733770
+0.114482 0.056148 0.189459 -0.509573 0.783824 -0.354901 0.147190 0.741800
+0.127109 0.070575 0.203192 -0.509573 0.783824 -0.354901 0.099430 0.733770
+0.126783 0.063450 0.187924 -0.509573 0.783824 -0.354901 0.123640 0.711560
+0.142189 0.063498 0.190741 0.131333 0.671051 -0.729686 0.092348 0.685197
+0.135284 0.055424 0.182073 0.131333 0.671051 -0.729686 0.119930 0.686806
+0.126783 0.063450 0.187924 0.131333 0.671051 -0.729686 0.123640 0.711560
+0.126783 0.063450 0.187924 0.307594 0.751384 -0.583788 0.123640 0.711560
+0.135284 0.055424 0.182073 0.307594 0.751384 -0.583788 0.119930 0.686806
+0.113399 0.048360 0.161450 0.307594 0.751384 -0.583788 0.194766 0.703056
+0.135284 0.055424 0.182073 0.542714 0.428171 -0.722586 0.119930 0.686806
+0.119302 0.038419 0.159993 0.542714 0.428171 -0.722586 0.182900 0.687000
+0.113399 0.048360 0.161450 0.542714 0.428171 -0.722586 0.194766 0.703056
+0.127109 0.070575 0.203192 -0.875533 0.402820 0.266792 0.099430 0.733770
+0.123289 0.059764 0.206979 -0.875533 0.402820 0.266792 0.100160 0.745330
+0.148253 0.104663 0.221112 -0.875533 0.402820 0.266792 0.025313 0.709884
+0.123289 0.059764 0.206979 -0.877269 0.408353 0.252284 0.100160 0.745330
+0.144382 0.092417 0.227473 -0.877269 0.408353 0.252284 0.026020 0.727900
+0.148253 0.104663 0.221112 -0.877269 0.408353 0.252284 0.025313 0.709884
+0.148253 0.104663 0.221112 -0.750055 0.605322 -0.266465 0.025313 0.709884
+0.126783 0.063450 0.187924 -0.750055 0.605322 -0.266465 0.123640 0.711560
+0.127109 0.070575 0.203192 -0.750055 0.605322 -0.266465 0.099430 0.733770
+0.160456 0.093784 0.213636 0.150116 0.536996 -0.830121 0.020110 0.678290
+0.142189 0.063498 0.190741 0.150116 0.536996 -0.830121 0.092348 0.685197
+0.126783 0.063450 0.187924 0.150116 0.536996 -0.830121 0.123640 0.711560
+0.154832 0.073607 0.208689 0.839615 -0.103280 -0.533273 0.052320 0.675130
+0.142189 0.063498 0.190741 0.839615 -0.103280 -0.533273 0.092348 0.685197
+0.160456 0.093784 0.213636 0.839615 -0.103280 -0.533273 0.020110 0.678290
+0.126783 0.063450 0.187924 0.056925 0.608025 -0.791874 0.123640 0.711560
+0.148253 0.104663 0.221112 0.056925 0.608025 -0.791874 0.025313 0.709884
+0.160456 0.093784 0.213636 0.056925 0.608025 -0.791874 0.020110 0.678290
+0.144382 0.092417 0.227473 -0.238748 0.505996 0.828835 0.026020 0.727900
+0.156812 0.096983 0.228266 -0.238748 0.505996 0.828835 0.004768 0.704040
+0.148253 0.104663 0.221112 -0.238748 0.505996 0.828835 0.025313 0.709884
+0.160456 0.093784 0.213636 0.666468 0.745528 0.002985 0.020110 0.678290
+0.148253 0.104663 0.221112 0.666468 0.745528 0.002985 0.025313 0.709884
+0.156812 0.096983 0.228266 0.666468 0.745528 0.002985 0.004768 0.704040
+0.053625 -0.028163 0.008548 0.886028 -0.175324 -0.429205 0.563770 0.587680
+0.022281 -0.052876 -0.046062 0.886028 -0.175324 -0.429205 0.707390 0.565900
+0.029309 -0.029773 -0.040991 0.886028 -0.175324 -0.429205 0.694998 0.593245
+0.142987 0.029221 0.148360 0.933907 -0.344071 0.097126 0.162840 0.613160
+0.142930 0.026574 0.139531 0.933907 -0.344071 0.097126 0.168030 0.609880
+0.150595 0.051764 0.155065 0.933907 -0.344071 0.097126 0.128281 0.611740
+0.142930 0.026574 0.139531 0.205114 0.467758 -0.859728 0.168030 0.609880
+0.130922 0.049236 0.148996 0.205114 0.467758 -0.859728 0.177100 0.643890
+0.150595 0.051764 0.155065 0.205114 0.467758 -0.859728 0.128281 0.611740
+0.136199 0.047431 0.134354 -0.626252 0.779220 -0.024993 0.190281 0.610355
+0.151269 0.059879 0.144841 -0.626252 0.779220 -0.024993 0.146100 0.591670
+0.143106 0.052573 0.121599 -0.626252 0.779220 -0.024993 0.196590 0.574980
+0.142930 0.026574 0.139531 -0.722055 -0.061990 0.689053 0.168030 0.609880
+0.159537 0.060161 0.159955 -0.722055 -0.061990 0.689053 0.117383 0.597700
+0.136199 0.047431 0.134354 -0.722055 -0.061990 0.689053 0.190281 0.610355
+0.136199 0.047431 0.134354 -0.732416 0.557915 0.390253 0.190281 0.610355
+0.159537 0.060161 0.159955 -0.732416 0.557915 0.390253 0.117383 0.597700
+0.151269 0.059879 0.144841 -0.732416 0.557915 0.390253 0.146100 0.591670
+0.143106 0.052573 0.121599 -0.141877 0.957493 -0.251153 0.196590 0.574980
+0.151269 0.059879 0.144841 -0.141877 0.957493 -0.251153 0.146100 0.591670
+0.161981 0.058897 0.135046 -0.141877 0.957493 -0.251153 0.137991 0.550913
+0.130922 0.049236 0.148996 0.892027 0.356758 0.277508 0.177100 0.643890
+0.142930 0.026574 0.139531 0.892027 0.356758 0.277508 0.168030 0.609880
+0.136199 0.047431 0.134354 0.892027 0.356758 0.277508 0.170267 0.618432
+0.150595 0.051764 0.155065 0.815290 0.051619 -0.576748 0.128281 0.611740
+0.158775 0.066912 0.167984 0.815290 0.051619 -0.576748 0.089401 0.614255
+0.164352 0.057553 0.175030 0.815290 0.051619 -0.576748 0.075400 0.602490
+0.150595 0.051764 0.155065 0.169529 0.584917 -0.793178 0.128281 0.611740
+0.130922 0.049236 0.148996 0.169529 0.584917 -0.793178 0.177100 0.643890
+0.158775 0.066912 0.167984 0.169529 0.584917 -0.793178 0.089401 0.614255
+0.159537 0.060161 0.159955 -0.596094 -0.180948 0.782259 0.117383 0.597700
+0.142930 0.026574 0.139531 -0.596094 -0.180948 0.782259 0.168030 0.609880
+0.156379 0.044803 0.153996 -0.596094 -0.180948 0.782259 0.124200 0.603440
+0.164352 0.057553 0.175030 0.762134 -0.032642 -0.646596 0.075400 0.602490
+0.158775 0.066912 0.167984 0.762134 -0.032642 -0.646596 0.089401 0.614255
+0.173943 0.072128 0.185599 0.762134 -0.032642 -0.646596 0.042490 0.600000
+0.158775 0.066912 0.167984 0.742392 0.085284 -0.664515 0.089401 0.614255
+0.169176 0.083665 0.181754 0.742392 0.085284 -0.664515 0.055650 0.607740
+0.173943 0.072128 0.185599 0.742392 0.085284 -0.664515 0.042490 0.600000
+0.158775 0.066912 0.167984 -0.066216 0.657796 -0.750280 0.089401 0.614255
+0.159528 0.087068 0.185589 -0.066216 0.657796 -0.750280 0.063021 0.631922
+0.169176 0.083665 0.181754 -0.066216 0.657796 -0.750280 0.055650 0.607740
+0.136199 0.047431 0.134354 -0.078967 0.938671 0.335651 0.190281 0.610355
+0.143106 0.052573 0.121599 -0.078967 0.938671 0.335651 0.196590 0.574980
+0.124840 0.048888 0.127607 -0.078967 0.938671 0.335651 0.225220 0.625490
+0.156379 0.044803 0.153996 -0.639687 -0.160153 0.751766 0.116410 0.602830
+0.168610 0.067224 0.169180 -0.639687 -0.160153 0.751766 0.071290 0.596710
+0.159537 0.060161 0.159955 -0.639687 -0.160153 0.751766 0.117383 0.597700
+0.159537 0.060161 0.159955 -0.649562 -0.141322 0.747060 0.117383 0.597700
+0.168610 0.067224 0.169180 -0.649562 -0.141322 0.747060 0.071290 0.596710
+0.174312 0.080673 0.176682 -0.649562 -0.141322 0.747060 0.046580 0.584910
+0.159537 0.060161 0.159955 -0.854005 0.237795 0.462740 0.102401 0.594698
+0.174312 0.080673 0.176682 -0.854005 0.237795 0.462740 0.046580 0.584910
+0.151269 0.059879 0.144841 -0.854005 0.237795 0.462740 0.134250 0.589370
+0.151269 0.059879 0.144841 -0.855175 0.294363 0.426645 0.134250 0.589370
+0.174312 0.080673 0.176682 -0.855175 0.294363 0.426645 0.046580 0.584910
+0.172340 0.088145 0.167574 -0.855175 0.294363 0.426645 0.061920 0.574880
+0.161018 0.070392 0.147527 -0.684546 0.692765 -0.226877 0.119270 0.573250
+0.151269 0.059879 0.144841 -0.684546 0.692765 -0.226877 0.134250 0.589370
+0.172340 0.088145 0.167574 -0.684546 0.692765 -0.226877 0.061920 0.574880
+0.172340 0.088145 0.167574 -0.217964 0.788476 -0.575150 0.061920 0.574880
+0.177914 0.078618 0.152401 -0.217964 0.788476 -0.575150 0.076558 0.550459
+0.161018 0.070392 0.147527 -0.217964 0.788476 -0.575150 0.119270 0.573250
+0.161981 0.058897 0.135046 -0.155900 0.720547 -0.675653 0.137991 0.550913
+0.161018 0.070392 0.147527 -0.155900 0.720547 -0.675653 0.119270 0.573250
+0.177914 0.078618 0.152401 -0.155900 0.720547 -0.675653 0.076558 0.550459
+0.161981 0.058897 0.135046 -0.498954 0.617936 -0.607618 0.137991 0.550913
+0.151269 0.059879 0.144841 -0.498954 0.617936 -0.607618 0.134250 0.589370
+0.161018 0.070392 0.147527 -0.498954 0.617936 -0.607618 0.119270 0.573250
+0.177914 0.078618 0.152401 0.491377 0.807443 -0.326473 0.076558 0.550459
+0.172340 0.088145 0.167574 0.491377 0.807443 -0.326473 0.061920 0.574880
+0.184018 0.079932 0.164838 0.491377 0.807443 -0.326473 0.046470 0.549800
+0.104757 0.078397 0.218252 -0.087544 0.744781 -0.661541 0.119475 0.806425
+0.118628 0.093343 0.233243 -0.087544 0.744781 -0.661541 0.068000 0.797370
+0.119937 0.077291 0.214998 -0.087544 0.744781 -0.661541 0.094384 0.772592
+0.119937 0.077291 0.214998 -0.302290 0.704802 -0.641775 0.094384 0.772592
+0.118628 0.093343 0.233243 -0.302290 0.704802 -0.641775 0.068000 0.797370
+0.128568 0.090583 0.225530 -0.302290 0.704802 -0.641775 0.062010 0.771769
+0.086585 0.059950 0.204852 -0.739178 -0.127042 0.661420 0.187156 0.190790
+0.075763 0.037689 0.188482 -0.739178 -0.127042 0.661420 0.246170 0.197800
+0.093507 0.047680 0.210231 -0.739178 -0.127042 0.661420 0.175260 0.199050
+0.097660 0.038978 0.172552 -0.194770 0.310967 0.930250 0.210540 0.264840
+0.095701 0.027930 0.175835 -0.194770 0.310967 0.930250 0.220030 0.259690
+0.109910 0.024071 0.180100 -0.194770 0.310967 0.930250 0.186271 0.283171
+0.109910 0.024071 0.180100 -0.489258 -0.485360 0.724605 0.186271 0.283171
+0.123196 0.044287 0.202612 -0.489258 -0.485360 0.724605 0.122720 0.274200
+0.107971 0.041337 0.190356 -0.489258 -0.485360 0.724605 0.166593 0.267539
+0.123289 0.059764 0.206979 -0.586337 -0.216715 0.780540 0.112692 0.264495
+0.107971 0.041337 0.190356 -0.586337 -0.216715 0.780540 0.166593 0.267539
+0.123196 0.044287 0.202612 -0.586337 -0.216715 0.780540 0.122720 0.274200
+0.109910 0.024071 0.180100 0.273703 -0.790311 0.548174 0.186271 0.283171
+0.141667 0.052178 0.204766 0.273703 -0.790311 0.548174 0.098020 0.295780
+0.123196 0.044287 0.202612 0.273703 -0.790311 0.548174 0.122720 0.274200
+0.109910 0.024071 0.180100 -0.775908 -0.384301 0.500279 0.186271 0.283171
+0.107971 0.041337 0.190356 -0.775908 -0.384301 0.500279 0.166593 0.267539
+0.097660 0.038978 0.172552 -0.775908 -0.384301 0.500279 0.210540 0.264840
+0.123289 0.059764 0.206979 -0.540231 -0.168491 0.824476 0.112692 0.264495
+0.141494 0.076623 0.222353 -0.540231 -0.168491 0.824476 0.059790 0.277920
+0.144382 0.092417 0.227473 -0.540231 -0.168491 0.824476 0.036651 0.275151
+0.141494 0.076623 0.222353 0.056107 -0.317159 0.946711 0.053758 0.278100
+0.156812 0.096983 0.228266 0.056107 -0.317159 0.946711 0.033925 0.290432
+0.144382 0.092417 0.227473 0.056107 -0.317159 0.946711 0.036651 0.275151
+0.141667 0.052178 0.204766 0.417494 -0.528731 0.739015 0.043468 0.298260
+0.156812 0.096983 0.228266 0.417494 -0.528731 0.739015 0.033925 0.290432
+0.141494 0.076623 0.222353 0.417494 -0.528731 0.739015 0.053758 0.278100
+0.141667 0.052178 0.204766 0.668676 -0.509881 0.541197 0.043468 0.298260
+0.154832 0.073607 0.208689 0.668676 -0.509881 0.541197 0.049561 0.321938
+0.156812 0.096983 0.228266 0.668676 -0.509881 0.541197 0.033925 0.290432
+0.154832 0.073607 0.208689 0.899394 -0.322928 0.294630 0.049561 0.321938
+0.160456 0.093784 0.213636 0.899394 -0.322928 0.294630 0.025280 0.326291
+0.156812 0.096983 0.228266 0.899394 -0.322928 0.294630 0.010440 0.305130
+0.154832 0.073607 0.208689 0.829718 -0.449044 -0.331555 0.048226 0.322009
+0.141667 0.052178 0.204766 0.829718 -0.449044 -0.331555 0.081990 0.304590
+0.142189 0.063498 0.190741 0.829718 -0.449044 -0.331555 0.087940 0.327060
+0.184018 0.079932 0.164838 0.591821 0.792575 0.146878 0.044750 0.458050
+0.172340 0.088145 0.167574 0.591821 0.792575 0.146878 0.035060 0.447515
+0.182614 0.078900 0.176064 0.591821 0.792575 0.146878 0.036999 0.436733
+0.141494 0.076623 0.222353 0.152655 -0.576454 0.802743 0.059790 0.277920
+0.123196 0.044287 0.202612 0.152655 -0.576454 0.802743 0.092590 0.280640
+0.141667 0.052178 0.204766 0.152655 -0.576454 0.802743 0.081990 0.304590
+0.172340 0.088145 0.167574 0.210187 0.777679 0.592483 0.036470 0.432090
+0.174312 0.080673 0.176682 0.210187 0.777679 0.592483 0.043260 0.424780
+0.182614 0.078900 0.176064 0.210187 0.777679 0.592483 0.036999 0.436733
+0.168610 0.067224 0.169180 0.319246 -0.644602 0.694673 0.067350 0.424040
+0.156379 0.044803 0.153996 0.319246 -0.644602 0.694673 0.116650 0.420010
+0.179213 0.068282 0.165289 0.319246 -0.644602 0.694673 0.085250 0.429680
+0.177914 0.078618 0.152401 0.739921 -0.005832 -0.672668 0.075080 0.466000
+0.172189 0.056197 0.146298 0.739921 -0.005832 -0.672668 0.098560 0.464290
+0.161981 0.058897 0.135046 0.739921 -0.005832 -0.672668 0.116410 0.468430
+0.172189 0.056197 0.146298 0.748595 0.211528 -0.628380 0.098560 0.464290
+0.153810 0.036576 0.117798 0.748595 0.211528 -0.628380 0.173706 0.466822
+0.161981 0.058897 0.135046 0.748595 0.211528 -0.628380 0.116410 0.468430
+0.156379 0.044803 0.153996 0.620950 -0.772732 0.131555 0.128690 0.429070
+0.153810 0.036576 0.117798 0.620950 -0.772732 0.131555 0.173706 0.466822
+0.172189 0.056197 0.146298 0.620950 -0.772732 0.131555 0.098560 0.464290
+0.142930 0.026574 0.139531 0.765804 -0.636695 0.090357 0.174691 0.414974
+0.153810 0.036576 0.117798 0.765804 -0.636695 0.090357 0.173706 0.466822
+0.156379 0.044803 0.153996 0.765804 -0.636695 0.090357 0.128690 0.429070
+0.172189 0.056197 0.146298 0.918839 -0.382700 -0.096309 0.098560 0.464290
+0.184018 0.079932 0.164838 0.918839 -0.382700 -0.096309 0.044750 0.458050
+0.179213 0.068282 0.165289 0.918839 -0.382700 -0.096309 0.058440 0.452240
+0.172189 0.056197 0.146298 0.896665 -0.112398 -0.428203 0.098560 0.464290
+0.177914 0.078618 0.152401 0.896665 -0.112398 -0.428203 0.075080 0.466000
+0.184018 0.079932 0.164838 0.896665 -0.112398 -0.428203 0.044750 0.458050
+0.179213 0.068282 0.165289 0.640173 -0.733080 0.229724 0.085250 0.429680
+0.156379 0.044803 0.153996 0.640173 -0.733080 0.229724 0.116650 0.420010
+0.172189 0.056197 0.146298 0.640173 -0.733080 0.229724 0.098560 0.464290
+0.003153 -0.046601 -0.089268 0.856116 0.056674 -0.513667 0.824771 0.564740
+-0.032947 -0.077119 -0.152802 0.856116 0.056674 -0.513667 0.958829 0.529430
+-0.024684 -0.048566 -0.135880 0.856116 0.056674 -0.513667 0.936076 0.565704
+0.097660 0.038978 0.172552 0.374096 0.371146 0.849884 0.207600 0.751920
+0.105390 0.048647 0.164927 0.374096 0.371146 0.849884 0.205070 0.725670
+0.088707 0.052685 0.170507 0.374096 0.371146 0.849884 0.230999 0.772807
+0.179213 0.068282 0.165289 0.922539 -0.377374 0.080688 0.055790 0.445457
+0.184018 0.079932 0.164838 0.922539 -0.377374 0.080688 0.049275 0.450166
+0.182614 0.078900 0.176064 0.922539 -0.377374 0.080688 0.039630 0.437958
+0.118713 0.062923 0.224846 0.130581 -0.574499 0.808022 0.104782 0.225075
+0.128282 0.088243 0.241302 0.130581 -0.574499 0.808022 0.086230 0.212680
+0.107296 0.067456 0.229914 0.130581 -0.574499 0.808022 0.116859 0.196548
+0.118713 0.062923 0.224846 0.546977 -0.591640 0.592265 0.102323 0.225356
+0.131489 0.078973 0.229080 0.546977 -0.591640 0.592265 0.069787 0.240614
+0.128282 0.088243 0.241302 0.546977 -0.591640 0.592265 0.086230 0.212680
+0.128282 0.088243 0.241302 -0.106723 -0.392687 0.913459 0.086230 0.212680
+0.113825 0.086887 0.239030 -0.106723 -0.392687 0.913459 0.085630 0.194932
+0.107296 0.067456 0.229914 -0.106723 -0.392687 0.913459 0.116859 0.196548
+0.158948 0.073733 0.201971 -0.747981 -0.134729 0.649902 0.045070 0.346890
+0.159469 0.086296 0.205175 -0.747981 -0.134729 0.649902 0.036382 0.346078
+0.149787 0.066389 0.189905 -0.747981 -0.134729 0.649902 0.078462 0.341776
+0.173943 0.072128 0.185599 0.987532 -0.153296 0.035792 0.037193 0.399844
+0.176435 0.092959 0.206061 0.987532 -0.153296 0.035792 0.016240 0.393960
+0.174433 0.077625 0.195623 0.987532 -0.153296 0.035792 0.020480 0.387770
+0.176435 0.092959 0.206061 0.047143 -0.248685 0.967436 0.007915 0.371467
+0.159469 0.086296 0.205175 0.047143 -0.248685 0.967436 0.021672 0.354337
+0.158948 0.073733 0.201971 0.047143 -0.248685 0.967436 0.030040 0.353640
+0.158948 0.073733 0.201971 0.431624 -0.545427 0.718477 0.030040 0.353640
+0.174433 0.077625 0.195623 0.431624 -0.545427 0.718477 0.019210 0.387510
+0.176435 0.092959 0.206061 0.431624 -0.545427 0.718477 0.007915 0.371467
+0.113825 0.086887 0.239030 -0.763241 -0.045901 0.644482 0.085630 0.194932
+0.086585 0.059950 0.204852 -0.763241 -0.045901 0.644482 0.187156 0.190790
+0.107296 0.067456 0.229914 -0.763241 -0.045901 0.644482 0.116859 0.196548
+0.142987 0.029221 0.148360 0.638381 -0.762440 -0.105617 0.161963 0.397901
+0.150255 0.033474 0.161588 0.638381 -0.762440 -0.105617 0.126154 0.391636
+0.135350 0.021139 0.160543 0.638381 -0.762440 -0.105617 0.158440 0.365650
+0.119302 0.038419 0.159993 0.369961 0.129537 0.919972 0.182860 0.335000
+0.119415 0.029098 0.161260 0.369961 0.129537 0.919972 0.188520 0.331390
+0.124638 0.045934 0.156789 0.369961 0.129537 0.919972 0.172400 0.336930
+0.142930 0.026574 0.139531 0.953163 -0.291356 0.081197 0.174691 0.414974
+0.142987 0.029221 0.148360 0.953163 -0.291356 0.081197 0.161963 0.397901
+0.140320 0.014932 0.128395 0.953163 -0.291356 0.081197 0.195960 0.427720
+0.131489 0.078973 0.229080 0.711285 -0.660514 0.240407 0.069787 0.240614
+0.107389 0.046282 0.210566 0.711285 -0.660514 0.240407 0.147990 0.226080
+0.108825 0.043741 0.199336 0.711285 -0.660514 0.240407 0.159984 0.245722
+0.053360 0.088072 0.187479 -0.311539 0.935424 -0.167110 0.282632 0.904939
+0.063223 0.090410 0.182179 -0.311539 0.935424 -0.167110 0.270710 0.887400
+0.053192 0.085858 0.175399 -0.311539 0.935424 -0.167110 0.298639 0.885778
+0.052997 0.025909 0.078557 0.193159 0.644918 -0.739440 0.450460 0.714650
+0.071762 0.046670 0.101566 0.193159 0.644918 -0.739440 0.374767 0.709924
+0.081658 0.033262 0.092457 0.193159 0.644918 -0.739440 0.369880 0.669890
+-0.060966 -0.016531 -0.020698 -0.651389 0.538782 0.534235 0.841232 0.965642
+-0.070686 -0.013774 -0.035330 -0.651389 0.538782 0.534235 0.884636 0.967596
+-0.071377 -0.019477 -0.030421 -0.651389 0.538782 0.534235 0.879123 0.988764
+-0.070686 -0.013774 -0.035330 -0.651354 0.538799 0.534260 0.884636 0.967596
+-0.080407 -0.011017 -0.049962 -0.651354 0.538799 0.534260 0.928039 0.969549
+-0.071377 -0.019477 -0.030421 -0.651354 0.538799 0.534260 0.879123 0.988764
+-0.030055 -0.019593 0.010739 -0.712964 0.001680 0.701198 0.733560 0.250650
+-0.045510 -0.018062 -0.004979 -0.712964 0.001680 0.701198 0.790600 0.243710
+-0.030500 -0.033141 0.010319 -0.712964 0.001680 0.701198 0.740681 0.264800
+-0.045510 -0.018062 -0.004979 -0.712964 0.001681 0.701199 0.790600 0.243710
+-0.060966 -0.016531 -0.020698 -0.712964 0.001681 0.701199 0.847640 0.236770
+-0.030500 -0.033141 0.010319 -0.712964 0.001681 0.701199 0.740681 0.264800
+-0.003552 -0.007667 0.034842 -0.709785 0.235454 0.663902 0.647650 0.255020
+-0.016803 -0.013630 0.022790 -0.709785 0.235454 0.663902 0.690605 0.252835
+-0.023862 -0.025303 0.019383 -0.709785 0.235454 0.663902 0.712310 0.264110
+-0.025528 -0.015002 -0.098342 0.523553 0.697209 -0.489685 0.894100 0.654540
+-0.008270 -0.013472 -0.077712 0.523553 0.697209 -0.489685 0.825605 0.640405
+-0.004285 -0.027983 -0.094112 0.523553 0.697209 -0.489685 0.848720 0.590070
+-0.008270 -0.013472 -0.077712 0.523510 0.697224 -0.489709 0.825605 0.640405
+0.008989 -0.011941 -0.057082 0.523510 0.697224 -0.489709 0.757109 0.626270
+-0.004285 -0.027983 -0.094112 0.523510 0.697224 -0.489709 0.848720 0.590070
+-0.047826 -0.000722 -0.060163 0.009873 0.938464 -0.345236 0.877380 0.822860
+-0.036677 -0.007862 -0.079253 0.009873 0.938464 -0.345236 0.885739 0.738700
+-0.082414 -0.008469 -0.082211 0.009873 0.938464 -0.345236 0.963945 0.892963
+-0.036677 -0.007862 -0.079253 0.009874 0.938458 -0.345252 0.885739 0.738700
+-0.025528 -0.015002 -0.098342 0.009874 0.938458 -0.345252 0.894100 0.654540
+-0.082414 -0.008469 -0.082211 0.009874 0.938458 -0.345252 0.963945 0.892963
+-0.080407 -0.011017 -0.049962 -0.273470 0.957407 0.092664 0.928039 0.969549
+-0.064116 -0.005870 -0.055063 -0.273470 0.957407 0.092664 0.902709 0.896205
+-0.082414 -0.008469 -0.082211 -0.273470 0.957407 0.092664 0.963945 0.892963
+0.045717 0.011490 0.032308 -0.182390 0.973132 -0.140527 0.534848 0.690702
+0.036877 0.011536 0.044100 -0.182390 0.973132 -0.140527 0.536104 0.738321
+0.062588 0.018440 0.058539 -0.182390 0.973132 -0.140527 0.462180 0.663670
+0.036877 0.011536 0.044100 -0.182362 0.973130 -0.140576 0.536104 0.738321
+0.028037 0.011583 0.055893 -0.182362 0.973130 -0.140576 0.537360 0.785940
+0.062588 0.018440 0.058539 -0.182362 0.973130 -0.140576 0.462180 0.663670
+0.028037 0.011583 0.055893 -0.631896 0.718168 0.291449 0.537360 0.785940
+0.012243 0.001958 0.045367 -0.631896 0.718168 0.291449 0.575925 0.816265
+0.016955 -0.004583 0.071701 -0.631896 0.718168 0.291449 0.536530 0.818012
+0.012243 0.001958 0.045367 -0.631858 0.718200 0.291451 0.575925 0.816265
+-0.003552 -0.007667 0.034842 -0.631858 0.718200 0.291451 0.614490 0.846590
+0.016955 -0.004583 0.071701 -0.631858 0.718200 0.291451 0.536530 0.818012
+0.029745 -0.010530 -0.025899 0.757465 0.392194 -0.521949 0.708175 0.630170
+0.040124 -0.009825 -0.010307 0.757465 0.392194 -0.521949 0.683707 0.632120
+0.029309 -0.029773 -0.040991 0.757465 0.392194 -0.521949 0.694998 0.593245
+0.040124 -0.009825 -0.010307 0.757479 0.392177 -0.521942 0.683707 0.632120
+0.050502 -0.009120 0.005284 0.757479 0.392177 -0.521942 0.659240 0.634070
+0.029309 -0.029773 -0.040991 0.757479 0.392177 -0.521942 0.694998 0.593245
+0.008989 -0.011941 -0.057082 0.757481 0.392213 -0.521912 0.757109 0.626270
+0.019367 -0.011236 -0.041490 0.757481 0.392213 -0.521912 0.732642 0.628220
+0.029309 -0.029773 -0.040991 0.757481 0.392213 -0.521912 0.694998 0.593245
+0.019367 -0.011236 -0.041490 0.757458 0.392199 -0.521955 0.732642 0.628220
+0.029745 -0.010530 -0.025899 0.757458 0.392199 -0.521955 0.708175 0.630170
+0.029309 -0.029773 -0.040991 0.757458 0.392199 -0.521955 0.694998 0.593245
+-0.064116 -0.005870 -0.055063 -0.273531 0.957385 0.092707 0.902709 0.896205
+-0.055971 -0.003296 -0.057613 -0.273531 0.957385 0.092707 0.890044 0.859532
+-0.082414 -0.008469 -0.082211 -0.273531 0.957385 0.092707 0.963945 0.892963
+-0.047826 -0.000722 -0.060163 -0.273531 0.957385 0.092707 0.877380 0.822860
+-0.016803 -0.013630 0.022790 -0.709692 0.235359 0.664036 0.690605 0.252835
+-0.023429 -0.016611 0.016765 -0.709692 0.235359 0.664036 0.712083 0.251742
+-0.023862 -0.025303 0.019383 -0.709692 0.235359 0.664036 0.712310 0.264110
+-0.023429 -0.016611 0.016765 -0.709760 0.235342 0.663969 0.712083 0.251742
+-0.030055 -0.019593 0.010739 -0.709760 0.235342 0.663969 0.733560 0.250650
+-0.023862 -0.025303 0.019383 -0.709760 0.235342 0.663969 0.712310 0.264110
+3 0 1 2
+3 3 4 5
+3 6 7 8
+3 9 10 11
+3 12 13 14
+3 15 16 17
+3 18 19 20
+3 21 22 23
+3 24 25 26
+3 27 28 29
+3 30 31 32
+3 33 34 35
+3 36 37 38
+3 39 40 41
+3 42 43 44
+3 45 46 47
+3 48 49 50
+3 51 52 53
+3 54 55 56
+3 57 58 59
+3 60 61 62
+3 63 64 65
+3 66 67 68
+3 69 70 71
+3 72 73 74
+3 75 76 77
+3 78 79 80
+3 81 82 83
+3 84 85 86
+3 87 88 89
+3 90 91 92
+3 93 94 95
+3 96 97 98
+3 99 100 101
+3 102 103 104
+3 105 106 107
+3 108 109 110
+3 111 112 113
+3 114 115 116
+3 117 118 119
+3 120 121 122
+3 123 124 125
+3 126 127 128
+3 129 130 131
+3 132 133 134
+3 135 136 137
+3 138 139 140
+3 141 142 143
+3 144 145 146
+3 147 148 149
+3 150 151 152
+3 153 154 155
+3 156 157 158
+3 159 160 161
+3 162 163 164
+3 165 166 167
+3 168 169 170
+3 171 172 173
+3 174 175 176
+3 177 178 179
+3 180 181 182
+3 183 184 185
+3 186 187 188
+3 189 190 191
+3 192 193 194
+3 195 196 197
+3 198 199 200
+3 201 202 203
+3 204 205 206
+3 207 208 209
+3 210 211 212
+3 213 214 215
+3 216 217 218
+3 219 220 221
+3 222 223 224
+3 225 226 227
+3 228 229 230
+3 231 232 233
+3 234 235 236
+3 237 238 239
+3 240 241 242
+3 243 244 245
+3 246 247 248
+3 249 250 251
+3 252 253 254
+3 255 256 257
+3 258 259 260
+3 261 262 263
+3 264 265 266
+3 267 268 269
+3 270 271 272
+3 273 274 275
+3 276 277 278
+3 279 280 281
+3 282 283 284
+3 285 286 287
+3 288 289 290
+3 291 292 293
+3 294 295 296
+3 297 298 299
+3 300 301 302
+3 303 304 305
+3 306 307 308
+3 309 310 311
+3 312 313 314
+3 315 316 317
+3 318 319 320
+3 321 322 323
+3 324 325 326
+3 327 328 329
+3 330 331 332
+3 333 334 335
+3 336 337 338
+3 339 340 341
+3 342 343 344
+3 345 346 347
+3 348 349 350
+3 351 352 353
+3 354 355 356
+3 357 358 359
+3 360 361 362
+3 363 364 365
+3 366 367 368
+3 369 370 371
+3 372 373 374
+3 375 376 377
+3 378 379 380
+3 381 382 383
+3 384 385 386
+3 387 388 389
+3 390 391 392
+3 393 394 395
+3 396 397 398
+3 399 400 401
+3 402 403 404
+3 405 406 407
+3 408 409 410
+3 411 412 413
+3 414 415 416
+3 417 418 419
+3 420 421 422
+3 423 424 425
+3 426 427 428
+3 429 430 431
+3 432 433 434
+3 435 436 437
+3 438 439 440
+3 441 442 443
+3 444 445 446
+3 447 448 449
+3 450 451 452
+3 453 454 455
+3 456 457 458
+3 459 460 461
+3 462 463 464
+3 465 466 467
+3 468 469 470
+3 471 472 473
+3 474 475 476
+3 477 478 479
+3 480 481 482
+3 483 484 485
+3 486 487 488
+3 489 490 491
+3 492 493 494
+3 495 496 497
+3 498 499 500
+3 501 502 503
+3 504 505 506
+3 507 508 509
+3 510 511 512
+3 513 514 515
+3 516 517 518
+3 519 520 521
+3 522 523 524
+3 525 526 527
+3 528 529 530
+3 531 532 533
+3 534 535 536
+3 537 538 539
+3 540 541 542
+3 543 544 545
+3 546 547 548
+3 549 550 551
+3 552 553 554
+3 555 556 557
+3 558 559 560
+3 561 562 563
+3 564 565 566
+3 567 568 569
+3 570 571 572
+3 573 574 575
+3 576 577 578
+3 579 580 581
+3 582 583 584
+3 585 586 587
+3 588 589 590
+3 591 592 593
+3 594 595 596
+3 597 598 599
+3 600 601 602
+3 603 604 605
+3 606 607 608
+3 609 610 611
+3 612 613 614
+3 615 616 617
+3 618 619 620
+3 621 622 623
+3 624 625 626
+3 627 628 629
+3 630 631 632
+3 633 634 635
+3 636 637 638
+3 639 640 641
+3 642 643 644
+3 645 646 647
+3 648 649 650
+3 651 652 653
+3 654 655 656
+3 657 658 659
+3 660 661 662
+3 663 664 665
+3 666 667 668
+3 669 670 671
+3 672 673 674
+3 675 676 677
+3 678 679 680
+3 681 682 683
+3 684 685 686
+3 687 688 689
+3 690 691 692
+3 693 694 695
+3 696 697 698
+3 699 700 701
+3 702 703 704
+3 705 706 707
+3 708 709 710
+3 711 712 713
+3 714 715 716
+3 717 718 719
+3 720 721 722
+3 723 724 725
+3 726 727 728
+3 729 730 731
+3 732 733 734
+3 735 736 737
+3 738 739 740
+3 741 742 743
+3 744 745 746
+3 747 748 749
+3 750 751 752
+3 753 754 755
+3 756 757 758
+3 759 760 761
+3 762 763 764
+3 765 766 767
+3 768 769 770
+3 771 772 773
+3 774 775 776
+3 777 778 779
+3 780 781 782
+3 783 784 785
+3 786 787 788
+3 789 790 791
+3 792 793 794
+3 795 796 797
+3 798 799 800
+3 801 802 803
+3 804 805 806
+3 807 808 809
+3 810 811 812
+3 813 814 815
+3 816 817 818
+3 819 820 821
+3 822 823 824
+3 825 826 827
+3 828 829 830
+3 831 832 833
+3 834 835 836
+3 837 838 839
+3 840 841 842
+3 843 844 845
+3 846 847 848
+3 849 850 851
+3 852 853 854
+3 855 856 857
+3 858 859 860
+3 861 862 863
+3 864 865 866
+3 867 868 869
+3 870 871 872
+3 873 874 875
+3 876 877 878
+3 879 880 881
+3 882 883 884
+3 885 886 887
+3 888 889 890
+3 891 892 893
+3 894 895 896
+3 897 898 899
+3 900 901 902
+3 903 904 905
+3 906 907 908
+3 909 910 911
+3 912 913 914
+3 915 916 917
+3 918 919 920
+3 921 922 923
+3 924 925 926
+3 927 928 929
+3 930 931 932
+3 933 934 935
+3 936 937 938
+3 939 940 941
+3 942 943 944
+3 945 946 947
+3 948 949 950
+3 951 952 953
+3 954 955 956
+3 957 958 959
+3 960 961 962
+3 963 964 965
+3 966 967 968
+3 969 970 971
+3 972 973 974
+3 975 976 977
+3 978 979 980
+3 981 982 983
+3 984 985 986
+3 987 988 989
+3 990 991 992
+3 993 994 995
+3 996 997 998
+3 999 1000 1001
+3 1002 1003 1004
+3 1005 1006 1007
+3 1008 1009 1010
+3 1011 1012 1013
+3 1014 1015 1016
+3 1017 1018 1019
+3 1020 1021 1022
+3 1023 1024 1025
+3 1026 1027 1028
+3 1029 1030 1031
+3 1032 1033 1034
+3 1035 1036 1037
+3 1038 1039 1040
+3 1041 1042 1043
+3 1044 1045 1046
+3 1047 1048 1049
+3 1050 1051 1052
+3 1053 1054 1055
+3 1056 1057 1058
+3 1059 1060 1061
+3 1062 1063 1064
+3 1065 1066 1067
+3 1068 1069 1070
+3 1071 1072 1073
+3 1074 1075 1076
+3 1077 1078 1079
+3 1080 1081 1082
+3 1083 1084 1085
+3 1086 1087 1088
+3 1089 1090 1091
+3 1092 1093 1094
+3 1095 1096 1097
+3 1098 1099 1100
+3 1101 1102 1103
+3 1104 1105 1106
+3 1107 1108 1109
+3 1110 1111 1112
+3 1113 1114 1115
+3 1116 1117 1118
+3 1119 1120 1121
+3 1122 1123 1124
+3 1125 1126 1127
+3 1128 1129 1130
+3 1131 1132 1133
+3 1134 1135 1136
+3 1137 1138 1139
+3 1140 1141 1142
+3 1143 1144 1145
+3 1146 1147 1148
+3 1149 1150 1151
+3 1152 1153 1154
+3 1155 1156 1157
+3 1158 1159 1160
+3 1161 1162 1163
+3 1164 1165 1166
+3 1167 1168 1169
+3 1170 1171 1172
+3 1173 1174 1175
+3 1176 1177 1178
+3 1179 1180 1181
+3 1182 1183 1184
+3 1185 1186 1187
+3 1188 1189 1190
+3 1191 1192 1193
+3 1194 1195 1196
+3 1197 1198 1199
+3 1200 1201 1202
+3 1203 1204 1205
+3 1206 1207 1208
+3 1209 1210 1211
+3 1212 1213 1214
+3 1215 1216 1217
+3 1218 1219 1220
+3 1221 1222 1223
+3 1224 1225 1226
+3 1227 1228 1229
+3 1230 1231 1232
+3 1233 1234 1235
+3 1236 1237 1238
+3 1239 1240 1241
+3 1242 1243 1244
+3 1245 1246 1247
+3 1248 1249 1250
+3 1251 1252 1253
+3 1254 1255 1256
+3 1257 1258 1259
+3 1260 1261 1262
+3 1263 1264 1265
+3 1266 1267 1268
+3 1269 1270 1271
+3 1272 1273 1274
+3 1275 1276 1277
+3 1278 1279 1280
+3 1281 1282 1283
+3 1284 1285 1286
+3 1287 1288 1289
+3 1290 1291 1292
+3 1293 1294 1295
+3 1296 1297 1298
+3 1299 1300 1301
+3 1302 1303 1304
+3 1305 1306 1307
+3 1308 1309 1310
+3 1311 1312 1313
+3 1314 1315 1316
+3 1317 1318 1319
+3 1320 1321 1322
+3 1323 1324 1325
+3 1326 1327 1328
+3 1329 1330 1331
+3 1332 1333 1334
+3 1335 1336 1337
+3 1338 1339 1340
+3 1341 1342 1343
+3 1344 1345 1346
+3 1347 1348 1349
+3 1350 1351 1352
+3 1353 1354 1355
+3 1356 1357 1358
+3 1359 1360 1361
+3 1362 1363 1364
+3 1365 1366 1367
+3 1368 1369 1370
+3 1371 1372 1373
+3 1374 1375 1376
+3 1377 1378 1379
+3 1380 1381 1382
+3 1383 1384 1385
+3 1386 1387 1388
+3 1389 1390 1391
+3 1392 1393 1394
+3 1395 1396 1397
+3 1398 1399 1400
+3 1401 1402 1403
+3 1404 1405 1406
+3 1407 1408 1409
+3 1410 1411 1412
+3 1413 1414 1415
+3 1416 1417 1418
+3 1419 1420 1421
+3 1422 1423 1424
+3 1425 1426 1427
+3 1428 1429 1430
+3 1431 1432 1433
+3 1434 1435 1436
+3 1437 1438 1439
+3 1440 1441 1442
+3 1443 1444 1445
+3 1446 1447 1448
+3 1449 1450 1451
+3 1452 1453 1454
+3 1455 1456 1457
+3 1458 1459 1460
+3 1461 1462 1463
+3 1464 1465 1466
+3 1467 1468 1469
+3 1470 1471 1472
+3 1473 1474 1475
+3 1476 1477 1478
+3 1479 1480 1481
+3 1482 1483 1484
+3 1485 1486 1487
+3 1488 1489 1490
+3 1491 1492 1493
+3 1494 1495 1496
+3 1497 1498 1499
+3 1500 1501 1502
+3 1503 1504 1505
+3 1506 1507 1508
+3 1509 1510 1511
+3 1512 1513 1514
+3 1515 1516 1517
+3 1518 1519 1520
+3 1521 1522 1523
+3 1524 1525 1526
+3 1527 1528 1529
+3 1530 1531 1532
+3 1533 1534 1535
+3 1536 1537 1538
+3 1539 1540 1541
+3 1542 1543 1544
+3 1545 1546 1547
+3 1548 1549 1550
+3 1551 1552 1553
+3 1554 1555 1556
+3 1557 1558 1559
+3 1560 1561 1562
+3 1563 1564 1565
+3 1566 1567 1568
+3 1569 1570 1571
+3 1572 1573 1574
+3 1573 1575 1574
+3 1576 1577 1578
+3 1579 1580 1581
diff --git a/SurgSim/Testing/Data/Geometry/wound_deformable.ply b/SurgSim/Testing/Data/Geometry/wound_deformable.ply
new file mode 100644
index 0000000..b4de671
--- /dev/null
+++ b/SurgSim/Testing/Data/Geometry/wound_deformable.ply
@@ -0,0 +1,2389 @@
+ply
+format ascii 1.0
+comment This file is a part of the OpenSurgSim project.
+comment Copyright 2013, SimQuest Solutions Inc.
+comment
+comment Licensed under the Apache License, Version 2.0 (the "License");
+comment you may not use this file except in compliance with the License.
+comment You may obtain a copy of the License at
+comment
+comment http://www.apache.org/licenses/LICENSE-2.0
+comment
+comment Unless required by applicable law or agreed to in writing, software
+comment distributed under the License is distributed on an "AS IS" BASIS,
+comment WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+comment See the License for the specific language governing permissions and
+comment limitations under the License.
+comment
+comment This file provides the geometry which describes a tetrahedral volume
+comment mesh and triangular surface mesh of an arm wound.
+comment
+element vertex 391
+property double x
+property double y
+property double z
+element face 407
+property list uint uint vertex_indices
+element 3d_element 1476
+property list uint uint vertex_indices
+element boundary_condition 79
+property uint vertex_index
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+end_header
+-0.017638 0.001427 -0.012363
+-0.018984 0.002069 -0.016867
+-0.014913 -0.001358 -0.015090
+-0.018090 -0.001832 -0.017359
+0.012643 -0.002455 0.001226
+0.009376 -0.006324 0.003409
+0.015479 -0.002289 0.003265
+0.014119 -0.002367 0.007046
+-0.000236 -0.006438 -0.015323
+-0.003732 -0.004010 -0.013609
+0.002155 -0.000546 -0.013612
+0.001262 -0.001523 -0.018049
+0.019667 -0.017785 0.008674
+0.029299 -0.005262 0.004546
+0.046990 -0.020879 0.007375
+0.037332 -0.020768 -0.005328
+0.033586 -0.016523 0.018450
+0.034101 -0.002031 0.016309
+0.033392 -0.002474 0.009623
+0.025349 -0.003774 0.014192
+0.019145 0.004850 0.012264
+0.020216 0.004879 0.011113
+0.020968 0.001399 0.010219
+0.018687 0.001314 0.008476
+-0.007352 0.001018 0.005860
+-0.007333 -0.005722 0.014291
+-0.013652 -0.000173 0.002866
+-0.007113 -0.008223 -0.002577
+-0.006477 -0.004144 -0.015932
+-0.010957 -0.004967 -0.018490
+-0.007629 -0.006377 -0.018801
+-0.009906 -0.007928 -0.014976
+0.014105 -0.020914 -0.009294
+0.009112 -0.006286 -0.008604
+0.006268 -0.009189 -0.014543
+0.002300 -0.008261 -0.009596
+0.006635 0.000026 -0.006193
+0.010115 -0.000309 -0.007852
+0.005102 -0.004418 -0.008996
+-0.006609 0.001420 -0.025080
+-0.011057 -0.007486 -0.026293
+-0.008811 0.000184 -0.034044
+-0.005338 -0.005470 -0.024596
+0.001259 0.002603 0.004418
+0.000123 0.002377 -0.000362
+0.002998 0.002713 0.001375
+0.002769 -0.002124 0.003090
+-0.020005 -0.002711 -0.020006
+-0.021803 0.001971 -0.019803
+-0.021059 0.001058 -0.014582
+0.014631 0.000524 0.027783
+0.022184 -0.002620 0.021520
+0.018739 -0.011699 0.030717
+0.015337 -0.003630 0.015467
+0.016049 0.003671 -0.002811
+0.018898 0.003898 -0.000351
+0.014287 0.000683 -0.000350
+0.019319 -0.000205 -0.003032
+0.046990 -0.007619 0.007375
+0.040075 -0.002131 -0.002012
+0.037332 -0.007509 -0.005328
+-0.011140 0.001039 -0.002776
+-0.014033 -0.006140 -0.001762
+-0.016684 0.000633 -0.005670
+-0.011809 -0.002190 -0.009170
+0.024204 0.005929 0.015670
+0.026606 0.006410 0.017058
+0.023576 0.006924 0.019679
+0.024566 0.001831 0.017759
+0.013743 0.004124 0.008547
+0.011935 0.004109 0.005502
+0.014670 0.004333 0.007344
+0.012704 0.000753 0.008033
+-0.003034 -0.009956 -0.020825
+-0.006038 -0.007999 -0.012282
+0.014736 0.003042 -0.006304
+0.014286 -0.003192 -0.009813
+0.014065 0.002151 -0.010793
+0.019818 -0.002939 -0.008113
+0.036755 0.006575 0.019271
+0.036907 0.001084 0.018946
+0.033263 0.001097 0.023070
+0.033376 0.002597 0.017918
+0.022543 0.001069 0.006601
+0.020245 -0.001947 0.007184
+0.019727 0.001041 0.004292
+0.023642 -0.002413 0.005541
+0.029219 0.001517 -0.003210
+0.022506 -0.007309 -0.002980
+0.013584 0.001132 0.004599
+0.015737 0.001218 0.006218
+0.015374 -0.005687 -0.005108
+0.019027 -0.006775 -0.014839
+0.030475 0.001257 0.015552
+0.027297 -0.001544 0.019319
+-0.006877 -0.002740 -0.012407
+-0.009683 -0.005474 -0.007552
+-0.008963 -0.000571 -0.011342
+-0.009123 -0.005772 -0.011405
+0.006753 0.004254 0.022359
+0.010408 0.004267 0.016270
+0.015903 0.005679 0.020105
+0.017492 0.000471 0.018668
+-0.018916 -0.006283 -0.013171
+-0.013552 -0.004660 -0.011679
+-0.015191 -0.008193 -0.008642
+-0.014454 -0.008852 -0.015804
+0.016241 0.004334 0.010184
+0.017210 0.004543 0.009049
+0.012207 0.000507 -0.002006
+0.015357 0.000245 -0.003791
+0.007154 -0.001854 0.005752
+0.011373 -0.002515 0.009976
+0.009600 -0.002702 0.003200
+0.022251 -0.002063 0.015960
+0.021468 -0.001789 0.012242
+0.017030 0.000365 0.013057
+0.020308 0.001792 0.013974
+0.013639 -0.003166 -0.002562
+0.011924 -0.003711 -0.006304
+-0.014146 -0.020429 -0.047689
+-0.014146 -0.007170 -0.047689
+-0.024140 -0.017967 -0.040188
+-0.012259 -0.006676 -0.033836
+0.002537 -0.008403 -0.037874
+-0.001499 -0.000057 -0.029561
+-0.000610 -0.002936 -0.039936
+0.031048 0.003384 0.005728
+0.031021 0.004616 0.009841
+0.029058 -0.002228 0.009819
+-0.017817 -0.005609 -0.020805
+-0.021077 -0.009144 -0.020053
+-0.019782 -0.005521 -0.017072
+-0.021431 -0.005952 -0.022335
+0.028581 0.004545 0.037841
+0.022848 0.002598 0.033651
+0.026518 0.008415 0.028378
+-0.008942 -0.024739 -0.008176
+-0.020167 -0.001455 -0.000035
+-0.019937 -0.006669 -0.033633
+-0.011533 -0.022757 -0.029013
+0.025681 0.001454 0.013298
+0.021352 0.005330 0.013823
+0.022555 0.005159 0.012575
+-0.018697 -0.003565 -0.023866
+-0.003741 -0.007976 -0.007476
+-0.004960 -0.004029 -0.006795
+0.035352 0.005531 0.016102
+0.033038 0.004995 0.012719
+0.036201 0.003833 0.011247
+-0.014781 0.002213 -0.020373
+-0.016880 -0.002824 -0.021921
+-0.016398 0.001977 -0.024375
+-0.013707 -0.002037 -0.021513
+-0.001967 0.002359 -0.017593
+-0.005030 0.002252 -0.019030
+-0.000406 0.001476 -0.021158
+-0.003015 -0.003057 -0.018944
+0.005527 0.000425 -0.000868
+0.005663 0.003045 0.003152
+0.003921 0.003399 0.000160
+-0.015527 -0.005152 -0.022539
+-0.015560 -0.006666 -0.026900
+-0.014996 -0.002896 -0.024296
+-0.013917 -0.007956 -0.024021
+0.004887 -0.021029 0.001820
+0.002057 -0.006870 -0.003212
+0.022552 -0.001675 0.009108
+-0.024489 0.000741 -0.016918
+-0.024391 0.001936 -0.022406
+-0.024601 -0.004739 -0.018778
+0.006850 -0.006797 -0.004277
+0.004032 -0.004089 -0.000551
+0.012333 0.004106 0.011509
+0.005130 0.003275 0.012820
+0.007317 0.003436 0.008224
+-0.004058 0.001878 0.001264
+-0.014203 0.001575 -0.029189
+-0.010570 -0.002455 -0.021860
+-0.012199 0.002106 -0.022621
+-0.025827 -0.009187 -0.021112
+-0.021601 -0.004637 -0.026044
+-0.017290 -0.008529 -0.023047
+-0.013967 -0.002130 -0.018341
+-0.014398 -0.004396 -0.019489
+-0.016250 -0.002623 -0.019238
+0.002921 -0.006003 -0.021793
+0.009600 0.002834 -0.010048
+0.012195 0.002939 -0.008156
+0.014492 -0.021349 -0.029300
+0.026117 -0.020765 -0.017965
+0.014492 -0.008090 -0.029300
+0.002621 -0.007830 -0.029082
+0.008423 0.000639 0.001153
+0.030678 0.007333 0.019744
+0.026656 0.007758 0.022058
+0.020949 0.007122 0.023900
+0.024451 0.002704 0.023354
+-0.023157 -0.024020 -0.019768
+0.029177 0.003608 0.018712
+0.012003 0.005360 0.025917
+0.004293 -0.015189 0.020860
+0.002537 -0.021663 -0.037874
+0.017564 -0.002515 -0.026912
+0.011421 -0.002833 -0.031689
+0.011215 0.000322 -0.020415
+0.002635 0.003021 -0.012333
+0.001235 0.002490 -0.015739
+0.004272 0.002632 -0.013690
+-0.002997 0.002057 -0.002458
+-0.002614 -0.004314 -0.002919
+0.003125 -0.022994 -0.021280
+-0.005395 -0.000823 -0.013764
+-0.008954 -0.001258 -0.015629
+-0.003834 0.002652 -0.016057
+-0.007047 -0.000971 -0.018908
+0.010433 -0.002408 -0.000327
+0.007502 -0.002301 -0.002415
+0.009372 0.000321 -0.004108
+0.025152 0.001195 0.009053
+0.016960 0.000956 0.001995
+0.019875 -0.003369 -0.000014
+0.030751 0.008190 0.022981
+0.006170 -0.006701 0.007879
+0.004293 -0.001930 0.020860
+-0.000653 -0.004919 0.008637
+-0.014173 -0.021266 0.011121
+-0.017277 -0.006019 -0.017796
+0.023544 -0.000939 0.002037
+-0.009768 -0.009134 -0.022027
+-0.009580 -0.002948 -0.014062
+-0.011797 -0.001691 -0.017176
+-0.010204 0.002309 -0.009891
+-0.011598 0.001574 -0.008061
+-0.008708 0.001627 -0.006285
+0.034635 0.001794 0.002947
+0.039472 0.002062 0.008665
+-0.015907 -0.007981 -0.018944
+-0.020598 0.001813 -0.025718
+-0.045621 -0.006163 -0.019740
+-0.040582 -0.008149 -0.025424
+-0.042936 -0.004004 -0.023210
+-0.038274 -0.003561 -0.017712
+-0.025030 -0.004697 -0.013625
+-0.023215 -0.000003 -0.009663
+-0.021118 -0.002997 -0.014161
+-0.004112 0.000695 0.015891
+-0.016389 -0.004557 -0.019826
+-0.003547 -0.002715 -0.010257
+-0.001840 -0.000524 -0.011726
+-0.000903 -0.002832 -0.008443
+-0.001161 -0.006198 -0.011121
+0.026165 -0.000632 0.016453
+0.025651 0.005503 0.014190
+-0.045621 -0.011579 -0.019740
+-0.040582 -0.021408 -0.025424
+-0.032217 -0.008366 -0.016728
+0.000124 -0.006312 0.001727
+0.017517 -0.002166 0.004909
+0.011949 -0.006639 -0.000263
+0.023286 0.001530 0.011808
+-0.013427 -0.006320 -0.020900
+0.005159 0.001722 -0.017378
+0.006008 -0.003221 -0.013176
+0.044680 -0.002154 0.004195
+-0.000907 0.002148 0.009167
+0.018182 0.005343 0.015586
+0.043927 0.001725 0.025605
+-0.012854 0.002261 -0.012050
+-0.014517 0.001574 -0.010230
+-0.011668 -0.001006 -0.012914
+-0.008316 0.002150 -0.020675
+-0.010664 0.002390 -0.018678
+-0.021013 -0.010291 0.007951
+-0.014146 -0.001754 -0.047689
+-0.008422 -0.002312 -0.044596
+-0.015684 0.000035 -0.039467
+-0.034331 -0.001228 -0.022520
+-0.031662 -0.007124 -0.023091
+-0.031224 -0.014703 0.002956
+-0.022628 -0.011995 -0.002126
+-0.025924 -0.004114 -0.023568
+0.027872 -0.000761 0.013734
+-0.034591 -0.000064 -0.031338
+-0.029145 0.000494 -0.024321
+-0.028017 -0.005001 -0.028032
+0.027902 0.001681 0.011326
+0.025087 -0.001165 0.011230
+-0.021752 0.000419 -0.042010
+-0.026528 0.000997 -0.038365
+-0.024140 -0.004708 -0.040188
+-0.020978 0.001288 -0.033343
+-0.040551 -0.026143 -0.010199
+-0.023197 -0.010713 -0.012877
+-0.040551 -0.012883 -0.010199
+0.004485 -0.000007 -0.025484
+0.010626 0.000849 0.002651
+-0.028255 0.000295 -0.019377
+0.013332 0.003514 -0.005048
+0.012798 0.000333 -0.005951
+-0.015803 0.002187 -0.014369
+-0.013341 -0.003209 -0.016395
+0.004675 -0.002400 -0.004481
+0.002779 0.000374 -0.003048
+0.026117 -0.007505 -0.017965
+0.028859 -0.001994 -0.015040
+-0.018952 -0.009957 -0.016014
+-0.017184 -0.003861 -0.014796
+0.020301 0.003439 -0.001538
+0.046716 -0.013576 0.023724
+0.055055 -0.021059 0.018567
+0.055055 -0.007800 0.018567
+-0.007396 0.002390 -0.007873
+0.001875 -0.002670 -0.006494
+0.038019 -0.008659 0.029998
+-0.008029 -0.002626 -0.003881
+-0.005500 -0.000144 -0.009081
+-0.016373 -0.003539 -0.018074
+-0.010554 -0.001307 0.012690
+0.049300 -0.002253 0.010554
+0.038759 -0.002166 0.014632
+0.005684 -0.003039 -0.035812
+0.038019 0.004600 0.029998
+0.038019 0.010016 0.029998
+-0.026286 -0.002491 -0.003954
+0.017414 0.003222 -0.004081
+0.044485 0.002423 0.015009
+0.040181 0.004389 0.016007
+-0.000017 0.000235 -0.005153
+-0.033970 -0.003806 -0.010656
+0.009082 0.001920 -0.014465
+0.017757 0.000902 -0.014705
+-0.030192 -0.001153 -0.014581
+0.025315 0.003889 0.003730
+0.024009 0.004399 0.004815
+-0.000443 0.002808 -0.014194
+0.005274 0.003135 -0.010676
+0.006826 0.002728 -0.011961
+0.008126 0.003261 -0.008819
+0.003879 -0.000228 -0.008173
+0.032342 0.006310 0.016942
+0.023610 0.001317 -0.009141
+0.028043 0.004252 0.006706
+0.028016 -0.000536 0.005381
+0.017259 0.006519 0.029649
+-0.032441 -0.018179 -0.033098
+0.022512 0.003646 0.000819
+0.027741 0.001325 0.014450
+0.028451 0.005830 0.015287
+-0.017863 -0.003577 0.009446
+0.021215 0.004148 0.001990
+0.025358 0.002875 -0.000357
+-0.025580 0.001252 -0.027579
+-0.032441 -0.004919 -0.033098
+-0.038228 -0.001462 -0.027638
+-0.019666 0.002058 -0.021535
+0.028581 -0.008715 0.037841
+0.010757 0.003389 -0.006914
+0.019618 0.002475 -0.006065
+-0.031224 -0.009287 0.002956
+-0.031224 -0.027962 0.002956
+0.034589 -0.002055 -0.008644
+-0.002831 -0.000029 -0.007070
+-0.005830 0.001763 -0.004361
+-0.038103 -0.007971 -0.006212
+0.031060 0.005869 0.013961
+-0.030292 0.001057 -0.034857
+-0.024162 -0.006173 0.006457
+-0.042999 -0.006964 -0.014187
+0.001834 0.002718 0.019362
+0.049505 -0.002359 0.021843
+0.011040 0.003784 0.006750
+-0.001834 0.002801 -0.003863
+0.001083 0.003102 -0.001764
+0.022848 0.008014 0.033651
+0.023375 -0.002185 -0.020891
+0.028581 0.009961 0.037841
+0.006652 0.003669 0.002024
+0.009164 0.003881 0.003676
+0.008207 0.003356 0.004901
+0.049505 0.003057 0.021843
+0.055055 -0.002384 0.018567
+0.001013 -0.000353 -0.009972
+0.029329 0.005348 0.010717
+-0.004550 0.002514 -0.005831
+-0.007071 0.002482 -0.017312
+0.046366 0.005522 0.023902
+-0.045621 -0.024839 -0.019740
+0.041488 0.008760 0.027307
+0.033206 0.010046 0.034162
+0.026563 0.004882 0.007661
+3 43 45 44
+3 65 67 66
+3 69 71 70
+3 99 101 100
+3 61 63 138
+3 65 143 142
+3 147 149 148
+3 154 156 155
+3 159 160 45
+3 168 48 169
+3 173 175 174
+3 43 44 176
+3 187 188 77
+3 67 196 195
+3 66 67 195
+3 150 179 152
+3 203 204 205
+3 206 208 207
+3 176 44 209
+3 232 233 234
+3 235 127 236
+3 48 49 1
+3 239 242 241
+3 253 143 65
+3 87 235 59
+3 152 179 177
+3 41 39 125
+3 179 272 271
+3 274 276 275
+3 283 277 284
+3 266 107 173
+3 288 289 291
+3 48 168 49
+3 208 262 207
+3 156 262 295
+3 297 169 284
+3 142 67 65
+3 188 298 75
+3 268 300 269
+3 300 0 269
+3 312 232 234
+3 1 49 0
+3 236 127 149
+3 233 63 61
+3 265 24 318
+3 319 264 236
+3 321 126 125
+3 79 222 323
+3 276 288 291
+3 107 71 69
+3 149 327 326
+3 269 63 233
+3 49 244 0
+3 77 331 330
+3 332 168 297
+3 268 269 233
+3 142 20 266
+3 325 54 55
+3 262 330 205
+3 0 244 63
+3 195 136 222
+3 336 338 337
+3 79 340 194
+3 246 174 265
+3 195 222 194
+3 26 138 349
+3 333 346 350
+3 351 333 127
+3 169 352 284
+3 325 55 308
+3 244 332 324
+3 241 242 277
+3 354 241 277
+3 39 271 155
+3 59 235 264
+3 169 355 238
+3 205 204 295
+3 20 108 107
+3 335 154 214
+3 87 341 358
+3 358 351 87
+3 107 69 173
+3 359 324 329
+3 41 177 39
+3 326 319 236
+3 100 173 174
+3 359 329 364
+3 214 154 155
+3 154 335 207
+3 283 354 277
+3 142 143 21
+3 289 366 291
+3 348 253 66
+3 138 367 349
+3 368 329 242
+3 361 305 341
+3 77 341 331
+3 232 268 233
+3 338 187 337
+3 174 369 100
+3 100 369 99
+3 43 265 174
+3 265 43 176
+3 127 333 342
+3 154 207 156
+3 196 200 344
+3 188 187 357
+3 188 75 77
+3 173 371 175
+3 340 348 194
+3 372 44 373
+3 147 340 79
+3 333 350 334
+3 126 41 125
+3 330 262 208
+3 291 366 352
+3 374 376 136
+3 351 346 333
+3 377 379 378
+3 331 375 205
+3 355 152 238
+3 380 381 326
+3 234 233 61
+3 367 324 359
+3 196 101 200
+3 342 128 127
+3 188 357 298
+3 379 371 378
+3 358 325 308
+3 383 148 128
+3 177 291 238
+3 138 324 367
+3 136 344 374
+3 173 101 266
+3 168 169 297
+3 149 326 236
+3 209 372 384
+3 108 71 107
+3 337 187 330
+3 149 127 128
+3 357 187 338
+3 373 45 160
+3 173 100 101
+3 265 318 246
+3 101 99 200
+3 21 108 20
+3 24 265 176
+3 55 346 308
+3 371 70 378
+3 152 177 238
+3 351 127 235
+3 168 244 49
+3 26 61 138
+3 305 375 331
+3 266 101 196
+3 156 295 125
+3 168 332 244
+3 308 346 351
+3 366 283 284
+3 329 332 242
+3 244 138 63
+3 177 179 39
+3 136 196 344
+3 361 87 59
+3 77 358 341
+3 373 44 45
+3 327 79 386
+3 207 262 156
+3 148 149 128
+3 55 350 346
+3 222 79 194
+3 332 297 277
+3 39 155 156
+3 147 148 365
+3 242 332 277
+3 187 77 330
+3 1 0 300
+3 372 209 44
+3 246 369 174
+3 26 349 318
+3 26 318 24
+3 332 329 324
+3 155 271 385
+3 156 125 39
+3 271 272 385
+3 364 329 368
+3 175 379 159
+3 348 66 194
+3 383 365 148
+3 136 389 222
+3 342 390 383
+3 326 327 386
+3 126 275 41
+3 323 222 389
+3 277 297 284
+3 142 266 67
+3 173 69 371
+3 75 298 54
+3 380 326 386
+3 381 319 326
+3 388 79 323
+3 308 351 358
+3 352 169 238
+3 386 79 388
+3 136 376 389
+3 239 368 242
+3 208 337 330
+3 358 75 325
+3 77 75 358
+3 253 65 66
+3 0 63 269
+3 361 341 87
+3 204 321 295
+3 266 20 107
+3 147 365 340
+3 43 174 175
+3 75 54 325
+3 128 342 383
+3 150 272 179
+3 266 196 67
+3 175 159 43
+3 312 363 384
+3 351 235 87
+3 355 150 152
+3 206 336 208
+3 214 155 385
+3 147 79 327
+3 208 336 337
+3 291 177 41
+3 363 234 61
+3 363 312 234
+3 371 69 70
+3 274 288 276
+3 159 377 160
+3 66 195 194
+3 330 331 205
+3 321 125 295
+3 342 334 390
+3 176 363 61
+3 26 176 61
+3 375 203 205
+3 379 175 371
+3 341 305 331
+3 291 41 276
+3 24 176 26
+3 262 205 295
+3 209 363 176
+3 276 41 275
+3 159 45 43
+3 335 206 207
+3 20 142 21
+3 366 284 352
+3 147 327 149
+3 39 179 271
+3 209 384 363
+3 379 377 159
+3 264 235 236
+3 333 334 342
+3 195 196 136
+3 244 324 138
+3 291 352 238
+3 1 2 3
+3 21 22 23
+3 47 48 3
+3 108 23 90
+3 48 1 3
+3 253 141 143
+3 268 270 2
+3 2 300 268
+3 97 232 312
+3 97 312 316
+3 23 22 167
+3 48 47 169
+3 6 90 258
+3 317 47 3
+3 193 4 216
+3 95 97 316
+3 4 193 296
+3 4 89 6
+3 282 347 93
+3 348 347 253
+3 316 250 248
+3 89 90 6
+3 372 373 303
+3 2 230 301
+3 143 141 260
+3 70 89 296
+3 377 378 296
+3 301 317 2
+3 282 141 347
+3 328 302 313
+3 141 253 347
+3 71 90 89
+3 328 384 372
+3 108 90 71
+3 328 303 302
+3 217 158 216
+3 373 160 158
+3 21 23 108
+3 378 70 296
+3 22 287 167
+3 312 362 316
+3 89 4 296
+3 143 260 21
+3 328 313 362
+3 1 300 2
+3 270 268 232
+3 22 260 287
+3 316 362 250
+3 84 90 23
+3 71 89 70
+3 141 282 287
+3 230 2 270
+3 193 216 158
+3 362 313 250
+3 270 232 97
+3 348 340 93
+3 84 258 90
+3 312 384 362
+3 377 296 193
+3 193 158 160
+3 373 158 303
+3 377 193 160
+3 317 3 2
+3 95 230 97
+3 230 270 97
+3 372 303 328
+3 217 303 158
+3 328 362 384
+3 248 95 316
+3 217 302 303
+3 84 23 167
+3 260 22 21
+3 260 141 287
+3 347 348 93
+3 54 56 55
+3 83 85 84
+3 56 54 109
+3 212 214 213
+3 216 218 217
+3 219 83 167
+3 230 213 231
+3 248 250 249
+3 56 220 55
+3 286 219 287
+3 85 220 258
+3 185 317 183
+3 150 183 272
+3 83 334 85
+3 336 339 338
+3 301 230 231
+3 95 248 212
+3 183 301 231
+3 109 216 4
+3 282 286 287
+3 169 47 355
+3 338 339 36
+3 335 214 212
+3 4 220 56
+3 339 313 302
+3 85 334 350
+3 47 317 185
+3 357 36 218
+3 382 206 335
+3 250 313 382
+3 334 83 219
+3 231 213 272
+3 218 36 217
+3 219 286 383
+3 220 4 6
+3 286 282 93
+3 183 317 301
+3 213 214 385
+3 167 83 84
+3 357 109 298
+3 36 357 338
+3 272 213 385
+3 55 85 350
+3 85 258 84
+3 336 382 339
+3 336 206 382
+3 249 250 382
+3 230 95 213
+3 47 185 355
+3 340 365 93
+3 248 249 212
+3 383 286 365
+3 219 383 390
+3 249 382 335
+3 218 216 109
+3 219 167 287
+3 286 93 365
+3 217 36 302
+3 54 298 109
+3 357 218 109
+3 185 150 355
+3 183 231 272
+3 36 339 302
+3 313 339 382
+3 334 219 390
+3 185 183 150
+3 85 55 220
+3 213 95 212
+3 258 220 6
+3 56 109 4
+3 335 212 249
+3 14 310 309
+4 0 1 2 3
+4 4 5 6 7
+4 8 9 10 11
+4 12 13 14 15
+4 16 17 18 19
+4 20 21 22 23
+4 24 25 26 27
+4 28 29 30 31
+4 32 33 34 35
+4 36 37 33 38
+4 39 40 41 42
+4 43 44 45 46
+4 47 3 48 49
+4 50 51 52 53
+4 54 55 56 57
+4 58 59 60 13
+4 61 62 63 64
+4 65 66 67 68
+4 69 70 71 72
+4 73 8 30 74
+4 75 76 77 78
+4 79 80 81 82
+4 83 84 85 86
+4 87 88 60 13
+4 89 7 90 72
+4 91 78 88 92
+4 93 82 94 17
+4 95 96 97 98
+4 99 100 101 102
+4 103 104 105 106
+4 107 108 23 90
+4 56 109 54 110
+4 111 7 112 113
+4 114 115 116 117
+4 91 118 110 119
+4 120 121 122 123
+4 124 125 126 123
+4 127 18 128 129
+4 130 131 132 133
+4 134 135 136 81
+4 105 27 137 74
+4 61 138 63 62
+4 122 139 140 123
+4 65 141 142 143
+4 47 144 130 133
+4 27 145 96 146
+4 147 148 149 82
+4 150 151 152 153
+4 154 155 156 157
+4 158 159 160 45
+4 161 162 163 164
+4 165 145 27 166
+4 167 12 84 86
+4 168 169 48 170
+4 5 171 166 172
+4 173 174 175 112
+4 43 176 44 46
+4 177 178 179 163
+4 180 181 182 139
+4 183 184 185 153
+4 8 186 157 11
+4 187 77 188 37
+4 173 116 112 72
+4 189 190 191 92
+4 192 186 125 42
+4 159 158 193 46
+4 79 81 194 82
+4 67 195 196 197
+4 122 198 140 182
+4 66 195 67 199
+4 150 152 179 153
+4 200 50 99 102
+4 12 201 52 53
+4 15 14 58 13
+4 202 120 140 123
+4 191 203 204 205
+4 152 151 163 153
+4 206 207 208 10
+4 176 209 44 210
+4 137 35 211 74
+4 212 213 214 215
+4 216 217 218 119
+4 219 167 83 86
+4 118 220 221 56
+4 195 199 222 197
+4 174 223 224 225
+4 137 226 165 27
+4 47 132 130 227
+4 88 228 221 86
+4 131 170 132 133
+4 106 31 137 229
+4 230 231 213 29
+4 232 233 97 234
+4 235 236 127 18
+4 48 3 1 49
+4 214 154 212 157
+4 130 237 182 161
+4 114 68 141 117
+4 125 123 192 42
+4 238 181 177 144
+4 239 240 241 242
+4 103 243 244 245
+4 193 111 159 46
+4 246 224 25 225
+4 184 161 247 151
+4 248 249 250 251
+4 94 199 93 252
+4 253 141 65 143
+4 254 255 240 256
+4 257 210 176 46
+4 258 221 12 259
+4 22 115 260 117
+4 260 115 114 117
+4 94 82 81 17
+4 96 146 145 98
+4 178 163 261 153
+4 262 263 208 10
+4 264 13 58 18
+4 24 265 25 225
+4 87 235 60 59
+4 152 177 179 163
+4 116 101 102 266
+4 34 35 33 38
+4 24 257 176 225
+4 41 125 39 42
+4 261 164 40 229
+4 94 17 16 19
+4 267 81 79 80
+4 268 269 2 270
+4 95 74 248 98
+4 179 271 272 178
+4 178 184 261 29
+4 25 27 273 62
+4 274 121 275 276
+4 277 256 242 278
+4 273 226 279 280
+4 210 145 250 166
+4 181 133 47 281
+4 282 17 94 19
+4 56 55 220 57
+4 283 284 277 285
+4 182 162 161 164
+4 286 287 219 129
+4 266 173 107 116
+4 174 224 246 225
+4 282 18 93 17
+4 51 114 94 19
+4 288 289 290 291
+4 223 257 165 225
+4 205 34 191 92
+4 47 48 168 49
+4 292 293 256 294
+4 208 207 262 10
+4 156 295 262 11
+4 149 80 147 82
+4 223 46 257 225
+4 296 111 193 113
+4 165 257 223 5
+4 297 284 169 281
+4 117 142 67 65
+4 188 75 298 299
+4 85 258 220 221
+4 170 133 180 281
+4 178 215 30 42
+4 270 104 230 64
+4 2 268 300 269
+4 184 106 301 227
+4 302 210 172 303
+4 106 237 261 229
+4 221 118 91 259
+4 300 0 2 269
+4 304 88 305 78
+4 182 131 198 306
+4 3 307 49 245
+4 55 308 220 57
+4 132 243 103 245
+4 14 309 310 311
+4 145 74 96 98
+4 178 29 183 153
+4 44 210 303 172
+4 97 312 232 234
+4 302 166 313 38
+4 1 49 3 0
+4 174 111 223 225
+4 94 81 314 17
+4 73 31 30 229
+4 177 139 291 123
+4 140 198 137 106
+4 211 192 73 186
+4 236 149 127 18
+4 100 224 174 112
+4 176 210 24 315
+4 261 29 184 237
+4 269 104 270 64
+4 292 294 280 293
+4 97 316 312 234
+4 250 145 248 251
+4 23 167 22 115
+4 135 51 50 197
+4 61 315 26 62
+4 48 169 47 170
+4 234 315 61 64
+4 178 229 40 42
+4 137 145 165 35
+4 233 61 63 64
+4 185 183 317 184
+4 46 113 5 172
+4 177 181 291 139
+4 6 258 90 7
+4 135 81 51 197
+4 265 24 25 318
+4 319 236 264 320
+4 181 162 177 144
+4 321 126 124 125
+4 8 10 263 11
+4 322 79 222 323
+4 244 103 324 243
+4 54 110 325 57
+4 73 34 211 186
+4 114 116 53 102
+4 189 211 191 192
+4 51 68 94 252
+4 317 3 47 132
+4 290 276 288 291
+4 282 94 93 252
+4 106 227 237 306
+4 91 299 110 76
+4 261 247 184 161
+4 294 292 280 279
+4 179 178 272 153
+4 150 272 183 153
+4 193 216 4 113
+4 107 90 69 71
+4 12 53 84 7
+4 33 171 302 38
+4 198 293 137 306
+4 149 326 327 320
+4 269 233 63 64
+4 49 0 244 245
+4 6 221 258 259
+4 166 171 35 38
+4 47 247 317 227
+4 291 139 276 123
+4 44 209 328 210
+4 168 170 47 245
+4 191 190 304 92
+4 181 285 291 139
+4 329 243 293 256
+4 95 316 97 96
+4 96 62 315 64
+4 60 88 15 13
+4 77 330 331 76
+4 293 103 105 306
+4 182 181 180 133
+4 137 27 165 145
+4 50 197 51 102
+4 332 297 168 170
+4 106 29 301 31
+4 34 76 33 92
+4 40 162 140 164
+4 320 80 149 17
+4 137 106 198 306
+4 30 157 73 42
+4 140 182 198 306
+4 268 233 269 64
+4 106 307 301 227
+4 142 266 20 117
+4 83 333 85 334
+4 47 170 169 281
+4 313 166 35 38
+4 325 55 54 57
+4 30 8 73 157
+4 262 205 330 263
+4 0 63 244 103
+4 125 186 156 42
+4 3 132 317 307
+4 12 112 53 7
+4 250 35 313 166
+4 94 114 51 252
+4 195 222 136 197
+4 335 10 249 11
+4 18 17 129 19
+4 111 46 223 225
+4 336 337 338 339
+4 266 68 101 102
+4 156 186 157 42
+4 13 86 12 19
+4 18 129 16 19
+4 294 255 254 256
+4 293 243 180 256
+4 137 73 140 229
+4 4 296 193 113
+4 79 194 340 82
+4 295 186 262 11
+4 305 78 341 92
+4 157 9 8 11
+4 342 286 219 343
+4 140 306 106 229
+4 196 135 344 197
+4 93 199 94 82
+4 198 182 345 180
+4 329 293 294 256
+4 5 46 111 113
+4 163 161 261 153
+4 242 254 240 256
+4 293 131 180 170
+4 216 259 4 113
+4 346 333 85 228
+4 207 10 335 11
+4 40 164 140 229
+4 246 265 174 225
+4 195 194 222 199
+4 66 347 348 199
+4 26 349 138 62
+4 333 346 85 350
+4 301 231 230 29
+4 347 199 68 252
+4 351 127 333 343
+4 95 212 248 9
+4 140 164 182 229
+4 73 192 40 42
+4 240 242 239 254
+4 114 68 51 252
+4 213 29 231 215
+4 248 145 250 146
+4 182 237 261 161
+4 52 51 12 53
+4 2 270 269 104
+4 258 6 5 7
+4 184 151 185 153
+4 81 199 94 197
+4 219 129 287 86
+4 97 96 316 64
+4 248 74 95 9
+4 183 231 301 29
+4 169 284 352 281
+4 314 80 320 17
+4 104 64 96 98
+4 217 5 216 172
+4 286 343 127 129
+4 292 198 255 256
+4 33 37 36 119
+4 121 276 290 139
+4 290 285 353 139
+4 130 131 237 227
+4 349 273 138 62
+4 169 181 238 144
+4 325 308 55 57
+4 33 76 37 119
+4 152 163 179 153
+4 111 46 193 113
+4 8 263 34 186
+4 244 324 332 243
+4 104 106 103 307
+4 109 4 216 118
+4 295 192 204 186
+4 241 242 240 277
+4 240 354 241 277
+4 204 192 191 186
+4 39 155 271 215
+4 36 37 218 119
+4 110 118 91 57
+4 353 122 290 139
+4 88 78 304 92
+4 282 287 286 129
+4 303 158 45 46
+4 314 322 267 81
+4 201 223 165 225
+4 230 31 28 98
+4 84 12 258 86
+4 28 29 213 215
+4 59 264 235 18
+4 4 6 89 7
+4 132 170 47 133
+4 51 197 68 102
+4 169 355 47 238
+4 141 68 114 252
+4 356 314 52 51
+4 35 251 8 74
+4 93 18 282 129
+4 218 37 357 299
+4 191 205 204 295
+4 20 23 107 108
+4 149 18 236 320
+4 39 215 178 42
+4 267 309 314 320
+4 47 181 169 144
+4 47 170 132 245
+4 338 337 36 339
+4 216 5 217 259
+4 335 154 212 214
+4 212 157 249 9
+4 173 112 175 72
+4 20 116 22 117
+4 87 358 341 57
+4 2 269 0 104
+4 358 87 351 57
+4 107 173 69 72
+4 217 113 158 172
+4 25 226 273 27
+4 359 329 324 280
+4 41 39 177 40
+4 360 226 137 280
+4 112 7 5 113
+4 4 220 118 56
+4 60 87 361 88
+4 257 5 165 166
+4 224 50 52 53
+4 326 236 319 320
+4 249 251 248 9
+4 218 299 109 118
+4 305 304 361 88
+4 47 133 170 281
+4 100 174 173 112
+4 121 139 122 123
+4 198 293 292 280
+4 316 362 363 146
+4 32 34 211 35
+4 226 280 273 62
+4 294 359 329 364
+4 85 228 83 86
+4 244 105 324 103
+4 40 178 177 163
+4 214 155 154 157
+4 161 151 184 153
+4 169 181 47 281
+4 302 33 36 171
+4 154 207 335 11
+4 302 171 217 172
+4 20 23 22 116
+4 293 103 324 280
+4 283 277 354 285
+4 255 180 345 278
+4 339 302 313 38
+4 154 249 212 157
+4 27 145 210 166
+4 365 93 148 82
+4 28 31 74 98
+4 282 93 347 252
+4 121 275 276 123
+4 261 178 40 164
+4 53 112 173 116
+4 142 21 143 117
+4 81 82 80 17
+4 289 291 366 285
+4 140 137 211 73
+4 360 137 198 280
+4 348 66 253 347
+4 137 27 105 280
+4 101 68 196 102
+4 138 349 367 273
+4 316 248 250 146
+4 51 81 94 197
+4 53 116 100 102
+4 140 73 211 192
+4 368 242 329 256
+4 63 104 269 64
+4 361 341 305 88
+4 182 164 237 229
+4 337 37 339 263
+4 180 243 293 170
+4 77 331 341 78
+4 140 162 182 164
+4 154 157 156 11
+4 44 46 210 172
+4 232 233 268 64
+4 263 10 262 11
+4 51 68 114 102
+4 363 315 316 146
+4 66 68 347 199
+4 140 162 40 123
+4 338 337 187 37
+4 327 320 326 80
+4 174 224 100 369
+4 100 224 99 369
+4 339 37 36 38
+4 43 111 174 265
+4 250 145 210 146
+4 265 176 43 225
+4 180 281 285 278
+4 127 342 333 343
+4 154 156 207 11
+4 69 71 90 72
+4 196 344 200 50
+4 209 210 176 315
+4 140 40 73 192
+4 165 5 12 259
+4 223 112 12 5
+4 220 118 221 6
+4 188 357 187 37
+4 188 77 75 299
+4 304 60 361 88
+4 220 228 221 57
+4 272 178 231 153
+4 182 161 261 164
+4 327 326 267 80
+4 319 58 370 320
+4 58 309 14 311
+4 211 34 73 35
+4 259 171 217 119
+4 238 163 152 144
+4 173 175 371 72
+4 12 53 51 19
+4 89 6 90 7
+4 83 343 219 86
+4 159 111 43 46
+4 182 180 198 131
+4 340 194 348 199
+4 177 162 181 139
+4 88 13 87 228
+4 303 46 44 172
+4 137 106 105 31
+4 69 90 107 72
+4 74 31 105 98
+4 372 303 373 44
+4 329 293 324 280
+4 258 12 84 7
+4 224 53 100 102
+4 47 238 355 144
+4 47 49 168 245
+4 314 51 356 81
+4 304 78 305 92
+4 136 134 374 135
+4 141 114 287 19
+4 165 27 226 257
+4 2 301 230 104
+4 90 7 23 72
+4 27 145 137 74
+4 263 186 8 11
+4 147 79 340 82
+4 359 273 279 280
+4 375 304 305 92
+4 5 7 4 113
+4 223 53 12 112
+4 137 105 293 280
+4 333 85 334 350
+4 148 93 149 17
+4 137 31 73 229
+4 34 33 32 92
+4 91 118 221 57
+4 126 125 41 123
+4 143 260 141 117
+4 330 208 262 263
+4 370 309 267 320
+4 47 185 317 247
+4 83 228 343 86
+4 304 203 191 92
+4 317 307 132 227
+4 30 29 178 229
+4 291 352 366 285
+4 91 57 221 78
+4 184 247 261 237
+4 374 134 136 376
+4 294 254 368 256
+4 371 70 89 296
+4 261 237 247 161
+4 357 218 36 37
+4 347 68 141 252
+4 59 13 264 18
+4 351 333 346 228
+4 124 120 202 123
+4 231 178 272 215
+4 316 315 96 146
+4 36 339 337 37
+4 152 144 163 151
+4 377 296 378 379
+4 255 198 345 180
+4 251 74 248 9
+4 324 103 105 280
+4 88 57 341 78
+4 354 277 240 278
+4 13 228 88 86
+4 192 123 40 42
+4 105 104 96 98
+4 100 53 173 116
+4 331 205 375 92
+4 352 181 169 281
+4 240 255 345 278
+4 355 238 152 144
+4 370 380 381 326
+4 234 61 233 64
+4 105 62 96 64
+4 30 229 178 42
+4 67 117 68 266
+4 63 103 0 104
+4 93 199 347 252
+4 301 2 317 307
+4 132 227 103 306
+4 110 57 91 78
+4 367 359 324 280
+4 293 243 132 170
+4 258 221 85 228
+4 332 256 297 170
+4 382 335 206 10
+4 250 382 313 38
+4 178 29 30 215
+4 333 334 83 219
+4 65 141 253 68
+4 53 116 114 115
+4 97 234 233 64
+4 237 164 261 229
+4 282 347 141 252
+4 326 320 267 80
+4 341 88 87 57
+4 210 328 302 313
+4 169 238 47 144
+4 34 263 8 35
+4 32 88 190 92
+4 40 162 177 123
+4 290 276 291 139
+4 231 272 213 215
+4 25 201 226 225
+4 196 200 101 50
+4 96 315 27 146
+4 342 286 127 128
+4 4 118 6 259
+4 317 247 184 227
+4 12 13 88 86
+4 188 298 357 299
+4 141 347 253 68
+4 290 274 276 121
+4 379 378 371 296
+4 63 62 105 64
+4 110 118 299 119
+4 166 171 302 172
+4 290 289 353 285
+4 358 308 325 57
+4 103 106 105 306
+4 193 46 158 113
+4 165 201 12 223
+4 383 148 286 128
+4 106 29 261 237
+4 12 5 112 7
+4 67 199 195 197
+4 177 238 291 181
+4 58 13 16 18
+4 138 367 324 280
+4 26 315 27 62
+4 37 263 330 76
+4 136 374 344 135
+4 116 173 101 266
+4 237 131 182 306
+4 35 251 250 38
+4 77 76 331 78
+4 168 297 169 170
+4 149 236 326 320
+4 71 89 90 72
+4 105 103 293 280
+4 209 328 384 372
+4 279 226 360 280
+4 354 285 277 278
+4 87 228 351 57
+4 68 199 94 252
+4 93 18 149 17
+4 368 254 242 256
+4 108 90 107 71
+4 196 50 101 102
+4 337 330 187 37
+4 328 210 302 303
+4 265 111 174 225
+4 341 78 331 92
+4 217 216 158 113
+4 345 285 180 139
+4 149 128 127 18
+4 226 257 27 225
+4 249 10 382 251
+4 105 106 137 306
+4 357 338 187 37
+4 130 144 47 151
+4 249 9 157 11
+4 373 158 160 45
+4 10 9 249 11
+4 114 115 287 19
+4 222 322 136 81
+4 173 116 101 100
+4 132 131 293 170
+4 165 223 12 5
+4 331 78 76 92
+4 345 182 122 139
+4 261 29 106 229
+4 265 25 246 318
+4 88 221 12 86
+4 101 200 99 102
+4 145 146 248 74
+4 32 33 91 92
+4 211 35 73 74
+4 177 162 163 144
+4 157 186 156 11
+4 218 217 36 119
+4 287 19 86 129
+4 145 35 137 74
+4 63 105 244 103
+4 6 118 221 259
+4 21 23 20 108
+4 321 125 124 192
+4 24 176 265 225
+4 283 354 353 285
+4 149 93 148 18
+4 165 166 5 171
+4 73 35 8 74
+4 25 27 24 225
+4 55 346 85 308
+4 36 171 33 119
+4 217 171 36 119
+4 103 307 132 245
+4 219 342 383 286
+4 371 378 70 296
+4 316 315 234 64
+4 152 238 177 163
+4 168 243 332 170
+4 138 280 105 62
+4 34 190 92 32
+4 223 5 257 172
+4 33 35 32 171
+4 18 343 13 129
+4 351 235 127 343
+4 13 12 14 16
+4 371 296 89 72
+4 97 233 232 64
+4 220 4 118 6
+4 16 14 58 309
+4 5 113 216 172
+4 73 229 30 42
+4 168 49 244 245
+4 26 138 61 62
+4 96 145 27 74
+4 210 145 27 146
+4 80 82 149 17
+4 305 331 375 92
+4 346 85 308 228
+4 0 307 103 245
+4 230 64 104 98
+4 266 196 101 68
+4 333 343 83 228
+4 226 27 25 225
+4 286 93 282 129
+4 116 53 84 115
+4 161 144 130 151
+4 6 258 5 259
+4 198 131 293 306
+4 50 224 99 102
+4 162 139 177 123
+4 220 221 56 57
+4 181 285 180 281
+4 168 48 47 170
+4 356 51 135 81
+4 103 227 106 306
+4 147 80 79 82
+4 76 78 91 92
+4 221 57 88 78
+4 156 125 295 186
+4 224 201 25 225
+4 183 301 317 184
+4 173 53 100 112
+4 226 201 165 225
+4 58 319 264 320
+4 250 35 145 251
+4 198 180 255 256
+4 230 104 301 31
+4 184 29 106 237
+4 208 263 337 10
+4 168 244 332 243
+4 236 18 264 320
+4 182 306 140 229
+4 283 353 366 285
+4 182 237 130 131
+4 284 285 352 281
+4 178 163 40 164
+4 181 144 47 133
+4 266 117 68 102
+4 308 351 346 228
+4 213 28 230 29
+4 261 237 182 164
+4 122 182 140 139
+4 22 167 287 115
+4 213 385 214 215
+4 267 380 370 326
+4 379 296 371 72
+4 297 281 170 278
+4 366 285 284 283
+4 210 46 257 172
+4 218 118 216 119
+4 146 74 145 98
+4 312 363 316 362
+4 260 114 141 117
+4 141 114 260 115
+4 329 242 332 256
+4 105 244 138 63
+4 262 186 263 11
+4 177 39 179 178
+4 94 199 68 197
+4 325 110 358 57
+4 167 84 83 86
+4 163 162 40 164
+4 136 344 196 135
+4 132 307 3 245
+4 263 251 382 10
+4 361 87 60 59
+4 60 235 87 13
+4 375 203 304 92
+4 321 124 191 192
+4 267 320 314 80
+4 47 132 3 245
+4 222 199 81 197
+4 176 210 44 46
+4 177 163 238 144
+4 299 357 109 298
+4 110 76 75 78
+4 77 341 358 78
+4 357 37 188 299
+4 224 223 201 225
+4 91 171 259 119
+4 34 263 33 76
+4 157 28 8 9
+4 36 338 357 37
+4 344 135 50 197
+4 271 178 39 215
+4 373 45 44 303
+4 105 31 104 98
+4 58 309 370 320
+4 250 210 362 146
+4 267 327 79 386
+4 211 34 190 189
+4 293 131 132 306
+4 8 35 263 251
+4 207 156 262 11
+4 40 192 140 123
+4 189 124 211 192
+4 105 104 63 64
+4 264 18 58 320
+4 272 385 213 215
+4 142 143 141 117
+4 205 295 191 186
+4 50 53 224 102
+4 148 128 149 18
+4 99 224 100 102
+4 261 184 178 153
+4 180 285 345 278
+4 4 7 89 113
+4 231 178 183 153
+4 55 346 350 85
+4 104 31 230 98
+4 222 194 79 81
+4 269 270 268 64
+4 258 5 12 7
+4 110 299 75 76
+4 5 166 257 172
+4 3 49 47 245
+4 93 94 282 17
+4 16 13 12 19
+4 292 255 294 256
+4 27 210 257 166
+4 337 263 336 10
+4 16 320 18 17
+4 332 277 297 256
+4 105 106 104 31
+4 41 40 177 123
+4 39 156 155 157
+4 161 162 182 144
+4 16 18 13 129
+4 131 227 132 306
+4 73 8 34 186
+4 89 296 4 113
+4 210 315 363 146
+4 132 307 103 227
+4 147 82 365 148
+4 339 263 382 10
+4 32 211 165 35
+4 345 180 182 139
+4 289 285 290 139
+4 106 306 237 229
+4 104 106 301 31
+4 242 277 332 256
+4 85 84 258 86
+4 103 132 293 243
+4 224 223 174 112
+4 143 21 260 117
+4 109 110 56 118
+4 336 339 382 10
+4 187 330 77 37
+4 328 362 313 210
+4 196 68 67 197
+4 58 311 370 309
+4 298 110 109 299
+4 1 0 2 300
+4 336 382 206 10
+4 372 44 209 328
+4 137 74 73 31
+4 272 178 271 215
+4 293 105 137 306
+4 145 35 250 166
+4 26 27 25 62
+4 301 106 184 29
+4 240 277 242 278
+4 263 251 35 38
+4 34 211 190 32
+4 249 382 250 251
+4 176 46 43 225
+4 294 293 329 280
+4 148 18 286 128
+4 39 178 40 42
+4 122 121 290 139
+4 261 161 163 164
+4 230 213 95 28
+4 12 223 201 53
+4 36 337 338 37
+4 75 299 77 76
+4 363 210 209 315
+4 216 118 4 259
+4 379 193 296 111
+4 163 144 161 151
+4 270 232 268 64
+4 335 249 154 11
+4 249 157 154 11
+4 130 237 247 227
+4 230 29 28 31
+4 294 255 292 387
+4 285 281 284 278
+4 263 10 8 251
+4 287 114 141 115
+4 282 252 141 19
+4 246 224 174 369
+4 47 247 130 151
+4 26 349 25 318
+4 26 25 24 318
+4 316 234 97 64
+4 182 162 181 144
+4 243 170 168 245
+4 279 329 359 280
+4 286 18 93 129
+4 84 12 167 115
+4 8 28 30 74
+4 32 15 190 88
+4 5 259 165 171
+4 22 287 260 115
+4 190 34 92 189
+4 221 228 88 57
+4 23 116 107 72
+4 353 354 240 278
+4 138 273 367 280
+4 222 81 136 197
+4 258 228 85 86
+4 107 116 173 72
+4 27 257 165 166
+4 254 255 294 387
+4 332 324 329 243
+4 230 28 95 98
+4 244 243 168 245
+4 47 355 185 151
+4 316 250 362 146
+4 40 163 177 162
+4 155 385 271 215
+4 180 256 243 170
+4 156 39 125 42
+4 248 146 95 98
+4 271 385 272 215
+4 84 23 90 7
+4 221 118 56 57
+4 364 368 329 294
+4 8 157 30 28
+4 175 159 379 111
+4 136 81 135 197
+4 204 321 191 192
+4 243 256 332 170
+4 217 259 5 171
+4 340 93 365 82
+4 180 285 181 139
+4 71 70 89 72
+4 84 53 12 115
+4 222 79 322 81
+4 58 14 16 13
+4 141 287 282 19
+4 303 45 44 46
+4 230 270 2 104
+4 33 263 37 76
+4 100 53 224 112
+4 112 116 53 7
+4 2 0 3 307
+4 105 280 27 62
+4 319 370 311 381
+4 35 171 33 38
+4 216 259 217 119
+4 337 339 336 263
+4 33 76 91 92
+4 188 37 77 299
+4 132 170 243 245
+4 248 212 249 9
+4 140 120 122 123
+4 181 162 182 139
+4 382 263 339 38
+4 22 116 23 115
+4 267 79 322 388
+4 41 123 125 42
+4 308 85 220 228
+4 284 281 297 278
+4 279 360 292 280
+4 175 112 174 111
+4 348 194 66 199
+4 18 320 149 17
+4 165 257 226 225
+4 77 299 37 76
+4 137 293 198 280
+4 51 53 50 102
+4 351 228 308 57
+4 32 165 12 259
+4 101 50 200 102
+4 184 29 178 153
+4 49 307 0 245
+4 97 64 230 98
+4 211 34 191 186
+4 282 129 18 17
+4 29 31 106 229
+4 136 135 196 197
+4 111 43 225 265
+4 205 263 34 76
+4 79 327 267 80
+4 291 181 352 285
+4 40 123 41 42
+4 67 68 66 199
+4 34 8 73 35
+4 352 285 181 281
+4 40 178 261 229
+4 43 111 225 46
+4 329 294 368 256
+4 383 365 286 148
+4 103 307 106 227
+4 193 158 216 113
+4 191 34 189 92
+4 293 132 103 306
+4 358 57 110 78
+4 140 106 137 229
+4 30 31 29 229
+4 301 106 104 307
+4 358 110 75 78
+4 52 201 224 53
+4 136 322 222 389
+4 342 219 383 390
+4 277 285 284 278
+4 16 51 314 94
+4 267 326 327 386
+4 23 7 116 72
+4 94 252 282 19
+4 356 135 134 81
+4 130 144 182 133
+4 126 41 275 123
+4 112 5 111 113
+4 134 322 136 389
+4 261 163 178 164
+4 203 205 191 92
+4 296 113 89 72
+4 53 114 51 19
+4 322 323 222 389
+4 257 166 210 172
+4 297 170 256 278
+4 105 74 137 31
+4 343 129 219 86
+4 0 103 244 245
+4 301 29 230 31
+4 88 228 87 57
+4 277 284 297 278
+4 81 199 194 82
+4 125 192 295 186
+4 142 117 67 266
+4 91 76 33 119
+4 173 371 69 72
+4 75 54 298 110
+4 166 302 210 172
+4 351 343 333 228
+4 183 29 184 153
+4 25 265 246 225
+4 324 105 138 280
+4 331 76 205 92
+4 53 116 84 7
+4 249 335 382 10
+4 380 326 267 386
+4 209 362 328 210
+4 370 381 319 326
+4 91 76 110 78
+4 218 109 216 118
+4 322 388 79 323
+4 13 343 87 228
+4 15 88 12 13
+4 353 285 354 278
+4 219 287 167 86
+4 240 256 255 278
+4 262 10 207 11
+4 308 358 351 57
+4 32 91 88 92
+4 170 281 180 278
+4 158 46 303 172
+4 352 238 169 181
+4 191 34 205 186
+4 27 96 105 62
+4 191 295 204 186
+4 267 386 79 388
+4 322 79 267 81
+4 73 74 30 31
+4 367 273 359 280
+4 324 103 293 243
+4 114 252 94 19
+4 136 376 134 389
+4 315 62 61 64
+4 8 74 251 9
+4 261 161 184 153
+4 198 180 293 131
+4 326 319 370 320
+4 214 157 212 215
+4 333 83 85 228
+4 33 171 91 119
+4 85 221 220 228
+4 159 193 379 111
+4 73 186 192 42
+4 353 289 366 285
+4 286 148 365 93
+4 10 251 249 9
+4 169 170 297 281
+4 95 146 96 98
+4 182 144 181 133
+4 182 131 130 133
+4 129 86 13 19
+4 141 252 114 19
+4 130 132 47 133
+4 148 82 93 17
+4 127 128 286 129
+4 128 18 286 129
+4 12 51 16 19
+4 314 320 16 17
+4 12 86 167 19
+4 140 192 124 123
+4 8 251 10 9
+4 239 242 368 254
+4 333 219 83 343
+4 217 302 36 171
+4 13 18 235 343
+4 208 330 337 263
+4 257 46 176 225
+4 118 259 216 119
+4 12 5 258 259
+4 304 190 60 88
+4 358 325 75 110
+4 116 7 112 72
+4 23 116 84 115
+4 32 35 165 171
+4 135 52 50 51
+4 51 114 53 102
+4 77 358 75 78
+4 27 315 96 62
+4 326 370 267 320
+4 253 66 65 68
+4 255 256 180 278
+4 209 363 362 210
+4 212 28 157 9
+4 16 129 13 19
+4 32 91 171 259
+4 149 320 327 80
+4 263 35 34 38
+4 0 269 63 104
+4 355 144 152 151
+4 361 87 341 88
+4 75 110 298 299
+4 290 291 289 139
+4 204 295 321 192
+4 297 256 277 278
+4 256 170 180 278
+4 223 111 112 5
+4 266 107 20 116
+4 28 74 95 98
+4 141 65 142 117
+4 362 250 313 210
+4 177 40 39 178
+4 211 137 165 35
+4 82 147 365 340
+4 191 211 189 34
+4 190 15 60 88
+4 111 43 174 175
+4 89 70 371 72
+4 163 151 161 153
+4 37 76 299 119
+4 330 37 337 263
+4 13 129 343 86
+4 54 109 298 110
+4 75 325 54 110
+4 356 52 135 51
+4 180 170 131 133
+4 273 27 226 62
+4 127 343 18 129
+4 27 257 24 225
+4 35 166 165 171
+4 52 314 16 51
+4 39 157 155 215
+4 345 285 353 278
+4 12 52 16 51
+4 198 106 140 306
+4 12 221 258 86
+4 251 263 382 38
+4 308 228 220 57
+4 286 128 342 383
+4 117 67 68 65
+4 73 157 8 186
+4 28 74 8 9
+4 3 0 49 307
+4 68 197 196 102
+4 293 292 256 198
+4 357 299 109 218
+4 179 163 178 153
+4 150 179 272 153
+4 257 46 223 172
+4 176 257 24 210
+4 266 67 196 68
+4 175 43 159 111
+4 235 13 59 18
+4 270 97 232 64
+4 124 121 120 123
+4 299 76 91 119
+4 348 93 340 199
+4 84 90 258 7
+4 91 259 118 119
+4 185 355 150 151
+4 313 35 250 38
+4 37 263 33 38
+4 301 184 183 29
+4 339 263 37 38
+4 231 29 178 215
+4 136 322 134 81
+4 312 362 384 363
+4 293 180 198 256
+4 68 117 114 102
+4 24 210 27 315
+4 158 113 46 172
+4 12 115 53 19
+4 53 115 114 19
+4 30 74 28 31
+4 185 184 317 247
+4 351 87 235 343
+4 124 192 125 123
+4 355 152 150 151
+4 301 104 2 307
+4 12 221 88 259
+4 212 28 213 215
+4 345 198 122 182
+4 211 73 137 74
+4 292 360 198 280
+4 138 105 63 62
+4 219 343 286 129
+4 206 208 336 10
+4 4 259 5 113
+4 377 296 379 193
+4 214 385 155 215
+4 12 15 32 88
+4 147 327 79 80
+4 217 171 5 172
+4 208 337 336 10
+4 183 272 231 153
+4 291 41 177 123
+4 73 40 140 229
+4 363 61 234 315
+4 36 302 339 38
+4 89 113 7 72
+4 30 215 157 42
+4 194 199 340 82
+4 193 160 158 159
+4 91 32 171 33
+4 60 59 235 13
+4 273 280 138 62
+4 363 234 312 316
+4 24 257 27 210
+4 235 18 127 343
+4 373 158 45 303
+4 16 309 58 320
+4 105 103 63 104
+4 287 115 167 19
+4 353 345 122 139
+4 247 161 130 151
+4 291 285 289 139
+4 313 210 250 166
+4 116 115 22 117
+4 0 104 103 307
+4 134 322 314 81
+4 248 251 145 74
+4 94 68 51 197
+4 60 15 58 13
+4 371 70 69 72
+4 248 74 146 98
+4 156 186 295 11
+4 330 263 205 76
+4 36 33 302 38
+4 18 148 286 93
+4 130 161 182 144
+4 94 199 81 82
+4 155 157 214 215
+4 129 17 282 19
+4 189 140 124 202
+4 290 274 288 276
+4 159 377 193 160
+4 182 162 140 139
+4 194 81 222 199
+4 22 21 20 117
+4 58 264 59 13
+4 379 111 296 72
+4 116 117 266 102
+4 66 194 195 199
+4 330 205 331 76
+4 16 94 314 17
+4 253 347 66 68
+4 185 247 47 151
+4 30 28 157 215
+4 2 104 0 307
+4 101 116 102 100
+4 88 91 32 259
+4 124 202 140 123
+4 84 116 23 7
+4 317 2 3 307
+4 321 295 125 192
+4 27 280 226 62
+4 313 382 339 38
+4 96 104 105 64
+4 342 334 219 390
+4 176 61 363 315
+4 26 61 176 315
+4 165 259 32 171
+4 47 144 355 151
+4 30 29 28 215
+4 95 97 230 98
+4 6 5 4 259
+4 157 186 73 42
+4 156 157 39 42
+4 149 82 148 17
+4 191 192 211 186
+4 185 151 150 153
+4 185 150 183 153
+4 96 315 316 64
+4 180 133 181 281
+4 230 97 270 64
+4 40 229 73 42
+4 157 28 212 215
+4 112 111 175 72
+4 88 221 91 259
+4 184 237 106 227
+4 353 285 345 139
+4 105 96 27 74
+4 301 307 317 227
+4 85 55 308 220
+4 77 37 330 76
+4 226 27 137 280
+4 247 237 130 161
+4 174 112 223 111
+4 126 275 124 123
+4 111 113 296 72
+4 329 279 294 280
+4 276 139 121 123
+4 12 88 32 259
+4 112 7 111 72
+4 110 299 91 119
+4 145 251 35 74
+4 332 243 329 256
+4 124 275 121 123
+4 375 205 203 92
+4 178 29 261 229
+4 379 175 72 371
+4 33 263 34 38
+4 56 110 54 57
+4 372 303 44 328
+4 302 166 210 313
+4 250 251 382 38
+4 341 331 305 92
+4 5 259 216 113
+4 68 199 67 197
+4 107 90 23 72
+4 7 113 111 72
+4 172 217 303 158
+4 184 247 185 151
+4 291 276 41 123
+4 141 68 65 117
+4 56 118 110 57
+4 24 26 176 315
+4 87 13 235 343
+4 262 295 205 186
+4 134 314 356 81
+4 279 294 359 329
+4 328 209 384 362
+4 165 35 145 166
+4 237 227 131 306
+4 24 27 26 315
+4 209 176 363 315
+4 27 315 210 146
+4 276 275 41 123
+4 266 116 20 117
+4 248 316 95 146
+4 340 199 93 82
+4 25 349 26 62
+4 96 74 105 98
+4 190 88 304 92
+4 58 18 16 320
+4 316 96 95 146
+4 159 43 45 46
+4 336 263 339 10
+4 217 172 303 302
+4 240 345 353 278
+4 213 212 95 28
+4 309 16 314 320
+4 84 167 23 115
+4 335 207 206 10
+4 159 45 158 46
+4 20 21 142 117
+4 314 94 51 81
+4 196 197 50 102
+4 285 366 284 352
+4 343 228 13 86
+4 34 263 205 186
+4 96 64 97 98
+4 260 21 22 117
+4 258 6 220 221
+4 333 342 219 343
+4 114 117 116 102
+4 180 131 182 133
+4 305 88 341 78
+4 56 4 109 118
+4 107 23 20 116
+4 247 237 184 227
+4 370 58 319 311
+4 147 149 327 80
+4 362 210 363 146
+4 260 287 141 115
+4 303 44 328 210
+4 335 212 154 249
+4 81 80 314 17
+4 39 271 179 178
+4 191 124 189 192
+4 209 362 363 384
+4 19 287 86 167
+4 91 221 88 78
+4 347 93 348 199
+4 205 263 262 186
+4 316 363 234 315
+4 237 306 182 229
+4 342 127 286 343
+4 317 132 47 227
+4 193 379 377 159
+4 221 228 258 86
+4 167 115 12 19
+4 140 139 162 123
+4 344 50 196 197
+4 242 256 240 278
+4 25 273 349 62
+4 341 57 358 78
+4 205 76 34 92
+4 216 113 217 172
+4 95 28 212 9
+4 189 140 211 124
+4 264 236 235 18
+4 183 178 231 29
+4 175 379 72 111
+4 132 131 130 227
+4 302 171 166 38
+4 317 184 301 227
+4 111 5 223 46
+4 333 334 219 342
+4 195 136 196 197
+4 109 299 110 118
+4 157 215 39 42
+4 130 247 47 227
+4 201 223 224 53
+4 244 105 138 324
+4 299 118 218 119
+4 224 53 223 112
+4 163 162 161 144
+4 287 129 282 19
+4 87 343 351 228
+4 124 140 211 192
+4 51 94 16 19
+4 324 293 329 243
+4 314 81 267 80
+4 95 74 28 9
+4 223 46 5 172
+4 291 238 352 181
+4 37 299 218 119
+389
+323
+388
+386
+380
+368
+364
+359
+274
+366
+283
+241
+239
+367
+349
+318
+369
+99
+200
+344
+374
+381
+319
+264
+59
+361
+305
+375
+204
+321
+126
+275
+203
+322
+267
+246
+309
+289
+288
+354
+376
+314
+370
+294
+279
+121
+353
+240
+254
+273
+25
+224
+50
+135
+311
+58
+60
+304
+191
+124
+290
+134
+292
+360
+120
+345
+255
+387
+226
+201
+52
+310
+14
+15
+190
+189
+202
+122
+356
+1000.0 0.45 7.5e4
diff --git a/SurgSim/Testing/Data/Textures/CheckerBoard.png b/SurgSim/Testing/Data/Textures/CheckerBoard.png
new file mode 100644
index 0000000..aafbea2
Binary files /dev/null and b/SurgSim/Testing/Data/Textures/CheckerBoard.png differ
diff --git a/SurgSim/Testing/Data/Textures/Testpattern.png b/SurgSim/Testing/Data/Textures/Testpattern.png
new file mode 100644
index 0000000..8fb6f9a
Binary files /dev/null and b/SurgSim/Testing/Data/Textures/Testpattern.png differ
diff --git a/SurgSim/Testing/MathUtilities.cpp b/SurgSim/Testing/MathUtilities.cpp
new file mode 100644
index 0000000..f390dd2
--- /dev/null
+++ b/SurgSim/Testing/MathUtilities.cpp
@@ -0,0 +1,67 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Testing/MathUtilities.h"
+
+#include "SurgSim/Math/Quaternion.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::makeRotationQuaternion;
+using SurgSim::Math::makeRigidTransform;
+
+
+namespace SurgSim
+{
+namespace Testing
+{
+
+
+SurgSim::Math::RigidTransform3d interpolatePose(
+ const Vector3d& startAngles,
+ const Vector3d& endAngles,
+ const Vector3d& startPosition,
+ const Vector3d& endPosition,
+ const double& t)
+{
+ Vector3d angles = interpolate(startAngles, endAngles, t);
+ Vector3d position = interpolate(startPosition, endPosition, t);
+ return makeRigidTransform<Quaterniond,Vector3d>(
+ Quaterniond(makeRotationQuaternion(angles.x(), Vector3d::UnitX().eval()) *
+ makeRotationQuaternion(angles.y(), Vector3d::UnitY().eval()) *
+ makeRotationQuaternion(angles.z(), Vector3d::UnitZ().eval())),
+ position);
+}
+
+template <>
+SurgSim::Math::Quaterniond interpolate(
+ const SurgSim::Math::Quaterniond& start,
+ const SurgSim::Math::Quaterniond& end,
+ const double& t)
+{
+ return SurgSim::Math::interpolate(start, end, t);
+}
+
+template <>
+SurgSim::Math::RigidTransform3d interpolate(
+ const SurgSim::Math::RigidTransform3d& start,
+ const SurgSim::Math::RigidTransform3d& end,
+ const double& t)
+{
+ return SurgSim::Math::interpolate(start, end, t);
+}
+
+}; // Testing
+}; // SurgSim
diff --git a/SurgSim/Testing/MathUtilities.h b/SurgSim/Testing/MathUtilities.h
new file mode 100644
index 0000000..8bd204f
--- /dev/null
+++ b/SurgSim/Testing/MathUtilities.h
@@ -0,0 +1,106 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_MATHUTILITIES_H
+#define SURGSIM_TESTING_MATHUTILITIES_H
+
+#include <utility>
+
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+
+#include <Eigen/Geometry>
+
+namespace SurgSim
+{
+namespace Testing
+{
+/// Does a linear interpolation between the start and the end dependent on t.
+/// \tparam T The type of value to be interpolated.
+/// \param start The start value.
+/// \param end The end value.
+/// \param t The percentage for the interpolation.
+/// \return the interpolated value.
+template <class T>
+T interpolate(const T& start, const T& end, const double& t)
+{
+ return (1 - t) * start + t * end;
+}
+
+template <class T>
+T interpolate(const std::pair<T, T>& values, const double& t)
+{
+ return interpolate<T>(values.first, values.second, t);
+}
+
+/// Specialized template to call the correct function for Quaterniond, might be superfluous,
+/// delegates to the eigen interpolation function for Quaterniond
+/// \param start The start quaternion.
+/// \param end The end quaternion.
+/// \param t The percentage for the interpolation.
+/// \return the interpolated quaternion.
+template <>
+SurgSim::Math::Quaterniond interpolate<SurgSim::Math::Quaterniond>(
+ const SurgSim::Math::Quaterniond& start,
+ const SurgSim::Math::Quaterniond& end,
+ const double& t);
+
+/// Specialized template to call the correct function for RigidTransform3d, might be superfluous,
+/// delegates to the eigen interpolation function for RigidTransform3d
+/// \param start The start quaternion.
+/// \param end The end quaternion.
+/// \param t The percentage for the interpolation.
+/// \return the interpolated RigidTransform3d.
+template <>
+SurgSim::Math::RigidTransform3d interpolate<SurgSim::Math::RigidTransform3d>(
+ const SurgSim::Math::RigidTransform3d& start,
+ const SurgSim::Math::RigidTransform3d& end,
+ const double& t);
+
+/// Does a linear interpolation on a pose, given Vector3d for angles and positions. The angles around the X, Y
+/// and Z axis are being passed in a Vector3d for a terser expression.
+/// \param startAngles The start angles in the order X/Y/Z angle axis value in radians.
+/// \param endAngles The end angles in the order X/Y/Z angle axis value in radians.
+/// \param startPosition The start position.
+/// \param endPosition The end position.
+/// \param t The percentage for the interpolation.
+/// \return The transform gained by interpolating and
+/// assembling the rotation and position values.
+SurgSim::Math::RigidTransform3d interpolatePose(
+ const SurgSim::Math::Vector3d& startAngles,
+ const SurgSim::Math::Vector3d& endAngles,
+ const SurgSim::Math::Vector3d& startPosition,
+ const SurgSim::Math::Vector3d& endPosition,
+ const double& t);
+
+
+}
+}
+
+
+namespace Eigen
+{
+
+template<class T, int Dim>
+::std::ostream& operator<<(::std::ostream& os, const Eigen::AlignedBox<T, Dim>& box)
+{
+ os << "[" << box.min().transpose() << ", " << box.max().transpose() << "]";
+ return os;
+}
+}
+#endif
+
+
diff --git a/SurgSim/Testing/MeshShapeData/InvalidMesh.ply b/SurgSim/Testing/MeshShapeData/InvalidMesh.ply
new file mode 100644
index 0000000..c0b6eb1
--- /dev/null
+++ b/SurgSim/Testing/MeshShapeData/InvalidMesh.ply
@@ -0,0 +1,12 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 1
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+end_header
+0.004801 0.000394 0.002520 0.934616 0.000000 -0.355657
diff --git a/SurgSim/Testing/MeshShapeData/staple_collision.ply b/SurgSim/Testing/MeshShapeData/staple_collision.ply
new file mode 100644
index 0000000..554e737
--- /dev/null
+++ b/SurgSim/Testing/MeshShapeData/staple_collision.ply
@@ -0,0 +1,741 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 504
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+element face 224
+property list uchar uint vertex_indices
+end_header
+0.004801 0.000394 0.002520 0.934616 0.000000 -0.355657
+0.004801 -0.000394 0.002520 0.934616 0.000000 -0.355657
+0.003882 0.000000 0.000105 0.934616 0.000000 -0.355657
+0.003892 0.000391 0.003619 -0.833198 0.000000 -0.552975
+0.003892 -0.000391 0.003619 -0.833198 0.000000 -0.552975
+0.003596 -0.000404 0.004065 -0.833198 0.000000 -0.552975
+0.003596 0.000404 0.004065 -0.833198 -0.000000 -0.552975
+0.004153 0.000404 0.004621 0.826688 -0.000000 0.562660
+0.004153 -0.000404 0.004621 0.826688 -0.000000 0.562660
+0.004592 -0.000391 0.003976 0.826688 -0.000000 0.562660
+0.004592 0.000391 0.003976 0.826688 0.000000 0.562660
+0.003596 0.000404 0.004065 -0.579143 0.000000 -0.815226
+0.003596 -0.000404 0.004065 -0.579143 0.000000 -0.815226
+0.003154 -0.000391 0.004379 -0.579143 0.000000 -0.815226
+0.003154 0.000391 0.004379 -0.579143 -0.000000 -0.815226
+0.004002 0.000391 0.003100 -0.978269 0.000000 -0.207340
+0.004002 -0.000391 0.003100 -0.978269 0.000000 -0.207340
+0.003892 -0.000391 0.003619 -0.978269 0.000000 -0.207340
+0.003892 0.000391 0.003619 -0.978269 -0.000000 -0.207340
+0.003154 0.000391 0.004379 -0.276560 0.000000 -0.960997
+0.003154 -0.000391 0.004379 -0.276560 0.000000 -0.960997
+0.002671 -0.000394 0.004518 -0.276560 0.000000 -0.960997
+0.002671 0.000394 0.004518 -0.276560 -0.000000 -0.960997
+0.003468 0.000391 0.005101 0.573863 -0.000000 0.818951
+0.003468 -0.000391 0.005101 0.573863 -0.000000 0.818951
+0.004153 -0.000404 0.004621 0.573863 -0.000000 0.818951
+0.004153 0.000404 0.004621 0.573863 0.000000 0.818951
+0.002757 0.000394 0.005300 0.269529 -0.000000 0.962992
+0.002757 -0.000394 0.005300 0.269529 -0.000000 0.962992
+0.003468 -0.000391 0.005101 0.269529 -0.000000 0.962992
+0.003468 0.000391 0.005101 0.269529 0.000000 0.962992
+0.004783 0.000391 0.003198 0.971162 -0.000000 0.238421
+0.004592 0.000391 0.003976 0.971162 -0.000000 0.238421
+0.004592 -0.000391 0.003976 0.971162 -0.000000 0.238421
+0.004783 -0.000391 0.003198 0.971162 0.000000 0.238421
+0.004013 0.000394 0.002520 -0.999820 0.000000 -0.018962
+0.004013 -0.000394 0.002520 -0.999820 0.000000 -0.018962
+0.004002 -0.000391 0.003100 -0.999820 0.000000 -0.018962
+0.004002 0.000391 0.003100 -0.999820 -0.000000 -0.018962
+0.004783 0.000391 0.003198 -0.000000 1.000000 0.000000
+0.004002 0.000391 0.003100 -0.000000 1.000000 0.000000
+0.003892 0.000391 0.003619 -0.000000 1.000000 0.000000
+0.004592 0.000391 0.003976 0.000000 1.000000 0.000000
+0.003596 0.000404 0.004065 0.011978 0.999856 -0.012000
+0.004153 0.000404 0.004621 0.011978 0.999856 -0.012000
+0.004592 0.000391 0.003976 0.011978 0.999856 -0.012000
+0.003596 0.000404 0.004065 0.011103 0.999701 -0.021770
+0.004592 0.000391 0.003976 0.011103 0.999701 -0.021770
+0.003892 0.000391 0.003619 0.011103 0.999701 -0.021770
+0.003154 0.000391 0.004379 -0.014544 0.999874 0.006325
+0.003468 0.000391 0.005101 -0.014544 0.999874 0.006325
+0.004153 0.000404 0.004621 -0.014544 0.999874 0.006325
+0.003154 0.000391 0.004379 -0.017178 0.999704 0.017209
+0.004153 0.000404 0.004621 -0.017178 0.999704 0.017209
+0.003596 0.000404 0.004065 -0.017178 0.999704 0.017209
+0.002671 0.000394 0.004518 0.004093 0.999992 -0.000450
+0.002757 0.000394 0.005300 0.004093 0.999992 -0.000450
+0.003468 0.000391 0.005101 0.004093 0.999992 -0.000450
+0.002671 0.000394 0.004518 0.005520 0.999982 -0.002401
+0.003468 0.000391 0.005101 0.005520 0.999982 -0.002401
+0.003154 0.000391 0.004379 0.005520 0.999982 -0.002401
+0.002757 -0.000394 0.005300 0.006021 -0.999982 -0.000662
+0.002671 -0.000394 0.004518 0.006021 -0.999982 -0.000662
+0.003154 -0.000391 0.004379 0.006021 -0.999982 -0.000662
+0.002757 -0.000394 0.005300 0.003762 -0.999992 -0.001636
+0.003154 -0.000391 0.004379 0.003762 -0.999992 -0.001636
+0.003468 -0.000391 0.005101 0.003762 -0.999992 -0.001636
+0.003596 -0.000404 0.004065 -0.011149 -0.999875 0.011169
+0.004153 -0.000404 0.004621 -0.011149 -0.999875 0.011169
+0.003468 -0.000391 0.005101 -0.011149 -0.999875 0.011169
+0.003596 -0.000404 0.004065 -0.022463 -0.999700 0.009769
+0.003468 -0.000391 0.005101 -0.022463 -0.999700 0.009769
+0.003154 -0.000391 0.004379 -0.022463 -0.999700 0.009769
+0.003892 -0.000391 0.003619 0.007629 -0.999859 -0.014959
+0.004592 -0.000391 0.003976 0.007629 -0.999859 -0.014959
+0.004153 -0.000404 0.004621 0.007629 -0.999859 -0.014959
+0.003892 -0.000391 0.003619 0.017496 -0.999693 -0.017527
+0.004153 -0.000404 0.004621 0.017496 -0.999693 -0.017527
+0.003596 -0.000404 0.004065 0.017496 -0.999693 -0.017527
+0.004002 -0.000391 0.003100 0.000000 -1.000000 -0.000000
+0.004783 -0.000391 0.003198 0.000000 -1.000000 -0.000000
+0.004592 -0.000391 0.003976 0.000000 -1.000000 -0.000000
+0.003892 -0.000391 0.003619 0.000000 -1.000000 -0.000000
+-0.001127 0.000391 -0.003682 -0.017168 0.999705 0.017168
+-0.000813 0.000404 -0.004125 -0.017168 0.999705 0.017168
+-0.001369 0.000404 -0.004681 -0.017168 0.999705 0.017168
+-0.001127 0.000391 -0.003682 -0.006315 0.999874 0.014541
+-0.001369 0.000404 -0.004681 -0.006315 0.999874 0.014541
+-0.001850 0.000391 -0.003996 -0.006315 0.999874 0.014541
+-0.000813 0.000404 -0.004125 0.021782 0.999701 -0.011124
+-0.000367 0.000391 -0.004420 0.021782 0.999701 -0.011124
+-0.000725 0.000391 -0.005121 0.021782 0.999701 -0.011124
+-0.000813 0.000404 -0.004125 0.011991 0.999856 -0.011991
+-0.000725 0.000391 -0.005121 0.011991 0.999856 -0.011991
+-0.001369 0.000404 -0.004681 0.011991 0.999856 -0.011991
+0.000053 0.000391 -0.005311 0.000000 1.000000 0.000000
+-0.000725 0.000391 -0.005121 0.000000 1.000000 0.000000
+-0.000367 0.000391 -0.004420 0.000000 1.000000 0.000000
+0.000152 0.000391 -0.004530 0.000000 1.000000 0.000000
+-0.000367 0.000391 -0.004420 0.551676 -0.000000 0.834059
+-0.000813 0.000404 -0.004125 0.551676 -0.000000 0.834059
+-0.000813 -0.000404 -0.004125 0.551676 -0.000000 0.834059
+-0.000367 -0.000391 -0.004420 0.551676 0.000000 0.834059
+-0.001369 0.000404 -0.004681 -0.564132 0.000000 -0.825684
+-0.000725 0.000391 -0.005121 -0.564132 0.000000 -0.825684
+-0.000725 -0.000391 -0.005121 -0.564132 0.000000 -0.825684
+-0.001369 -0.000404 -0.004681 -0.564132 0.000000 -0.825684
+-0.000813 0.000404 -0.004125 0.815843 -0.000000 0.578273
+-0.001127 0.000391 -0.003682 0.815843 -0.000000 0.578273
+-0.001127 -0.000391 -0.003682 0.815843 -0.000000 0.578273
+-0.000813 -0.000404 -0.004125 0.815843 0.000000 0.578273
+0.000152 0.000391 -0.004530 0.207340 -0.000000 0.978269
+-0.000367 0.000391 -0.004420 0.207340 -0.000000 0.978269
+-0.000367 -0.000391 -0.004420 0.207340 -0.000000 0.978269
+0.000152 -0.000391 -0.004530 0.207340 0.000000 0.978269
+-0.001127 0.000391 -0.003682 0.960844 -0.000000 0.277090
+-0.001266 0.000394 -0.003200 0.960844 -0.000000 0.277090
+-0.001266 -0.000394 -0.003200 0.960844 -0.000000 0.277090
+-0.001127 -0.000391 -0.003682 0.960844 0.000000 0.277090
+-0.001850 0.000391 -0.003996 -0.818389 0.000000 -0.574665
+-0.001369 0.000404 -0.004681 -0.818389 0.000000 -0.574665
+-0.001369 -0.000404 -0.004681 -0.818389 0.000000 -0.574665
+-0.001850 -0.000391 -0.003996 -0.818389 0.000000 -0.574665
+-0.002048 0.000394 -0.003285 -0.963343 0.000000 -0.268273
+-0.001850 0.000391 -0.003996 -0.963343 0.000000 -0.268273
+-0.001850 -0.000391 -0.003996 -0.963343 0.000000 -0.268273
+-0.002048 -0.000394 -0.003285 -0.963343 0.000000 -0.268273
+0.000053 0.000391 -0.005311 -0.237244 0.000000 -0.971450
+0.000053 -0.000391 -0.005311 -0.237244 0.000000 -0.971450
+-0.000725 -0.000391 -0.005121 -0.237244 0.000000 -0.971450
+-0.000725 0.000391 -0.005121 -0.237244 -0.000000 -0.971450
+-0.001266 0.000394 -0.003200 0.002402 0.999982 -0.005531
+-0.001127 0.000391 -0.003682 0.002402 0.999982 -0.005531
+-0.001850 0.000391 -0.003996 0.002402 0.999982 -0.005531
+-0.001266 0.000394 -0.003200 0.000445 0.999992 -0.004095
+-0.001850 0.000391 -0.003996 0.000445 0.999992 -0.004095
+-0.002048 0.000394 -0.003285 0.000445 0.999992 -0.004095
+-0.002048 -0.000394 -0.003285 0.001635 -0.999992 -0.003764
+-0.001850 -0.000391 -0.003996 0.001635 -0.999992 -0.003764
+-0.001127 -0.000391 -0.003682 0.001635 -0.999992 -0.003764
+-0.002048 -0.000394 -0.003285 0.000656 -0.999982 -0.006035
+-0.001127 -0.000391 -0.003682 0.000656 -0.999982 -0.006035
+-0.001266 -0.000394 -0.003200 0.000656 -0.999982 -0.006035
+-0.000813 -0.000404 -0.004125 -0.009742 -0.999701 0.022431
+-0.001127 -0.000391 -0.003682 -0.009742 -0.999701 0.022431
+-0.001850 -0.000391 -0.003996 -0.009742 -0.999701 0.022431
+-0.000813 -0.000404 -0.004125 -0.011148 -0.999876 0.011148
+-0.001850 -0.000391 -0.003996 -0.011148 -0.999876 0.011148
+-0.001369 -0.000404 -0.004681 -0.011148 -0.999876 0.011148
+-0.000367 -0.000391 -0.004420 0.017538 -0.999692 -0.017538
+-0.000813 -0.000404 -0.004125 0.017538 -0.999692 -0.017538
+-0.001369 -0.000404 -0.004681 0.017538 -0.999692 -0.017538
+-0.000367 -0.000391 -0.004420 0.014963 -0.999859 -0.007641
+-0.001369 -0.000404 -0.004681 0.014963 -0.999859 -0.007641
+-0.000725 -0.000391 -0.005121 0.014963 -0.999859 -0.007641
+0.000152 -0.000391 -0.004530 0.000000 -1.000000 0.000000
+-0.000367 -0.000391 -0.004420 0.000000 -1.000000 0.000000
+-0.000725 -0.000391 -0.005121 0.000000 -1.000000 0.000000
+0.000053 -0.000391 -0.005311 0.000000 -1.000000 0.000000
+-0.002077 0.000394 0.002660 -0.998925 0.000000 0.046350
+-0.002077 -0.000394 0.002660 -0.998925 0.000000 0.046350
+-0.002048 -0.000394 0.003285 -0.998925 0.000000 0.046350
+-0.002048 0.000394 0.003285 -0.998925 0.000000 0.046350
+0.000053 0.000391 0.005311 -0.237244 0.000000 0.971450
+-0.000725 0.000391 0.005121 -0.237244 0.000000 0.971450
+-0.000725 -0.000391 0.005121 -0.237244 0.000000 0.971450
+0.000053 -0.000391 0.005311 -0.237244 0.000000 0.971450
+-0.002048 0.000394 0.003285 -0.963343 0.000000 0.268273
+-0.002048 -0.000394 0.003285 -0.963343 0.000000 0.268273
+-0.001850 -0.000391 0.003996 -0.963343 0.000000 0.268273
+-0.001850 0.000391 0.003996 -0.963343 0.000000 0.268273
+-0.001850 0.000391 0.003996 -0.818389 0.000000 0.574665
+-0.001850 -0.000391 0.003996 -0.818389 0.000000 0.574665
+-0.001369 -0.000404 0.004681 -0.818389 0.000000 0.574665
+-0.001369 0.000404 0.004681 -0.818389 0.000000 0.574665
+-0.001127 0.000391 0.003682 0.960844 0.000000 -0.277090
+-0.001127 -0.000391 0.003682 0.960844 0.000000 -0.277090
+-0.001266 -0.000394 0.003200 0.960844 0.000000 -0.277090
+-0.001266 0.000394 0.003200 0.960844 0.000000 -0.277090
+-0.001289 0.000394 -0.002660 1.000000 -0.000000 0.000000
+-0.001289 0.000394 0.002660 1.000000 -0.000000 0.000000
+-0.001289 -0.000394 0.002660 1.000000 -0.000000 0.000000
+-0.001289 -0.000394 -0.002660 1.000000 -0.000000 0.000000
+-0.002077 0.000394 0.002660 -1.000000 0.000000 0.000000
+-0.002077 0.000394 -0.002660 -1.000000 0.000000 0.000000
+-0.002077 -0.000394 -0.002660 -1.000000 0.000000 0.000000
+-0.002077 -0.000394 0.002660 -1.000000 0.000000 0.000000
+-0.001289 -0.000394 -0.002660 0.000000 -1.000000 0.000000
+-0.001289 -0.000394 0.002660 0.000000 -1.000000 0.000000
+-0.002077 -0.000394 0.002660 0.000000 -1.000000 0.000000
+-0.002077 -0.000394 -0.002660 0.000000 -1.000000 0.000000
+-0.002077 0.000394 -0.002660 0.000000 1.000000 0.000000
+-0.002077 0.000394 0.002660 0.000000 1.000000 0.000000
+-0.001289 0.000394 0.002660 0.000000 1.000000 0.000000
+-0.001289 0.000394 -0.002660 0.000000 1.000000 -0.000000
+0.000152 0.000391 0.004530 0.207340 0.000000 -0.978269
+0.000152 -0.000391 0.004530 0.207340 0.000000 -0.978269
+-0.000367 -0.000391 0.004420 0.207340 0.000000 -0.978269
+-0.000367 0.000391 0.004420 0.207340 0.000000 -0.978269
+-0.000813 0.000404 0.004125 0.815843 0.000000 -0.578273
+-0.000813 -0.000404 0.004125 0.815843 0.000000 -0.578273
+-0.001127 -0.000391 0.003682 0.815843 0.000000 -0.578273
+-0.001127 0.000391 0.003682 0.815843 0.000000 -0.578273
+-0.001369 0.000404 0.004681 -0.564132 0.000000 0.825684
+-0.001369 -0.000404 0.004681 -0.564132 0.000000 0.825684
+-0.000725 -0.000391 0.005121 -0.564132 0.000000 0.825684
+-0.000725 0.000391 0.005121 -0.564132 0.000000 0.825684
+-0.000367 0.000391 0.004420 0.551676 0.000000 -0.834059
+-0.000367 -0.000391 0.004420 0.551676 0.000000 -0.834059
+-0.000813 -0.000404 0.004125 0.551676 0.000000 -0.834059
+-0.000813 0.000404 0.004125 0.551676 0.000000 -0.834059
+-0.001266 0.000394 0.003200 0.999094 0.000000 -0.042554
+-0.001266 -0.000394 0.003200 0.999094 0.000000 -0.042554
+-0.001289 -0.000394 0.002660 0.999094 0.000000 -0.042554
+-0.001289 0.000394 0.002660 0.999094 0.000000 -0.042554
+0.000053 0.000391 0.005311 0.000000 1.000000 -0.000000
+0.000152 0.000391 0.004530 0.000000 1.000000 -0.000000
+-0.000367 0.000391 0.004420 0.000000 1.000000 -0.000000
+-0.000725 0.000391 0.005121 -0.000000 1.000000 0.000000
+-0.000813 0.000404 0.004125 0.011991 0.999856 0.011991
+-0.001369 0.000404 0.004681 0.011991 0.999856 0.011991
+-0.000725 0.000391 0.005121 0.011991 0.999856 0.011991
+-0.000813 0.000404 0.004125 0.021782 0.999701 0.011124
+-0.000725 0.000391 0.005121 0.021782 0.999701 0.011124
+-0.000367 0.000391 0.004420 0.021782 0.999701 0.011124
+-0.001127 0.000391 0.003682 -0.006315 0.999874 -0.014541
+-0.001850 0.000391 0.003996 -0.006315 0.999874 -0.014541
+-0.001369 0.000404 0.004681 -0.006315 0.999874 -0.014541
+-0.001127 0.000391 0.003682 -0.017168 0.999705 -0.017168
+-0.001369 0.000404 0.004681 -0.017168 0.999705 -0.017168
+-0.000813 0.000404 0.004125 -0.017168 0.999705 -0.017168
+-0.001266 0.000394 0.003200 0.000445 0.999992 0.004095
+-0.002048 0.000394 0.003285 0.000445 0.999992 0.004095
+-0.001850 0.000391 0.003996 0.000445 0.999992 0.004095
+-0.001266 0.000394 0.003200 0.002402 0.999982 0.005531
+-0.001850 0.000391 0.003996 0.002402 0.999982 0.005531
+-0.001127 0.000391 0.003682 0.002402 0.999982 0.005531
+-0.001266 0.000394 0.003200 0.000000 1.000000 0.000000
+-0.002048 0.000394 0.003285 -0.000000 1.000000 0.000000
+-0.002048 -0.000394 0.003285 0.000000 -1.000000 0.000000
+-0.001266 -0.000394 0.003200 -0.000000 -1.000000 0.000000
+-0.002048 -0.000394 0.003285 0.000656 -0.999982 0.006035
+-0.001266 -0.000394 0.003200 0.000656 -0.999982 0.006035
+-0.001127 -0.000391 0.003682 0.000656 -0.999982 0.006035
+-0.002048 -0.000394 0.003285 0.001635 -0.999992 0.003764
+-0.001127 -0.000391 0.003682 0.001635 -0.999992 0.003764
+-0.001850 -0.000391 0.003996 0.001635 -0.999992 0.003764
+-0.000813 -0.000404 0.004125 -0.011148 -0.999876 -0.011148
+-0.001369 -0.000404 0.004681 -0.011148 -0.999876 -0.011148
+-0.001850 -0.000391 0.003996 -0.011148 -0.999876 -0.011148
+-0.000813 -0.000404 0.004125 -0.009742 -0.999701 -0.022431
+-0.001850 -0.000391 0.003996 -0.009742 -0.999701 -0.022431
+-0.001127 -0.000391 0.003682 -0.009742 -0.999701 -0.022431
+-0.000367 -0.000391 0.004420 0.014963 -0.999859 0.007641
+-0.000725 -0.000391 0.005121 0.014963 -0.999859 0.007641
+-0.001369 -0.000404 0.004681 0.014963 -0.999859 0.007641
+-0.000367 -0.000391 0.004420 0.017538 -0.999692 0.017538
+-0.001369 -0.000404 0.004681 0.017538 -0.999692 0.017538
+-0.000813 -0.000404 0.004125 0.017538 -0.999692 0.017538
+0.000152 -0.000391 0.004530 0.000000 -1.000000 0.000000
+0.000053 -0.000391 0.005311 0.000000 -1.000000 0.000000
+-0.000725 -0.000391 0.005121 0.000000 -1.000000 0.000000
+-0.000367 -0.000391 0.004420 0.000000 -1.000000 0.000000
+-0.001266 0.000394 -0.003200 0.999094 -0.000000 0.042554
+-0.001289 0.000394 -0.002660 0.999094 -0.000000 0.042554
+-0.001289 -0.000394 -0.002660 0.999094 -0.000000 0.042554
+-0.001266 -0.000394 -0.003200 0.999094 0.000000 0.042554
+-0.001266 0.000394 -0.003200 -0.000000 1.000000 0.000000
+-0.002048 0.000394 -0.003285 -0.000000 1.000000 0.000000
+-0.002048 -0.000394 -0.003285 0.000000 -1.000000 -0.000000
+-0.001266 -0.000394 -0.003200 0.000000 -1.000000 -0.000000
+-0.002077 0.000394 -0.002660 -0.998925 0.000000 -0.046350
+-0.002048 0.000394 -0.003285 -0.998925 0.000000 -0.046350
+-0.002048 -0.000394 -0.003285 -0.998925 0.000000 -0.046350
+-0.002077 -0.000394 -0.002660 -0.998925 0.000000 -0.046350
+0.003892 0.000391 -0.003619 -0.833198 0.000000 0.552975
+0.003596 0.000404 -0.004065 -0.833198 0.000000 0.552975
+0.003596 -0.000404 -0.004065 -0.833198 0.000000 0.552975
+0.003892 -0.000391 -0.003619 -0.833198 0.000000 0.552975
+0.004153 0.000404 -0.004621 0.826688 0.000000 -0.562660
+0.004592 0.000391 -0.003976 0.826688 0.000000 -0.562660
+0.004592 -0.000391 -0.003976 0.826688 0.000000 -0.562660
+0.004153 -0.000404 -0.004621 0.826688 0.000000 -0.562660
+0.003596 0.000404 -0.004065 -0.579143 0.000000 0.815226
+0.003154 0.000391 -0.004379 -0.579143 0.000000 0.815226
+0.003154 -0.000391 -0.004379 -0.579143 0.000000 0.815226
+0.003596 -0.000404 -0.004065 -0.579143 0.000000 0.815226
+0.004002 0.000391 -0.003100 -0.978269 0.000000 0.207340
+0.003892 0.000391 -0.003619 -0.978269 0.000000 0.207340
+0.003892 -0.000391 -0.003619 -0.978269 0.000000 0.207340
+0.004002 -0.000391 -0.003100 -0.978269 0.000000 0.207340
+0.003154 0.000391 -0.004379 -0.276560 0.000000 0.960997
+0.002671 0.000394 -0.004518 -0.276560 0.000000 0.960997
+0.002671 -0.000394 -0.004518 -0.276560 0.000000 0.960997
+0.003154 -0.000391 -0.004379 -0.276560 0.000000 0.960997
+0.003468 0.000391 -0.005101 0.573863 0.000000 -0.818951
+0.004153 0.000404 -0.004621 0.573863 0.000000 -0.818951
+0.004153 -0.000404 -0.004621 0.573863 0.000000 -0.818951
+0.003468 -0.000391 -0.005101 0.573863 0.000000 -0.818951
+0.002757 0.000394 -0.005300 0.269529 0.000000 -0.962992
+0.003468 0.000391 -0.005101 0.269529 0.000000 -0.962992
+0.003468 -0.000391 -0.005101 0.269529 0.000000 -0.962992
+0.002757 -0.000394 -0.005300 0.269529 0.000000 -0.962992
+0.004783 0.000391 -0.003198 0.971162 0.000000 -0.238421
+0.004783 -0.000391 -0.003198 0.971162 0.000000 -0.238421
+0.004592 -0.000391 -0.003976 0.971162 0.000000 -0.238421
+0.004592 0.000391 -0.003976 0.971162 0.000000 -0.238421
+0.004783 0.000391 -0.003198 -0.000000 1.000000 0.000000
+0.004592 0.000391 -0.003976 -0.000000 1.000000 0.000000
+0.003892 0.000391 -0.003619 -0.000000 1.000000 0.000000
+0.004002 0.000391 -0.003100 -0.000000 1.000000 0.000000
+0.003596 0.000404 -0.004065 0.011103 0.999701 0.021770
+0.003892 0.000391 -0.003619 0.011103 0.999701 0.021770
+0.004592 0.000391 -0.003976 0.011103 0.999701 0.021770
+0.003596 0.000404 -0.004065 0.011978 0.999856 0.012000
+0.004592 0.000391 -0.003976 0.011978 0.999856 0.012000
+0.004153 0.000404 -0.004621 0.011978 0.999856 0.012000
+0.003154 0.000391 -0.004379 -0.017178 0.999704 -0.017209
+0.003596 0.000404 -0.004065 -0.017178 0.999704 -0.017209
+0.004153 0.000404 -0.004621 -0.017178 0.999704 -0.017209
+0.003154 0.000391 -0.004379 -0.014544 0.999874 -0.006325
+0.004153 0.000404 -0.004621 -0.014544 0.999874 -0.006325
+0.003468 0.000391 -0.005101 -0.014544 0.999874 -0.006325
+0.002671 0.000394 -0.004518 0.005520 0.999982 0.002401
+0.003154 0.000391 -0.004379 0.005520 0.999982 0.002401
+0.003468 0.000391 -0.005101 0.005520 0.999982 0.002401
+0.002671 0.000394 -0.004518 0.004093 0.999992 0.000450
+0.003468 0.000391 -0.005101 0.004093 0.999992 0.000450
+0.002757 0.000394 -0.005300 0.004093 0.999992 0.000450
+0.002757 -0.000394 -0.005300 0.003761 -0.999992 0.001636
+0.003468 -0.000391 -0.005101 0.003761 -0.999992 0.001636
+0.003154 -0.000391 -0.004379 0.003761 -0.999992 0.001636
+0.002757 -0.000394 -0.005300 0.006021 -0.999982 0.000662
+0.003154 -0.000391 -0.004379 0.006021 -0.999982 0.000662
+0.002671 -0.000394 -0.004518 0.006021 -0.999982 0.000662
+0.003596 -0.000404 -0.004065 -0.022463 -0.999700 -0.009769
+0.003154 -0.000391 -0.004379 -0.022463 -0.999700 -0.009769
+0.003468 -0.000391 -0.005101 -0.022463 -0.999700 -0.009769
+0.003596 -0.000404 -0.004065 -0.011149 -0.999875 -0.011169
+0.003468 -0.000391 -0.005101 -0.011149 -0.999875 -0.011169
+0.004153 -0.000404 -0.004621 -0.011149 -0.999875 -0.011169
+0.003892 -0.000391 -0.003619 0.017496 -0.999693 0.017527
+0.003596 -0.000404 -0.004065 0.017496 -0.999693 0.017527
+0.004153 -0.000404 -0.004621 0.017496 -0.999693 0.017527
+0.003892 -0.000391 -0.003619 0.007629 -0.999859 0.014959
+0.004153 -0.000404 -0.004621 0.007629 -0.999859 0.014959
+0.004592 -0.000391 -0.003976 0.007629 -0.999859 0.014959
+0.004002 -0.000391 -0.003100 0.000000 -1.000000 0.000000
+0.003892 -0.000391 -0.003619 0.000000 -1.000000 0.000000
+0.004592 -0.000391 -0.003976 0.000000 -1.000000 0.000000
+0.004783 -0.000391 -0.003198 -0.000000 -1.000000 0.000000
+0.000732 0.000394 -0.004541 0.000000 1.000000 -0.000000
+0.002132 0.000394 -0.004541 0.000000 1.000000 -0.000000
+0.002132 0.000394 -0.005329 0.000000 1.000000 -0.000000
+0.000732 0.000394 -0.005329 0.000000 1.000000 -0.000000
+0.002132 0.000394 -0.004541 0.000000 0.000000 1.000000
+0.000732 0.000394 -0.004541 0.000000 0.000000 1.000000
+0.000732 -0.000394 -0.004541 0.000000 0.000000 1.000000
+0.002132 -0.000394 -0.004541 0.000000 -0.000000 1.000000
+0.002132 -0.000394 -0.004541 0.000000 -1.000000 0.000000
+0.000732 -0.000394 -0.004541 0.000000 -1.000000 0.000000
+0.000732 -0.000394 -0.005329 0.000000 -1.000000 0.000000
+0.002132 -0.000394 -0.005329 0.000000 -1.000000 0.000000
+0.000732 0.000394 -0.005329 0.000000 0.000000 -1.000000
+0.002132 0.000394 -0.005329 0.000000 0.000000 -1.000000
+0.002132 -0.000394 -0.005329 0.000000 0.000000 -1.000000
+0.000732 -0.000394 -0.005329 0.000000 0.000000 -1.000000
+0.004013 0.000394 0.002520 0.000000 0.986951 -0.161018
+0.004801 0.000394 0.002520 0.000000 0.986951 -0.161018
+0.003882 0.000000 0.000105 0.000000 0.986951 -0.161018
+0.003882 0.000000 0.000105 -0.998532 0.000000 0.054165
+0.004013 -0.000394 0.002520 -0.998532 0.000000 0.054165
+0.004013 0.000394 0.002520 -0.998532 0.000000 0.054165
+0.003882 0.000000 0.000105 0.000000 -0.986951 -0.161018
+0.004801 -0.000394 0.002520 0.000000 -0.986951 -0.161018
+0.004013 -0.000394 0.002520 0.000000 -0.986951 -0.161018
+0.002671 0.000394 0.004518 -0.042633 0.000000 -0.999091
+0.002671 -0.000394 0.004518 -0.042633 0.000000 -0.999091
+0.002132 -0.000394 0.004541 -0.042633 0.000000 -0.999091
+0.002132 0.000394 0.004541 -0.042633 -0.000000 -0.999091
+0.004801 0.000394 0.002520 0.999648 -0.000000 0.026540
+0.004783 0.000391 0.003198 0.999648 -0.000000 0.026540
+0.004783 -0.000391 0.003198 0.999648 -0.000000 0.026540
+0.004801 -0.000394 0.002520 0.999648 0.000000 0.026540
+0.002132 0.000394 0.005329 0.046350 -0.000000 0.998925
+0.002132 -0.000394 0.005329 0.046350 -0.000000 0.998925
+0.002757 -0.000394 0.005300 0.046350 -0.000000 0.998925
+0.002757 0.000394 0.005300 0.046350 0.000000 0.998925
+0.004801 0.000394 0.002520 -0.000000 0.999987 0.005172
+0.004013 0.000394 0.002520 -0.000000 0.999987 0.005172
+0.004002 0.000391 0.003100 -0.000000 0.999987 0.005172
+0.004801 0.000394 0.002520 -0.000553 0.999990 0.004410
+0.004002 0.000391 0.003100 -0.000553 0.999990 0.004410
+0.004783 0.000391 0.003198 -0.000553 0.999990 0.004410
+0.002671 0.000394 0.004518 0.000000 1.000000 0.000000
+0.002132 0.000394 0.004541 0.000000 1.000000 0.000000
+0.002132 0.000394 0.005329 0.000000 1.000000 0.000000
+0.002757 0.000394 0.005300 0.000000 1.000000 0.000000
+0.002757 -0.000394 0.005300 0.000000 -1.000000 0.000000
+0.002132 -0.000394 0.005329 0.000000 -1.000000 0.000000
+0.002132 -0.000394 0.004541 0.000000 -1.000000 0.000000
+0.002671 -0.000394 0.004518 0.000000 -1.000000 0.000000
+0.004013 -0.000394 0.002520 0.000000 -0.999990 0.004425
+0.004801 -0.000394 0.002520 0.000000 -0.999990 0.004425
+0.004783 -0.000391 0.003198 0.000000 -0.999990 0.004425
+0.004013 -0.000394 0.002520 -0.000647 -0.999986 0.005160
+0.004783 -0.000391 0.003198 -0.000647 -0.999986 0.005160
+0.004002 -0.000391 0.003100 -0.000647 -0.999986 0.005160
+0.000732 0.000394 0.004541 0.000000 1.000000 0.000000
+0.000732 0.000394 0.005329 0.000000 1.000000 0.000000
+0.002132 0.000394 0.004541 0.000000 0.000000 -1.000000
+0.002132 -0.000394 0.004541 0.000000 0.000000 -1.000000
+0.000732 -0.000394 0.004541 0.000000 0.000000 -1.000000
+0.000732 0.000394 0.004541 0.000000 0.000000 -1.000000
+0.000732 -0.000394 0.005329 0.000000 -1.000000 0.000000
+0.000732 -0.000394 0.004541 0.000000 -1.000000 0.000000
+0.000732 0.000394 0.005329 0.000000 -0.000000 1.000000
+0.000732 -0.000394 0.005329 0.000000 -0.000000 1.000000
+0.002132 -0.000394 0.005329 0.000000 -0.000000 1.000000
+0.002132 0.000394 0.005329 0.000000 0.000000 1.000000
+0.000732 0.000394 -0.004541 0.018962 -0.000000 0.999820
+0.000152 0.000391 -0.004530 0.018962 -0.000000 0.999820
+0.000152 -0.000391 -0.004530 0.018962 -0.000000 0.999820
+0.000732 -0.000394 -0.004541 0.018962 0.000000 0.999820
+0.000152 0.000391 -0.004530 -0.005172 0.999987 0.000000
+0.000732 0.000394 -0.004541 -0.005172 0.999987 0.000000
+0.000732 0.000394 -0.005329 -0.005172 0.999987 0.000000
+0.000152 0.000391 -0.004530 -0.004403 0.999990 0.000558
+0.000732 0.000394 -0.005329 -0.004403 0.999990 0.000558
+0.000053 0.000391 -0.005311 -0.004403 0.999990 0.000558
+0.000732 0.000394 -0.005329 -0.026500 0.000000 -0.999649
+0.000732 -0.000394 -0.005329 -0.026500 0.000000 -0.999649
+0.000053 -0.000391 -0.005311 -0.026500 0.000000 -0.999649
+0.000053 0.000391 -0.005311 -0.026500 -0.000000 -0.999649
+0.000053 -0.000391 -0.005311 -0.004418 -0.999990 -0.000000
+0.000732 -0.000394 -0.005329 -0.004418 -0.999990 -0.000000
+0.000732 -0.000394 -0.004541 -0.004418 -0.999990 -0.000000
+0.000053 -0.000391 -0.005311 -0.005160 -0.999986 0.000654
+0.000732 -0.000394 -0.004541 -0.005160 -0.999986 0.000654
+0.000152 -0.000391 -0.004530 -0.005160 -0.999986 0.000654
+0.000732 0.000394 0.004541 0.018962 0.000000 -0.999820
+0.000732 -0.000394 0.004541 0.018962 0.000000 -0.999820
+0.000152 -0.000391 0.004530 0.018962 0.000000 -0.999820
+0.000152 0.000391 0.004530 0.018962 0.000000 -0.999820
+0.000732 0.000394 0.005329 -0.026500 0.000000 0.999649
+0.000053 0.000391 0.005311 -0.026500 0.000000 0.999649
+0.000053 -0.000391 0.005311 -0.026500 0.000000 0.999649
+0.000732 -0.000394 0.005329 -0.026500 0.000000 0.999649
+0.000152 0.000391 0.004530 -0.004403 0.999990 -0.000558
+0.000053 0.000391 0.005311 -0.004403 0.999990 -0.000558
+0.000732 0.000394 0.005329 -0.004403 0.999990 -0.000558
+0.000152 0.000391 0.004530 -0.005172 0.999987 0.000000
+0.000732 0.000394 0.005329 -0.005172 0.999987 0.000000
+0.000732 0.000394 0.004541 -0.005172 0.999987 0.000000
+0.000053 -0.000391 0.005311 -0.005160 -0.999986 -0.000654
+0.000152 -0.000391 0.004530 -0.005160 -0.999986 -0.000654
+0.000732 -0.000394 0.004541 -0.005160 -0.999986 -0.000654
+0.000053 -0.000391 0.005311 -0.004418 -0.999990 -0.000000
+0.000732 -0.000394 0.004541 -0.004418 -0.999990 -0.000000
+0.000732 -0.000394 0.005329 -0.004418 -0.999990 -0.000000
+0.002671 0.000394 -0.004518 -0.042633 0.000000 0.999091
+0.002132 0.000394 -0.004541 -0.042633 0.000000 0.999091
+0.002132 -0.000394 -0.004541 -0.042633 0.000000 0.999091
+0.002671 -0.000394 -0.004518 -0.042633 0.000000 0.999091
+0.004801 0.000394 -0.002520 0.999648 0.000000 -0.026540
+0.004801 -0.000394 -0.002520 0.999648 0.000000 -0.026540
+0.004783 -0.000391 -0.003198 0.999648 0.000000 -0.026540
+0.004783 0.000391 -0.003198 0.999648 0.000000 -0.026540
+0.002132 0.000394 -0.005329 0.046350 0.000000 -0.998925
+0.002757 0.000394 -0.005300 0.046350 0.000000 -0.998925
+0.002757 -0.000394 -0.005300 0.046350 0.000000 -0.998925
+0.002132 -0.000394 -0.005329 0.046350 0.000000 -0.998925
+0.004002 0.000391 -0.003100 0.000000 0.999987 -0.005172
+0.004013 0.000394 -0.002520 0.000000 0.999987 -0.005172
+0.004801 0.000394 -0.002520 0.000000 0.999987 -0.005172
+0.004002 0.000391 -0.003100 -0.000553 0.999990 -0.004410
+0.004801 0.000394 -0.002520 -0.000553 0.999990 -0.004410
+0.004783 0.000391 -0.003198 -0.000553 0.999990 -0.004410
+0.002671 0.000394 -0.004518 0.000000 1.000000 -0.000000
+0.002757 0.000394 -0.005300 0.000000 1.000000 -0.000000
+0.002757 -0.000394 -0.005300 0.000000 -1.000000 0.000000
+0.002671 -0.000394 -0.004518 0.000000 -1.000000 0.000000
+0.003882 0.000000 -0.000105 -0.000000 0.986951 0.161018
+0.004801 0.000394 -0.002520 -0.000000 0.986951 0.161018
+0.004013 0.000394 -0.002520 -0.000000 0.986951 0.161018
+0.003882 0.000000 -0.000105 -0.998532 0.000000 -0.054165
+0.004013 0.000394 -0.002520 -0.998532 0.000000 -0.054165
+0.004013 -0.000394 -0.002520 -0.998532 0.000000 -0.054165
+0.004013 -0.000394 -0.002520 0.000000 -0.986951 0.161018
+0.004801 -0.000394 -0.002520 0.000000 -0.986951 0.161018
+0.003882 0.000000 -0.000105 0.000000 -0.986951 0.161018
+0.004801 -0.000394 -0.002520 0.934616 0.000000 0.355657
+0.004801 0.000394 -0.002520 0.934616 0.000000 0.355657
+0.003882 0.000000 -0.000105 0.934616 0.000000 0.355657
+0.004013 0.000394 -0.002520 -0.999820 0.000000 0.018962
+0.004002 0.000391 -0.003100 -0.999820 0.000000 0.018962
+0.004002 -0.000391 -0.003100 -0.999820 0.000000 0.018962
+0.004013 -0.000394 -0.002520 -0.999820 0.000000 0.018962
+0.004783 -0.000391 -0.003198 0.000000 -0.999990 -0.004425
+0.004801 -0.000394 -0.002520 0.000000 -0.999990 -0.004425
+0.004013 -0.000394 -0.002520 0.000000 -0.999990 -0.004425
+0.004783 -0.000391 -0.003198 -0.000647 -0.999986 -0.005160
+0.004013 -0.000394 -0.002520 -0.000647 -0.999986 -0.005160
+0.004002 -0.000391 -0.003100 -0.000647 -0.999986 -0.005160
+3 0 1 2
+3 3 4 5
+3 3 5 6
+3 7 8 9
+3 7 9 10
+3 11 12 13
+3 11 13 14
+3 15 16 17
+3 15 17 18
+3 19 20 21
+3 19 21 22
+3 23 24 25
+3 23 25 26
+3 27 28 29
+3 27 29 30
+3 31 32 33
+3 31 33 34
+3 35 36 37
+3 35 37 38
+3 39 40 41
+3 39 41 42
+3 43 44 45
+3 46 47 48
+3 49 50 51
+3 52 53 54
+3 55 56 57
+3 58 59 60
+3 61 62 63
+3 64 65 66
+3 67 68 69
+3 70 71 72
+3 73 74 75
+3 76 77 78
+3 79 80 81
+3 79 81 82
+3 83 84 85
+3 86 87 88
+3 89 90 91
+3 92 93 94
+3 95 96 97
+3 95 97 98
+3 99 100 101
+3 99 101 102
+3 103 104 105
+3 103 105 106
+3 107 108 109
+3 107 109 110
+3 111 112 113
+3 111 113 114
+3 115 116 117
+3 115 117 118
+3 119 120 121
+3 119 121 122
+3 123 124 125
+3 123 125 126
+3 127 128 129
+3 127 129 130
+3 131 132 133
+3 134 135 136
+3 137 138 139
+3 140 141 142
+3 143 144 145
+3 146 147 148
+3 149 150 151
+3 152 153 154
+3 155 156 157
+3 155 157 158
+3 159 160 161
+3 159 161 162
+3 163 164 165
+3 163 165 166
+3 167 168 169
+3 167 169 170
+3 171 172 173
+3 171 173 174
+3 175 176 177
+3 175 177 178
+3 179 180 181
+3 179 181 182
+3 183 184 185
+3 183 185 186
+3 187 188 189
+3 187 189 190
+3 191 192 193
+3 191 193 194
+3 195 196 197
+3 195 197 198
+3 199 200 201
+3 199 201 202
+3 203 204 205
+3 203 205 206
+3 207 208 209
+3 207 209 210
+3 211 212 213
+3 211 213 214
+3 215 216 217
+3 215 217 218
+3 219 220 221
+3 222 223 224
+3 225 226 227
+3 228 229 230
+3 231 232 233
+3 234 235 236
+3 237 193 192
+3 237 192 238
+3 239 189 188
+3 239 188 240
+3 241 242 243
+3 244 245 246
+3 247 248 249
+3 250 251 252
+3 253 254 255
+3 256 257 258
+3 259 260 261
+3 259 261 262
+3 263 264 265
+3 263 265 266
+3 267 268 191
+3 267 191 194
+3 269 270 187
+3 269 187 190
+3 271 272 273
+3 271 273 274
+3 275 276 277
+3 275 277 278
+3 279 280 281
+3 279 281 282
+3 283 284 285
+3 283 285 286
+3 287 288 289
+3 287 289 290
+3 291 292 293
+3 291 293 294
+3 295 296 297
+3 295 297 298
+3 299 300 301
+3 299 301 302
+3 303 304 305
+3 303 305 306
+3 307 308 309
+3 307 309 310
+3 311 312 313
+3 314 315 316
+3 317 318 319
+3 320 321 322
+3 323 324 325
+3 326 327 328
+3 329 330 331
+3 332 333 334
+3 335 336 337
+3 338 339 340
+3 341 342 343
+3 344 345 346
+3 347 348 349
+3 347 349 350
+3 351 352 353
+3 351 353 354
+3 355 356 357
+3 355 357 358
+3 359 360 361
+3 359 361 362
+3 363 364 365
+3 363 365 366
+3 367 368 369
+3 370 371 372
+3 373 374 375
+3 376 377 378
+3 376 378 379
+3 380 381 382
+3 380 382 383
+3 384 385 386
+3 384 386 387
+3 388 389 390
+3 391 392 393
+3 394 395 396
+3 394 396 397
+3 398 399 400
+3 398 400 401
+3 402 403 404
+3 405 406 407
+3 408 409 396
+3 408 396 395
+3 410 411 412
+3 410 412 413
+3 400 399 414
+3 400 414 415
+3 416 417 418
+3 416 418 419
+3 420 421 422
+3 420 422 423
+3 424 425 426
+3 427 428 429
+3 430 431 432
+3 430 432 433
+3 434 435 436
+3 437 438 439
+3 440 441 442
+3 440 442 443
+3 444 445 446
+3 444 446 447
+3 448 449 450
+3 451 452 453
+3 454 455 456
+3 457 458 459
+3 460 461 462
+3 460 462 463
+3 464 465 466
+3 464 466 467
+3 468 469 470
+3 468 470 471
+3 472 473 474
+3 475 476 477
+3 478 479 353
+3 478 353 352
+3 480 481 359
+3 480 359 362
+3 482 483 484
+3 485 486 487
+3 488 489 490
+3 491 492 493
+3 494 495 496
+3 494 496 497
+3 498 499 500
+3 501 502 503
diff --git a/SurgSim/Testing/MlcpIO/CMakeLists.txt b/SurgSim/Testing/MlcpIO/CMakeLists.txt
new file mode 100644
index 0000000..8eb1e53
--- /dev/null
+++ b/SurgSim/Testing/MlcpIO/CMakeLists.txt
@@ -0,0 +1,55 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+)
+
+set(MLCP_IO_SOURCES
+ MlcpTestData.cpp
+ ReadText.cpp
+ TextLabels.cpp
+ WriteText.cpp
+)
+
+set(MLCP_IO_HEADERS
+ MlcpTestData.h
+ ReadText.h
+ TextLabels.h
+ WriteText.h
+)
+
+# The headers etc. for this do not need to be shipped, so do NOT use
+# surgsim_add_library here.
+
+surgsim_add_library(
+ MlcpTestIO
+ "${MLCP_IO_SOURCES}"
+ "${MLCP_IO_HEADERS}"
+ "SurgSim/MlcpTestIO"
+)
+
+SET(LIBS
+ SurgSimMath
+ SurgSimTesting
+)
+
+target_link_libraries(MlcpTestIO ${LIBS})
+
+
+surgsim_show_ide_folders("${MLCP_IO_SOURCES}" "${MLCP_IO_HEADERS}")
+
+# Put MlcpTestIO into folder "Testing"
+set_target_properties(MlcpTestIO PROPERTIES FOLDER "Testing")
diff --git a/SurgSim/Testing/MlcpIO/MlcpTestData.cpp b/SurgSim/Testing/MlcpIO/MlcpTestData.cpp
new file mode 100644
index 0000000..d991bcd
--- /dev/null
+++ b/SurgSim/Testing/MlcpIO/MlcpTestData.cpp
@@ -0,0 +1,36 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <iomanip>
+
+#include "SurgSim/Testing/MlcpIO/MlcpTestData.h"
+#include "SurgSim/Testing/MlcpIO/ReadText.h"
+
+std::shared_ptr<MlcpTestData> loadTestData(const std::string& fileName)
+{
+ std::shared_ptr<MlcpTestData> data = std::make_shared<MlcpTestData>();
+ if (! readMlcpTestDataAsText("MlcpTestData/" + fileName, data.get()))
+ {
+ data.reset();
+ }
+ return data;
+}
+
+std::string getTestFileName(const std::string& prefix, int index, const std::string& suffix)
+{
+ std::ostringstream stream;
+ stream << prefix << std::setfill('0') << std::setw(3) << index << suffix;
+ return stream.str();
+}
diff --git a/SurgSim/Testing/MlcpIO/MlcpTestData.h b/SurgSim/Testing/MlcpIO/MlcpTestData.h
new file mode 100644
index 0000000..2f6c754
--- /dev/null
+++ b/SurgSim/Testing/MlcpIO/MlcpTestData.h
@@ -0,0 +1,80 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_MLCPIO_MLCPTESTDATA_H
+#define SURGSIM_TESTING_MLCPIO_MLCPTESTDATA_H
+
+#include <memory>
+#include <string>
+
+#include <vector>
+#include <Eigen/Core>
+#include "SurgSim/Math/MlcpProblem.h"
+#include "SurgSim/Math/MlcpConstraintType.h"
+
+
+struct MlcpTestData
+{
+public:
+ // Description of an MLCP problem.
+ SurgSim::Math::MlcpProblem problem;
+
+ // Expected output for the solver (as recorded from old Gauss-Seidel code).
+ Eigen::VectorXd expectedLambda;
+
+ // Other metadata
+ std::string description;
+ std::vector<std::string> flags;
+ int numDegreesOfFreedom;
+
+ MlcpTestData() :
+ numDegreesOfFreedom(-1)
+ {
+ }
+
+ MlcpTestData(const MlcpTestData& other) :
+ problem(other.problem),
+ expectedLambda(other.expectedLambda),
+ description(other.description),
+ flags(other.flags),
+ numDegreesOfFreedom(other.numDegreesOfFreedom)
+ {
+ }
+
+ MlcpTestData& operator= (const MlcpTestData& other)
+ {
+ problem = other.problem;
+ expectedLambda = other.expectedLambda;
+
+ description = other.description;
+ flags = other.flags;
+ numDegreesOfFreedom = other.numDegreesOfFreedom;
+
+ return *this;
+ }
+
+ size_t getSize() const
+ {
+ return (problem.b.rows() >= 0) ? static_cast<size_t>(problem.b.rows()) : 0;
+ }
+};
+
+const MlcpTestData* getTestProblem1();
+
+std::shared_ptr<MlcpTestData> loadTestData(const std::string& fileName);
+
+std::string getTestFileName(const std::string& prefix, int index, const std::string& suffix);
+
+#endif // SURGSIM_TESTING_MLCPIO_MLCPTESTDATA_H
diff --git a/SurgSim/Testing/MlcpIO/ReadText.cpp b/SurgSim/Testing/MlcpIO/ReadText.cpp
new file mode 100644
index 0000000..746aee5
--- /dev/null
+++ b/SurgSim/Testing/MlcpIO/ReadText.cpp
@@ -0,0 +1,509 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Testing/MlcpIO/ReadText.h"
+
+#include <string>
+#include <vector>
+#include <stdio.h>
+
+#include <Eigen/Core>
+
+#include "SurgSim/Math/MlcpConstraintType.h"
+#include "SurgSim/Math/MlcpConstraintTypeName.h"
+
+#include "SurgSim/Testing/MlcpIO/MlcpTestData.h"
+#include "SurgSim/Testing/MlcpIO/TextLabels.h"
+
+
+// input helpers
+
+static std::string getRawLine(FILE* in)
+{
+ char buffer[1024];
+ if (! fgets(buffer, sizeof(buffer), in))
+ {
+ return "";
+ }
+ std::string line(buffer);
+ if ((line.length() > 0) && (line[line.length()-1] == '\n'))
+ {
+ line.resize(line.length()-1); // remove the last character
+ }
+ if ((line.length() > 0) && (line[line.length()-1] == '\r'))
+ {
+ line.resize(line.length()-1); // remove the last character
+ }
+ return line;
+}
+
+static bool getLine(const std::string& fileName, FILE* in, std::string* line, bool skipEmptyLines = true)
+{
+ if (ferror(in) || feof(in))
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+ *line = getRawLine(in);
+ while (skipEmptyLines && (line->length() == 0))
+ {
+ if (ferror(in) || feof(in))
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+ *line = getRawLine(in);
+ }
+ if (ferror(in))
+ {
+ fprintf(stderr, "Unexpected error in file '%s'\n", fileName.c_str());
+ return false;
+ }
+ return true;
+}
+
+static bool readInt(const std::string& fileName, FILE* in, const char* label, int* value)
+{
+ if (ferror(in) || feof(in))
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+ std::string format = std::string(" ") + label + " %d";
+ if (fscanf(in, format.c_str(), value) != 1)
+ {
+ fprintf(stderr, "Bad integer input in '%s'\n near text '%s'\n", fileName.c_str(), getRawLine(in).c_str());
+ return false;
+ }
+ if (ferror(in))
+ {
+ fprintf(stderr, "Unexpected error in file '%s'\n", fileName.c_str());
+ return false;
+ }
+ return true;
+}
+
+static bool readEigenRowVector(const std::string& fileName, FILE* in, const char* label, Eigen::RowVectorXd* vector)
+{
+ // Read the label.
+ {
+ if (ferror(in) || feof(in))
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+
+ std::string format = std::string(" ") + label + " (";
+ if (fscanf(in, format.c_str()) != 0)
+ {
+ fprintf(stderr, "Bad label for Eigen input in '%s'\n near text '%s'\n",
+ fileName.c_str(), getRawLine(in).c_str());
+ return false;
+ }
+ }
+
+ // Read elements until ')' is found.
+ std::vector<double> values;
+ while (true)
+ {
+ // Scan for a non-whitespace character and check it.
+ while (true)
+ {
+ if (ferror(in) || feof(in))
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+
+ int nextCharacter = getc(in);
+ if (nextCharacter == EOF) // either EOF or read error
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+ else if (nextCharacter == ')')
+ {
+ // We're done reading input.
+ if (ferror(in))
+ {
+ fprintf(stderr, "Unexpected error in file '%s'\n", fileName.c_str());
+ return false;
+ }
+
+ vector->resize(values.size());
+ for (size_t i = 0; i < values.size(); i++)
+ {
+ (*vector)[i] = values[i];
+ }
+ return true;
+ }
+ else if (! isspace(nextCharacter))
+ {
+ // The next character isn't ')'. Pretend we never read it, and go on reading data.
+ ungetc(nextCharacter, in);
+ break;
+ }
+ }
+
+ if (ferror(in) || feof(in))
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+
+ // Ready to read the next element.
+ double nextValue = 0;
+ if (fscanf(in, " %lg", &nextValue) != 1)
+ {
+ fprintf(stderr, "Bad data for Eigen input in '%s'\n near text '%s'\n",
+ fileName.c_str(), getRawLine(in).c_str());
+ return false;
+ }
+
+ values.push_back(nextValue);
+ }
+}
+
+static bool readEigenVector(const std::string& fileName, FILE* in, const char* label, Eigen::VectorXd* vector)
+{
+ Eigen::RowVectorXd rowVector;
+ if (! readEigenRowVector(fileName, in, label, &rowVector))
+ {
+ return false;
+ }
+ *vector = rowVector.transpose();
+ return true;
+}
+
+static bool readEigenMatrix(const std::string& fileName, FILE* in, const char* label, Eigen::MatrixXd* matrix)
+{
+ // Read the label.
+ {
+ if (ferror(in) || feof(in))
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+
+ std::string format = std::string(" ") + label + " (";
+ if (fscanf(in, format.c_str()) != 0)
+ {
+ fprintf(stderr, "Bad label for Eigen input in '%s'\n near text '%s'\n",
+ fileName.c_str(), getRawLine(in).c_str());
+ return false;
+ }
+ }
+
+ // Read rows until ')' is found.
+ bool firstRow = true;
+ matrix->resize(0, 0);
+ while (true)
+ {
+ // Scan for a non-whitespace character and check it.
+ while (true)
+ {
+ if (ferror(in) || feof(in))
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+
+ int nextCharacter = getc(in);
+ if (nextCharacter == EOF) // either EOF or read error
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+ else if (nextCharacter == ')')
+ {
+ // We're done reading input.
+ if (ferror(in))
+ {
+ fprintf(stderr, "Unexpected error in file '%s'\n", fileName.c_str());
+ return false;
+ }
+ return true;
+ }
+ else if (! isspace(nextCharacter))
+ {
+ // The next character isn't ')'. Pretend we never read it, and go on reading data.
+ ungetc(nextCharacter, in);
+ break;
+ }
+ }
+
+ if (ferror(in) || feof(in))
+ {
+ fprintf(stderr, "Unexpected error or EOF in file '%s'\n", fileName.c_str());
+ return false;
+ }
+
+ // Ready to read the next row.
+ Eigen::RowVectorXd rowVector;
+ if (! readEigenRowVector(fileName, in, "", &rowVector))
+ {
+ return false;
+ }
+ if (firstRow)
+ {
+ *matrix = rowVector;
+ firstRow = false;
+ }
+ else
+ {
+ ptrdiff_t numCols = matrix->cols();
+ ptrdiff_t newNumRows = matrix->rows() + 1;
+ if (rowVector.cols() != numCols)
+ {
+ fprintf(stderr, "Inconsistent number of columns for Eigen matrix (%lld vs %lld)\n in file '%s'\n",
+ static_cast<long long int>(numCols), static_cast<long long int>(rowVector.cols()), // NOLINT
+ fileName.c_str());
+ return false;
+ }
+ matrix->conservativeResize(newNumRows, numCols);
+ matrix->row(newNumRows-1) = rowVector;
+ }
+ }
+}
+
+static bool extractWordList(const std::string& fileName, const std::string& line, const char* label,
+ std::vector<std::string>* words)
+{
+ std::string labelString(label);
+ if (line.substr(0, labelString.length()) != labelString)
+ {
+ fprintf(stderr,
+ "Unexpected input line: '%s'\n"
+ " expected: '%s'...\n"
+ " in file '%s'\n",
+ line.c_str(), label, fileName.c_str());
+ return false;
+ }
+
+ words->clear();
+ size_t start = line.find_first_not_of(" \t", labelString.length());
+ while (start != std::string::npos)
+ {
+ size_t end = line.find_first_of(" \t", start+1);
+ if (end == std::string::npos)
+ {
+ words->push_back(line.substr(start));
+ break;
+ }
+ words->push_back(line.substr(start, end - start));
+ start = line.find_first_not_of(" \t", end+1);
+ }
+ return true;
+}
+
+static bool readWordList(const std::string& fileName, FILE* in, const char* label, std::vector<std::string>* words)
+{
+ std::string line;
+ if (! getLine(fileName, in, &line))
+ {
+ return false;
+ }
+
+ return extractWordList(fileName, line, label, words);
+}
+
+static bool checkInputLine(const std::string& fileName, const std::string& expected, const std::string& line)
+{
+ if (line != expected)
+ {
+ fprintf(stderr,
+ "Unexpected input line: '%s'\n"
+ " expected: '%s'\n"
+ " in file '%s'\n",
+ line.c_str(), expected.c_str(), fileName.c_str());
+ return false;
+ }
+ return true;
+}
+
+
+
+// actually write the data
+
+bool readMlcpTestDataAsText(const std::string& fileName, MlcpTestData* testData)
+{
+ FILE* in = fopen(fileName.c_str(), "rt");
+ if (! in)
+ {
+ fprintf(stderr, "File '%s' could not be opened to read the current MLCP for GTest\n", fileName.c_str());
+ return false;
+ }
+
+ testData->description.clear();
+
+ std::string line;
+ if (! getLine(fileName, in, &line))
+ {
+ fprintf(stderr, "Failed to read first line from the file.\n");
+ return false;
+ }
+
+ while ((line.length() > 0) && (line[0] == '#'))
+ {
+ if (testData->description.length() > 0)
+ {
+ testData->description += "\n";
+ }
+ if ((line.length() > 1) && (line[1] == ' '))
+ {
+ testData->description += line.substr(2);
+ }
+ else
+ {
+ testData->description += line.substr(1);
+ }
+
+ if (! getLine(fileName, in, &line))
+ {
+ fprintf(stderr, "Failed to read header comment block from the file.\n");
+ return false;
+ }
+ }
+
+ // We have already read the next line, so we just need to parse it.
+ if (! extractWordList(fileName, line, TEXT_LABEL_FLAGS_LIST, &(testData->flags)))
+ {
+ return false;
+ }
+
+ if (! readInt(fileName, in, TEXT_LABEL_NUM_DEGREES_OF_FREEDOM, &(testData->numDegreesOfFreedom)))
+ {
+ return false;
+ }
+
+ int numConstraints;
+ if (! readInt(fileName, in, TEXT_LABEL_NUM_CONSTRAINTS, &numConstraints))
+ {
+ return false;
+ }
+
+ int numAtomicConstraints;
+ if (! readInt(fileName, in, TEXT_LABEL_NUM_ATOMIC_CONSTRAINTS, &numAtomicConstraints))
+ {
+ return false;
+ }
+
+ std::vector<std::string> constraintTypeNames;
+ if (! readWordList(fileName, in, TEXT_LABEL_CONSTRAINT_TYPES_LIST, &constraintTypeNames))
+ {
+ return false;
+ }
+ if (static_cast<int>(constraintTypeNames.size()) != numConstraints)
+ {
+ fprintf(stderr, "Expected %d constraint types, saw %u\n in file '%s'\n",
+ numConstraints, static_cast<unsigned int>(constraintTypeNames.size()), fileName.c_str());
+ return false;
+ }
+ testData->problem.constraintTypes.resize(numConstraints);
+ for (size_t i = 0; i < testData->problem.constraintTypes.size(); ++i)
+ {
+ SurgSim::Math::MlcpConstraintType currentType =
+ SurgSim::Math::getMlcpConstraintTypeValue(constraintTypeNames[i]);
+ if (currentType == SurgSim::Math::MLCP_INVALID_CONSTRAINT)
+ {
+ fprintf(stderr, "Unexpected constraint type string: '%s'\n in file '%s'\n",
+ constraintTypeNames[i].c_str(), fileName.c_str());
+ return false;
+ }
+ testData->problem.constraintTypes[i] = currentType;
+ }
+
+ {
+ Eigen::VectorXd b;
+ Eigen::MatrixXd A;
+ Eigen::VectorXd mu;
+ Eigen::VectorXd expectedLambda;
+
+ if (! readEigenVector(fileName, in, TEXT_LABEL_E_VIOLATIONS_VECTOR, &b))
+ {
+ return false;
+ }
+
+ if (! readEigenMatrix(fileName, in, TEXT_LABEL_HCHt_MLCP_MATRIX, &A))
+ {
+ return false;
+ }
+
+ if (! readEigenVector(fileName, in, TEXT_LABEL_MU_FRICTION_VECTOR, &mu))
+ {
+ return false;
+ }
+
+ if (! readEigenVector(fileName, in, TEXT_LABEL_LAMBDA_VECTOR, &expectedLambda))
+ {
+ return false;
+ }
+
+ testData->problem.b = b;
+ testData->problem.A = A;
+ testData->problem.mu = mu;
+ testData->expectedLambda = expectedLambda;
+ }
+
+ if ((testData->problem.b.rows() != numAtomicConstraints) || (testData->problem.b.cols() != 1))
+ {
+ fprintf(stderr, "Expected %dx%d vector E, saw %dx%d\n in file '%s'\n",
+ numAtomicConstraints, 1, static_cast<int>(testData->problem.b.rows()),
+ static_cast<int>(testData->problem.b.cols()), fileName.c_str());
+ return false;
+ }
+ if ((testData->problem.A.rows() != numAtomicConstraints) || (testData->problem.A.cols() != numAtomicConstraints))
+ {
+ fprintf(stderr, "Expected %dx%d matrix A, saw %dx%d\n in file '%s'\n",
+ numAtomicConstraints, numAtomicConstraints, static_cast<int>(testData->problem.A.rows()),
+ static_cast<int>(testData->problem.A.cols()), fileName.c_str());
+ return false;
+ }
+ if ((testData->problem.mu.rows() != numConstraints) || (testData->problem.mu.cols() != 1))
+ {
+ fprintf(stderr, "Expected %dx%d vector mu, saw %dx%d\n in file '%s'\n",
+ numConstraints, 1, static_cast<int>(testData->problem.mu.rows()),
+ static_cast<int>(testData->problem.mu.cols()), fileName.c_str());
+ return false;
+ }
+ if ((testData->expectedLambda.rows() != numAtomicConstraints) || (testData->expectedLambda.cols() != 1))
+ {
+ fprintf(stderr, "Expected %dx%d vector lambda, saw %dx%d\n in file '%s'\n",
+ numAtomicConstraints, 1, static_cast<int>(testData->expectedLambda.rows()),
+ static_cast<int>(testData->expectedLambda.cols()), fileName.c_str());
+ return false;
+ }
+
+ if (! getLine(fileName, in, &line) || ! checkInputLine(fileName, TEXT_LABEL_END_OF_FILE, line))
+ {
+ fprintf(stderr, "Expected %d constraint types, saw %u\n", numConstraints,
+ static_cast<unsigned int>(constraintTypeNames.size()));
+ return false;
+ }
+
+ if (ferror(in))
+ {
+ fprintf(stderr, "Unexpected error near the end of file '%s'\n", fileName.c_str());
+ return false;
+
+ }
+ if (fclose(in))
+ {
+ fprintf(stderr, "Error closing file '%s' for reading.\n", fileName.c_str());
+ return false;
+ }
+
+ return true;
+}
diff --git a/SurgSim/Testing/MlcpIO/ReadText.h b/SurgSim/Testing/MlcpIO/ReadText.h
new file mode 100644
index 0000000..72e233e
--- /dev/null
+++ b/SurgSim/Testing/MlcpIO/ReadText.h
@@ -0,0 +1,24 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_MLCPIO_READTEXT_H
+#define SURGSIM_TESTING_MLCPIO_READTEXT_H
+
+#include <string>
+#include "SurgSim/Testing/MlcpIO/MlcpTestData.h"
+
+bool readMlcpTestDataAsText(const std::string& fileName, MlcpTestData* testData);
+
+#endif // SURGSIM_TESTING_MLCPIO_READTEXT_H
diff --git a/SurgSim/Testing/MlcpIO/TextLabels.cpp b/SurgSim/Testing/MlcpIO/TextLabels.cpp
new file mode 100644
index 0000000..128e6fe
--- /dev/null
+++ b/SurgSim/Testing/MlcpIO/TextLabels.cpp
@@ -0,0 +1,27 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Testing/MlcpIO/TextLabels.h"
+
+const char* const TEXT_LABEL_FLAGS_LIST = "flags =";
+const char* const TEXT_LABEL_NUM_DEGREES_OF_FREEDOM = "total degrees of freedom =";
+const char* const TEXT_LABEL_NUM_CONSTRAINTS = "number of constraints =";
+const char* const TEXT_LABEL_NUM_ATOMIC_CONSTRAINTS = "number of constraint DOFs (\"atomic constraints\") =";
+const char* const TEXT_LABEL_CONSTRAINT_TYPES_LIST = "constraint types =";
+const char* const TEXT_LABEL_E_VIOLATIONS_VECTOR = "E =";
+const char* const TEXT_LABEL_HCHt_MLCP_MATRIX = "HCHt =";
+const char* const TEXT_LABEL_MU_FRICTION_VECTOR = "friction =";
+const char* const TEXT_LABEL_LAMBDA_VECTOR = "lambda =";
+const char* const TEXT_LABEL_END_OF_FILE = "== END ==";
\ No newline at end of file
diff --git a/SurgSim/Testing/MlcpIO/TextLabels.h b/SurgSim/Testing/MlcpIO/TextLabels.h
new file mode 100644
index 0000000..db5c96c
--- /dev/null
+++ b/SurgSim/Testing/MlcpIO/TextLabels.h
@@ -0,0 +1,30 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_MLCPIO_TEXTLABELS_H
+#define SURGSIM_TESTING_MLCPIO_TEXTLABELS_H
+
+extern const char* const TEXT_LABEL_FLAGS_LIST;
+extern const char* const TEXT_LABEL_NUM_DEGREES_OF_FREEDOM;
+extern const char* const TEXT_LABEL_NUM_CONSTRAINTS;
+extern const char* const TEXT_LABEL_NUM_ATOMIC_CONSTRAINTS;
+extern const char* const TEXT_LABEL_CONSTRAINT_TYPES_LIST;
+extern const char* const TEXT_LABEL_E_VIOLATIONS_VECTOR;
+extern const char* const TEXT_LABEL_HCHt_MLCP_MATRIX;
+extern const char* const TEXT_LABEL_MU_FRICTION_VECTOR;
+extern const char* const TEXT_LABEL_LAMBDA_VECTOR;
+extern const char* const TEXT_LABEL_END_OF_FILE;
+
+#endif // SURGSIM_TESTING_MLCPIO_TEXTLABELS_H
diff --git a/SurgSim/Testing/MlcpIO/WriteText.cpp b/SurgSim/Testing/MlcpIO/WriteText.cpp
new file mode 100644
index 0000000..951abaf
--- /dev/null
+++ b/SurgSim/Testing/MlcpIO/WriteText.cpp
@@ -0,0 +1,151 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Testing/MlcpIO/WriteText.h"
+
+#include <string>
+#include <vector>
+#include <stdio.h>
+
+#include <Eigen/Core>
+
+#include "SurgSim/Math/MlcpConstraintType.h"
+#include "SurgSim/Math/MlcpConstraintTypeName.h"
+
+#include "SurgSim/Testing/MlcpIO/MlcpTestData.h"
+#include "SurgSim/Testing/MlcpIO/TextLabels.h"
+
+
+// output helpers
+
+static void writeCommentBlock(FILE* out, const std::string& text)
+{
+ const std::string prefix = "# ";
+ const std::string suffix = "";
+
+ size_t start = 0;
+ while (start < text.length())
+ {
+ size_t next = text.find_first_of('\n', start);
+ if (next == std::string::npos)
+ {
+ break;
+ }
+ fprintf(out, "%s%s%s\n", prefix.c_str(), text.substr(start, next - start).c_str(), suffix.c_str());
+ start = next+1;
+ }
+ if (start >= text.length())
+ {
+ fprintf(out, "%s%s\n", prefix.c_str(), suffix.c_str());
+ }
+ else
+ {
+ fprintf(out, "%s%s%s\n", prefix.c_str(), text.substr(start).c_str(), suffix.c_str());
+ }
+}
+
+static void writeEigenRowVector(FILE* out, const char* label, const Eigen::RowVectorXd& vector)
+{
+ fprintf(out, "%s (", label);
+ for (int i = 0; i < vector.cols(); i++)
+ {
+ fprintf(out, " %.20g", vector[i]);
+ }
+ fprintf(out, ")\n");
+}
+
+static void writeEigenVector(FILE* out, const char* label, const Eigen::VectorXd& vector)
+{
+ writeEigenRowVector(out, label, vector.transpose());
+}
+
+static void writeEigenMatrix(FILE* out, const char* label, const Eigen::MatrixXd& matrix)
+{
+ fprintf(out, "%s (\n", label);
+ for (int i = 0; i < matrix.rows(); ++i)
+ {
+ writeEigenRowVector(out, " ", matrix.row(i));
+ }
+ fprintf(out, ")\n");
+}
+
+
+
+
+bool writeMlcpTestDataAsText(const std::string& fileName, const MlcpTestData& testData)
+{
+ FILE* out = fopen(fileName.c_str(), "wt");
+ if (! out)
+ {
+ fprintf(stderr, "File '%s' could not be created to export the current MLCP for GTest\n", fileName.c_str());
+ return false;
+ }
+
+ size_t numConstraints = testData.problem.constraintTypes.size();
+ ptrdiff_t numAtomicConstraints = testData.problem.b.rows();
+ if (testData.problem.b.rows() != numAtomicConstraints ||
+ testData.problem.A.rows() != numAtomicConstraints || testData.problem.A.cols() != numAtomicConstraints ||
+ testData.expectedLambda.rows() != numAtomicConstraints)
+ {
+ fprintf(stderr, "Inconsistent dimensions in file '%s'!\n", fileName.c_str());
+ fclose(out);
+ return false;
+ }
+
+ writeCommentBlock(out, testData.description);
+
+ fprintf(out, "%s", TEXT_LABEL_FLAGS_LIST);
+ for (auto it = testData.flags.begin(); it != testData.flags.end(); ++it)
+ {
+ fprintf(out, " %s", it->c_str());
+ }
+ fprintf(out, "\n");
+
+
+ fprintf(out, "%s %d\n", TEXT_LABEL_NUM_DEGREES_OF_FREEDOM, testData.numDegreesOfFreedom);
+ fprintf(out, "%s %llu\n", TEXT_LABEL_NUM_CONSTRAINTS,
+ static_cast<unsigned long long int>(numConstraints)); // NOLINT
+ fprintf(out, "%s %lld\n", TEXT_LABEL_NUM_ATOMIC_CONSTRAINTS,
+ static_cast<long long int>(numAtomicConstraints)); // NOLINT
+
+ fprintf(out, "%s", TEXT_LABEL_CONSTRAINT_TYPES_LIST);
+ for (auto it = testData.problem.constraintTypes.begin(); it != testData.problem.constraintTypes.end(); ++it)
+ {
+ fprintf(out, " %s", getMlcpConstraintTypeName(*it).c_str());
+ }
+ fprintf(out, "\n");
+
+ writeEigenVector(out, TEXT_LABEL_E_VIOLATIONS_VECTOR, testData.problem.b);
+ writeEigenMatrix(out, TEXT_LABEL_HCHt_MLCP_MATRIX, testData.problem.A);
+ writeEigenVector(out, TEXT_LABEL_MU_FRICTION_VECTOR, testData.problem.mu);
+ writeEigenVector(out, TEXT_LABEL_LAMBDA_VECTOR, testData.expectedLambda);
+
+
+ fprintf(out, "%s\n", TEXT_LABEL_END_OF_FILE);
+
+ if (ferror(out))
+ {
+ fprintf(stderr, "Unexpected error writing file '%s'\n", fileName.c_str());
+ return false;
+
+ }
+ if (fclose(out))
+ {
+ fprintf(stderr, "Error closing file '%s' for writing.\n", fileName.c_str());
+ return false;
+ }
+
+ return true;
+}
diff --git a/SurgSim/Testing/MlcpIO/WriteText.h b/SurgSim/Testing/MlcpIO/WriteText.h
new file mode 100644
index 0000000..8fdd2ea
--- /dev/null
+++ b/SurgSim/Testing/MlcpIO/WriteText.h
@@ -0,0 +1,24 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_MLCPIO_WRITETEXT_H
+#define SURGSIM_TESTING_MLCPIO_WRITETEXT_H
+
+#include <string>
+#include "SurgSim/Testing/MlcpIO/MlcpTestData.h"
+
+bool writeMlcpTestDataAsText(const std::string& fileName, const MlcpTestData& testData);
+
+#endif // SURGSIM_TESTING_MLCPIO_WRITETEXT_H
diff --git a/SurgSim/Testing/MockInputOutput.cpp b/SurgSim/Testing/MockInputOutput.cpp
new file mode 100644
index 0000000..675b9b7
--- /dev/null
+++ b/SurgSim/Testing/MockInputOutput.cpp
@@ -0,0 +1,56 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::DataStructures::DataGroup;
+
+namespace SurgSim
+{
+namespace Testing
+{
+
+MockInputOutput::MockInputOutput() :
+ m_numTimesInitializedInput(0),
+ m_numTimesReceivedInput(0),
+ m_numTimesRequestedOutput(0)
+{
+}
+
+bool MockInputOutput::requestOutput(const std::string& device, DataGroup* outputData)
+{
+ ++m_numTimesRequestedOutput;
+ bool result = false;
+ if (m_output.hasValue())
+ {
+ *outputData = m_output.getValue();
+ result = true;
+ }
+ return result;
+}
+
+void MockInputOutput::handleInput(const std::string& device, const DataGroup& inputData)
+{
+ ++m_numTimesReceivedInput;
+ m_lastReceivedInput = inputData;
+}
+
+void MockInputOutput::initializeInput(const std::string& device, const DataGroup& inputData)
+{
+ ++m_numTimesInitializedInput;
+}
+
+}; // Testing
+}; // SurgSim
diff --git a/SurgSim/Testing/MockInputOutput.h b/SurgSim/Testing/MockInputOutput.h
new file mode 100644
index 0000000..9fef424
--- /dev/null
+++ b/SurgSim/Testing/MockInputOutput.h
@@ -0,0 +1,47 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_MOCKINPUTOUTPUT_H
+#define SURGSIM_TESTING_MOCKINPUTOUTPUT_H
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+
+namespace SurgSim
+{
+namespace Testing
+{
+
+struct MockInputOutput : public SurgSim::Input::InputConsumerInterface, public SurgSim::Input::OutputProducerInterface
+{
+public:
+ MockInputOutput();
+
+ virtual void initializeInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData);
+ virtual void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData);
+ virtual bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData);
+
+ int m_numTimesInitializedInput;
+ int m_numTimesReceivedInput;
+ int m_numTimesRequestedOutput;
+ SurgSim::DataStructures::DataGroup m_lastReceivedInput;
+ SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::DataGroup> m_output;
+};
+
+};
+};
+#endif // SURGSIM_TESTING_MOCKINPUTOUTPUT_H
diff --git a/SurgSim/Testing/MockPhysicsManager.h b/SurgSim/Testing/MockPhysicsManager.h
new file mode 100644
index 0000000..3eb6bea
--- /dev/null
+++ b/SurgSim/Testing/MockPhysicsManager.h
@@ -0,0 +1,59 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_MOCKPHYSICSMANAGER_H
+#define SURGSIM_TESTING_MOCKPHYSICSMANAGER_H
+
+#include "SurgSim/Physics/PhysicsManager.h"
+
+namespace SurgSim
+{
+namespace Testing
+{
+
+/// Testing class used to publicly expose PhysicsManager's protected member functions
+class MockPhysicsManager : public SurgSim::Physics::PhysicsManager
+{
+public:
+ virtual bool executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component) override
+ {
+ return SurgSim::Physics::PhysicsManager::executeAdditions(component);
+ }
+
+ virtual bool executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component) override
+ {
+ return SurgSim::Physics::PhysicsManager::executeRemovals(component);
+ }
+
+ virtual bool doInitialize() override
+ {
+ return SurgSim::Physics::PhysicsManager::doInitialize();
+ }
+
+ virtual bool doStartUp() override
+ {
+ return SurgSim::Physics::PhysicsManager::doStartUp();
+ }
+
+ virtual bool doUpdate(double dt)
+ {
+ return SurgSim::Physics::PhysicsManager::doUpdate(dt);
+ }
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_TESTING_MOCKPHYSICSMANAGER_H
diff --git a/SurgSim/Testing/OctreeShapeData/invalid-staple.vox b/SurgSim/Testing/OctreeShapeData/invalid-staple.vox
new file mode 100644
index 0000000..2fc83f8
--- /dev/null
+++ b/SurgSim/Testing/OctreeShapeData/invalid-staple.vox
@@ -0,0 +1,26 @@
+7 11 1
+-0.001 -0.001 -0.001
+0.00532899983227 -0.000403999991249 0.000403999991249 -0.00207699998282 0.00480100000277 -0.00532899983227
+-0.000576999982819 -0.00482899983227 9.60000087507e-05
+0.000423000017181 -0.00482899983227 9.60000087507e-05
+0.00142300001718 -0.00482899983227 9.60000087507e-05
+0.00242300001718 -0.00482899983227 9.60000087507e-05
+0.00342300001718 -0.00482899983227 9.60000087507e-05
+-0.00157699998282 -0.00382899983227 9.60000087507e-05
+0.00442300001718 -0.00382899983227 9.60000087507e-05
+-0.00157699998282 -0.00282899983227 9.60000087507e-05
+0.00442300001718 -0.00282899983227 9.60000087507e-05
+-0.00157699998282 -0.00182899983227 9.60000087507e-05
+0.00442300001718 -0.00182899983227 9.60000087507e-05
+-0.00157699998282 -0.000828999832273 9.60000087507e-05
+-0.00157699998282 0.000171000167727 9.60000087507e-05
+-0.00157699998282 0.00117100016773 9.60000087507e-05
+-0.00157699998282 0.00217100016773 9.60000087507e-05
+0.00442300001718 0.00217100016773 9.60000087507e-05
+-0.00157699998282 0.00317100016773 9.60000087507e-05
+0.00442300001718 0.00317100016773 9.60000087507e-05
+-0.00157699998282 0.00417100016773 9.60000087507e-05
+0.00442300001718 0.00417100016773 9.60000087507e-05
+0.000423000017181 0.00517100016773 9.60000087507e-05
+0.00142300001718 0.00517100016773 9.60000087507e-05
+0.00242300001718 0.00517100016773 9.60000087507e-05
diff --git a/SurgSim/Testing/OctreeShapeData/staple.vox b/SurgSim/Testing/OctreeShapeData/staple.vox
new file mode 100644
index 0000000..e2a3ad7
--- /dev/null
+++ b/SurgSim/Testing/OctreeShapeData/staple.vox
@@ -0,0 +1,26 @@
+7 11 1
+0.001 0.001 0.001
+-0.00207699998282 0.00480100000277 -0.00532899983227 0.00532899983227 -0.000403999991249 0.000403999991249
+-0.000576999982819 -0.00482899983227 9.60000087507e-05
+0.000423000017181 -0.00482899983227 9.60000087507e-05
+0.00142300001718 -0.00482899983227 9.60000087507e-05
+0.00242300001718 -0.00482899983227 9.60000087507e-05
+0.00342300001718 -0.00482899983227 9.60000087507e-05
+-0.00157699998282 -0.00382899983227 9.60000087507e-05
+0.00442300001718 -0.00382899983227 9.60000087507e-05
+-0.00157699998282 -0.00282899983227 9.60000087507e-05
+0.00442300001718 -0.00282899983227 9.60000087507e-05
+-0.00157699998282 -0.00182899983227 9.60000087507e-05
+0.00442300001718 -0.00182899983227 9.60000087507e-05
+-0.00157699998282 -0.000828999832273 9.60000087507e-05
+-0.00157699998282 0.000171000167727 9.60000087507e-05
+-0.00157699998282 0.00117100016773 9.60000087507e-05
+-0.00157699998282 0.00217100016773 9.60000087507e-05
+0.00442300001718 0.00217100016773 9.60000087507e-05
+-0.00157699998282 0.00317100016773 9.60000087507e-05
+0.00442300001718 0.00317100016773 9.60000087507e-05
+-0.00157699998282 0.00417100016773 9.60000087507e-05
+0.00442300001718 0.00417100016773 9.60000087507e-05
+0.000423000017181 0.00517100016773 9.60000087507e-05
+0.00142300001718 0.00517100016773 9.60000087507e-05
+0.00242300001718 0.00517100016773 9.60000087507e-05
diff --git a/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.mtl b/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.mtl
new file mode 100644
index 0000000..d159b36
--- /dev/null
+++ b/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.mtl
@@ -0,0 +1,11 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl Material.001
+Ns 96.078431
+Ka 0.000000 0.000000 0.000000
+Kd 0.060738 0.205554 0.640000
+Ks 0.700000 0.700000 0.700000
+Ni 1.000000
+d 1.000000
+illum 2
diff --git a/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.obj b/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.obj
new file mode 100644
index 0000000..18dd6c8
--- /dev/null
+++ b/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.obj
@@ -0,0 +1,2694 @@
+# Blender v2.66 (sub 1) OBJ File: ''
+# www.blender.org
+mtllib Torus.mtl
+o Torus
+v 1.000000 0.000000 0.000000
+v 0.978386 0.101684 0.000000
+v 0.917283 0.185786 0.000000
+v 0.827254 0.237764 0.000000
+v 0.723868 0.248630 0.000000
+v 0.625000 0.216506 0.000000
+v 0.547746 0.146946 0.000000
+v 0.505463 0.051978 0.000000
+v 0.505463 -0.051978 0.000000
+v 0.547746 -0.146946 0.000000
+v 0.625000 -0.216506 0.000000
+v 0.723868 -0.248630 0.000000
+v 0.827254 -0.237764 0.000000
+v 0.917283 -0.185786 0.000000
+v 0.978386 -0.101684 0.000000
+v 0.987688 0.000000 -0.156435
+v 0.966341 0.101684 -0.153053
+v 0.905989 0.185786 -0.143495
+v 0.817069 0.237764 -0.129411
+v 0.714956 0.248630 -0.113238
+v 0.617305 0.216506 -0.097772
+v 0.541002 0.146946 -0.085686
+v 0.499240 0.051978 -0.079072
+v 0.499240 -0.051978 -0.079072
+v 0.541002 -0.146946 -0.085686
+v 0.617305 -0.216506 -0.097772
+v 0.714956 -0.248630 -0.113238
+v 0.817069 -0.237764 -0.129411
+v 0.905989 -0.185786 -0.143495
+v 0.966341 -0.101684 -0.153053
+v 0.951057 0.000000 -0.309017
+v 0.930501 0.101684 -0.302338
+v 0.872388 0.185786 -0.283456
+v 0.786766 0.237764 -0.255636
+v 0.688439 0.248630 -0.223687
+v 0.594410 0.216506 -0.193136
+v 0.520937 0.146946 -0.169263
+v 0.480724 0.051978 -0.156197
+v 0.480724 -0.051978 -0.156197
+v 0.520937 -0.146946 -0.169263
+v 0.594410 -0.216506 -0.193136
+v 0.688439 -0.248630 -0.223687
+v 0.786766 -0.237764 -0.255636
+v 0.872388 -0.185786 -0.283456
+v 0.930501 -0.101684 -0.302338
+v 0.891007 0.000000 -0.453990
+v 0.871749 0.101684 -0.444178
+v 0.817305 0.185786 -0.416438
+v 0.737089 0.237764 -0.375566
+v 0.644971 0.248630 -0.328629
+v 0.556879 0.216506 -0.283744
+v 0.488045 0.146946 -0.248671
+v 0.450371 0.051978 -0.229475
+v 0.450371 -0.051978 -0.229475
+v 0.488045 -0.146946 -0.248671
+v 0.556879 -0.216506 -0.283744
+v 0.644971 -0.248630 -0.328629
+v 0.737089 -0.237764 -0.375566
+v 0.817305 -0.185786 -0.416438
+v 0.871749 -0.101684 -0.444178
+v 0.809017 0.000000 -0.587785
+v 0.791531 0.101684 -0.575081
+v 0.742097 0.185786 -0.539165
+v 0.669263 0.237764 -0.486248
+v 0.585621 0.248630 -0.425479
+v 0.505636 0.216506 -0.367366
+v 0.443136 0.146946 -0.321957
+v 0.408928 0.051978 -0.297104
+v 0.408928 -0.051978 -0.297104
+v 0.443136 -0.146946 -0.321957
+v 0.505636 -0.216506 -0.367366
+v 0.585621 -0.248630 -0.425479
+v 0.669263 -0.237764 -0.486248
+v 0.742097 -0.185786 -0.539165
+v 0.791531 -0.101684 -0.575081
+v 0.707107 0.000000 -0.707107
+v 0.691824 0.101684 -0.691824
+v 0.648617 0.185786 -0.648617
+v 0.584957 0.237764 -0.584957
+v 0.511852 0.248630 -0.511852
+v 0.441942 0.216506 -0.441942
+v 0.387315 0.146946 -0.387315
+v 0.357416 0.051978 -0.357416
+v 0.357416 -0.051978 -0.357416
+v 0.387315 -0.146946 -0.387315
+v 0.441942 -0.216506 -0.441942
+v 0.511852 -0.248630 -0.511852
+v 0.584957 -0.237764 -0.584957
+v 0.648617 -0.185786 -0.648617
+v 0.691824 -0.101684 -0.691824
+v 0.587785 0.000000 -0.809017
+v 0.575081 0.101684 -0.791531
+v 0.539165 0.185786 -0.742097
+v 0.486248 0.237764 -0.669263
+v 0.425479 0.248630 -0.585621
+v 0.367366 0.216506 -0.505636
+v 0.321957 0.146946 -0.443136
+v 0.297104 0.051978 -0.408928
+v 0.297104 -0.051978 -0.408928
+v 0.321957 -0.146946 -0.443136
+v 0.367366 -0.216506 -0.505636
+v 0.425479 -0.248630 -0.585621
+v 0.486248 -0.237764 -0.669263
+v 0.539165 -0.185786 -0.742097
+v 0.575081 -0.101684 -0.791531
+v 0.453991 0.000000 -0.891006
+v 0.444178 0.101684 -0.871748
+v 0.416438 0.185786 -0.817305
+v 0.375566 0.237764 -0.737089
+v 0.328629 0.248630 -0.644971
+v 0.283744 0.216506 -0.556879
+v 0.248672 0.146946 -0.488045
+v 0.229476 0.051978 -0.450371
+v 0.229476 -0.051978 -0.450371
+v 0.248672 -0.146946 -0.488045
+v 0.283744 -0.216506 -0.556879
+v 0.328629 -0.248630 -0.644971
+v 0.375566 -0.237764 -0.737089
+v 0.416438 -0.185786 -0.817305
+v 0.444178 -0.101684 -0.871748
+v 0.309017 0.000000 -0.951056
+v 0.302338 0.101684 -0.930501
+v 0.283456 0.185786 -0.872388
+v 0.255636 0.237764 -0.786765
+v 0.223688 0.248630 -0.688439
+v 0.193136 0.216506 -0.594410
+v 0.169263 0.146946 -0.520937
+v 0.156197 0.051978 -0.480724
+v 0.156197 -0.051978 -0.480724
+v 0.169263 -0.146946 -0.520937
+v 0.193136 -0.216506 -0.594410
+v 0.223688 -0.248630 -0.688439
+v 0.255636 -0.237764 -0.786765
+v 0.283456 -0.185786 -0.872388
+v 0.302338 -0.101684 -0.930501
+v 0.156435 0.000000 -0.987688
+v 0.153053 0.101684 -0.966341
+v 0.143495 0.185786 -0.905989
+v 0.129411 0.237764 -0.817069
+v 0.113238 0.248630 -0.714956
+v 0.097772 0.216506 -0.617305
+v 0.085686 0.146946 -0.541002
+v 0.079072 0.051978 -0.499240
+v 0.079072 -0.051978 -0.499240
+v 0.085686 -0.146946 -0.541002
+v 0.097772 -0.216506 -0.617305
+v 0.113238 -0.248630 -0.714956
+v 0.129411 -0.237764 -0.817069
+v 0.143495 -0.185786 -0.905989
+v 0.153053 -0.101684 -0.966341
+v 0.000000 0.000000 -1.000000
+v 0.000000 0.101684 -0.978386
+v 0.000000 0.185786 -0.917283
+v 0.000000 0.237764 -0.827254
+v 0.000000 0.248630 -0.723868
+v 0.000000 0.216506 -0.625000
+v 0.000000 0.146946 -0.547746
+v 0.000000 0.051978 -0.505463
+v 0.000000 -0.051978 -0.505463
+v 0.000000 -0.146946 -0.547746
+v 0.000000 -0.216506 -0.625000
+v 0.000000 -0.248630 -0.723868
+v 0.000000 -0.237764 -0.827254
+v 0.000000 -0.185786 -0.917283
+v 0.000000 -0.101684 -0.978386
+v -0.156434 0.000000 -0.987688
+v -0.153053 0.101684 -0.966341
+v -0.143495 0.185786 -0.905989
+v -0.129411 0.237764 -0.817069
+v -0.113238 0.248630 -0.714956
+v -0.097772 0.216506 -0.617305
+v -0.085686 0.146946 -0.541002
+v -0.079072 0.051978 -0.499240
+v -0.079072 -0.051978 -0.499240
+v -0.085686 -0.146946 -0.541002
+v -0.097772 -0.216506 -0.617305
+v -0.113238 -0.248630 -0.714956
+v -0.129411 -0.237764 -0.817069
+v -0.143495 -0.185786 -0.905989
+v -0.153053 -0.101684 -0.966341
+v -0.309017 0.000000 -0.951057
+v -0.302338 0.101684 -0.930501
+v -0.283456 0.185786 -0.872388
+v -0.255636 0.237764 -0.786766
+v -0.223687 0.248630 -0.688439
+v -0.193136 0.216506 -0.594410
+v -0.169263 0.146946 -0.520937
+v -0.156197 0.051978 -0.480724
+v -0.156197 -0.051978 -0.480724
+v -0.169263 -0.146946 -0.520937
+v -0.193136 -0.216506 -0.594410
+v -0.223687 -0.248630 -0.688439
+v -0.255636 -0.237764 -0.786766
+v -0.283456 -0.185786 -0.872388
+v -0.302338 -0.101684 -0.930501
+v -0.453991 0.000000 -0.891006
+v -0.444178 0.101684 -0.871749
+v -0.416438 0.185786 -0.817305
+v -0.375566 0.237764 -0.737089
+v -0.328629 0.248630 -0.644971
+v -0.283744 0.216506 -0.556879
+v -0.248671 0.146946 -0.488045
+v -0.229476 0.051978 -0.450371
+v -0.229476 -0.051978 -0.450371
+v -0.248671 -0.146946 -0.488045
+v -0.283744 -0.216506 -0.556879
+v -0.328629 -0.248630 -0.644971
+v -0.375566 -0.237764 -0.737089
+v -0.416438 -0.185786 -0.817305
+v -0.444178 -0.101684 -0.871749
+v -0.587785 0.000000 -0.809017
+v -0.575081 0.101684 -0.791531
+v -0.539165 0.185786 -0.742097
+v -0.486248 0.237764 -0.669263
+v -0.425479 0.248630 -0.585621
+v -0.367366 0.216506 -0.505636
+v -0.321957 0.146946 -0.443136
+v -0.297104 0.051978 -0.408928
+v -0.297104 -0.051978 -0.408928
+v -0.321957 -0.146946 -0.443136
+v -0.367366 -0.216506 -0.505636
+v -0.425479 -0.248630 -0.585621
+v -0.486248 -0.237764 -0.669263
+v -0.539165 -0.185786 -0.742097
+v -0.575081 -0.101684 -0.791531
+v -0.707107 0.000000 -0.707107
+v -0.691824 0.101684 -0.691823
+v -0.648617 0.185786 -0.648617
+v -0.584957 0.237764 -0.584957
+v -0.511852 0.248630 -0.511852
+v -0.441942 0.216506 -0.441942
+v -0.387315 0.146946 -0.387315
+v -0.357416 0.051978 -0.357416
+v -0.357416 -0.051978 -0.357416
+v -0.387315 -0.146946 -0.387315
+v -0.441942 -0.216506 -0.441942
+v -0.511852 -0.248630 -0.511852
+v -0.584957 -0.237764 -0.584957
+v -0.648617 -0.185786 -0.648617
+v -0.691824 -0.101684 -0.691823
+v -0.809017 0.000000 -0.587785
+v -0.791531 0.101684 -0.575081
+v -0.742097 0.185786 -0.539165
+v -0.669263 0.237764 -0.486248
+v -0.585622 0.248630 -0.425479
+v -0.505636 0.216506 -0.367366
+v -0.443136 0.146946 -0.321957
+v -0.408928 0.051978 -0.297104
+v -0.408928 -0.051978 -0.297104
+v -0.443136 -0.146946 -0.321957
+v -0.505636 -0.216506 -0.367366
+v -0.585622 -0.248630 -0.425479
+v -0.669263 -0.237764 -0.486248
+v -0.742097 -0.185786 -0.539165
+v -0.791531 -0.101684 -0.575081
+v -0.891006 0.000000 -0.453991
+v -0.871749 0.101684 -0.444178
+v -0.817305 0.185786 -0.416438
+v -0.737089 0.237764 -0.375566
+v -0.644971 0.248630 -0.328629
+v -0.556879 0.216506 -0.283744
+v -0.488045 0.146946 -0.248671
+v -0.450371 0.051978 -0.229475
+v -0.450371 -0.051978 -0.229475
+v -0.488045 -0.146946 -0.248671
+v -0.556879 -0.216506 -0.283744
+v -0.644971 -0.248630 -0.328629
+v -0.737089 -0.237764 -0.375566
+v -0.817305 -0.185786 -0.416438
+v -0.871749 -0.101684 -0.444178
+v -0.951057 0.000000 -0.309017
+v -0.930501 0.101684 -0.302338
+v -0.872388 0.185786 -0.283456
+v -0.786766 0.237764 -0.255636
+v -0.688439 0.248631 -0.223688
+v -0.594410 0.216506 -0.193136
+v -0.520937 0.146946 -0.169263
+v -0.480724 0.051978 -0.156197
+v -0.480724 -0.051978 -0.156197
+v -0.520937 -0.146946 -0.169263
+v -0.594410 -0.216506 -0.193136
+v -0.688439 -0.248631 -0.223688
+v -0.786766 -0.237764 -0.255636
+v -0.872388 -0.185786 -0.283456
+v -0.930501 -0.101684 -0.302338
+v -0.987688 0.000000 -0.156434
+v -0.966341 0.101684 -0.153053
+v -0.905989 0.185786 -0.143495
+v -0.817069 0.237764 -0.129411
+v -0.714956 0.248630 -0.113238
+v -0.617305 0.216506 -0.097772
+v -0.541002 0.146946 -0.085686
+v -0.499240 0.051978 -0.079072
+v -0.499240 -0.051978 -0.079072
+v -0.541002 -0.146946 -0.085686
+v -0.617305 -0.216506 -0.097772
+v -0.714956 -0.248630 -0.113238
+v -0.817069 -0.237764 -0.129411
+v -0.905989 -0.185786 -0.143495
+v -0.966341 -0.101684 -0.153053
+v -1.000000 0.000000 -0.000000
+v -0.978386 0.101684 -0.000000
+v -0.917283 0.185786 -0.000000
+v -0.827254 0.237764 -0.000000
+v -0.723868 0.248630 -0.000000
+v -0.625000 0.216506 -0.000000
+v -0.547746 0.146946 -0.000000
+v -0.505463 0.051978 -0.000000
+v -0.505463 -0.051978 -0.000000
+v -0.547746 -0.146946 -0.000000
+v -0.625000 -0.216506 -0.000000
+v -0.723868 -0.248630 -0.000000
+v -0.827254 -0.237764 -0.000000
+v -0.917283 -0.185786 -0.000000
+v -0.978386 -0.101684 -0.000000
+v -0.987688 0.000000 0.156434
+v -0.966341 0.101684 0.153053
+v -0.905989 0.185786 0.143494
+v -0.817069 0.237764 0.129411
+v -0.714956 0.248630 0.113238
+v -0.617305 0.216506 0.097771
+v -0.541002 0.146946 0.085686
+v -0.499240 0.051978 0.079072
+v -0.499240 -0.051978 0.079072
+v -0.541002 -0.146946 0.085686
+v -0.617305 -0.216506 0.097771
+v -0.714956 -0.248630 0.113238
+v -0.817069 -0.237764 0.129411
+v -0.905989 -0.185786 0.143494
+v -0.966341 -0.101684 0.153053
+v -0.951057 0.000000 0.309017
+v -0.930501 0.101684 0.302338
+v -0.872388 0.185786 0.283456
+v -0.786766 0.237764 0.255635
+v -0.688439 0.248630 0.223687
+v -0.594410 0.216506 0.193135
+v -0.520937 0.146946 0.169263
+v -0.480724 0.051978 0.156196
+v -0.480724 -0.051978 0.156196
+v -0.520937 -0.146946 0.169263
+v -0.594410 -0.216506 0.193135
+v -0.688439 -0.248630 0.223687
+v -0.786766 -0.237764 0.255635
+v -0.872388 -0.185786 0.283456
+v -0.930501 -0.101684 0.302338
+v -0.891007 0.000000 0.453990
+v -0.871749 0.101684 0.444178
+v -0.817305 0.185786 0.416437
+v -0.737089 0.237764 0.375565
+v -0.644971 0.248630 0.328629
+v -0.556879 0.216506 0.283744
+v -0.488045 0.146946 0.248671
+v -0.450371 0.051978 0.229475
+v -0.450371 -0.051978 0.229475
+v -0.488045 -0.146946 0.248671
+v -0.556879 -0.216506 0.283744
+v -0.644971 -0.248630 0.328629
+v -0.737089 -0.237764 0.375565
+v -0.817305 -0.185786 0.416437
+v -0.871749 -0.101684 0.444178
+v -0.809017 0.000000 0.587785
+v -0.791531 0.101684 0.575081
+v -0.742097 0.185786 0.539165
+v -0.669263 0.237764 0.486248
+v -0.585622 0.248630 0.425479
+v -0.505636 0.216506 0.367366
+v -0.443136 0.146946 0.321957
+v -0.408928 0.051978 0.297104
+v -0.408928 -0.051978 0.297104
+v -0.443136 -0.146946 0.321957
+v -0.505636 -0.216506 0.367366
+v -0.585622 -0.248630 0.425479
+v -0.669263 -0.237764 0.486248
+v -0.742097 -0.185786 0.539165
+v -0.791531 -0.101684 0.575081
+v -0.707107 0.000000 0.707107
+v -0.691824 0.101684 0.691823
+v -0.648617 0.185786 0.648617
+v -0.584957 0.237764 0.584957
+v -0.511852 0.248630 0.511852
+v -0.441942 0.216506 0.441942
+v -0.387315 0.146946 0.387315
+v -0.357416 0.051978 0.357416
+v -0.357416 -0.051978 0.357416
+v -0.387315 -0.146946 0.387315
+v -0.441942 -0.216506 0.441942
+v -0.511852 -0.248630 0.511852
+v -0.584957 -0.237764 0.584957
+v -0.648617 -0.185786 0.648617
+v -0.691824 -0.101684 0.691823
+v -0.587785 0.000000 0.809017
+v -0.575081 0.101684 0.791531
+v -0.539165 0.185786 0.742097
+v -0.486248 0.237764 0.669263
+v -0.425479 0.248630 0.585621
+v -0.367366 0.216506 0.505636
+v -0.321957 0.146946 0.443136
+v -0.297104 0.051978 0.408928
+v -0.297104 -0.051978 0.408928
+v -0.321957 -0.146946 0.443136
+v -0.367366 -0.216506 0.505636
+v -0.425479 -0.248630 0.585621
+v -0.486248 -0.237764 0.669263
+v -0.539165 -0.185786 0.742097
+v -0.575081 -0.101684 0.791531
+v -0.453991 0.000000 0.891006
+v -0.444178 0.101684 0.871749
+v -0.416438 0.185786 0.817305
+v -0.375566 0.237764 0.737089
+v -0.328629 0.248630 0.644971
+v -0.283744 0.216506 0.556879
+v -0.248671 0.146946 0.488045
+v -0.229476 0.051978 0.450371
+v -0.229476 -0.051978 0.450371
+v -0.248671 -0.146946 0.488045
+v -0.283744 -0.216506 0.556879
+v -0.328629 -0.248630 0.644971
+v -0.375566 -0.237764 0.737089
+v -0.416438 -0.185786 0.817305
+v -0.444178 -0.101684 0.871749
+v -0.309017 0.000000 0.951057
+v -0.302338 0.101684 0.930501
+v -0.283456 0.185786 0.872388
+v -0.255636 0.237764 0.786766
+v -0.223687 0.248630 0.688439
+v -0.193136 0.216506 0.594410
+v -0.169263 0.146946 0.520937
+v -0.156197 0.051978 0.480724
+v -0.156197 -0.051978 0.480724
+v -0.169263 -0.146946 0.520937
+v -0.193136 -0.216506 0.594410
+v -0.223687 -0.248630 0.688439
+v -0.255636 -0.237764 0.786766
+v -0.283456 -0.185786 0.872388
+v -0.302338 -0.101684 0.930501
+v -0.156434 0.000000 0.987688
+v -0.153053 0.101684 0.966341
+v -0.143495 0.185786 0.905989
+v -0.129411 0.237764 0.817069
+v -0.113238 0.248630 0.714956
+v -0.097772 0.216506 0.617305
+v -0.085686 0.146946 0.541002
+v -0.079072 0.051978 0.499240
+v -0.079072 -0.051978 0.499240
+v -0.085686 -0.146946 0.541002
+v -0.097772 -0.216506 0.617305
+v -0.113238 -0.248630 0.714956
+v -0.129411 -0.237764 0.817069
+v -0.143495 -0.185786 0.905989
+v -0.153053 -0.101684 0.966341
+v 0.000000 0.000000 1.000000
+v 0.000000 0.101684 0.978386
+v 0.000000 0.185786 0.917283
+v 0.000000 0.237764 0.827254
+v 0.000000 0.248630 0.723868
+v 0.000000 0.216506 0.625000
+v 0.000000 0.146946 0.547746
+v 0.000000 0.051978 0.505463
+v 0.000000 -0.051978 0.505463
+v 0.000000 -0.146946 0.547746
+v 0.000000 -0.216506 0.625000
+v 0.000000 -0.248630 0.723868
+v 0.000000 -0.237764 0.827254
+v 0.000000 -0.185786 0.917283
+v 0.000000 -0.101684 0.978386
+v 0.156435 0.000000 0.987688
+v 0.153053 0.101684 0.966341
+v 0.143495 0.185786 0.905989
+v 0.129411 0.237764 0.817069
+v 0.113238 0.248630 0.714956
+v 0.097772 0.216506 0.617305
+v 0.085686 0.146946 0.541002
+v 0.079072 0.051978 0.499240
+v 0.079072 -0.051978 0.499240
+v 0.085686 -0.146946 0.541002
+v 0.097772 -0.216506 0.617305
+v 0.113238 -0.248630 0.714956
+v 0.129411 -0.237764 0.817069
+v 0.143495 -0.185786 0.905989
+v 0.153053 -0.101684 0.966341
+v 0.309017 0.000000 0.951056
+v 0.302338 0.101684 0.930501
+v 0.283456 0.185786 0.872388
+v 0.255636 0.237764 0.786765
+v 0.223688 0.248630 0.688439
+v 0.193136 0.216506 0.594410
+v 0.169263 0.146946 0.520937
+v 0.156197 0.051978 0.480724
+v 0.156197 -0.051978 0.480724
+v 0.169263 -0.146946 0.520937
+v 0.193136 -0.216506 0.594410
+v 0.223688 -0.248630 0.688439
+v 0.255636 -0.237764 0.786765
+v 0.283456 -0.185786 0.872388
+v 0.302338 -0.101684 0.930501
+v 0.453991 0.000000 0.891006
+v 0.444178 0.101684 0.871748
+v 0.416438 0.185786 0.817305
+v 0.375566 0.237764 0.737089
+v 0.328629 0.248630 0.644971
+v 0.283744 0.216506 0.556879
+v 0.248672 0.146946 0.488045
+v 0.229476 0.051978 0.450371
+v 0.229476 -0.051978 0.450371
+v 0.248672 -0.146946 0.488045
+v 0.283744 -0.216506 0.556879
+v 0.328629 -0.248630 0.644971
+v 0.375566 -0.237764 0.737089
+v 0.416438 -0.185786 0.817305
+v 0.444178 -0.101684 0.871748
+v 0.587785 0.000000 0.809017
+v 0.575081 0.101684 0.791532
+v 0.539165 0.185786 0.742098
+v 0.486247 0.237764 0.669263
+v 0.425479 0.248630 0.585622
+v 0.367365 0.216506 0.505636
+v 0.321957 0.146946 0.443136
+v 0.297104 0.051978 0.408928
+v 0.297104 -0.051978 0.408928
+v 0.321957 -0.146946 0.443136
+v 0.367365 -0.216506 0.505636
+v 0.425479 -0.248630 0.585622
+v 0.486247 -0.237764 0.669263
+v 0.539165 -0.185786 0.742098
+v 0.575081 -0.101684 0.791532
+v 0.707106 0.000000 0.707107
+v 0.691823 0.101684 0.691824
+v 0.648616 0.185786 0.648617
+v 0.584957 0.237764 0.584957
+v 0.511852 0.248630 0.511852
+v 0.441941 0.216506 0.441942
+v 0.387315 0.146946 0.387315
+v 0.357416 0.051978 0.357417
+v 0.357416 -0.051978 0.357417
+v 0.387315 -0.146946 0.387315
+v 0.441941 -0.216506 0.441942
+v 0.511852 -0.248630 0.511852
+v 0.584957 -0.237764 0.584957
+v 0.648616 -0.185786 0.648617
+v 0.691823 -0.101684 0.691824
+v 0.809017 0.000000 0.587786
+v 0.791531 0.101684 0.575081
+v 0.742097 0.185786 0.539166
+v 0.669262 0.237764 0.486248
+v 0.585621 0.248630 0.425479
+v 0.505635 0.216506 0.367366
+v 0.443135 0.146946 0.321957
+v 0.408928 0.051978 0.297104
+v 0.408928 -0.051978 0.297104
+v 0.443135 -0.146946 0.321957
+v 0.505635 -0.216506 0.367366
+v 0.585621 -0.248630 0.425479
+v 0.669262 -0.237764 0.486248
+v 0.742097 -0.185786 0.539166
+v 0.791531 -0.101684 0.575081
+v 0.891006 0.000000 0.453991
+v 0.871748 0.101684 0.444178
+v 0.817305 0.185786 0.416438
+v 0.737089 0.237764 0.375566
+v 0.644971 0.248630 0.328629
+v 0.556879 0.216506 0.283744
+v 0.488045 0.146946 0.248672
+v 0.450371 0.051978 0.229476
+v 0.450371 -0.051978 0.229476
+v 0.488045 -0.146946 0.248672
+v 0.556879 -0.216506 0.283744
+v 0.644971 -0.248630 0.328629
+v 0.737089 -0.237764 0.375566
+v 0.817305 -0.185786 0.416438
+v 0.871748 -0.101684 0.444178
+v 0.951056 0.000000 0.309017
+v 0.930501 0.101684 0.302338
+v 0.872388 0.185786 0.283456
+v 0.786765 0.237764 0.255636
+v 0.688439 0.248630 0.223688
+v 0.594410 0.216506 0.193136
+v 0.520937 0.146946 0.169263
+v 0.480724 0.051978 0.156197
+v 0.480724 -0.051978 0.156197
+v 0.520937 -0.146946 0.169263
+v 0.594410 -0.216506 0.193136
+v 0.688439 -0.248630 0.223688
+v 0.786765 -0.237764 0.255636
+v 0.872388 -0.185786 0.283456
+v 0.930501 -0.101684 0.302338
+v 0.987688 0.000000 0.156435
+v 0.966341 0.101684 0.153054
+v 0.905989 0.185786 0.143495
+v 0.817069 0.237764 0.129411
+v 0.714956 0.248630 0.113238
+v 0.617305 0.216506 0.097772
+v 0.541002 0.146946 0.085686
+v 0.499240 0.051978 0.079072
+v 0.499240 -0.051978 0.079072
+v 0.541002 -0.146946 0.085686
+v 0.617305 -0.216506 0.097772
+v 0.714956 -0.248630 0.113238
+v 0.817069 -0.237764 0.129411
+v 0.905989 -0.185786 0.143495
+v 0.966341 -0.101684 0.153054
+vn 0.975262 0.207298 -0.076755
+vn 0.807382 0.586598 -0.063543
+vn 0.499613 0.865356 -0.039321
+vn 0.104525 0.994488 -0.008226
+vn -0.308926 0.950775 0.024313
+vn -0.668205 0.742116 0.052589
+vn -0.911193 0.405689 0.071713
+vn -0.996917 0.000000 0.078459
+vn -0.911193 -0.405689 0.071712
+vn -0.668205 -0.742116 0.052589
+vn -0.308926 -0.950775 0.024313
+vn 0.104525 -0.994488 -0.008226
+vn 0.499613 -0.865356 -0.039321
+vn 0.807382 -0.586598 -0.063543
+vn 0.975262 -0.207299 -0.076755
+vn 0.951248 0.207299 -0.228373
+vn 0.787502 0.586597 -0.189063
+vn 0.487311 0.865356 -0.116992
+vn 0.101951 0.994488 -0.024476
+vn -0.301319 0.950775 0.072341
+vn -0.651751 0.742116 0.156471
+vn -0.888757 0.405689 0.213372
+vn -0.972370 0.000000 0.233445
+vn -0.888757 -0.405689 0.213372
+vn -0.651751 -0.742117 0.156472
+vn -0.301319 -0.950775 0.072340
+vn 0.101951 -0.994488 -0.024476
+vn 0.487311 -0.865356 -0.116993
+vn 0.787502 -0.586597 -0.189062
+vn 0.951248 -0.207299 -0.228374
+vn 0.903811 0.207299 -0.374371
+vn 0.748230 0.586597 -0.309927
+vn 0.463010 0.865356 -0.191785
+vn 0.096867 0.994488 -0.040124
+vn -0.286293 0.950775 0.118586
+vn -0.619249 0.742117 0.256501
+vn -0.844436 0.405689 0.349777
+vn -0.923879 0.000000 0.382684
+vn -0.844436 -0.405689 0.349777
+vn -0.619249 -0.742117 0.256501
+vn -0.286293 -0.950775 0.118586
+vn 0.096867 -0.994488 -0.040124
+vn 0.463010 -0.865356 -0.191785
+vn 0.748230 -0.586597 -0.309928
+vn 0.903811 -0.207299 -0.374371
+vn 0.834119 0.207298 -0.511148
+vn 0.690535 0.586597 -0.423160
+vn 0.427308 0.865356 -0.261854
+vn 0.089398 0.994488 -0.054783
+vn -0.264217 0.950775 0.161912
+vn -0.571500 0.742117 0.350215
+vn -0.779323 0.405689 0.477569
+vn -0.852640 0.000000 0.522498
+vn -0.779323 -0.405689 0.477569
+vn -0.571500 -0.742117 0.350215
+vn -0.264217 -0.950775 0.161912
+vn 0.089398 -0.994488 -0.054783
+vn 0.427308 -0.865356 -0.261854
+vn 0.690535 -0.586597 -0.423160
+vn 0.834119 -0.207298 -0.511148
+vn 0.743888 0.207299 -0.635341
+vn 0.615836 0.586597 -0.525975
+vn 0.381084 0.865356 -0.325477
+vn 0.079727 0.994488 -0.068094
+vn -0.235635 0.950775 0.201252
+vn -0.509678 0.742117 0.435306
+vn -0.695019 0.405689 0.593603
+vn -0.760406 0.000000 0.649449
+vn -0.695019 -0.405689 0.593603
+vn -0.509678 -0.742117 0.435306
+vn -0.235635 -0.950775 0.201252
+vn 0.079727 -0.994488 -0.068094
+vn 0.381084 -0.865355 -0.325477
+vn 0.615836 -0.586597 -0.525975
+vn 0.743888 -0.207299 -0.635341
+vn 0.635340 0.207298 -0.743888
+vn 0.525974 0.586597 -0.615837
+vn 0.325476 0.865356 -0.381083
+vn 0.068093 0.994488 -0.079727
+vn -0.201251 0.950775 0.235635
+vn -0.435306 0.742117 0.509678
+vn -0.593603 0.405689 0.695019
+vn -0.649448 0.000000 0.760406
+vn -0.593603 -0.405689 0.695020
+vn -0.435306 -0.742117 0.509678
+vn -0.201252 -0.950775 0.235635
+vn 0.068093 -0.994488 -0.079727
+vn 0.325476 -0.865356 -0.381084
+vn 0.525974 -0.586597 -0.615837
+vn 0.635341 -0.207298 -0.743888
+vn 0.511149 0.207298 -0.834119
+vn 0.423160 0.586597 -0.690535
+vn 0.261854 0.865356 -0.427308
+vn 0.054783 0.994488 -0.089398
+vn -0.161912 0.950775 0.264217
+vn -0.350215 0.742116 0.571500
+vn -0.477570 0.405689 0.779322
+vn -0.522499 0.000000 0.852640
+vn -0.477569 -0.405689 0.779323
+vn -0.350216 -0.742117 0.571500
+vn -0.161912 -0.950775 0.264217
+vn 0.054783 -0.994488 -0.089398
+vn 0.261854 -0.865356 -0.427308
+vn 0.423160 -0.586597 -0.690535
+vn 0.511149 -0.207298 -0.834119
+vn 0.374371 0.207298 -0.903811
+vn 0.309928 0.586597 -0.748230
+vn 0.191785 0.865356 -0.463009
+vn 0.040124 0.994488 -0.096867
+vn -0.118586 0.950775 0.286293
+vn -0.256502 0.742116 0.619250
+vn -0.349777 0.405690 0.844436
+vn -0.382684 0.000000 0.923879
+vn -0.349777 -0.405690 0.844436
+vn -0.256502 -0.742116 0.619250
+vn -0.118587 -0.950775 0.286293
+vn 0.040124 -0.994488 -0.096867
+vn 0.191785 -0.865356 -0.463009
+vn 0.309928 -0.586597 -0.748230
+vn 0.374372 -0.207298 -0.903811
+vn 0.228374 0.207298 -0.951248
+vn 0.189062 0.586598 -0.787502
+vn 0.116993 0.865356 -0.487311
+vn 0.024476 0.994488 -0.101951
+vn -0.072340 0.950775 0.301319
+vn -0.156471 0.742117 0.651751
+vn -0.213372 0.405689 0.888757
+vn -0.233446 0.000000 0.972370
+vn -0.213372 -0.405689 0.888757
+vn -0.156472 -0.742117 0.651751
+vn -0.072340 -0.950775 0.301319
+vn 0.024476 -0.994488 -0.101951
+vn 0.116993 -0.865356 -0.487311
+vn 0.189062 -0.586598 -0.787502
+vn 0.228374 -0.207298 -0.951248
+vn 0.076755 0.207298 -0.975262
+vn 0.063543 0.586597 -0.807382
+vn 0.039321 0.865356 -0.499613
+vn 0.008227 0.994488 -0.104525
+vn -0.024313 0.950775 0.308926
+vn -0.052589 0.742116 0.668205
+vn -0.071713 0.405689 0.911193
+vn -0.078459 0.000000 0.996917
+vn -0.071712 -0.405689 0.911193
+vn -0.052589 -0.742116 0.668205
+vn -0.024313 -0.950775 0.308926
+vn 0.008227 -0.994488 -0.104525
+vn 0.039321 -0.865356 -0.499613
+vn 0.063543 -0.586597 -0.807382
+vn 0.076755 -0.207298 -0.975262
+vn -0.076755 0.207298 -0.975262
+vn -0.063543 0.586598 -0.807382
+vn -0.039321 0.865356 -0.499613
+vn -0.008227 0.994488 -0.104525
+vn 0.024313 0.950775 0.308926
+vn 0.052590 0.742116 0.668205
+vn 0.071713 0.405689 0.911193
+vn 0.078459 -0.000000 0.996917
+vn 0.071712 -0.405689 0.911194
+vn 0.052589 -0.742116 0.668205
+vn 0.024313 -0.950775 0.308925
+vn -0.008227 -0.994488 -0.104525
+vn -0.039321 -0.865356 -0.499613
+vn -0.063543 -0.586597 -0.807382
+vn -0.076755 -0.207298 -0.975262
+vn -0.228374 0.207298 -0.951248
+vn -0.189062 0.586598 -0.787501
+vn -0.116993 0.865356 -0.487311
+vn -0.024476 0.994488 -0.101951
+vn 0.072340 0.950775 0.301319
+vn 0.156471 0.742116 0.651752
+vn 0.213371 0.405690 0.888757
+vn 0.233446 -0.000000 0.972370
+vn 0.213372 -0.405690 0.888757
+vn 0.156471 -0.742116 0.651752
+vn 0.072340 -0.950775 0.301319
+vn -0.024476 -0.994488 -0.101951
+vn -0.116993 -0.865355 -0.487312
+vn -0.189062 -0.586598 -0.787501
+vn -0.228374 -0.207298 -0.951248
+vn -0.374371 0.207299 -0.903811
+vn -0.309927 0.586598 -0.748230
+vn -0.191785 0.865355 -0.463010
+vn -0.040124 0.994488 -0.096867
+vn 0.118586 0.950775 0.286293
+vn 0.256501 0.742117 0.619250
+vn 0.349778 0.405689 0.844436
+vn 0.382683 -0.000000 0.923880
+vn 0.349777 -0.405689 0.844436
+vn 0.256502 -0.742117 0.619249
+vn 0.118586 -0.950775 0.286293
+vn -0.040124 -0.994488 -0.096867
+vn -0.191785 -0.865356 -0.463010
+vn -0.309927 -0.586598 -0.748230
+vn -0.374371 -0.207298 -0.903811
+vn -0.511149 0.207299 -0.834119
+vn -0.423160 0.586597 -0.690535
+vn -0.261855 0.865356 -0.427308
+vn -0.054783 0.994488 -0.089398
+vn 0.161912 0.950775 0.264217
+vn 0.350216 0.742116 0.571500
+vn 0.477569 0.405690 0.779323
+vn 0.522499 -0.000000 0.852640
+vn 0.477570 -0.405690 0.779322
+vn 0.350215 -0.742116 0.571500
+vn 0.161912 -0.950775 0.264217
+vn -0.054783 -0.994488 -0.089398
+vn -0.261854 -0.865356 -0.427308
+vn -0.423161 -0.586597 -0.690535
+vn -0.511148 -0.207299 -0.834119
+vn -0.635341 0.207298 -0.743888
+vn -0.525974 0.586598 -0.615836
+vn -0.325477 0.865355 -0.381084
+vn -0.068094 0.994488 -0.079727
+vn 0.201252 0.950775 0.235635
+vn 0.435307 0.742116 0.509678
+vn 0.593604 0.405689 0.695019
+vn 0.649448 -0.000000 0.760406
+vn 0.593603 -0.405689 0.695019
+vn 0.435307 -0.742116 0.509678
+vn 0.201252 -0.950775 0.235635
+vn -0.068094 -0.994488 -0.079727
+vn -0.325477 -0.865355 -0.381084
+vn -0.525974 -0.586598 -0.615836
+vn -0.635341 -0.207298 -0.743888
+vn -0.743888 0.207299 -0.635340
+vn -0.615837 0.586598 -0.525974
+vn -0.381084 0.865356 -0.325476
+vn -0.079727 0.994488 -0.068093
+vn 0.235635 0.950775 0.201252
+vn 0.509678 0.742117 0.435306
+vn 0.695019 0.405689 0.593603
+vn 0.760406 -0.000000 0.649448
+vn 0.695020 -0.405690 0.593602
+vn 0.509678 -0.742117 0.435306
+vn 0.235635 -0.950775 0.201252
+vn -0.079727 -0.994488 -0.068093
+vn -0.381084 -0.865356 -0.325476
+vn -0.615837 -0.586598 -0.525974
+vn -0.743888 -0.207299 -0.635340
+vn -0.834119 0.207298 -0.511149
+vn -0.690535 0.586597 -0.423160
+vn -0.427308 0.865355 -0.261855
+vn -0.089398 0.994488 -0.054783
+vn 0.264217 0.950775 0.161912
+vn 0.571500 0.742117 0.350216
+vn 0.779322 0.405690 0.477570
+vn 0.852640 -0.000000 0.522499
+vn 0.779322 -0.405690 0.477570
+vn 0.571500 -0.742117 0.350216
+vn 0.264217 -0.950775 0.161912
+vn -0.089398 -0.994488 -0.054783
+vn -0.427308 -0.865356 -0.261855
+vn -0.690535 -0.586598 -0.423161
+vn -0.834119 -0.207298 -0.511149
+vn -0.903811 0.207299 -0.374371
+vn -0.748230 0.586597 -0.309928
+vn -0.463010 0.865356 -0.191785
+vn -0.096867 0.994488 -0.040124
+vn 0.286293 0.950775 0.118586
+vn 0.619249 0.742117 0.256501
+vn 0.844436 0.405689 0.349777
+vn 0.923879 -0.000000 0.382684
+vn 0.844436 -0.405689 0.349777
+vn 0.619249 -0.742117 0.256501
+vn 0.286293 -0.950775 0.118586
+vn -0.096867 -0.994488 -0.040124
+vn -0.463010 -0.865356 -0.191785
+vn -0.748230 -0.586597 -0.309927
+vn -0.903811 -0.207299 -0.374371
+vn -0.951248 0.207299 -0.228374
+vn -0.787502 0.586597 -0.189062
+vn -0.487311 0.865356 -0.116993
+vn -0.101951 0.994488 -0.024476
+vn 0.301319 0.950775 0.072341
+vn 0.651751 0.742117 0.156471
+vn 0.888757 0.405689 0.213371
+vn 0.972370 -0.000000 0.233445
+vn 0.888757 -0.405689 0.213372
+vn 0.651751 -0.742117 0.156472
+vn 0.301319 -0.950775 0.072340
+vn -0.101951 -0.994488 -0.024476
+vn -0.487312 -0.865355 -0.116992
+vn -0.787502 -0.586597 -0.189062
+vn -0.951248 -0.207299 -0.228374
+vn -0.975262 0.207298 -0.076755
+vn -0.807382 0.586598 -0.063542
+vn -0.499613 0.865356 -0.039321
+vn -0.104525 0.994488 -0.008226
+vn 0.308926 0.950775 0.024313
+vn 0.668205 0.742116 0.052589
+vn 0.911193 0.405689 0.071713
+vn 0.996917 -0.000000 0.078459
+vn 0.911193 -0.405689 0.071712
+vn 0.668205 -0.742117 0.052589
+vn 0.308926 -0.950775 0.024313
+vn -0.104525 -0.994488 -0.008226
+vn -0.499613 -0.865356 -0.039321
+vn -0.807382 -0.586598 -0.063543
+vn -0.975262 -0.207298 -0.076755
+vn -0.975262 0.207299 0.076754
+vn -0.807382 0.586597 0.063542
+vn -0.499613 0.865356 0.039320
+vn -0.104525 0.994488 0.008226
+vn 0.308926 0.950775 -0.024313
+vn 0.668205 0.742116 -0.052589
+vn 0.911193 0.405689 -0.071712
+vn 0.996917 0.000000 -0.078459
+vn 0.911193 -0.405689 -0.071712
+vn 0.668205 -0.742117 -0.052589
+vn 0.308926 -0.950775 -0.024313
+vn -0.104525 -0.994488 0.008226
+vn -0.499613 -0.865356 0.039320
+vn -0.807382 -0.586597 0.063542
+vn -0.975262 -0.207299 0.076754
+vn -0.951248 0.207298 0.228374
+vn -0.787502 0.586597 0.189063
+vn -0.487311 0.865356 0.116993
+vn -0.101951 0.994488 0.024476
+vn 0.301319 0.950775 -0.072340
+vn 0.651751 0.742117 -0.156471
+vn 0.888757 0.405689 -0.213372
+vn 0.972370 0.000000 -0.233445
+vn 0.888757 -0.405689 -0.213371
+vn 0.651751 -0.742117 -0.156472
+vn 0.301319 -0.950775 -0.072340
+vn -0.101951 -0.994488 0.024476
+vn -0.487311 -0.865356 0.116993
+vn -0.787502 -0.586597 0.189063
+vn -0.951248 -0.207298 0.228375
+vn -0.903811 0.207299 0.374370
+vn -0.748231 0.586597 0.309927
+vn -0.463010 0.865356 0.191785
+vn -0.096867 0.994488 0.040124
+vn 0.286293 0.950775 -0.118586
+vn 0.619250 0.742116 -0.256502
+vn 0.844436 0.405689 -0.349776
+vn 0.923880 0.000000 -0.382683
+vn 0.844436 -0.405689 -0.349776
+vn 0.619250 -0.742116 -0.256501
+vn 0.286292 -0.950775 -0.118586
+vn -0.096867 -0.994488 0.040124
+vn -0.463010 -0.865356 0.191785
+vn -0.748231 -0.586597 0.309927
+vn -0.903811 -0.207299 0.374370
+vn -0.834119 0.207299 0.511149
+vn -0.690535 0.586597 0.423160
+vn -0.427308 0.865356 0.261855
+vn -0.089398 0.994488 0.054783
+vn 0.264217 0.950775 -0.161912
+vn 0.571500 0.742117 -0.350215
+vn 0.779322 0.405690 -0.477569
+vn 0.852640 0.000000 -0.522499
+vn 0.779322 -0.405690 -0.477570
+vn 0.571500 -0.742117 -0.350216
+vn 0.264217 -0.950775 -0.161912
+vn -0.089398 -0.994488 0.054783
+vn -0.427308 -0.865356 0.261855
+vn -0.690535 -0.586597 0.423161
+vn -0.834119 -0.207299 0.511148
+vn -0.743888 0.207298 0.635340
+vn -0.615837 0.586597 0.525974
+vn -0.381084 0.865355 0.325476
+vn -0.079727 0.994488 0.068093
+vn 0.235635 0.950775 -0.201251
+vn 0.509678 0.742116 -0.435307
+vn 0.695019 0.405689 -0.593603
+vn 0.760406 0.000000 -0.649448
+vn 0.695020 -0.405689 -0.593602
+vn 0.509678 -0.742116 -0.435306
+vn 0.235635 -0.950775 -0.201252
+vn -0.079727 -0.994488 0.068093
+vn -0.381084 -0.865355 0.325476
+vn -0.615837 -0.586598 0.525974
+vn -0.743888 -0.207298 0.635340
+vn -0.635341 0.207299 0.743888
+vn -0.525975 0.586598 0.615836
+vn -0.325477 0.865355 0.381084
+vn -0.068094 0.994488 0.079727
+vn 0.201252 0.950775 -0.235635
+vn 0.435307 0.742116 -0.509678
+vn 0.593603 0.405690 -0.695019
+vn 0.649449 0.000000 -0.760406
+vn 0.593603 -0.405690 -0.695019
+vn 0.435307 -0.742116 -0.509678
+vn 0.201252 -0.950775 -0.235635
+vn -0.068094 -0.994488 0.079727
+vn -0.325477 -0.865355 0.381084
+vn -0.525974 -0.586598 0.615836
+vn -0.635341 -0.207299 0.743888
+vn -0.511149 0.207298 0.834119
+vn -0.423160 0.586598 0.690535
+vn -0.261855 0.865356 0.427308
+vn -0.054783 0.994488 0.089398
+vn 0.161912 0.950775 -0.264217
+vn 0.350216 0.742117 -0.571500
+vn 0.477569 0.405689 -0.779323
+vn 0.522499 0.000000 -0.852640
+vn 0.477570 -0.405689 -0.779323
+vn 0.350215 -0.742117 -0.571500
+vn 0.161913 -0.950775 -0.264217
+vn -0.054783 -0.994488 0.089398
+vn -0.261854 -0.865356 0.427308
+vn -0.423160 -0.586598 0.690535
+vn -0.511148 -0.207298 0.834119
+vn -0.374371 0.207299 0.903811
+vn -0.309928 0.586597 0.748230
+vn -0.191785 0.865356 0.463010
+vn -0.040124 0.994488 0.096867
+vn 0.118586 0.950775 -0.286293
+vn 0.256501 0.742116 -0.619250
+vn 0.349777 0.405690 -0.844436
+vn 0.382683 0.000000 -0.923880
+vn 0.349777 -0.405690 -0.844436
+vn 0.256502 -0.742116 -0.619250
+vn 0.118586 -0.950775 -0.286293
+vn -0.040124 -0.994488 0.096867
+vn -0.191785 -0.865356 0.463010
+vn -0.309927 -0.586597 0.748230
+vn -0.374371 -0.207299 0.903811
+vn -0.228374 0.207298 0.951248
+vn -0.189062 0.586597 0.787502
+vn -0.116993 0.865356 0.487311
+vn -0.024476 0.994488 0.101951
+vn 0.072340 0.950775 -0.301319
+vn 0.156471 0.742116 -0.651752
+vn 0.213371 0.405689 -0.888757
+vn 0.233446 0.000000 -0.972370
+vn 0.213372 -0.405689 -0.888757
+vn 0.156471 -0.742116 -0.651752
+vn 0.072340 -0.950776 -0.301319
+vn -0.024476 -0.994488 0.101951
+vn -0.116993 -0.865355 0.487312
+vn -0.189062 -0.586597 0.787502
+vn -0.228374 -0.207298 0.951248
+vn -0.076755 0.207298 0.975262
+vn -0.063543 0.586598 0.807382
+vn -0.039321 0.865356 0.499613
+vn -0.008227 0.994488 0.104525
+vn 0.024313 0.950775 -0.308926
+vn 0.052590 0.742117 -0.668204
+vn 0.071713 0.405689 -0.911193
+vn 0.078459 0.000000 -0.996917
+vn 0.071712 -0.405689 -0.911193
+vn 0.052589 -0.742117 -0.668205
+vn 0.024313 -0.950775 -0.308925
+vn -0.008227 -0.994488 0.104525
+vn -0.039321 -0.865356 0.499613
+vn -0.063543 -0.586597 0.807382
+vn -0.076755 -0.207298 0.975262
+vn 0.076755 0.207298 0.975262
+vn 0.063543 0.586598 0.807382
+vn 0.039321 0.865356 0.499613
+vn 0.008226 0.994488 0.104525
+vn -0.024313 0.950775 -0.308926
+vn -0.052589 0.742117 -0.668205
+vn -0.071713 0.405689 -0.911193
+vn -0.078459 0.000000 -0.996917
+vn -0.071712 -0.405689 -0.911193
+vn -0.052589 -0.742117 -0.668205
+vn -0.024313 -0.950775 -0.308926
+vn 0.008227 -0.994488 0.104525
+vn 0.039321 -0.865356 0.499613
+vn 0.063543 -0.586598 0.807382
+vn 0.076755 -0.207298 0.975262
+vn 0.228374 0.207298 0.951248
+vn 0.189062 0.586597 0.787502
+vn 0.116993 0.865356 0.487311
+vn 0.024476 0.994488 0.101951
+vn -0.072340 0.950775 -0.301319
+vn -0.156472 0.742116 -0.651752
+vn -0.213372 0.405689 -0.888757
+vn -0.233446 0.000000 -0.972370
+vn -0.213372 -0.405689 -0.888757
+vn -0.156472 -0.742116 -0.651751
+vn -0.072340 -0.950775 -0.301319
+vn 0.024476 -0.994488 0.101951
+vn 0.116993 -0.865356 0.487311
+vn 0.189062 -0.586597 0.787502
+vn 0.228374 -0.207298 0.951248
+vn 0.374371 0.207298 0.903811
+vn 0.309928 0.586597 0.748230
+vn 0.191785 0.865356 0.463009
+vn 0.040124 0.994488 0.096867
+vn -0.118586 0.950775 -0.286293
+vn -0.256502 0.742117 -0.619249
+vn -0.349777 0.405689 -0.844436
+vn -0.382684 0.000000 -0.923879
+vn -0.349777 -0.405689 -0.844436
+vn -0.256501 -0.742117 -0.619249
+vn -0.118587 -0.950775 -0.286293
+vn 0.040124 -0.994488 0.096867
+vn 0.191785 -0.865356 0.463010
+vn 0.309928 -0.586597 0.748230
+vn 0.374371 -0.207298 0.903811
+vn 0.511148 0.207299 0.834119
+vn 0.423160 0.586597 0.690535
+vn 0.261854 0.865356 0.427308
+vn 0.054783 0.994488 0.089398
+vn -0.161912 0.950775 -0.264217
+vn -0.350215 0.742116 -0.571500
+vn -0.477569 0.405690 -0.779323
+vn -0.522498 0.000000 -0.852641
+vn -0.477569 -0.405690 -0.779323
+vn -0.350216 -0.742116 -0.571500
+vn -0.161912 -0.950775 -0.264217
+vn 0.054783 -0.994488 0.089398
+vn 0.261854 -0.865356 0.427308
+vn 0.423160 -0.586597 0.690535
+vn 0.511148 -0.207299 0.834119
+vn 0.635341 0.207299 0.743888
+vn 0.525974 0.586597 0.615837
+vn 0.325476 0.865356 0.381084
+vn 0.068093 0.994488 0.079727
+vn -0.201251 0.950775 -0.235635
+vn -0.435306 0.742116 -0.509678
+vn -0.593602 0.405689 -0.695020
+vn -0.649448 0.000000 -0.760406
+vn -0.593603 -0.405689 -0.695020
+vn -0.435306 -0.742117 -0.509678
+vn -0.201251 -0.950775 -0.235635
+vn 0.068093 -0.994488 0.079727
+vn 0.325476 -0.865356 0.381084
+vn 0.525974 -0.586597 0.615837
+vn 0.635340 -0.207299 0.743888
+vn 0.743888 0.207298 0.635341
+vn 0.615836 0.586598 0.525974
+vn 0.381084 0.865355 0.325476
+vn 0.079727 0.994488 0.068093
+vn -0.235635 0.950775 -0.201251
+vn -0.509678 0.742116 -0.435306
+vn -0.695019 0.405689 -0.593603
+vn -0.760406 0.000000 -0.649448
+vn -0.695019 -0.405690 -0.593603
+vn -0.509678 -0.742116 -0.435306
+vn -0.235635 -0.950775 -0.201251
+vn 0.079727 -0.994488 0.068093
+vn 0.381084 -0.865355 0.325476
+vn 0.615836 -0.586598 0.525974
+vn 0.743888 -0.207298 0.635341
+vn 0.834119 0.207299 0.511149
+vn 0.690535 0.586597 0.423160
+vn 0.427308 0.865356 0.261855
+vn 0.089398 0.994488 0.054783
+vn -0.264217 0.950776 -0.161912
+vn -0.571500 0.742116 -0.350216
+vn -0.779322 0.405689 -0.477570
+vn -0.852640 0.000000 -0.522499
+vn -0.779322 -0.405689 -0.477570
+vn -0.571500 -0.742116 -0.350216
+vn -0.264216 -0.950775 -0.161913
+vn 0.089398 -0.994488 0.054783
+vn 0.427308 -0.865356 0.261855
+vn 0.690535 -0.586597 0.423161
+vn 0.834119 -0.207299 0.511148
+vn 0.903811 0.207298 0.374371
+vn 0.748230 0.586597 0.309928
+vn 0.463010 0.865356 0.191785
+vn 0.096867 0.994488 0.040124
+vn -0.286293 0.950775 -0.118587
+vn -0.619250 0.742116 -0.256502
+vn -0.844436 0.405689 -0.349778
+vn -0.923879 0.000000 -0.382684
+vn -0.844436 -0.405689 -0.349777
+vn -0.619249 -0.742116 -0.256502
+vn -0.286293 -0.950776 -0.118586
+vn 0.096867 -0.994488 0.040124
+vn 0.463010 -0.865356 0.191785
+vn 0.748230 -0.586597 0.309928
+vn 0.903811 -0.207298 0.374371
+vn 0.951248 0.207298 0.228375
+vn 0.787501 0.586598 0.189063
+vn 0.487311 0.865356 0.116993
+vn 0.101951 0.994488 0.024476
+vn -0.301319 0.950775 -0.072340
+vn -0.651752 0.742116 -0.156472
+vn -0.888757 0.405690 -0.213371
+vn -0.972370 0.000000 -0.233446
+vn -0.888757 -0.405690 -0.213372
+vn -0.651752 -0.742116 -0.156471
+vn -0.301319 -0.950775 -0.072340
+vn 0.101951 -0.994488 0.024476
+vn 0.487311 -0.865356 0.116993
+vn 0.787502 -0.586598 0.189063
+vn 0.951248 -0.207298 0.228375
+vn 0.975262 0.207299 0.076755
+vn 0.807382 0.586597 0.063543
+vn 0.499613 0.865356 0.039321
+vn 0.104525 0.994488 0.008226
+vn -0.308926 0.950775 -0.024313
+vn -0.668205 0.742117 -0.052589
+vn -0.911194 0.405689 -0.071713
+vn -0.996917 0.000000 -0.078459
+vn -0.911194 -0.405689 -0.071713
+vn -0.668205 -0.742117 -0.052589
+vn -0.308926 -0.950775 -0.024313
+vn 0.104525 -0.994488 0.008226
+vn 0.499613 -0.865356 0.039321
+vn 0.807382 -0.586597 0.063543
+vn 0.975262 -0.207299 0.076755
+vn 0.975262 0.207299 -0.076755
+vn 0.807382 0.586597 -0.063543
+vn -0.668205 0.742117 0.052589
+vn -0.911194 0.405689 0.071712
+vn -0.911194 -0.405689 0.071713
+vn -0.668205 -0.742117 0.052589
+vn 0.807382 -0.586597 -0.063543
+vn 0.975262 -0.207298 -0.076755
+vn 0.951248 0.207298 -0.228374
+vn 0.787502 0.586598 -0.189062
+vn 0.487311 0.865356 -0.116993
+vn -0.301319 0.950775 0.072340
+vn -0.651751 0.742117 0.156472
+vn -0.651751 -0.742117 0.156471
+vn -0.301319 -0.950775 0.072341
+vn 0.487311 -0.865356 -0.116992
+vn 0.787502 -0.586598 -0.189062
+vn 0.951248 -0.207298 -0.228374
+vn 0.748231 0.586597 -0.309928
+vn -0.619250 0.742117 0.256501
+vn -0.619250 -0.742117 0.256502
+vn 0.748231 -0.586597 -0.309927
+vn 0.903810 -0.207299 -0.374371
+vn 0.834119 0.207299 -0.511148
+vn 0.834119 -0.207299 -0.511148
+vn 0.743888 0.207298 -0.635341
+vn 0.381084 0.865356 -0.325476
+vn -0.509678 0.742117 0.435307
+vn 0.381083 -0.865356 -0.325477
+vn 0.615836 -0.586597 -0.525974
+vn 0.743888 -0.207298 -0.635341
+vn 0.635341 0.207299 -0.743888
+vn 0.525974 0.586597 -0.615836
+vn 0.325476 0.865356 -0.381084
+vn -0.201252 0.950775 0.235635
+vn -0.593603 0.405689 0.695020
+vn -0.593603 -0.405689 0.695019
+vn -0.435306 -0.742116 0.509678
+vn -0.201251 -0.950775 0.235635
+vn 0.325476 -0.865355 -0.381084
+vn 0.635340 -0.207299 -0.743888
+vn 0.261854 0.865356 -0.427307
+vn -0.350216 0.742116 0.571500
+vn -0.477569 0.405690 0.779322
+vn -0.477570 -0.405689 0.779322
+vn -0.350215 -0.742116 0.571500
+vn -0.161912 -0.950776 0.264217
+vn 0.261854 -0.865356 -0.427307
+vn 0.191785 0.865356 -0.463010
+vn -0.118587 0.950775 0.286293
+vn -0.256501 0.742117 0.619250
+vn -0.349777 0.405689 0.844436
+vn -0.349777 -0.405689 0.844436
+vn -0.256502 -0.742117 0.619249
+vn -0.118586 -0.950775 0.286293
+vn 0.374371 -0.207298 -0.903811
+vn 0.189062 0.586597 -0.787502
+vn -0.156472 0.742116 0.651751
+vn -0.156472 -0.742116 0.651752
+vn 0.189062 -0.586597 -0.787502
+vn 0.063543 0.586598 -0.807382
+vn -0.052589 0.742117 0.668205
+vn -0.071712 0.405689 0.911193
+vn -0.071713 -0.405689 0.911193
+vn -0.052589 -0.742117 0.668205
+vn 0.063543 -0.586598 -0.807382
+vn -0.063543 0.586597 -0.807382
+vn 0.024313 0.950775 0.308925
+vn 0.052589 0.742117 0.668205
+vn 0.071712 0.405689 0.911193
+vn 0.071713 -0.405689 0.911193
+vn 0.052590 -0.742117 0.668204
+vn 0.024313 -0.950775 0.308926
+vn -0.063543 -0.586598 -0.807382
+vn -0.189062 0.586597 -0.787502
+vn -0.116993 0.865355 -0.487312
+vn 0.072340 0.950776 0.301319
+vn 0.213372 0.405689 0.888757
+vn 0.213371 -0.405689 0.888757
+vn -0.116993 -0.865356 -0.487311
+vn -0.189062 -0.586598 -0.787502
+vn -0.309927 0.586597 -0.748230
+vn -0.191785 0.865356 -0.463010
+vn 0.256502 0.742116 0.619250
+vn 0.349777 0.405690 0.844436
+vn 0.349777 -0.405690 0.844436
+vn 0.256501 -0.742116 0.619250
+vn -0.309928 -0.586597 -0.748230
+vn -0.374371 -0.207299 -0.903811
+vn -0.511148 0.207298 -0.834119
+vn -0.423160 0.586598 -0.690535
+vn -0.261854 0.865355 -0.427308
+vn 0.161913 0.950775 0.264217
+vn 0.350215 0.742117 0.571500
+vn 0.477570 0.405689 0.779323
+vn 0.477569 -0.405689 0.779323
+vn 0.350216 -0.742117 0.571500
+vn -0.261855 -0.865355 -0.427308
+vn -0.423160 -0.586598 -0.690535
+vn -0.511149 -0.207298 -0.834119
+vn -0.635341 0.207299 -0.743888
+vn 0.593603 0.405690 0.695019
+vn 0.649449 0.000000 0.760406
+vn 0.593603 -0.405690 0.695019
+vn -0.525975 -0.586598 -0.615836
+vn -0.635341 -0.207299 -0.743888
+vn -0.743888 0.207298 -0.635340
+vn -0.615837 0.586597 -0.525974
+vn -0.381084 0.865355 -0.325476
+vn 0.509678 0.742116 0.435306
+vn 0.695020 0.405689 0.593602
+vn 0.695019 -0.405689 0.593603
+vn 0.509678 -0.742116 0.435307
+vn 0.235635 -0.950775 0.201251
+vn -0.381084 -0.865355 -0.325476
+vn -0.615837 -0.586597 -0.525974
+vn -0.743888 -0.207298 -0.635340
+vn -0.834119 0.207299 -0.511149
+vn -0.690535 0.586597 -0.423161
+vn -0.427308 0.865356 -0.261854
+vn -0.690535 -0.586597 -0.423161
+vn -0.834119 -0.207299 -0.511149
+vn -0.748230 0.586597 -0.309927
+vn 0.619250 0.742117 0.256501
+vn 0.844436 0.405690 0.349777
+vn 0.844436 -0.405690 0.349777
+vn -0.748230 -0.586597 -0.309928
+vn -0.951248 0.207298 -0.228374
+vn -0.787501 0.586598 -0.189062
+vn -0.487312 0.865355 -0.116992
+vn 0.301319 0.950775 0.072340
+vn 0.651751 0.742117 0.156472
+vn 0.888757 0.405689 0.213372
+vn 0.888757 -0.405689 0.213371
+vn 0.651751 -0.742117 0.156471
+vn 0.301319 -0.950775 0.072341
+vn -0.487311 -0.865356 -0.116993
+vn -0.787501 -0.586598 -0.189062
+vn -0.951248 -0.207298 -0.228374
+vn -0.975262 0.207299 -0.076755
+vn -0.807382 0.586597 -0.063543
+vn 0.668205 0.742117 0.052589
+vn 0.911194 0.405689 0.071712
+vn 0.911194 -0.405689 0.071713
+vn 0.668205 -0.742116 0.052589
+vn -0.807382 -0.586597 -0.063542
+vn -0.975262 -0.207299 -0.076755
+vn -0.975262 0.207298 0.076754
+vn -0.975262 -0.207298 0.076754
+vn -0.951248 0.207299 0.228375
+vn 0.651751 0.742117 -0.156472
+vn 0.888757 0.405689 -0.213371
+vn 0.888757 -0.405689 -0.213372
+vn 0.651751 -0.742117 -0.156471
+vn -0.487311 -0.865355 0.116993
+vn -0.951248 -0.207299 0.228374
+vn 0.286293 0.950775 -0.118587
+vn 0.619250 0.742117 -0.256501
+vn 0.844437 -0.405689 -0.349776
+vn 0.619249 -0.742117 -0.256502
+vn 0.286293 -0.950775 -0.118586
+vn -0.834119 0.207298 0.511148
+vn -0.427308 0.865355 0.261855
+vn 0.571500 0.742116 -0.350216
+vn 0.779322 0.405689 -0.477570
+vn 0.779322 -0.405689 -0.477569
+vn 0.571500 -0.742116 -0.350215
+vn -0.427308 -0.865355 0.261855
+vn -0.690535 -0.586597 0.423160
+vn -0.834119 -0.207298 0.511149
+vn -0.743888 0.207299 0.635340
+vn -0.615837 0.586598 0.525974
+vn -0.381084 0.865356 0.325476
+vn 0.235635 0.950775 -0.201252
+vn 0.509678 0.742117 -0.435306
+vn 0.695020 0.405690 -0.593602
+vn 0.695019 -0.405689 -0.593603
+vn 0.509678 -0.742117 -0.435306
+vn -0.381084 -0.865356 0.325476
+vn -0.743888 -0.207299 0.635340
+vn -0.635341 0.207298 0.743888
+vn -0.525974 0.586598 0.615836
+vn 0.593603 0.405689 -0.695019
+vn 0.649448 0.000000 -0.760406
+vn 0.593603 -0.405689 -0.695019
+vn -0.635341 -0.207298 0.743888
+vn -0.511148 0.207299 0.834119
+vn -0.423161 0.586597 0.690535
+vn -0.261854 0.865356 0.427308
+vn 0.350215 0.742116 -0.571500
+vn 0.477570 0.405690 -0.779322
+vn 0.477569 -0.405690 -0.779323
+vn 0.350216 -0.742116 -0.571500
+vn 0.161912 -0.950775 -0.264217
+vn -0.423160 -0.586597 0.690535
+vn -0.511149 -0.207299 0.834119
+vn -0.374371 0.207298 0.903811
+vn -0.309927 0.586598 0.748230
+vn -0.191785 0.865355 0.463010
+vn 0.256502 0.742117 -0.619249
+vn 0.349777 0.405689 -0.844436
+vn 0.349777 -0.405689 -0.844436
+vn 0.256501 -0.742117 -0.619250
+vn -0.191785 -0.865355 0.463010
+vn -0.309927 -0.586598 0.748230
+vn -0.374371 -0.207298 0.903811
+vn -0.189062 0.586598 0.787501
+vn -0.116993 0.865355 0.487312
+vn 0.213372 0.405690 -0.888757
+vn 0.213371 -0.405690 -0.888757
+vn 0.072340 -0.950775 -0.301319
+vn -0.116993 -0.865356 0.487311
+vn -0.189062 -0.586598 0.787501
+vn -0.063543 0.586597 0.807382
+vn 0.024313 0.950775 -0.308925
+vn 0.052589 0.742116 -0.668205
+vn 0.071712 0.405689 -0.911193
+vn 0.071713 -0.405689 -0.911193
+vn 0.052590 -0.742116 -0.668205
+vn 0.024313 -0.950775 -0.308926
+vn -0.063543 -0.586598 0.807382
+vn 0.063543 0.586597 0.807382
+vn 0.008227 0.994488 0.104525
+vn -0.052589 0.742116 -0.668205
+vn -0.071712 0.405689 -0.911193
+vn -0.071713 -0.405689 -0.911193
+vn -0.052589 -0.742116 -0.668205
+vn 0.008226 -0.994488 0.104525
+vn 0.063543 -0.586597 0.807382
+vn 0.189062 0.586598 0.787502
+vn -0.156472 0.742117 -0.651751
+vn -0.156471 -0.742117 -0.651751
+vn 0.189062 -0.586598 0.787502
+vn 0.374372 0.207298 0.903811
+vn -0.118587 0.950775 -0.286293
+vn -0.256502 0.742116 -0.619250
+vn -0.349777 0.405690 -0.844436
+vn -0.349777 -0.405690 -0.844436
+vn -0.256502 -0.742116 -0.619250
+vn -0.118586 -0.950775 -0.286293
+vn 0.191785 -0.865356 0.463009
+vn 0.511148 0.207298 0.834119
+vn -0.350216 0.742116 -0.571500
+vn -0.477569 0.405689 -0.779323
+vn -0.477569 -0.405689 -0.779323
+vn -0.350215 -0.742116 -0.571500
+vn 0.511148 -0.207298 0.834119
+vn 0.635340 0.207299 0.743888
+vn 0.068094 0.994488 0.079727
+vn -0.593602 0.405690 -0.695019
+vn -0.593602 -0.405690 -0.695020
+vn -0.435306 -0.742116 -0.509678
+vn 0.743888 0.207299 0.635341
+vn 0.615837 0.586597 0.525974
+vn 0.381084 0.865356 0.325476
+vn -0.695019 -0.405689 -0.593603
+vn 0.381084 -0.865356 0.325476
+vn 0.615836 -0.586597 0.525974
+vn 0.743888 -0.207299 0.635341
+vn 0.834119 0.207298 0.511149
+vn 0.690534 0.586598 0.423161
+vn 0.427308 0.865355 0.261855
+vn -0.264217 0.950775 -0.161913
+vn -0.264217 -0.950775 -0.161912
+vn 0.427308 -0.865355 0.261855
+vn 0.690535 -0.586598 0.423160
+vn 0.834119 -0.207298 0.511149
+vn 0.903810 0.207299 0.374371
+vn -0.286293 0.950776 -0.118586
+vn -0.844436 0.405690 -0.349777
+vn -0.844436 -0.405690 -0.349778
+vn -0.619250 -0.742116 -0.256502
+vn -0.286293 -0.950775 -0.118587
+vn 0.903811 -0.207299 0.374371
+vn 0.787502 0.586597 0.189063
+vn -0.301319 0.950775 -0.072341
+vn -0.651751 0.742117 -0.156471
+vn -0.888757 0.405689 -0.213372
+vn -0.888757 -0.405689 -0.213371
+vn -0.651751 -0.742116 -0.156472
+vn 0.787502 -0.586597 0.189063
+vn 0.975262 0.207298 0.076755
+vn 0.807382 0.586598 0.063543
+vn -0.668205 0.742116 -0.052589
+vn -0.911193 0.405689 -0.071713
+vn -0.911193 -0.405689 -0.071713
+vn -0.668205 -0.742116 -0.052589
+vn 0.975262 -0.207298 0.076755
+usemtl Material.001
+s off
+f 1//1 16//1 17//1
+f 2//2 17//2 18//2
+f 3//3 18//3 19//3
+f 4//4 19//4 20//4
+f 5//5 20//5 6//5
+f 6//6 21//6 7//6
+f 7//7 22//7 8//7
+f 8//8 23//8 9//8
+f 9//9 24//9 10//9
+f 10//10 25//10 11//10
+f 11//11 26//11 12//11
+f 12//12 27//12 28//12
+f 13//13 28//13 29//13
+f 14//14 29//14 30//14
+f 1//15 15//15 30//15
+f 16//16 31//16 32//16
+f 17//17 32//17 33//17
+f 18//18 33//18 34//18
+f 19//19 34//19 35//19
+f 20//20 35//20 21//20
+f 21//21 36//21 22//21
+f 22//22 37//22 23//22
+f 23//23 38//23 24//23
+f 24//24 39//24 25//24
+f 25//25 40//25 26//25
+f 26//26 41//26 27//26
+f 27//27 42//27 43//27
+f 28//28 43//28 44//28
+f 29//29 44//29 45//29
+f 30//30 45//30 31//30
+f 31//31 46//31 47//31
+f 32//32 47//32 48//32
+f 33//33 48//33 49//33
+f 34//34 49//34 50//34
+f 35//35 50//35 36//35
+f 36//36 51//36 37//36
+f 37//37 52//37 38//37
+f 38//38 53//38 39//38
+f 39//39 54//39 40//39
+f 40//40 55//40 41//40
+f 41//41 56//41 42//41
+f 42//42 57//42 58//42
+f 43//43 58//43 59//43
+f 44//44 59//44 60//44
+f 45//45 60//45 46//45
+f 46//46 61//46 62//46
+f 47//47 62//47 63//47
+f 48//48 63//48 64//48
+f 49//49 64//49 65//49
+f 50//50 65//50 51//50
+f 51//51 66//51 52//51
+f 52//52 67//52 53//52
+f 53//53 68//53 54//53
+f 54//54 69//54 55//54
+f 55//55 70//55 56//55
+f 56//56 71//56 72//56
+f 57//57 72//57 73//57
+f 58//58 73//58 59//58
+f 59//59 74//59 75//59
+f 60//60 75//60 61//60
+f 61//61 76//61 77//61
+f 62//62 77//62 78//62
+f 63//63 78//63 79//63
+f 64//64 79//64 80//64
+f 65//65 80//65 66//65
+f 66//66 81//66 82//66
+f 67//67 82//67 68//67
+f 68//68 83//68 69//68
+f 69//69 84//69 70//69
+f 70//70 85//70 71//70
+f 71//71 86//71 72//71
+f 72//72 87//72 88//72
+f 73//73 88//73 89//73
+f 74//74 89//74 90//74
+f 75//75 90//75 76//75
+f 76//76 91//76 92//76
+f 77//77 92//77 93//77
+f 78//78 93//78 94//78
+f 79//79 94//79 95//79
+f 80//80 95//80 81//80
+f 81//81 96//81 82//81
+f 82//82 97//82 83//82
+f 83//83 98//83 84//83
+f 84//84 99//84 85//84
+f 85//85 100//85 86//85
+f 86//86 101//86 87//86
+f 87//87 102//87 103//87
+f 88//88 103//88 104//88
+f 89//89 104//89 105//89
+f 90//90 105//90 91//90
+f 91//91 106//91 107//91
+f 92//92 107//92 108//92
+f 93//93 108//93 109//93
+f 94//94 109//94 110//94
+f 95//95 110//95 96//95
+f 96//96 111//96 97//96
+f 97//97 112//97 98//97
+f 98//98 113//98 99//98
+f 99//99 114//99 100//99
+f 100//100 115//100 101//100
+f 101//101 116//101 102//101
+f 102//102 117//102 118//102
+f 103//103 118//103 119//103
+f 104//104 119//104 120//104
+f 105//105 120//105 106//105
+f 106//106 121//106 122//106
+f 107//107 122//107 123//107
+f 108//108 123//108 124//108
+f 109//109 124//109 125//109
+f 110//110 125//110 111//110
+f 111//111 126//111 112//111
+f 112//112 127//112 113//112
+f 113//113 128//113 114//113
+f 114//114 129//114 115//114
+f 115//115 130//115 116//115
+f 116//116 131//116 132//116
+f 117//117 132//117 133//117
+f 118//118 133//118 134//118
+f 119//119 134//119 135//119
+f 120//120 135//120 121//120
+f 121//121 136//121 137//121
+f 122//122 137//122 138//122
+f 123//123 138//123 139//123
+f 124//124 139//124 140//124
+f 125//125 140//125 141//125
+f 126//126 141//126 127//126
+f 127//127 142//127 128//127
+f 128//128 143//128 129//128
+f 129//129 144//129 130//129
+f 130//130 145//130 131//130
+f 131//131 146//131 132//131
+f 132//132 147//132 148//132
+f 133//133 148//133 149//133
+f 134//134 149//134 150//134
+f 135//135 150//135 136//135
+f 136//136 151//136 152//136
+f 137//137 152//137 153//137
+f 138//138 153//138 154//138
+f 139//139 154//139 155//139
+f 140//140 155//140 141//140
+f 141//141 156//141 142//141
+f 142//142 157//142 143//142
+f 143//143 158//143 144//143
+f 144//144 159//144 145//144
+f 145//145 160//145 146//145
+f 146//146 161//146 162//146
+f 147//147 162//147 163//147
+f 148//148 163//148 149//148
+f 149//149 164//149 165//149
+f 150//150 165//150 151//150
+f 151//151 166//151 152//151
+f 152//152 167//152 168//152
+f 153//153 168//153 154//153
+f 154//154 169//154 170//154
+f 155//155 170//155 156//155
+f 156//156 171//156 172//156
+f 157//157 172//157 173//157
+f 158//158 173//158 174//158
+f 159//159 174//159 175//159
+f 160//160 175//160 176//160
+f 161//161 176//161 177//161
+f 162//162 177//162 163//162
+f 163//163 178//163 164//163
+f 164//164 179//164 165//164
+f 165//165 180//165 151//165
+f 166//166 181//166 167//166
+f 167//167 182//167 168//167
+f 168//168 183//168 169//168
+f 169//169 184//169 170//169
+f 170//170 185//170 186//170
+f 171//171 186//171 187//171
+f 172//172 187//172 188//172
+f 173//173 188//173 189//173
+f 174//174 189//174 190//174
+f 175//175 190//175 191//175
+f 176//176 191//176 192//176
+f 177//177 192//177 193//177
+f 178//178 193//178 194//178
+f 179//179 194//179 180//179
+f 180//180 195//180 166//180
+f 181//181 196//181 182//181
+f 182//182 197//182 183//182
+f 183//183 198//183 184//183
+f 184//184 199//184 185//184
+f 185//185 200//185 201//185
+f 186//186 201//186 202//186
+f 187//187 202//187 203//187
+f 188//188 203//188 204//188
+f 189//189 204//189 205//189
+f 190//190 205//190 206//190
+f 191//191 206//191 207//191
+f 192//192 207//192 193//192
+f 193//193 208//193 194//193
+f 194//194 209//194 195//194
+f 195//195 210//195 181//195
+f 196//196 211//196 197//196
+f 197//197 212//197 198//197
+f 198//198 213//198 199//198
+f 199//199 214//199 200//199
+f 200//200 215//200 216//200
+f 201//201 216//201 217//201
+f 202//202 217//202 218//202
+f 203//203 218//203 219//203
+f 204//204 219//204 220//204
+f 205//205 220//205 221//205
+f 206//206 221//206 222//206
+f 207//207 222//207 223//207
+f 208//208 223//208 209//208
+f 209//209 224//209 210//209
+f 210//210 225//210 196//210
+f 211//211 226//211 212//211
+f 212//212 227//212 213//212
+f 213//213 228//213 214//213
+f 214//214 229//214 215//214
+f 215//215 230//215 216//215
+f 216//216 231//216 217//216
+f 217//217 232//217 233//217
+f 218//218 233//218 234//218
+f 219//219 234//219 235//219
+f 220//220 235//220 236//220
+f 221//221 236//221 237//221
+f 222//222 237//222 223//222
+f 223//223 238//223 224//223
+f 224//224 239//224 225//224
+f 225//225 240//225 211//225
+f 226//226 241//226 227//226
+f 227//227 242//227 228//227
+f 228//228 243//228 229//228
+f 229//229 244//229 230//229
+f 230//230 245//230 246//230
+f 231//231 246//231 247//231
+f 232//232 247//232 248//232
+f 233//233 248//233 249//233
+f 234//234 249//234 250//234
+f 235//235 250//235 251//235
+f 236//236 251//236 252//236
+f 237//237 252//237 238//237
+f 238//238 253//238 239//238
+f 239//239 254//239 240//239
+f 240//240 255//240 226//240
+f 241//241 256//241 242//241
+f 242//242 257//242 243//242
+f 243//243 258//243 244//243
+f 244//244 259//244 245//244
+f 245//245 260//245 261//245
+f 246//246 261//246 262//246
+f 247//247 262//247 263//247
+f 248//248 263//248 264//248
+f 249//249 264//249 265//249
+f 250//250 265//250 266//250
+f 251//251 266//251 252//251
+f 252//252 267//252 253//252
+f 253//253 268//253 254//253
+f 254//254 269//254 255//254
+f 255//255 270//255 241//255
+f 256//256 271//256 257//256
+f 257//257 272//257 258//257
+f 258//258 273//258 259//258
+f 259//259 274//259 260//259
+f 260//260 275//260 276//260
+f 261//261 276//261 277//261
+f 262//262 277//262 278//262
+f 263//263 278//263 279//263
+f 264//264 279//264 280//264
+f 265//265 280//265 281//265
+f 266//266 281//266 282//266
+f 267//267 282//267 268//267
+f 268//268 283//268 269//268
+f 269//269 284//269 270//269
+f 270//270 285//270 256//270
+f 271//271 286//271 272//271
+f 272//272 287//272 273//272
+f 273//273 288//273 289//273
+f 274//274 289//274 290//274
+f 275//275 290//275 276//275
+f 276//276 291//276 277//276
+f 277//277 292//277 293//277
+f 278//278 293//278 294//278
+f 279//279 294//279 295//279
+f 280//280 295//280 296//280
+f 281//281 296//281 297//281
+f 282//282 297//282 283//282
+f 283//283 298//283 284//283
+f 284//284 299//284 285//284
+f 285//285 300//285 271//285
+f 286//286 301//286 287//286
+f 287//287 302//287 288//287
+f 288//288 303//288 289//288
+f 289//289 304//289 290//289
+f 290//290 305//290 306//290
+f 291//291 306//291 307//291
+f 292//292 307//292 308//292
+f 293//293 308//293 309//293
+f 294//294 309//294 310//294
+f 295//295 310//295 296//295
+f 296//296 311//296 312//296
+f 297//297 312//297 298//297
+f 298//298 313//298 299//298
+f 299//299 314//299 300//299
+f 300//300 315//300 286//300
+f 301//301 316//301 302//301
+f 302//302 317//302 303//302
+f 303//303 318//303 304//303
+f 304//304 319//304 305//304
+f 305//305 320//305 321//305
+f 306//306 321//306 322//306
+f 307//307 322//307 323//307
+f 308//308 323//308 324//308
+f 309//309 324//309 325//309
+f 310//310 325//310 326//310
+f 311//311 326//311 327//311
+f 312//312 327//312 313//312
+f 313//313 328//313 314//313
+f 314//314 329//314 315//314
+f 315//315 330//315 301//315
+f 316//316 331//316 317//316
+f 317//317 332//317 318//317
+f 318//318 333//318 319//318
+f 319//319 334//319 335//319
+f 320//320 335//320 336//320
+f 321//321 336//321 337//321
+f 322//322 337//322 338//322
+f 323//323 338//323 339//323
+f 324//324 339//324 340//324
+f 325//325 340//325 341//325
+f 326//326 341//326 342//326
+f 327//327 342//327 328//327
+f 328//328 343//328 329//328
+f 329//329 344//329 330//329
+f 330//330 345//330 316//330
+f 331//331 346//331 332//331
+f 332//332 347//332 333//332
+f 333//333 348//333 334//333
+f 334//334 349//334 335//334
+f 335//335 350//335 351//335
+f 336//336 351//336 352//336
+f 337//337 352//337 353//337
+f 338//338 353//338 354//338
+f 339//339 354//339 355//339
+f 340//340 355//340 356//340
+f 341//341 356//341 357//341
+f 342//342 357//342 343//342
+f 343//343 358//343 344//343
+f 344//344 359//344 345//344
+f 345//345 360//345 331//345
+f 346//346 361//346 347//346
+f 347//347 362//347 348//347
+f 348//348 363//348 349//348
+f 349//349 364//349 350//349
+f 350//350 365//350 366//350
+f 351//351 366//351 367//351
+f 352//352 367//352 368//352
+f 353//353 368//353 369//353
+f 354//354 369//354 370//354
+f 355//355 370//355 371//355
+f 356//356 371//356 372//356
+f 357//357 372//357 358//357
+f 358//358 373//358 359//358
+f 359//359 374//359 360//359
+f 360//360 375//360 346//360
+f 361//361 376//361 362//361
+f 362//362 377//362 363//362
+f 363//363 378//363 364//363
+f 364//364 379//364 365//364
+f 365//365 380//365 381//365
+f 366//366 381//366 382//366
+f 367//367 382//367 383//367
+f 368//368 383//368 384//368
+f 369//369 384//369 385//369
+f 370//370 385//370 386//370
+f 371//371 386//371 387//371
+f 372//372 387//372 373//372
+f 373//373 388//373 374//373
+f 374//374 389//374 375//374
+f 375//375 390//375 361//375
+f 376//376 391//376 377//376
+f 377//377 392//377 378//377
+f 378//378 393//378 379//378
+f 379//379 394//379 380//379
+f 380//380 395//380 396//380
+f 381//381 396//381 397//381
+f 382//382 397//382 398//382
+f 383//383 398//383 399//383
+f 384//384 399//384 400//384
+f 385//385 400//385 386//385
+f 386//386 401//386 387//386
+f 387//387 402//387 388//387
+f 388//388 403//388 389//388
+f 389//389 404//389 390//389
+f 390//390 405//390 376//390
+f 391//391 406//391 392//391
+f 392//392 407//392 393//392
+f 393//393 408//393 394//393
+f 394//394 409//394 410//394
+f 395//395 410//395 411//395
+f 396//396 411//396 412//396
+f 397//397 412//397 413//397
+f 398//398 413//398 414//398
+f 399//399 414//399 415//399
+f 400//400 415//400 416//400
+f 401//401 416//401 417//401
+f 402//402 417//402 403//402
+f 403//403 418//403 404//403
+f 404//404 419//404 405//404
+f 405//405 420//405 391//405
+f 406//406 421//406 407//406
+f 407//407 422//407 408//407
+f 408//408 423//408 409//408
+f 409//409 424//409 410//409
+f 410//410 425//410 426//410
+f 411//411 426//411 427//411
+f 412//412 427//412 428//412
+f 413//413 428//413 429//413
+f 414//414 429//414 430//414
+f 415//415 430//415 431//415
+f 416//416 431//416 432//416
+f 417//417 432//417 418//417
+f 418//418 433//418 419//418
+f 419//419 434//419 420//419
+f 420//420 435//420 406//420
+f 421//421 436//421 422//421
+f 422//422 437//422 423//422
+f 423//423 438//423 439//423
+f 424//424 439//424 440//424
+f 425//425 440//425 441//425
+f 426//426 441//426 442//426
+f 427//427 442//427 443//427
+f 428//428 443//428 444//428
+f 429//429 444//429 445//429
+f 430//430 445//430 446//430
+f 431//431 446//431 447//431
+f 432//432 447//432 433//432
+f 433//433 448//433 434//433
+f 434//434 449//434 435//434
+f 435//435 450//435 421//435
+f 436//436 451//436 437//436
+f 437//437 452//437 438//437
+f 438//438 453//438 439//438
+f 439//439 454//439 440//439
+f 440//440 455//440 456//440
+f 441//441 456//441 457//441
+f 442//442 457//442 458//442
+f 443//443 458//443 459//443
+f 444//444 459//444 460//444
+f 445//445 460//445 461//445
+f 446//446 461//446 447//446
+f 447//447 462//447 463//447
+f 448//448 463//448 449//448
+f 449//449 464//449 465//449
+f 450//450 465//450 436//450
+f 451//451 466//451 467//451
+f 452//452 467//452 468//452
+f 453//453 468//453 454//453
+f 454//454 469//454 470//454
+f 455//455 470//455 471//455
+f 456//456 471//456 457//456
+f 457//457 472//457 458//457
+f 458//458 473//458 459//458
+f 459//459 474//459 460//459
+f 460//460 475//460 461//460
+f 461//461 476//461 462//461
+f 462//462 477//462 478//462
+f 463//463 478//463 479//463
+f 464//464 479//464 480//464
+f 465//465 480//465 466//465
+f 466//466 481//466 482//466
+f 467//467 482//467 483//467
+f 468//468 483//468 484//468
+f 469//469 484//469 485//469
+f 470//470 485//470 471//470
+f 471//471 486//471 472//471
+f 472//472 487//472 473//472
+f 473//473 488//473 474//473
+f 474//474 489//474 475//474
+f 475//475 490//475 476//475
+f 476//476 491//476 492//476
+f 477//477 492//477 493//477
+f 478//478 493//478 494//478
+f 479//479 494//479 495//479
+f 480//480 495//480 481//480
+f 481//481 496//481 497//481
+f 482//482 497//482 498//482
+f 483//483 498//483 499//483
+f 484//484 499//484 500//484
+f 485//485 500//485 501//485
+f 486//486 501//486 487//486
+f 487//487 502//487 488//487
+f 488//488 503//488 489//488
+f 489//489 504//489 490//489
+f 490//490 505//490 491//490
+f 491//491 506//491 492//491
+f 492//492 507//492 508//492
+f 493//493 508//493 509//493
+f 494//494 509//494 510//494
+f 495//495 510//495 496//495
+f 496//496 511//496 512//496
+f 497//497 512//497 513//497
+f 498//498 513//498 514//498
+f 499//499 514//499 515//499
+f 500//500 515//500 501//500
+f 501//501 516//501 502//501
+f 502//502 517//502 503//502
+f 503//503 518//503 504//503
+f 504//504 519//504 505//504
+f 505//505 520//505 521//505
+f 506//506 521//506 522//506
+f 507//507 522//507 508//507
+f 508//508 523//508 524//508
+f 509//509 524//509 525//509
+f 510//510 525//510 511//510
+f 511//511 526//511 527//511
+f 512//512 527//512 528//512
+f 513//513 528//513 529//513
+f 514//514 529//514 530//514
+f 515//515 530//515 516//515
+f 516//516 531//516 517//516
+f 517//517 532//517 518//517
+f 518//518 533//518 519//518
+f 519//519 534//519 520//519
+f 520//520 535//520 521//520
+f 521//521 536//521 522//521
+f 522//522 537//522 538//522
+f 523//523 538//523 539//523
+f 524//524 539//524 540//524
+f 525//525 540//525 526//525
+f 526//526 541//526 542//526
+f 527//527 542//527 543//527
+f 528//528 543//528 544//528
+f 529//529 544//529 530//529
+f 530//530 545//530 546//530
+f 531//531 546//531 547//531
+f 532//532 547//532 533//532
+f 533//533 548//533 534//533
+f 534//534 549//534 535//534
+f 535//535 550//535 536//535
+f 536//536 551//536 537//536
+f 537//537 552//537 553//537
+f 538//538 553//538 554//538
+f 539//539 554//539 555//539
+f 540//540 555//540 541//540
+f 541//541 556//541 557//541
+f 542//542 557//542 558//542
+f 543//543 558//543 559//543
+f 544//544 559//544 560//544
+f 545//545 560//545 546//545
+f 546//546 561//546 547//546
+f 547//547 562//547 548//547
+f 548//548 563//548 549//548
+f 549//549 564//549 550//549
+f 550//550 565//550 551//550
+f 551//551 566//551 552//551
+f 552//552 567//552 568//552
+f 553//553 568//553 569//553
+f 554//554 569//554 570//554
+f 555//555 570//555 556//555
+f 556//556 571//556 572//556
+f 557//557 572//557 573//557
+f 558//558 573//558 574//558
+f 559//559 574//559 575//559
+f 560//560 575//560 561//560
+f 561//561 576//561 562//561
+f 562//562 577//562 563//562
+f 563//563 578//563 564//563
+f 564//564 579//564 565//564
+f 565//565 580//565 566//565
+f 566//566 581//566 582//566
+f 567//567 582//567 583//567
+f 568//568 583//568 584//568
+f 569//569 584//569 585//569
+f 570//570 585//570 571//570
+f 571//571 586//571 587//571
+f 572//572 587//572 588//572
+f 573//573 588//573 589//573
+f 574//574 589//574 590//574
+f 575//575 590//575 576//575
+f 576//576 591//576 577//576
+f 577//577 592//577 578//577
+f 578//578 593//578 579//578
+f 579//579 594//579 580//579
+f 580//580 595//580 581//580
+f 581//581 596//581 582//581
+f 582//582 597//582 598//582
+f 583//583 598//583 599//583
+f 584//584 599//584 600//584
+f 585//585 600//585 586//585
+f 586//586 1//586 2//586
+f 587//587 2//587 3//587
+f 588//588 3//588 4//588
+f 589//589 4//589 5//589
+f 590//590 5//590 591//590
+f 591//591 6//591 592//591
+f 592//592 7//592 593//592
+f 593//593 8//593 594//593
+f 594//594 9//594 595//594
+f 595//595 10//595 596//595
+f 596//596 11//596 597//596
+f 597//597 12//597 13//597
+f 598//598 13//598 14//598
+f 599//599 14//599 15//599
+f 600//600 15//600 1//600
+f 2//601 1//601 17//601
+f 3//602 2//602 18//602
+f 4//3 3//3 19//3
+f 5//4 4//4 20//4
+f 20//5 21//5 6//5
+f 21//603 22//603 7//603
+f 22//604 23//604 8//604
+f 23//8 24//8 9//8
+f 24//605 25//605 10//605
+f 25//606 26//606 11//606
+f 26//11 27//11 12//11
+f 13//12 12//12 28//12
+f 14//13 13//13 29//13
+f 15//607 14//607 30//607
+f 16//608 1//608 30//608
+f 17//609 16//609 32//609
+f 18//610 17//610 33//610
+f 19//611 18//611 34//611
+f 20//19 19//19 35//19
+f 35//612 36//612 21//612
+f 36//613 37//613 22//613
+f 37//22 38//22 23//22
+f 38//23 39//23 24//23
+f 39//24 40//24 25//24
+f 40//614 41//614 26//614
+f 41//615 42//615 27//615
+f 28//27 27//27 43//27
+f 29//616 28//616 44//616
+f 30//617 29//617 45//617
+f 16//618 30//618 31//618
+f 32//31 31//31 47//31
+f 33//619 32//619 48//619
+f 34//33 33//33 49//33
+f 35//34 34//34 50//34
+f 50//35 51//35 36//35
+f 51//620 52//620 37//620
+f 52//37 53//37 38//37
+f 53//38 54//38 39//38
+f 54//39 55//39 40//39
+f 55//621 56//621 41//621
+f 56//41 57//41 42//41
+f 43//42 42//42 58//42
+f 44//43 43//43 59//43
+f 45//622 44//622 60//622
+f 31//623 45//623 46//623
+f 47//624 46//624 62//624
+f 48//47 47//47 63//47
+f 49//48 48//48 64//48
+f 50//49 49//49 65//49
+f 65//50 66//50 51//50
+f 66//51 67//51 52//51
+f 67//52 68//52 53//52
+f 68//53 69//53 54//53
+f 69//54 70//54 55//54
+f 70//55 71//55 56//55
+f 57//56 56//56 72//56
+f 58//57 57//57 73//57
+f 73//58 74//58 59//58
+f 60//59 59//59 75//59
+f 46//625 60//625 61//625
+f 62//626 61//626 77//626
+f 63//62 62//62 78//62
+f 64//627 63//627 79//627
+f 65//64 64//64 80//64
+f 80//65 81//65 66//65
+f 67//628 66//628 82//628
+f 82//67 83//67 68//67
+f 83//68 84//68 69//68
+f 84//69 85//69 70//69
+f 85//70 86//70 71//70
+f 86//71 87//71 72//71
+f 73//72 72//72 88//72
+f 74//629 73//629 89//629
+f 75//630 74//630 90//630
+f 61//631 75//631 76//631
+f 77//632 76//632 92//632
+f 78//633 77//633 93//633
+f 79//634 78//634 94//634
+f 80//79 79//79 95//79
+f 95//635 96//635 81//635
+f 96//81 97//81 82//81
+f 97//636 98//636 83//636
+f 98//83 99//83 84//83
+f 99//637 100//637 85//637
+f 100//638 101//638 86//638
+f 101//639 102//639 87//639
+f 88//87 87//87 103//87
+f 89//640 88//640 104//640
+f 90//89 89//89 105//89
+f 76//641 90//641 91//641
+f 92//91 91//91 107//91
+f 93//92 92//92 108//92
+f 94//642 93//642 109//642
+f 95//94 94//94 110//94
+f 110//95 111//95 96//95
+f 111//643 112//643 97//643
+f 112//644 113//644 98//644
+f 113//98 114//98 99//98
+f 114//645 115//645 100//645
+f 115//646 116//646 101//646
+f 116//647 117//647 102//647
+f 103//102 102//102 118//102
+f 104//648 103//648 119//648
+f 105//104 104//104 120//104
+f 91//105 105//105 106//105
+f 107//106 106//106 122//106
+f 108//107 107//107 123//107
+f 109//649 108//649 124//649
+f 110//109 109//109 125//109
+f 125//650 126//650 111//650
+f 126//651 127//651 112//651
+f 127//652 128//652 113//652
+f 128//113 129//113 114//113
+f 129//653 130//653 115//653
+f 130//654 131//654 116//654
+f 117//655 116//655 132//655
+f 118//117 117//117 133//117
+f 119//118 118//118 134//118
+f 120//119 119//119 135//119
+f 106//656 120//656 121//656
+f 122//121 121//121 137//121
+f 123//657 122//657 138//657
+f 124//123 123//123 139//123
+f 125//124 124//124 140//124
+f 126//125 125//125 141//125
+f 141//658 142//658 127//658
+f 142//127 143//127 128//127
+f 143//128 144//128 129//128
+f 144//129 145//129 130//129
+f 145//659 146//659 131//659
+f 146//131 147//131 132//131
+f 133//132 132//132 148//132
+f 134//133 133//133 149//133
+f 135//660 134//660 150//660
+f 121//135 135//135 136//135
+f 137//136 136//136 152//136
+f 138//661 137//661 153//661
+f 139//138 138//138 154//138
+f 140//139 139//139 155//139
+f 155//140 156//140 141//140
+f 156//662 157//662 142//662
+f 157//663 158//663 143//663
+f 158//143 159//143 144//143
+f 159//664 160//664 145//664
+f 160//665 161//665 146//665
+f 147//146 146//146 162//146
+f 148//147 147//147 163//147
+f 163//148 164//148 149//148
+f 150//666 149//666 165//666
+f 136//150 150//150 151//150
+f 166//151 167//151 152//151
+f 153//667 152//667 168//667
+f 168//153 169//153 154//153
+f 155//154 154//154 170//154
+f 170//668 171//668 156//668
+f 157//669 156//669 172//669
+f 158//670 157//670 173//670
+f 159//158 158//158 174//158
+f 160//671 159//671 175//671
+f 161//672 160//672 176//672
+f 162//673 161//673 177//673
+f 177//162 178//162 163//162
+f 178//163 179//163 164//163
+f 179//674 180//674 165//674
+f 180//165 166//165 151//165
+f 181//166 182//166 167//166
+f 182//675 183//675 168//675
+f 183//676 184//676 169//676
+f 184//169 185//169 170//169
+f 171//677 170//677 186//677
+f 172//171 171//171 187//171
+f 173//678 172//678 188//678
+f 174//173 173//173 189//173
+f 175//679 174//679 190//679
+f 176//175 175//175 191//175
+f 177//176 176//176 192//176
+f 178//177 177//177 193//177
+f 179//680 178//680 194//680
+f 194//681 195//681 180//681
+f 195//180 181//180 166//180
+f 196//181 197//181 182//181
+f 197//682 198//682 183//682
+f 198//683 199//683 184//683
+f 199//184 200//184 185//184
+f 186//185 185//185 201//185
+f 187//684 186//684 202//684
+f 188//685 187//685 203//685
+f 189//188 188//188 204//188
+f 190//686 189//686 205//686
+f 191//687 190//687 206//687
+f 192//191 191//191 207//191
+f 207//192 208//192 193//192
+f 208//193 209//193 194//193
+f 209//688 210//688 195//688
+f 210//689 196//689 181//689
+f 211//690 212//690 197//690
+f 212//691 213//691 198//691
+f 213//692 214//692 199//692
+f 214//199 215//199 200//199
+f 201//693 200//693 216//693
+f 202//694 201//694 217//694
+f 203//695 202//695 218//695
+f 204//203 203//203 219//203
+f 205//696 204//696 220//696
+f 206//697 205//697 221//697
+f 207//206 206//206 222//206
+f 208//207 207//207 223//207
+f 223//698 224//698 209//698
+f 224//699 225//699 210//699
+f 225//700 211//700 196//700
+f 226//701 227//701 212//701
+f 227//212 228//212 213//212
+f 228//213 229//213 214//213
+f 229//214 230//214 215//214
+f 230//215 231//215 216//215
+f 231//216 232//216 217//216
+f 218//702 217//702 233//702
+f 219//703 218//703 234//703
+f 220//704 219//704 235//704
+f 221//220 220//220 236//220
+f 222//221 221//221 237//221
+f 237//222 238//222 223//222
+f 238//223 239//223 224//223
+f 239//705 240//705 225//705
+f 240//706 226//706 211//706
+f 241//707 242//707 227//707
+f 242//708 243//708 228//708
+f 243//709 244//709 229//709
+f 244//229 245//229 230//229
+f 231//230 230//230 246//230
+f 232//710 231//710 247//710
+f 233//711 232//711 248//711
+f 234//233 233//233 249//233
+f 235//712 234//712 250//712
+f 236//713 235//713 251//713
+f 237//714 236//714 252//714
+f 252//237 253//237 238//237
+f 253//715 254//715 239//715
+f 254//716 255//716 240//716
+f 255//717 241//717 226//717
+f 256//718 257//718 242//718
+f 257//719 258//719 243//719
+f 258//720 259//720 244//720
+f 259//244 260//244 245//244
+f 246//245 245//245 261//245
+f 247//246 246//246 262//246
+f 248//247 247//247 263//247
+f 249//248 248//248 264//248
+f 250//249 249//249 265//249
+f 251//250 250//250 266//250
+f 266//251 267//251 252//251
+f 267//252 268//252 253//252
+f 268//253 269//253 254//253
+f 269//721 270//721 255//721
+f 270//722 256//722 241//722
+f 271//256 272//256 257//256
+f 272//723 273//723 258//723
+f 273//258 274//258 259//258
+f 274//259 275//259 260//259
+f 261//260 260//260 276//260
+f 262//724 261//724 277//724
+f 263//725 262//725 278//725
+f 264//263 263//263 279//263
+f 265//726 264//726 280//726
+f 266//265 265//265 281//265
+f 267//266 266//266 282//266
+f 282//267 283//267 268//267
+f 283//268 284//268 269//268
+f 284//727 285//727 270//727
+f 285//270 271//270 256//270
+f 286//728 287//728 272//728
+f 287//729 288//729 273//729
+f 274//730 273//730 289//730
+f 275//274 274//274 290//274
+f 290//731 291//731 276//731
+f 291//732 292//732 277//732
+f 278//733 277//733 293//733
+f 279//278 278//278 294//278
+f 280//734 279//734 295//734
+f 281//735 280//735 296//735
+f 282//736 281//736 297//736
+f 297//282 298//282 283//282
+f 298//737 299//737 284//737
+f 299//738 300//738 285//738
+f 300//739 286//739 271//739
+f 301//740 302//740 287//740
+f 302//741 303//741 288//741
+f 303//288 304//288 289//288
+f 304//289 305//289 290//289
+f 291//290 290//290 306//290
+f 292//742 291//742 307//742
+f 293//743 292//743 308//743
+f 294//293 293//293 309//293
+f 295//744 294//744 310//744
+f 310//745 311//745 296//745
+f 297//296 296//296 312//296
+f 312//297 313//297 298//297
+f 313//298 314//298 299//298
+f 314//746 315//746 300//746
+f 315//747 301//747 286//747
+f 316//748 317//748 302//748
+f 317//302 318//302 303//302
+f 318//303 319//303 304//303
+f 319//304 320//304 305//304
+f 306//305 305//305 321//305
+f 307//306 306//306 322//306
+f 308//307 307//307 323//307
+f 309//308 308//308 324//308
+f 310//309 309//309 325//309
+f 311//310 310//310 326//310
+f 312//311 311//311 327//311
+f 327//312 328//312 313//312
+f 328//313 329//313 314//313
+f 329//314 330//314 315//314
+f 330//749 316//749 301//749
+f 331//750 332//750 317//750
+f 332//317 333//317 318//317
+f 333//318 334//318 319//318
+f 320//319 319//319 335//319
+f 321//320 320//320 336//320
+f 322//751 321//751 337//751
+f 323//752 322//752 338//752
+f 324//323 323//323 339//323
+f 325//753 324//753 340//753
+f 326//754 325//754 341//754
+f 327//326 326//326 342//326
+f 342//327 343//327 328//327
+f 343//755 344//755 329//755
+f 344//329 345//329 330//329
+f 345//756 331//756 316//756
+f 346//331 347//331 332//331
+f 347//332 348//332 333//332
+f 348//333 349//333 334//333
+f 349//334 350//334 335//334
+f 336//757 335//757 351//757
+f 337//758 336//758 352//758
+f 338//337 337//337 353//337
+f 339//338 338//338 354//338
+f 340//759 339//759 355//759
+f 341//760 340//760 356//760
+f 342//761 341//761 357//761
+f 357//342 358//342 343//342
+f 358//343 359//343 344//343
+f 359//344 360//344 345//344
+f 360//345 346//345 331//345
+f 361//762 362//762 347//762
+f 362//347 363//347 348//347
+f 363//763 364//763 349//763
+f 364//349 365//349 350//349
+f 351//350 350//350 366//350
+f 352//764 351//764 367//764
+f 353//765 352//765 368//765
+f 354//353 353//353 369//353
+f 355//766 354//766 370//766
+f 356//767 355//767 371//767
+f 357//356 356//356 372//356
+f 372//357 373//357 358//357
+f 373//768 374//768 359//768
+f 374//769 375//769 360//769
+f 375//770 361//770 346//770
+f 376//771 377//771 362//771
+f 377//772 378//772 363//772
+f 378//773 379//773 364//773
+f 379//364 380//364 365//364
+f 366//774 365//774 381//774
+f 367//775 366//775 382//775
+f 368//776 367//776 383//776
+f 369//368 368//368 384//368
+f 370//777 369//777 385//777
+f 371//778 370//778 386//778
+f 372//371 371//371 387//371
+f 387//372 388//372 373//372
+f 388//779 389//779 374//779
+f 389//374 390//374 375//374
+f 390//780 376//780 361//780
+f 391//781 392//781 377//781
+f 392//782 393//782 378//782
+f 393//378 394//378 379//378
+f 394//379 395//379 380//379
+f 381//380 380//380 396//380
+f 382//381 381//381 397//381
+f 383//783 382//783 398//783
+f 384//784 383//784 399//784
+f 385//785 384//785 400//785
+f 400//385 401//385 386//385
+f 401//386 402//386 387//386
+f 402//387 403//387 388//387
+f 403//388 404//388 389//388
+f 404//389 405//389 390//389
+f 405//786 391//786 376//786
+f 406//787 407//787 392//787
+f 407//788 408//788 393//788
+f 408//789 409//789 394//789
+f 395//394 394//394 410//394
+f 396//395 395//395 411//395
+f 397//790 396//790 412//790
+f 398//791 397//791 413//791
+f 399//398 398//398 414//398
+f 400//792 399//792 415//792
+f 401//793 400//793 416//793
+f 402//794 401//794 417//794
+f 417//402 418//402 403//402
+f 418//403 419//403 404//403
+f 419//795 420//795 405//795
+f 420//796 406//796 391//796
+f 421//797 422//797 407//797
+f 422//798 423//798 408//798
+f 423//799 424//799 409//799
+f 424//409 425//409 410//409
+f 411//410 410//410 426//410
+f 412//800 411//800 427//800
+f 413//801 412//801 428//801
+f 414//413 413//413 429//413
+f 415//802 414//802 430//802
+f 416//803 415//803 431//803
+f 417//416 416//416 432//416
+f 432//417 433//417 418//417
+f 433//804 434//804 419//804
+f 434//805 435//805 420//805
+f 435//806 421//806 406//806
+f 436//421 437//421 422//421
+f 437//807 438//807 423//807
+f 424//808 423//808 439//808
+f 425//424 424//424 440//424
+f 426//425 425//425 441//425
+f 427//426 426//426 442//426
+f 428//809 427//809 443//809
+f 429//428 428//428 444//428
+f 430//810 429//810 445//810
+f 431//430 430//430 446//430
+f 432//811 431//811 447//811
+f 447//432 448//432 433//432
+f 448//812 449//812 434//812
+f 449//813 450//813 435//813
+f 450//435 436//435 421//435
+f 451//436 452//436 437//436
+f 452//814 453//814 438//814
+f 453//438 454//438 439//438
+f 454//439 455//439 440//439
+f 441//815 440//815 456//815
+f 442//816 441//816 457//816
+f 443//817 442//817 458//817
+f 444//443 443//443 459//443
+f 445//818 444//818 460//818
+f 446//819 445//819 461//819
+f 461//820 462//820 447//820
+f 448//447 447//447 463//447
+f 463//448 464//448 449//448
+f 450//821 449//821 465//821
+f 465//450 451//450 436//450
+f 452//451 451//451 467//451
+f 453//822 452//822 468//822
+f 468//453 469//453 454//453
+f 455//823 454//823 470//823
+f 456//455 455//455 471//455
+f 471//824 472//824 457//824
+f 472//825 473//825 458//825
+f 473//458 474//458 459//458
+f 474//826 475//826 460//826
+f 475//827 476//827 461//827
+f 476//461 477//461 462//461
+f 463//828 462//828 478//828
+f 464//463 463//463 479//463
+f 465//829 464//829 480//829
+f 451//465 465//465 466//465
+f 467//466 466//466 482//466
+f 468//830 467//830 483//830
+f 469//468 468//468 484//468
+f 470//469 469//469 485//469
+f 485//470 486//470 471//470
+f 486//831 487//831 472//831
+f 487//472 488//472 473//472
+f 488//473 489//473 474//473
+f 489//474 490//474 475//474
+f 490//832 491//832 476//832
+f 477//476 476//476 492//476
+f 478//477 477//477 493//477
+f 479//478 478//478 494//478
+f 480//833 479//833 495//833
+f 466//480 480//480 481//480
+f 482//834 481//834 497//834
+f 483//482 482//482 498//482
+f 484//483 483//483 499//483
+f 485//484 484//484 500//484
+f 486//835 485//835 501//835
+f 501//836 502//836 487//836
+f 502//837 503//837 488//837
+f 503//488 504//488 489//488
+f 504//838 505//838 490//838
+f 505//839 506//839 491//839
+f 506//840 507//840 492//840
+f 493//492 492//492 508//492
+f 494//841 493//841 509//841
+f 495//494 494//494 510//494
+f 481//495 495//495 496//495
+f 497//842 496//842 512//842
+f 498//497 497//497 513//497
+f 499//498 498//498 514//498
+f 500//499 499//499 515//499
+f 515//500 516//500 501//500
+f 516//843 517//843 502//843
+f 517//844 518//844 503//844
+f 518//503 519//503 504//503
+f 519//845 520//845 505//845
+f 506//846 505//846 521//846
+f 507//506 506//506 522//506
+f 522//507 523//507 508//507
+f 509//508 508//508 524//508
+f 510//509 509//509 525//509
+f 496//847 510//847 511//847
+f 512//848 511//848 527//848
+f 513//512 512//512 528//512
+f 514//513 513//513 529//513
+f 515//849 514//849 530//849
+f 530//515 531//515 516//515
+f 531//516 532//516 517//516
+f 532//850 533//850 518//850
+f 533//518 534//518 519//518
+f 534//851 535//851 520//851
+f 535//852 536//852 521//852
+f 536//521 537//521 522//521
+f 523//522 522//522 538//522
+f 524//523 523//523 539//523
+f 525//524 524//524 540//524
+f 511//525 525//525 526//525
+f 527//853 526//853 542//853
+f 528//854 527//854 543//854
+f 529//855 528//855 544//855
+f 544//529 545//529 530//529
+f 531//530 530//530 546//530
+f 532//531 531//531 547//531
+f 547//532 548//532 533//532
+f 548//533 549//533 534//533
+f 549//856 550//856 535//856
+f 550//535 551//535 536//535
+f 551//536 552//536 537//536
+f 538//537 537//537 553//537
+f 539//857 538//857 554//857
+f 540//858 539//858 555//858
+f 526//859 540//859 541//859
+f 542//860 541//860 557//860
+f 543//861 542//861 558//861
+f 544//862 543//862 559//862
+f 545//544 544//544 560//544
+f 560//863 561//863 546//863
+f 561//546 562//546 547//546
+f 562//547 563//547 548//547
+f 563//548 564//548 549//548
+f 564//549 565//549 550//549
+f 565//550 566//550 551//550
+f 566//864 567//864 552//864
+f 553//552 552//552 568//552
+f 554//865 553//865 569//865
+f 555//866 554//866 570//866
+f 541//867 555//867 556//867
+f 557//868 556//868 572//868
+f 558//557 557//557 573//557
+f 559//558 558//558 574//558
+f 560//559 559//559 575//559
+f 575//869 576//869 561//869
+f 576//561 577//561 562//561
+f 577//870 578//870 563//870
+f 578//563 579//563 564//563
+f 579//871 580//871 565//871
+f 580//872 581//872 566//872
+f 567//873 566//873 582//873
+f 568//567 567//567 583//567
+f 569//568 568//568 584//568
+f 570//569 569//569 585//569
+f 556//874 570//874 571//874
+f 572//571 571//571 587//571
+f 573//875 572//875 588//875
+f 574//573 573//573 589//573
+f 575//574 574//574 590//574
+f 590//876 591//876 576//876
+f 591//877 592//877 577//877
+f 592//878 593//878 578//878
+f 593//578 594//578 579//578
+f 594//879 595//879 580//879
+f 595//880 596//880 581//880
+f 596//581 597//581 582//581
+f 583//582 582//582 598//582
+f 584//583 583//583 599//583
+f 585//881 584//881 600//881
+f 571//585 585//585 586//585
+f 587//882 586//882 2//882
+f 588//883 587//883 3//883
+f 589//588 588//588 4//588
+f 590//589 589//589 5//589
+f 5//590 6//590 591//590
+f 6//884 7//884 592//884
+f 7//885 8//885 593//885
+f 8//593 9//593 594//593
+f 9//886 10//886 595//886
+f 10//887 11//887 596//887
+f 11//596 12//596 597//596
+f 598//597 597//597 13//597
+f 599//598 598//598 14//598
+f 600//599 599//599 15//599
+f 586//888 600//888 1//888
diff --git a/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.osgb b/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.osgb
new file mode 100644
index 0000000..f8aeec2
Binary files /dev/null and b/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.osgb differ
diff --git a/SurgSim/Testing/SerializationMockComponent.cpp b/SurgSim/Testing/SerializationMockComponent.cpp
new file mode 100644
index 0000000..0d2f860
--- /dev/null
+++ b/SurgSim/Testing/SerializationMockComponent.cpp
@@ -0,0 +1,71 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Testing/SerializationMockComponent.h"
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SerializationMockComponent, SerializationMockComponent);
+
+SerializationMockComponent::SerializationMockComponent(
+ const std::string& name,
+ bool succeedInit,
+ bool succeedWakeUp) :
+ Component(name),
+ succeedWithInit(succeedInit),
+ succeedWithWakeUp(succeedWakeUp),
+ didWakeUp(false),
+ didInit(false)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(
+ SerializationMockComponent, bool, SucceedWithInit, getSucceedWithInit, setSucceedWithInit);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(
+ SerializationMockComponent, bool, SucceedWithWakeUp, getSucceedWithWakeUp, setSucceedWithWakeUp);
+}
+
+SerializationMockComponent::~SerializationMockComponent()
+{
+
+}
+
+bool SerializationMockComponent::doInitialize()
+{
+ didInit = true;
+ return succeedWithInit;
+}
+
+bool SerializationMockComponent::doWakeUp()
+{
+ didWakeUp = true;
+ return succeedWithWakeUp;
+}
+
+bool SerializationMockComponent::getSucceedWithInit() const
+{
+ return succeedWithInit;
+}
+
+void SerializationMockComponent::setSucceedWithInit(bool val)
+{
+ succeedWithInit = val;
+}
+
+bool SerializationMockComponent::getSucceedWithWakeUp() const
+{
+ return succeedWithWakeUp;
+}
+
+void SerializationMockComponent::setSucceedWithWakeUp(bool val)
+{
+ succeedWithWakeUp = val;
+}
diff --git a/SurgSim/Testing/SerializationMockComponent.h b/SurgSim/Testing/SerializationMockComponent.h
new file mode 100644
index 0000000..acf844d
--- /dev/null
+++ b/SurgSim/Testing/SerializationMockComponent.h
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_SERIALIZATIONMOCKCOMPONENT_H
+#define SURGSIM_TESTING_SERIALIZATIONMOCKCOMPONENT_H
+
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+SURGSIM_STATIC_REGISTRATION(SerializationMockComponent);
+
+/// This class is for testing the linker and checking if the definition stays in the
+/// executable even if there is not direct reference to it, DO NOT define a member
+/// of this class explicitly anywhere.
+class SerializationMockComponent : public SurgSim::Framework::Component
+{
+public:
+ explicit SerializationMockComponent(const std::string& name, bool succeedInit = true, bool succeedWakeUp = true);
+
+ virtual ~SerializationMockComponent();
+
+ SURGSIM_CLASSNAME(SerializationMockComponent);
+
+ virtual bool doInitialize();
+ virtual bool doWakeUp();
+
+ bool getSucceedWithInit() const;
+ void setSucceedWithInit(bool val);
+
+ bool getSucceedWithWakeUp() const;
+ void setSucceedWithWakeUp(bool val);
+
+ bool succeedWithInit;
+ bool succeedWithWakeUp;
+ bool didWakeUp;
+ bool didInit;
+};
+
+#endif // SURGSIM_TESTING_SERIALIZATIONMOCKCOMPONENT_H
\ No newline at end of file
diff --git a/SurgSim/Testing/TestCube.cpp b/SurgSim/Testing/TestCube.cpp
new file mode 100644
index 0000000..36216d9
--- /dev/null
+++ b/SurgSim/Testing/TestCube.cpp
@@ -0,0 +1,158 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Testing/TestCube.h"
+
+namespace SurgSim
+{
+namespace Testing
+{
+namespace Cube
+{
+
+int numVertices = 24;
+
+double vertexData[] =
+{
+ -0.5,-0.5,-0.5,
+ -0.5,-0.5, 0.5,
+ 0.5,-0.5, 0.5,
+ 0.5,-0.5,-0.5,
+ -0.5, 0.5,-0.5,
+ -0.5, 0.5, 0.5,
+ 0.5, 0.5, 0.5,
+ 0.5, 0.5,-0.5,
+
+ -0.5,-0.5,-0.5,
+ -0.5,-0.5, 0.5,
+ 0.5,-0.5, 0.5,
+ 0.5,-0.5,-0.5,
+ -0.5, 0.5,-0.5,
+ -0.5, 0.5, 0.5,
+ 0.5, 0.5, 0.5,
+ 0.5, 0.5,-0.5,
+
+ -0.5,-0.5,-0.5,
+ -0.5,-0.5, 0.5,
+ 0.5,-0.5, 0.5,
+ 0.5,-0.5,-0.5,
+ -0.5, 0.5,-0.5,
+ -0.5, 0.5, 0.5,
+ 0.5, 0.5, 0.5,
+ 0.5, 0.5,-0.5
+};
+
+double colorData[] =
+{
+ 0.9,0.0,0.0,1.0,
+ 0.9,0.0,0.0,1.0,
+ 0.9,0.0,0.0,1.0,
+ 0.9,0.0,0.0,1.0,
+ 0.9,0.5,0.5,1.0,
+ 0.9,0.5,0.5,1.0,
+ 0.9,0.5,0.5,1.0,
+ 0.9,0.5,0.5,1.0,
+
+ 0.0,0.9,0.0,1.0,
+ 0.0,0.9,0.0,1.0,
+ 0.5,0.9,0.5,1.0,
+ 0.5,0.9,0.5,1.0,
+ 0.0,0.9,0.0,1.0,
+ 0.0,0.9,0.0,1.0,
+ 0.5,0.9,0.5,1.0,
+ 0.5,0.9,0.5,1.0,
+
+ 0.0,0.0,0.9,1.0,
+ 0.5,0.5,0.9,1.0,
+ 0.5,0.5,0.9,1.0,
+ 0.0,0.0,0.9,1.0,
+ 0.0,0.0,0.9,1.0,
+ 0.5,0.5,0.9,1.0,
+ 0.5,0.5,0.9,1.0,
+ 0.0,0.0,0.9,1.0
+};
+
+double textureData[] =
+{
+ 0.25,0.25,
+ 0.25,0.50,
+ 0.0, 0.50,
+ 0.0, 0.25,
+ 0.50,0.25,
+ 0.50,0.50,
+ 0.50,0.50,
+ 0.50,0.25,
+
+ 0.25,0.25,
+ 0.25,0.50,
+ 1.0, 0.50,
+ 1.0, 0.25,
+ 0.50,0.25,
+ 0.50,0.50,
+ 0.75,0.50,
+ 0.75,0.25,
+
+ 0.50,0.0 ,
+ 0.50,0.75,
+ 0.75,0.75,
+ 0.75,0.0,
+ 0.50,0.25,
+ 0.50,0.50,
+ 0.75,0.50,
+ 0.75,0.25
+};
+
+unsigned int numTriangles = 12;
+unsigned int triangleData[] =
+{
+ 0, 3, 2,
+ 0, 2, 1,
+ 4, 6, 7,
+ 4, 5, 6,
+ 8, 9,13,
+ 8,13,12,
+ 10,15,14,
+ 10,11,15,
+ 16,20,23,
+ 16,23,19,
+ 17,18,22,
+ 17,22,21
+};
+
+void makeCube(std::vector<Vector3d>* vertices,
+ std::vector<Vector4d>* colors,
+ std::vector<Vector2d>* textures,
+ std::vector<size_t>* triangles)
+{
+vertices->resize(numVertices);
+colors->resize(numVertices);
+textures->resize(numVertices);
+
+double scale = 1.0;
+
+for (int i=0; i<numVertices; ++i)
+{
+ (*vertices)[i] = Vector3d(vertexData[3*i]*scale, vertexData[3*i+1]*scale, vertexData[3*i+2]*scale);
+ (*colors)[i] = Vector4d(colorData[4*i], colorData[4*i+1], colorData[4*i+2], colorData[4*i+3]);
+ (*textures)[i] = Vector2d(textureData[2*i], textureData[2*i+1]);
+}
+
+triangles->resize(numTriangles*3);
+std::copy(triangleData, triangleData+12*3,std::begin(*triangles));
+}
+
+}; // Cube
+}; // Testing
+}; // SurgSim
diff --git a/SurgSim/Testing/TestCube.h b/SurgSim/Testing/TestCube.h
new file mode 100644
index 0000000..16a8965
--- /dev/null
+++ b/SurgSim/Testing/TestCube.h
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_TESTCUBE_H
+#define SURGSIM_TESTING_TESTCUBE_H
+
+#include <vector>
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+
+/// \file
+/// Data for a cube, vertices, color, textures and triangles
+/// This cube is for using as a mesh. OpenGl does not support per face attributes, therefore
+/// to implement sharp edges vertices are duplicated over different faces, for a cube all
+/// vertices need to exist as three copies
+
+#include <memory>
+
+namespace SurgSim
+{
+
+namespace Testing
+{
+namespace Cube
+{
+
+/// Fill our standard structures with the correct data types from the cube data
+void makeCube(std::vector<Vector3d>* vertices,
+ std::vector<Vector4d>* colors,
+ std::vector<Vector2d>* textures,
+ std::vector<size_t>* triangles);
+
+
+}; // Cube
+}; // Testing
+}; // SurgSim
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Testing/TestingMain.cpp b/SurgSim/Testing/TestingMain.cpp
new file mode 100644
index 0000000..646863b
--- /dev/null
+++ b/SurgSim/Testing/TestingMain.cpp
@@ -0,0 +1,30 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gmock/gmock.h>
+
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/Framework/LogOutput.h"
+
+int main(int argc, char** argv)
+{
+ //Disable logging during tests
+ std::shared_ptr<SurgSim::Framework::LoggerManager> loggerManager;
+ loggerManager = SurgSim::Framework::Logger::getLoggerManager();
+ loggerManager->setDefaultOutput(std::make_shared<SurgSim::Framework::NullOutput>());
+
+ testing::InitGoogleMock(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/SurgSim/Testing/TriangleMeshBaseTests/Cube.ply b/SurgSim/Testing/TriangleMeshBaseTests/Cube.ply
new file mode 100644
index 0000000..33fe119
--- /dev/null
+++ b/SurgSim/Testing/TriangleMeshBaseTests/Cube.ply
@@ -0,0 +1,51 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.69 (sub 0) - www.blender.org, source file: ''
+element vertex 26
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+element face 12
+property list uchar uint vertex_indices
+end_header
+1.000000 1.000000 -1.000000 0.000000 0.000000 -1.000000
+1.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000
+-1.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000
+1.000000 0.999999 1.000000 -0.000000 -0.000000 1.000000
+-1.000000 1.000000 1.000000 -0.000000 -0.000000 1.000000
+0.999999 -1.000001 1.000000 -0.000000 -0.000000 1.000000
+1.000000 1.000000 -1.000000 1.000000 0.000000 -0.000000
+1.000000 0.999999 1.000000 1.000000 0.000000 -0.000000
+1.000000 -1.000000 -1.000000 1.000000 0.000000 -0.000000
+1.000000 -1.000000 -1.000000 -0.000000 -1.000000 -0.000000
+0.999999 -1.000001 1.000000 -0.000000 -1.000000 -0.000000
+-1.000000 -1.000000 -1.000000 -0.000000 -1.000000 -0.000000
+-1.000000 -1.000000 -1.000000 -1.000000 0.000000 -0.000000
+-1.000000 -1.000000 1.000000 -1.000000 0.000000 -0.000000
+-1.000000 1.000000 1.000000 -1.000000 0.000000 -0.000000
+1.000000 0.999999 1.000000 0.000000 1.000000 0.000000
+1.000000 1.000000 -1.000000 0.000000 1.000000 0.000000
+-1.000000 1.000000 1.000000 0.000000 1.000000 0.000000
+-1.000000 1.000000 -1.000000 0.000000 0.000000 -1.000000
+1.000000 0.999999 1.000000 1.000000 -0.000001 0.000000
+0.999999 -1.000001 1.000000 1.000000 -0.000001 0.000000
+1.000000 -1.000000 -1.000000 1.000000 -0.000001 0.000000
+-1.000000 1.000000 -1.000000 0.000000 1.000000 0.000000
+-1.000000 -1.000000 1.000000 0.000000 -0.000000 1.000000
+-1.000000 1.000000 -1.000000 -1.000000 0.000000 -0.000000
+-1.000000 -1.000000 1.000000 -0.000000 -1.000000 0.000000
+3 0 1 2
+3 3 4 5
+3 6 7 8
+3 9 10 11
+3 12 13 14
+3 15 16 17
+3 18 0 2
+3 19 20 21
+3 16 22 17
+3 4 23 5
+3 24 12 14
+3 10 25 11
diff --git a/SurgSim/Testing/VisualTestCommon/CMakeLists.txt b/SurgSim/Testing/VisualTestCommon/CMakeLists.txt
new file mode 100644
index 0000000..4925014
--- /dev/null
+++ b/SurgSim/Testing/VisualTestCommon/CMakeLists.txt
@@ -0,0 +1,55 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${GLUT_INCLUDE_DIR}"
+)
+
+set(VISUAL_TEST_COMMON_SOURCES
+ GlutRenderer.cpp
+ MovingSquareForce.cpp
+ MovingSquareGlutWindow.cpp
+ ToolSquareTest.cpp
+)
+
+set(VISUAL_TEST_COMMON_HEADERS
+ GlutRenderer.h
+ MovingSquareForce.h
+ MovingSquareGlutWindow.h
+ ToolSquareTest.h
+)
+
+set(LIBS
+ ${GLUT_LIBRARIES}
+)
+
+
+# The headers etc. for this do not need to be shipped, so do NOT use
+# surgsim_add_library here.
+add_library(VisualTestCommon
+ ${VISUAL_TEST_COMMON_SOURCES} ${VISUAL_TEST_COMMON_HEADERS})
+
+target_link_libraries(VisualTestCommon ${LIBS})
+
+surgsim_show_ide_folders(
+ "${VISUAL_TEST_COMMON_SOURCES}" "${VISUAL_TEST_COMMON_HEADERS}")
+
+set_target_properties(VisualTestCommon PROPERTIES FOLDER "Testing")
diff --git a/SurgSim/Testing/VisualTestCommon/GlutRenderer.cpp b/SurgSim/Testing/VisualTestCommon/GlutRenderer.cpp
new file mode 100644
index 0000000..aecfeab
--- /dev/null
+++ b/SurgSim/Testing/VisualTestCommon/GlutRenderer.cpp
@@ -0,0 +1,187 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Testing/VisualTestCommon/GlutRenderer.h"
+
+#include <string>
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::RigidTransform3d;
+
+
+int GlutRenderer::m_width = 1024;
+int GlutRenderer::m_height = 768;
+
+std::shared_ptr<GlutCamera> GlutRenderer::m_camera = nullptr;
+std::vector< std::shared_ptr<GlutRenderObject> > GlutRenderer::m_objects =
+ std::vector< std::shared_ptr<GlutRenderObject> >();
+
+
+GlutRenderObject::~GlutRenderObject()
+{
+}
+
+void GlutSquare::draw() const
+{
+ glEnable(GL_LIGHTING);
+
+ glPushMatrix();
+
+ glTranslated(pose.translation().x(), pose.translation().y(), pose.translation().z());
+ Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(pose.rotation());
+ glRotated(angleAxis.angle() * 180.0 / M_PI, angleAxis.axis().x(), angleAxis.axis().y(), angleAxis.axis().z());
+
+ Vector3d squarePoints[4];
+ squarePoints[0] = - (halfSize * planeDirectionX) + (halfSize * planeDirectionY);
+ squarePoints[1] = (halfSize * planeDirectionX) + (halfSize * planeDirectionY);
+ squarePoints[2] = (halfSize * planeDirectionX) - (halfSize * planeDirectionY);
+ squarePoints[3] = - (halfSize * planeDirectionX) - (halfSize * planeDirectionY);
+
+ Vector3d normal = (squarePoints[1] - squarePoints[0]).cross(squarePoints[2] - squarePoints[0]);
+ normal.normalize();
+
+ glColor3d(color.x(), color.y(), color.z());
+ glBegin(GL_QUADS);
+
+ // Front side
+ glNormal3d(normal.x(), normal.y(), normal.z());
+ for (int i = 0; i < 4; ++i)
+ {
+ glVertex3d(squarePoints[i].x(), squarePoints[i].y(), squarePoints[i].z());
+ }
+ // Back side
+ glNormal3d(- normal.x(), - normal.y(), - normal.z());
+ for (int i = 0; i < 4; ++i)
+ {
+ glVertex3d(squarePoints[3 - i].x(), squarePoints[3 - i].y(), squarePoints[3 - i].z());
+ }
+
+ glEnd();
+
+ glPopMatrix();
+}
+
+void GlutAxes::draw() const
+{
+ glDisable(GL_LIGHTING);
+
+ glPushMatrix();
+
+ glTranslated(pose.translation().x(), pose.translation().y(), pose.translation().z());
+ Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(pose.rotation());
+ glRotated(angleAxis.angle() * 180.0 / M_PI, angleAxis.axis().x(), angleAxis.axis().y(), angleAxis.axis().z());
+
+ Vector3d axesPoints[4];
+ axesPoints[0] = Vector3d(0.0, 0.0, 0.0);
+ axesPoints[1] = (length * Vector3d(1.0, 0.0, 0.0));
+ axesPoints[2] = (length * Vector3d(0.0, 1.0, 0.0));
+ axesPoints[3] = (length * Vector3d(0.0, 0.0, 1.0));
+
+ glLineWidth(width);
+
+ glBegin(GL_LINES);
+ glColor3d(1.0, 0.0, 0.0);
+ glVertex3d(axesPoints[0].x(), axesPoints[0].y(), axesPoints[0].z());
+ glVertex3d(axesPoints[1].x(), axesPoints[1].y(), axesPoints[1].z());
+ glColor3d(0.0, 1.0, 0.0);
+ glVertex3d(axesPoints[0].x(), axesPoints[0].y(), axesPoints[0].z());
+ glVertex3d(axesPoints[2].x(), axesPoints[2].y(), axesPoints[2].z());
+ glColor3d(0.0, 0.0, 1.0);
+ glVertex3d(axesPoints[0].x(), axesPoints[0].y(), axesPoints[0].z());
+ glVertex3d(axesPoints[3].x(), axesPoints[3].y(), axesPoints[3].z());
+ glEnd();
+
+ glPopMatrix();
+}
+
+void GlutSphere::draw() const
+{
+ glEnable(GL_LIGHTING);
+
+ glPushMatrix();
+
+ glTranslated(pose.translation().x(), pose.translation().y(), pose.translation().z());
+ Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(pose.rotation());
+ glRotated(angleAxis.angle() * 180.0 / M_PI, angleAxis.axis().x(), angleAxis.axis().y(), angleAxis.axis().z());
+
+ glColor3d(color.x(), color.y(), color.z());
+
+ gluQuadricOrientation(quadratic, GLU_OUTSIDE);
+ gluSphere(quadratic, radius, 32, 32);
+
+ glPopMatrix();
+}
+
+void GlutGroup::draw() const
+{
+ glPushMatrix();
+
+ glTranslated(pose.translation().x(), pose.translation().y(), pose.translation().z());
+ Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(pose.rotation());
+ glRotated(angleAxis.angle() * 180.0 / M_PI, angleAxis.axis().x(), angleAxis.axis().y(), angleAxis.axis().z());
+
+ for (auto it = children.cbegin(); it != children.cend(); ++it)
+ {
+ (*it)->draw();
+ }
+
+ glPopMatrix();
+}
+
+void GlutRenderer::initialize()
+{
+ char* argv[1];
+ int argc = 1;
+ std::string title = "GlutWindow";
+ std::vector<char> charString(title.size() + 1);
+ std::copy(title.begin(), title.end(), charString.begin());
+ argv[0] = &charString[0];
+ glutInit(&argc, argv);
+ glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA);
+ glutInitWindowPosition(100, 100);
+ glutInitWindowSize(m_width, m_height);
+ glutCreateWindow("Simple GLUT display");
+
+ glEnable(GL_COLOR_MATERIAL);
+ glEnable(GL_CULL_FACE);
+ glCullFace(GL_BACK);
+ glEnable(GL_DEPTH_TEST);
+ glShadeModel(GL_SMOOTH);
+ glClearColor(0.1f, 0.1f, 0.1f, 1.0f);
+
+ glutDisplayFunc(display);
+ glutReshapeFunc(reshape);
+}
+void GlutRenderer::display()
+{
+ glViewport(0, 0, (GLsizei) m_width, (GLsizei) m_height);
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ gluPerspective(m_camera->fovY, static_cast<float>(m_width) / m_height, m_camera->zNear, m_camera->zFar);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ gluLookAt(m_camera->eye.x(), m_camera->eye.y(), m_camera->eye.z(),
+ m_camera->center.x(), m_camera->center.y(), m_camera->center.z(),
+ m_camera->up.x(), m_camera->up.y(), m_camera->up.z());
+
+ glEnable(GL_LIGHT0);
+
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+ drawObjects();
+ glutSwapBuffers();
+
+ glutPostRedisplay();
+}
diff --git a/SurgSim/Testing/VisualTestCommon/GlutRenderer.h b/SurgSim/Testing/VisualTestCommon/GlutRenderer.h
new file mode 100644
index 0000000..19176ee
--- /dev/null
+++ b/SurgSim/Testing/VisualTestCommon/GlutRenderer.h
@@ -0,0 +1,232 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_VISUALTESTCOMMON_GLUTRENDERER_H
+#define SURGSIM_TESTING_VISUALTESTCOMMON_GLUTRENDERER_H
+
+#include <vector>
+#include <memory>
+
+#ifdef __APPLE__
+#include <GLUT/glut.h>
+#else
+#include <GL/glut.h>
+#endif
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+/// Abstract definition of an object that can render itself with Glut.
+struct GlutRenderObject
+{
+ /// Pose (rotation and translation) of the object.
+ SurgSim::Math::RigidTransform3d pose;
+
+ /// Constructor initializes pose as identity (no rotation or translation)
+ GlutRenderObject() : pose(SurgSim::Math::RigidTransform3d::Identity())
+ {
+ }
+
+ virtual ~GlutRenderObject();
+
+ /// Pure virtual draw method for subclasses to define how to draw themselves with Glut.
+ virtual void draw() const = 0;
+};
+
+/// Square with center at local origin.
+struct GlutSquare : public GlutRenderObject
+{
+ /// The unit direction along one of the pairs edges of the square.
+ SurgSim::Math::Vector3d planeDirectionX;
+ /// The unit direction along the other pair of edges of the square.
+ SurgSim::Math::Vector3d planeDirectionY;
+ /// One half of the edge length of the square, in meters.
+ double halfSize;
+ /// Color of the square.
+ SurgSim::Math::Vector3d color;
+
+ /// Constructor
+ /// \param halfSize One half of the edge length of the square, in meters.
+ /// \param color Color of the square.
+ /// \param planeDirectionX The unit direction along one of the pairs edges of the square, default is X-axis.
+ /// \param planeDirectionY The unit direction along the other pair of edges of the square, default is Y-axis.
+ GlutSquare(double halfSize, const SurgSim::Math::Vector3d& color,
+ const SurgSim::Math::Vector3d& planeDirectionX = SurgSim::Math::Vector3d(1.0, 0.0, 0.0),
+ const SurgSim::Math::Vector3d& planeDirectionY = SurgSim::Math::Vector3d(0.0, 1.0, 0.0)) :
+ GlutRenderObject(), planeDirectionX(planeDirectionX), planeDirectionY(planeDirectionY),
+ halfSize(halfSize), color(color)
+ {
+ }
+
+ /// Draws the square with Glut.
+ virtual void draw() const;
+};
+
+/// Axes with center at local origin, red axis along the local X-axis, green axis along the local Y-axis, and blue axis
+/// along the local Z-axis.
+struct GlutAxes : GlutRenderObject
+{
+ /// Length of each axis, in meters.
+ double length;
+ /// Width of each axis, in pixels.
+ float width;
+
+ /// Constructor
+ /// \param length Length of each axis, in meters.
+ /// \param width Width of each axis, in pixels.
+ GlutAxes(double length, float width) : GlutRenderObject(), length(length), width(width)
+ {
+ }
+
+ /// Draws the axes with Glut.
+ virtual void draw() const;
+};
+
+/// Sphere with center at local origin.
+struct GlutSphere : GlutRenderObject
+{
+ /// Radius of the sphere, in meters.
+ double radius;
+ /// Color of the sphere.
+ SurgSim::Math::Vector3d color;
+
+ /// Constructor
+ /// \param radius Radius of the sphere, in meters.
+ /// \param color Color of the sphere.
+ GlutSphere(double radius, const SurgSim::Math::Vector3d& color) :
+ radius(radius), color(color), quadratic(gluNewQuadric())
+ {
+ }
+
+ /// Draws the sphere with Glut.
+ virtual void draw() const;
+
+private:
+ /// GLU quadric object for the quadric operations required to build the sphere.
+ GLUquadric* quadratic;
+};
+
+/// Group of objects which provides a transform hierarchy.
+struct GlutGroup : public GlutRenderObject
+{
+ /// Children of this group.
+ std::vector< std::shared_ptr<GlutRenderObject> > children;
+
+ /// Constructor. The group is initialized with no children.
+ GlutGroup() : GlutRenderObject()
+ {
+ }
+
+ /// Draws the group with Glut and iterates through its children to draw them.
+ virtual void draw() const;
+};
+
+/// Camera which controls the view of the scene.
+struct GlutCamera
+{
+ /// Eye position.
+ SurgSim::Math::Vector3d eye;
+ /// Center (look at) position.
+ SurgSim::Math::Vector3d center;
+ /// Up direction.
+ SurgSim::Math::Vector3d up;
+ /// Field of view angle (in degrees) in the vertical direction.
+ double fovY;
+ /// Near clipping plane distance from camera, in meters.
+ double zNear;
+ /// Far clipping plane distance from camera, in meters.
+ double zFar;
+
+ /// Constructor
+ /// \param eye_ Eye position.
+ /// \param center_ Center (look at) position.
+ /// \param up_ Up direction.
+ /// \param fovY_ Field of view angle (in degrees) in the vertical direction.
+ /// \param zNear_ Near clipping plane distance from camera, in meters.
+ /// \param zFar_ Far clipping plane distance from camera, in meters.
+ GlutCamera(const SurgSim::Math::Vector3d& eye_, const SurgSim::Math::Vector3d& center_,
+ const SurgSim::Math::Vector3d& up_, const double fovY_, double zNear_, double zFar_) :
+ eye(eye_),
+ center(center_),
+ up(up_),
+ fovY(fovY_),
+ zNear(zNear_),
+ zFar(zFar_)
+ {
+ }
+};
+
+/// Simple static class renderer built on Glut.
+class GlutRenderer
+{
+public:
+ /// Initializes and runs the Glut main loop. This function will block until the Glut graphics window is closed.
+ static void run()
+ {
+ initialize();
+ glutMainLoop();
+ }
+
+ /// Sets the camera used to control the view of the scene.
+ /// \param camera View camera.
+ static void setCamera(std::shared_ptr<GlutCamera> camera)
+ {
+ m_camera = camera;
+ }
+
+ /// Adds an object to the scene.
+ /// \param object Scene object.
+ static void addObject(std::shared_ptr<GlutRenderObject> object)
+ {
+ m_objects.push_back(object);
+ }
+
+private:
+ /// Width of the window.
+ static int m_width;
+ /// Height of the window.
+ static int m_height;
+
+ /// Camera which controls the view of the scene.
+ static std::shared_ptr<GlutCamera> m_camera;
+ /// Objects in the scene.
+ static std::vector< std::shared_ptr<GlutRenderObject> > m_objects;
+
+ /// Initializes the Glut window.
+ static void initialize();
+
+ /// Glut reshape function which handles the resizing of the window.
+ static void reshape(GLint width, GLint height)
+ {
+ m_width = width;
+ m_height = height;
+ glViewport(0, 0, m_width, m_height);
+ }
+
+ /// Glut display function which handles the drawing of the scene.
+ static void display();
+
+ /// Iterates through the scene objects to draw them.
+ static void drawObjects()
+ {
+ for (auto it = m_objects.cbegin(); it != m_objects.cend(); ++it)
+ {
+ (*it)->draw();
+ }
+ }
+};
+
+
+#endif // SURGSIM_TESTING_VISUALTESTCOMMON_GLUTRENDERER_H
diff --git a/SurgSim/Testing/VisualTestCommon/MovingSquareForce.cpp b/SurgSim/Testing/VisualTestCommon/MovingSquareForce.cpp
new file mode 100644
index 0000000..f91c9d7
--- /dev/null
+++ b/SurgSim/Testing/VisualTestCommon/MovingSquareForce.cpp
@@ -0,0 +1,147 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Testing/VisualTestCommon/MovingSquareForce.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+#include "SurgSim/Framework/Assert.h"
+
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::RigidTransform3d;
+
+
+MovingSquareForce::SquarePoseVectors::SquarePoseVectors() :
+ normal(0, -1, 0),
+ edgeDirectionX(1, 0, 0),
+ edgeDirectionY(0, 0, 1),
+ center(0, 0, 0)
+{
+}
+
+
+MovingSquareForce::MovingSquareForce(const std::string& toolDeviceName, const std::string& squareDeviceName) :
+ m_toolDeviceName(toolDeviceName),
+ m_squareDeviceName(squareDeviceName),
+ m_squareHalfSize(0.050),
+ m_surfaceStiffness(250.0),
+ m_forceLimit(5.0),
+ m_squareNormalDirection(+1),
+ m_tipPoint(0, 0, 0)
+{
+ DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::FORCE);
+ builder.addVector(SurgSim::DataStructures::Names::TORQUE);
+ m_outputData = std::move(builder.createData());
+
+ m_outputData.vectors().set(SurgSim::DataStructures::Names::FORCE, Vector3d(0, 0, 0));
+ m_outputData.vectors().set(SurgSim::DataStructures::Names::TORQUE, Vector3d(0, 0, 0));
+}
+
+void MovingSquareForce::initializeInput(const std::string& device, const DataGroup& inputData)
+{
+}
+
+void MovingSquareForce::handleInput(const std::string& device, const DataGroup& inputData)
+{
+ if (device == m_toolDeviceName)
+ {
+ updateTool(inputData);
+ }
+ else if (device == m_squareDeviceName)
+ {
+ updateSquare(inputData);
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Unknown device name '" << device << "'";
+ }
+}
+
+void MovingSquareForce::updateTool(const DataGroup& inputData)
+{
+ m_outputData.resetAll();
+
+ RigidTransform3d devicePose;
+ if (! inputData.poses().get(SurgSim::DataStructures::Names::POSE, &devicePose))
+ {
+ m_outputData.vectors().set(SurgSim::DataStructures::Names::FORCE, Vector3d(0, 0, 0));
+ m_outputData.vectors().set(SurgSim::DataStructures::Names::TORQUE, Vector3d(0, 0, 0));
+ return;
+ }
+
+ Vector3d tipPosition = devicePose * m_tipPoint;
+ m_outputData.vectors().set(SurgSim::DataStructures::Names::FORCE, computeForce(tipPosition));
+ m_outputData.vectors().set(SurgSim::DataStructures::Names::TORQUE, Vector3d(0, 0, 0));
+}
+
+void MovingSquareForce::updateSquare(const DataGroup& inputData)
+{
+ RigidTransform3d devicePose;
+ if (! inputData.poses().get(SurgSim::DataStructures::Names::POSE, &devicePose))
+ {
+ return;
+ }
+
+ SquarePoseVectors square;
+ square.normal = devicePose.linear() * Vector3d(0, -1, 0);
+ square.edgeDirectionX = devicePose.linear() * Vector3d(1, 0, 0);
+ square.edgeDirectionY = devicePose.linear() * Vector3d(0, 0, 1);
+ square.center = devicePose * Vector3d(0, 0, 0);
+
+ // Push into thread-safe storage so the other device can safely access it
+ m_square.set(square);
+}
+
+bool MovingSquareForce::requestOutput(const std::string& device, DataGroup* outputData)
+{
+ *outputData = m_outputData;
+ return true;
+}
+
+Vector3d MovingSquareForce::computeForce(const Vector3d& position)
+{
+ Vector3d force(0, 0, 0);
+
+ SquarePoseVectors square;
+ m_square.get(&square);
+ Vector3d offset = position - square.center;
+ double orthogonalDistance = offset.dot(square.normal);
+
+ if ((orthogonalDistance * m_squareNormalDirection) > 0)
+ {
+ double planeDistanceX = offset.dot(square.edgeDirectionX);
+ double planeDistanceY = offset.dot(square.edgeDirectionY);
+ bool insideSquare = (fabs(planeDistanceX) < m_squareHalfSize) && (fabs(planeDistanceY) < m_squareHalfSize);
+
+ force = -m_surfaceStiffness * orthogonalDistance * square.normal;
+
+ if ((force.norm() > m_forceLimit) || ! insideSquare)
+ {
+ force.setZero();
+ m_squareNormalDirection = -m_squareNormalDirection;
+ }
+
+ }
+ return force;
+}
diff --git a/SurgSim/Testing/VisualTestCommon/MovingSquareForce.h b/SurgSim/Testing/VisualTestCommon/MovingSquareForce.h
new file mode 100644
index 0000000..70b8340
--- /dev/null
+++ b/SurgSim/Testing/VisualTestCommon/MovingSquareForce.h
@@ -0,0 +1,102 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_VISUALTESTCOMMON_MOVINGSQUAREFORCE_H
+#define SURGSIM_TESTING_VISUALTESTCOMMON_MOVINGSQUAREFORCE_H
+
+#include <string>
+
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+
+#include "SurgSim/Framework/LockedContainer.h"
+
+
+/// A simple listener to calculate collision force against a square area for the example application.
+/// Includes support for the square being moved by a second tool.
+/// \sa SurgSim::Input::InputConsumerInterface, SurgSim::Input::OutputProducerInterface
+class MovingSquareForce : public SurgSim::Input::InputConsumerInterface, public SurgSim::Input::OutputProducerInterface
+{
+public:
+ /// Constructor.
+ MovingSquareForce(const std::string& toolDeviceName, const std::string& squareDeviceName);
+
+ virtual void initializeInput(const std::string& device,
+ const SurgSim::DataStructures::DataGroup& inputData) override;
+
+ virtual void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override;
+
+ virtual bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) override;
+
+protected:
+ /// Updates the state of the tool as described by toolInputData.
+ /// \param toolInputData The state of the device controlling the tool.
+ void updateTool(const SurgSim::DataStructures::DataGroup& toolInputData);
+
+ /// Updates the state of the square as described by squareInputData.
+ /// \param squareInputData The state of the device controlling the colliding square.
+ void updateSquare(const SurgSim::DataStructures::DataGroup& squareInputData);
+
+ /// Calculates the force as a function of device tip position.
+ /// The calculation is very simple, for a simple demo of the device input/output functionality.
+ ///
+ /// \param position The device tip position.
+ /// \return The computed force.
+ SurgSim::Math::Vector3d computeForce(const SurgSim::Math::Vector3d& position);
+
+private:
+ /// State defined by the pose of the square.
+ struct SquarePoseVectors
+ {
+ /// Constructor.
+ SquarePoseVectors();
+
+ /// The unit normal vector of the square.
+ SurgSim::Math::Vector3d normal;
+ /// The unit direction along one of the pairs edges of the square.
+ SurgSim::Math::Vector3d edgeDirectionX;
+ /// The unit direction along the other pair of edges of the square.
+ SurgSim::Math::Vector3d edgeDirectionY;
+ /// The location of the center of the square in world coordinates.
+ SurgSim::Math::Vector3d center;
+ };
+
+
+ /// Name of the device used as the tool.
+ const std::string m_toolDeviceName;
+ /// Name of the device used to move the square.
+ const std::string m_squareDeviceName;
+
+ /// Internally stored output data (force and torque).
+ SurgSim::DataStructures::DataGroup m_outputData;
+
+ /// One half of the edge length of the square we're colliding against, in meters.
+ double m_squareHalfSize;
+ /// The surface stiffness, in newtons per meter.
+ double m_surfaceStiffness;
+ /// The maximum force before the application allows the tool to pop through.
+ double m_forceLimit;
+
+ /// Points and directions defined by the pose of the square.
+ SurgSim::Framework::LockedContainer<SquarePoseVectors> m_square;
+ /// The current sign of the direction of the normal vector of the square.
+ double m_squareNormalDirection;
+
+ /// The location of the "tip" (i.e. interacting point) of the tool, in the local frame relative to the tool pose.
+ SurgSim::Math::Vector3d m_tipPoint;
+};
+
+#endif // SURGSIM_TESTING_VISUALTESTCOMMON_MOVINGSQUAREFORCE_H
diff --git a/SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.cpp b/SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.cpp
new file mode 100644
index 0000000..99c1889
--- /dev/null
+++ b/SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.cpp
@@ -0,0 +1,145 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.h"
+
+using SurgSim::DataStructures::DataGroup;
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::RigidTransform3d;
+
+MovingSquareGlutWindow::MovingSquareGlutWindow(const std::string& toolDeviceName, const std::string& squareDeviceName) :
+ m_toolDeviceName(toolDeviceName),
+ m_squareDeviceName(squareDeviceName),
+ m_tool(std::make_shared<GlutGroup>())
+{
+ m_camera = std::make_shared<GlutCamera>(Vector3d(-0.15, 0.15, 0.3), Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0), 45.0, 0.001, 1.0);
+ GlutRenderer::setCamera(m_camera);
+
+ m_square = std::make_shared<GlutSquare>(0.050, Vector3d(1.0, 1.0, 1.0), Vector3d(1.0, 0.0, 0.0),
+ Vector3d(0.0, 0.0, 1.0));
+ GlutRenderer::addObject(m_square);
+
+ m_toolSphere = std::make_shared<GlutSphere>(0.010, Vector3d(1.0, 1.0, 1.0));
+ std::shared_ptr<GlutAxes> toolAxes = std::make_shared<GlutAxes>(0.025, 5.0f);
+ m_tool->children.push_back(m_toolSphere);
+ m_tool->children.push_back(toolAxes);
+
+ GlutRenderer::addObject(m_tool);
+
+ m_renderThread = boost::thread(boost::ref(GlutRenderer::run));
+}
+
+MovingSquareGlutWindow::~MovingSquareGlutWindow()
+{
+ if (m_renderThread.joinable())
+ {
+ m_renderThread.join();
+ }
+}
+
+void MovingSquareGlutWindow::initializeInput(const std::string& device, const DataGroup& inputData)
+{
+}
+
+void MovingSquareGlutWindow::handleInput(const std::string& device, const DataGroup& inputData)
+{
+ if (device == m_toolDeviceName)
+ {
+ updateTool(inputData);
+ }
+ else if (device == m_squareDeviceName)
+ {
+ updateSquare(inputData);
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Unknown device name '" << device << "'";
+ }
+}
+
+void MovingSquareGlutWindow::updateTool(const DataGroup& inputData)
+{
+ RigidTransform3d devicePose;
+ bool button1, button2, button3, button4, isHomed;
+ if (! inputData.poses().get(SurgSim::DataStructures::Names::POSE, &devicePose))
+ {
+ return; // not much we can do without a pose...
+ }
+ if (! inputData.booleans().get(SurgSim::DataStructures::Names::BUTTON_1, &button1))
+ {
+ button1 = false;
+ }
+ if (! inputData.booleans().get(SurgSim::DataStructures::Names::BUTTON_2, &button2))
+ {
+ button2 = false;
+ }
+ if (! inputData.booleans().get(SurgSim::DataStructures::Names::BUTTON_3, &button3))
+ {
+ button3 = false;
+ }
+ if (! inputData.booleans().get(SurgSim::DataStructures::Names::BUTTON_4, &button4))
+ {
+ button4 = false;
+ }
+ if (! inputData.booleans().get(SurgSim::DataStructures::Names::IS_HOMED, &isHomed))
+ {
+ isHomed = true; // if device has no homing indication, never show as un-homed
+ }
+
+ m_tool->pose = devicePose;
+
+ // We generate the color as a linear combination from various states and buttons. We start from plain white.
+ Vector3d unscaledColor(1.0, 1.0, 1.0);
+ double colorScale = 1;
+ if (! isHomed)
+ {
+ unscaledColor += Vector3d(1.0, 0.0, 0.0); // if un-homed, show the device as red
+ colorScale += 1;
+ }
+ if (button1)
+ {
+ unscaledColor += 1.5 * Vector3d(0.0, 1.0, 1.0);
+ colorScale += 1.5;
+ }
+ if (button2)
+ {
+ unscaledColor += 1.5 * Vector3d(1.0, 0.0, 1.0);
+ colorScale += 1.5;
+ }
+ if (button3)
+ {
+ unscaledColor += 1.5 * Vector3d(1.0, 1.0, 0.0);
+ colorScale += 1.5;
+ }
+ if (button4)
+ {
+ unscaledColor += 1.5 * Vector3d(0.7, 0.7, 0.7);
+ colorScale += 1.5;
+ }
+ m_toolSphere->color = unscaledColor * (1.0 / colorScale);
+}
+
+void MovingSquareGlutWindow::updateSquare(const DataGroup& inputData)
+{
+ RigidTransform3d devicePose;
+ if (! inputData.poses().get(SurgSim::DataStructures::Names::POSE, &devicePose))
+ {
+ return;
+ }
+
+ m_square->pose = devicePose;
+}
diff --git a/SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.h b/SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.h
new file mode 100644
index 0000000..fd9f0e1
--- /dev/null
+++ b/SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.h
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_VISUALTESTCOMMON_MOVINGSQUAREGLUTWINDOW_H
+#define SURGSIM_TESTING_VISUALTESTCOMMON_MOVINGSQUAREGLUTWINDOW_H
+
+#include <boost/thread.hpp>
+
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+
+#include "SurgSim/Testing/VisualTestCommon/GlutRenderer.h"
+
+/// A simple listener to display the simple scene composed of a square and tool for the example application.
+/// Includes support for the square being moved by a second tool.
+/// \sa SurgSim::Input::InputConsumerInterface
+class MovingSquareGlutWindow : public SurgSim::Input::InputConsumerInterface
+{
+public:
+ /// Constructor.
+ MovingSquareGlutWindow(const std::string& toolDeviceName, const std::string& squareDeviceName);
+ /// Destructor.
+ ~MovingSquareGlutWindow();
+
+ virtual void initializeInput(const std::string& device,
+ const SurgSim::DataStructures::DataGroup& inputData) override;
+
+ virtual void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override;
+
+protected:
+
+private:
+ /// Render thread which runs the Glut main loop.
+ boost::thread m_renderThread;
+
+ /// Name of the tool device.
+ const std::string m_toolDeviceName;
+ /// Name of the square device.
+ const std::string m_squareDeviceName;
+
+ /// Camera which controls the view of the scene.
+ std::shared_ptr<GlutCamera> m_camera;
+
+ /// Tool composed of a sphere and axes that are moved with device input.
+ std::shared_ptr<GlutGroup> m_tool;
+ /// Sphere of the tool. Pointer is kept here so that the color can easily be changed based on the device's button
+ /// state.
+ std::shared_ptr<GlutSphere> m_toolSphere;
+
+ /// Square that is moved with device input.
+ std::shared_ptr<GlutSquare> m_square;
+
+ /// Updates the tool based on the device input.
+ /// \param inputData Input data from the device.
+ void updateTool(const SurgSim::DataStructures::DataGroup& inputData);
+ /// Updates the square based on the device input.
+ /// \param inputData Input data from the device.
+ void updateSquare(const SurgSim::DataStructures::DataGroup& inputData);
+
+};
+
+#endif // SURGSIM_TESTING_VISUALTESTCOMMON_MOVINGSQUAREGLUTWINDOW_H
diff --git a/SurgSim/Testing/VisualTestCommon/ToolSquareTest.cpp b/SurgSim/Testing/VisualTestCommon/ToolSquareTest.cpp
new file mode 100644
index 0000000..891c567
--- /dev/null
+++ b/SurgSim/Testing/VisualTestCommon/ToolSquareTest.cpp
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#ifdef __APPLE__
+#include <GLUT/glut.h>
+#else
+#include <GL/glut.h>
+#endif
+
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Framework/Assert.h"
+
+using SurgSim::Input::DeviceInterface;
+
+#include "SurgSim/Testing/VisualTestCommon/MovingSquareForce.h"
+#include "SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.h"
+
+
+void runToolSquareTest(std::shared_ptr<DeviceInterface> toolDevice, std::shared_ptr<DeviceInterface> squareDevice,
+ const char* testDescriptionMessage)
+{
+ SURGSIM_ASSERT(toolDevice && squareDevice);
+ if (! toolDevice->initialize())
+ {
+ std::cout << std::endl << "Could not initialize device named '" << toolDevice->getName() <<
+ "' for the tool." << std::endl << "--- Press Enter to quit the application! ---" << std::endl;
+ getc(stdin);
+ return;
+ }
+ if (! squareDevice->initialize())
+ {
+ std::cout << std::endl << "Could not initialize device named '" << squareDevice->getName() <<
+ "' for the square." << std::endl << "--- Press Enter to quit the application! ---" << std::endl;
+ getc(stdin);
+ return;
+ }
+ std::shared_ptr<MovingSquareForce> squareForce =
+ std::make_shared<MovingSquareForce>(toolDevice->getName(), squareDevice->getName());
+ toolDevice->addInputConsumer(squareForce);
+ toolDevice->setOutputProducer(squareForce);
+ squareDevice->addInputConsumer(squareForce);
+ // NB: the code does not currently support exerting the reaction force on the square.
+
+ std::shared_ptr<MovingSquareGlutWindow> squareGlutWindow =
+ std::make_shared<MovingSquareGlutWindow>(toolDevice->getName(), squareDevice->getName());
+ toolDevice->addInputConsumer(squareGlutWindow);
+ squareDevice->addInputConsumer(squareGlutWindow);
+
+ std::cout << std::endl << "**********************************************************************" << std::endl <<
+ testDescriptionMessage << std::endl << std::endl << "When done, press Enter to quit the application." <<
+ std::endl << "**********************************************************************" << std::endl;
+
+ // Wait for a key; the display, force generation, etc. all happen in separate threads.
+ getc(stdin);
+
+ toolDevice->removeInputConsumer(squareForce);
+ toolDevice->removeOutputProducer(squareForce);
+ squareDevice->removeInputConsumer(squareForce);
+
+ toolDevice->removeInputConsumer(squareGlutWindow);
+ squareDevice->removeInputConsumer(squareGlutWindow);
+
+ // Right now you can't just tear down the MovingSquareGlutWindow, so we exit with prejudice instead.
+ exit(0);
+}
diff --git a/SurgSim/Testing/VisualTestCommon/ToolSquareTest.h b/SurgSim/Testing/VisualTestCommon/ToolSquareTest.h
new file mode 100644
index 0000000..eda385e
--- /dev/null
+++ b/SurgSim/Testing/VisualTestCommon/ToolSquareTest.h
@@ -0,0 +1,39 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_TESTING_VISUALTESTCOMMON_TOOLSQUARETEST_H
+#define SURGSIM_TESTING_VISUALTESTCOMMON_TOOLSQUARETEST_H
+
+#include <memory>
+
+namespace SurgSim
+{
+namespace Input
+{
+class DeviceInterface;
+};
+};
+
+/// Creates a GLUT window containing a sphere and a square each controlled by a device, with interaction forces.
+/// \warning Does not return, instead calls exit(0). Therefore, will not destruct the device or its scaffold.
+/// \sa MovingSquareForce, MovingSquareGlutWindow
+/// \param toolDevice The device providing an input pose to control the sphere.
+/// \param squareDevice The device providing an input pose to control the square.
+/// \param testDescriptionMessage A message to be printed to the screen, e.g., instructions for operation.
+void runToolSquareTest(std::shared_ptr<SurgSim::Input::DeviceInterface> toolDevice,
+ std::shared_ptr<SurgSim::Input::DeviceInterface> squareDevice,
+ const char* testDescriptionMessage);
+
+#endif // SURGSIM_TESTING_VISUALTESTCOMMON_TOOLSQUARETEST_H
diff --git a/ThirdParty/README_THIRD_PARTY b/ThirdParty/README_THIRD_PARTY
new file mode 100644
index 0000000..be9475c
--- /dev/null
+++ b/ThirdParty/README_THIRD_PARTY
@@ -0,0 +1,3 @@
+This directory contains third-party software that is not a part of
+OpenSurgSim, but is packaged with it for convenience.
+
diff --git a/ThirdParty/google-style-lint/cpplint.py b/ThirdParty/google-style-lint/cpplint.py
new file mode 100644
index 0000000..526b955
--- /dev/null
+++ b/ThirdParty/google-style-lint/cpplint.py
@@ -0,0 +1,3361 @@
+#!/usr/bin/python
+#
+# Copyright (c) 2009 Google Inc. 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 Google Inc. 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.
+
+# Here are some issues that I've had people identify in my code during reviews,
+# that I think are possible to flag automatically in a lint tool. If these were
+# caught by lint, it would save time both for myself and that of my reviewers.
+# Most likely, some of these are beyond the scope of the current lint framework,
+# but I think it is valuable to retain these wish-list items even if they cannot
+# be immediately implemented.
+#
+# Suggestions
+# -----------
+# - Check for no 'explicit' for multi-arg ctor
+# - Check for boolean assign RHS in parens
+# - Check for ctor initializer-list colon position and spacing
+# - Check that if there's a ctor, there should be a dtor
+# - Check accessors that return non-pointer member variables are
+# declared const
+# - Check accessors that return non-const pointer member vars are
+# *not* declared const
+# - Check for using public includes for testing
+# - Check for spaces between brackets in one-line inline method
+# - Check for no assert()
+# - Check for spaces surrounding operators
+# - Check for 0 in pointer context (should be NULL)
+# - Check for 0 in char context (should be '\0')
+# - Check for camel-case method name conventions for methods
+# that are not simple inline getters and setters
+# - Check that base classes have virtual destructors
+# put " // namespace" after } that closes a namespace, with
+# namespace's name after 'namespace' if it is named.
+# - Do not indent namespace contents
+# - Avoid inlining non-trivial constructors in header files
+# include base/basictypes.h if DISALLOW_EVIL_CONSTRUCTORS is used
+# - Check for old-school (void) cast for call-sites of functions
+# ignored return value
+# - Check gUnit usage of anonymous namespace
+# - Check for class declaration order (typedefs, consts, enums,
+# ctor(s?), dtor, friend declarations, methods, member vars)
+#
+
+"""Does google-lint on c++ files.
+
+The goal of this script is to identify places in the code that *may*
+be in non-compliance with google style. It does not attempt to fix
+up these problems -- the point is to educate. It does also not
+attempt to find all problems, or to ensure that everything it does
+find is legitimately a problem.
+
+In particular, we can get very confused by /* and // inside strings!
+We do a small hack, which is to ignore //'s with "'s after them on the
+same line, but it is far from perfect (in either direction).
+"""
+
+import codecs
+import getopt
+import math # for log
+import os
+import re
+import sre_compile
+import string
+import sys
+import unicodedata
+
+
+_USAGE = """
+Syntax: cpplint.py [--verbose=#] [--output=vs7] [--filter=-x,+y,...]
+ [--counting=total|toplevel|detailed]
+ <file> [file] ...
+
+ The style guidelines this tries to follow are those in
+ http://google-styleguide.googlecode.com/svn/trunk/cppguide.xml
+
+ Every problem is given a confidence score from 1-5, with 5 meaning we are
+ certain of the problem, and 1 meaning it could be a legitimate construct.
+ This will miss some errors, and is not a substitute for a code review.
+
+ To suppress false-positive errors of a certain category, add a
+ 'NOLINT(category)' comment to the line. NOLINT or NOLINT(*)
+ suppresses errors of all categories on that line.
+
+ The files passed in will be linted; at least one file must be provided.
+ Linted extensions are .cc, .cpp, and .h. Other file types will be ignored.
+
+ Flags:
+
+ output=vs7
+ By default, the output is formatted to ease emacs parsing. Visual Studio
+ compatible output (vs7) may also be used. Other formats are unsupported.
+
+ verbose=#
+ Specify a number 0-5 to restrict errors to certain verbosity levels.
+
+ filter=-x,+y,...
+ Specify a comma-separated list of category-filters to apply: only
+ error messages whose category names pass the filters will be printed.
+ (Category names are printed with the message and look like
+ "[whitespace/indent]".) Filters are evaluated left to right.
+ "-FOO" and "FOO" means "do not print categories that start with FOO".
+ "+FOO" means "do print categories that start with FOO".
+
+ Examples: --filter=-whitespace,+whitespace/braces
+ --filter=whitespace,runtime/printf,+runtime/printf_format
+ --filter=-,+build/include_what_you_use
+
+ To see a list of all the categories used in cpplint, pass no arg:
+ --filter=
+
+ counting=total|toplevel|detailed
+ The total number of errors found is always printed. If
+ 'toplevel' is provided, then the count of errors in each of
+ the top-level categories like 'build' and 'whitespace' will
+ also be printed. If 'detailed' is provided, then a count
+ is provided for each category like 'build/class'.
+"""
+
+# We categorize each error message we print. Here are the categories.
+# We want an explicit list so we can list them all in cpplint --filter=.
+# If you add a new error message with a new category, add it to the list
+# here! cpplint_unittest.py should tell you if you forget to do this.
+# \ used for clearer layout -- pylint: disable-msg=C6013
+_ERROR_CATEGORIES = [
+ 'build/class',
+ 'build/deprecated',
+ 'build/endif_comment',
+ 'build/explicit_make_pair',
+ 'build/forward_decl',
+ 'build/header_guard',
+ 'build/include',
+ 'build/include_alpha',
+ 'build/include_order',
+ 'build/include_what_you_use',
+ 'build/namespaces',
+ 'build/printf_format',
+ 'build/storage_class',
+ 'legal/copyright',
+ 'readability/braces',
+ 'readability/casting',
+ 'readability/check',
+ 'readability/constructors',
+ 'readability/fn_size',
+ 'readability/function',
+ 'readability/multiline_comment',
+ 'readability/multiline_string',
+ 'readability/nolint',
+ 'readability/streams',
+ 'readability/todo',
+ 'readability/utf8',
+ 'runtime/arrays',
+ 'runtime/casting',
+ 'runtime/explicit',
+ 'runtime/int',
+ 'runtime/init',
+ 'runtime/invalid_increment',
+ 'runtime/member_string_references',
+ 'runtime/memset',
+ 'runtime/operator',
+ 'runtime/printf',
+ 'runtime/printf_format',
+ 'runtime/references',
+ 'runtime/rtti',
+ 'runtime/sizeof',
+ 'runtime/string',
+ 'runtime/threadsafe_fn',
+ 'runtime/virtual',
+ 'whitespace/blank_line',
+ 'whitespace/braces',
+ 'whitespace/comma',
+ 'whitespace/comments',
+ 'whitespace/end_of_line',
+ 'whitespace/ending_newline',
+ 'whitespace/indent',
+ 'whitespace/labels',
+ 'whitespace/line_length',
+ 'whitespace/newline',
+ 'whitespace/operators',
+ 'whitespace/parens',
+ 'whitespace/semicolon',
+ 'whitespace/tab',
+ 'whitespace/todo'
+ ]
+
+# The default state of the category filter. This is overrided by the --filter=
+# flag. By default all errors are on, so only add here categories that should be
+# off by default (i.e., categories that must be enabled by the --filter= flags).
+# All entries here should start with a '-' or '+', as in the --filter= flag.
+_DEFAULT_FILTERS = ['-build/include_alpha']
+
+# We used to check for high-bit characters, but after much discussion we
+# decided those were OK, as long as they were in UTF-8 and didn't represent
+# hard-coded international strings, which belong in a separate i18n file.
+
+# Headers that we consider STL headers.
+_STL_HEADERS = frozenset([
+ 'algobase.h', 'algorithm', 'alloc.h', 'bitset', 'deque', 'exception',
+ 'function.h', 'functional', 'hash_map', 'hash_map.h', 'hash_set',
+ 'hash_set.h', 'iterator', 'list', 'list.h', 'map', 'memory', 'new',
+ 'pair.h', 'pthread_alloc', 'queue', 'set', 'set.h', 'sstream', 'stack',
+ 'stl_alloc.h', 'stl_relops.h', 'type_traits.h',
+ 'utility', 'vector', 'vector.h',
+ ])
+
+
+# Non-STL C++ system headers.
+_CPP_HEADERS = frozenset([
+ 'algo.h', 'builtinbuf.h', 'bvector.h', 'cassert', 'cctype',
+ 'cerrno', 'cfloat', 'ciso646', 'climits', 'clocale', 'cmath',
+ 'complex', 'complex.h', 'csetjmp', 'csignal', 'cstdarg', 'cstddef',
+ 'cstdio', 'cstdlib', 'cstring', 'ctime', 'cwchar', 'cwctype',
+ 'defalloc.h', 'deque.h', 'editbuf.h', 'exception', 'fstream',
+ 'fstream.h', 'hashtable.h', 'heap.h', 'indstream.h', 'iomanip',
+ 'iomanip.h', 'ios', 'iosfwd', 'iostream', 'iostream.h', 'istream',
+ 'istream.h', 'iterator.h', 'limits', 'map.h', 'multimap.h', 'multiset.h',
+ 'numeric', 'ostream', 'ostream.h', 'parsestream.h', 'pfstream.h',
+ 'PlotFile.h', 'procbuf.h', 'pthread_alloc.h', 'rope', 'rope.h',
+ 'ropeimpl.h', 'SFile.h', 'slist', 'slist.h', 'stack.h', 'stdexcept',
+ 'stdiostream.h', 'streambuf.h', 'stream.h', 'strfile.h', 'string',
+ 'strstream', 'strstream.h', 'tempbuf.h', 'tree.h', 'typeinfo', 'valarray',
+ ])
+
+
+# Assertion macros. These are defined in base/logging.h and
+# testing/base/gunit.h. Note that the _M versions need to come first
+# for substring matching to work.
+_CHECK_MACROS = [
+ 'DCHECK', 'CHECK',
+ 'EXPECT_TRUE_M', 'EXPECT_TRUE',
+ 'ASSERT_TRUE_M', 'ASSERT_TRUE',
+ 'EXPECT_FALSE_M', 'EXPECT_FALSE',
+ 'ASSERT_FALSE_M', 'ASSERT_FALSE',
+ ]
+
+# Replacement macros for CHECK/DCHECK/EXPECT_TRUE/EXPECT_FALSE
+_CHECK_REPLACEMENT = dict([(m, {}) for m in _CHECK_MACROS])
+
+for op, replacement in [('==', 'EQ'), ('!=', 'NE'),
+ ('>=', 'GE'), ('>', 'GT'),
+ ('<=', 'LE'), ('<', 'LT')]:
+ _CHECK_REPLACEMENT['DCHECK'][op] = 'DCHECK_%s' % replacement
+ _CHECK_REPLACEMENT['CHECK'][op] = 'CHECK_%s' % replacement
+ _CHECK_REPLACEMENT['EXPECT_TRUE'][op] = 'EXPECT_%s' % replacement
+ _CHECK_REPLACEMENT['ASSERT_TRUE'][op] = 'ASSERT_%s' % replacement
+ _CHECK_REPLACEMENT['EXPECT_TRUE_M'][op] = 'EXPECT_%s_M' % replacement
+ _CHECK_REPLACEMENT['ASSERT_TRUE_M'][op] = 'ASSERT_%s_M' % replacement
+
+for op, inv_replacement in [('==', 'NE'), ('!=', 'EQ'),
+ ('>=', 'LT'), ('>', 'LE'),
+ ('<=', 'GT'), ('<', 'GE')]:
+ _CHECK_REPLACEMENT['EXPECT_FALSE'][op] = 'EXPECT_%s' % inv_replacement
+ _CHECK_REPLACEMENT['ASSERT_FALSE'][op] = 'ASSERT_%s' % inv_replacement
+ _CHECK_REPLACEMENT['EXPECT_FALSE_M'][op] = 'EXPECT_%s_M' % inv_replacement
+ _CHECK_REPLACEMENT['ASSERT_FALSE_M'][op] = 'ASSERT_%s_M' % inv_replacement
+
+
+# These constants define types of headers for use with
+# _IncludeState.CheckNextIncludeOrder().
+_C_SYS_HEADER = 1
+_CPP_SYS_HEADER = 2
+_LIKELY_MY_HEADER = 3
+_POSSIBLE_MY_HEADER = 4
+_OTHER_HEADER = 5
+
+
+_regexp_compile_cache = {}
+
+# Finds occurrences of NOLINT or NOLINT(...).
+_RE_SUPPRESSION = re.compile(r'\bNOLINT\b(\([^)]*\))?')
+
+# {str, set(int)}: a map from error categories to sets of linenumbers
+# on which those errors are expected and should be suppressed.
+_error_suppressions = {}
+
+def ParseNolintSuppressions(filename, raw_line, linenum, error):
+ """Updates the global list of error-suppressions.
+
+ Parses any NOLINT comments on the current line, updating the global
+ error_suppressions store. Reports an error if the NOLINT comment
+ was malformed.
+
+ Args:
+ filename: str, the name of the input file.
+ raw_line: str, the line of input text, with comments.
+ linenum: int, the number of the current line.
+ error: function, an error handler.
+ """
+ # FIXME(adonovan): "NOLINT(" is misparsed as NOLINT(*).
+ matched = _RE_SUPPRESSION.search(raw_line)
+ if matched:
+ category = matched.group(1)
+ if category in (None, '(*)'): # => "suppress all"
+ _error_suppressions.setdefault(None, set()).add(linenum)
+ else:
+ if category.startswith('(') and category.endswith(')'):
+ category = category[1:-1]
+ if category in _ERROR_CATEGORIES:
+ _error_suppressions.setdefault(category, set()).add(linenum)
+ else:
+ error(filename, linenum, 'readability/nolint', 5,
+ 'Unknown NOLINT error category: %s' % category)
+
+
+def ResetNolintSuppressions():
+ "Resets the set of NOLINT suppressions to empty."
+ _error_suppressions.clear()
+
+
+def IsErrorSuppressedByNolint(category, linenum):
+ """Returns true if the specified error category is suppressed on this line.
+
+ Consults the global error_suppressions map populated by
+ ParseNolintSuppressions/ResetNolintSuppressions.
+
+ Args:
+ category: str, the category of the error.
+ linenum: int, the current line number.
+ Returns:
+ bool, True iff the error should be suppressed due to a NOLINT comment.
+ """
+ return (linenum in _error_suppressions.get(category, set()) or
+ linenum in _error_suppressions.get(None, set()))
+
+def Match(pattern, s):
+ """Matches the string with the pattern, caching the compiled regexp."""
+ # The regexp compilation caching is inlined in both Match and Search for
+ # performance reasons; factoring it out into a separate function turns out
+ # to be noticeably expensive.
+ if not pattern in _regexp_compile_cache:
+ _regexp_compile_cache[pattern] = sre_compile.compile(pattern)
+ return _regexp_compile_cache[pattern].match(s)
+
+
+def Search(pattern, s):
+ """Searches the string for the pattern, caching the compiled regexp."""
+ if not pattern in _regexp_compile_cache:
+ _regexp_compile_cache[pattern] = sre_compile.compile(pattern)
+ return _regexp_compile_cache[pattern].search(s)
+
+
+class _IncludeState(dict):
+ """Tracks line numbers for includes, and the order in which includes appear.
+
+ As a dict, an _IncludeState object serves as a mapping between include
+ filename and line number on which that file was included.
+
+ Call CheckNextIncludeOrder() once for each header in the file, passing
+ in the type constants defined above. Calls in an illegal order will
+ raise an _IncludeError with an appropriate error message.
+
+ """
+ # self._section will move monotonically through this set. If it ever
+ # needs to move backwards, CheckNextIncludeOrder will raise an error.
+ _INITIAL_SECTION = 0
+ _MY_H_SECTION = 1
+ _C_SECTION = 2
+ _CPP_SECTION = 3
+ _OTHER_H_SECTION = 4
+
+ _TYPE_NAMES = {
+ _C_SYS_HEADER: 'C system header',
+ _CPP_SYS_HEADER: 'C++ system header',
+ _LIKELY_MY_HEADER: 'header this file implements',
+ _POSSIBLE_MY_HEADER: 'header this file may implement',
+ _OTHER_HEADER: 'other header',
+ }
+ _SECTION_NAMES = {
+ _INITIAL_SECTION: "... nothing. (This can't be an error.)",
+ _MY_H_SECTION: 'a header this file implements',
+ _C_SECTION: 'C system header',
+ _CPP_SECTION: 'C++ system header',
+ _OTHER_H_SECTION: 'other header',
+ }
+
+ def __init__(self):
+ dict.__init__(self)
+ # The name of the current section.
+ self._section = self._INITIAL_SECTION
+ # The path of last found header.
+ self._last_header = ''
+
+ def CanonicalizeAlphabeticalOrder(self, header_path):
+ """Returns a path canonicalized for alphabetical comparison.
+
+ - replaces "-" with "_" so they both cmp the same.
+ - removes '-inl' since we don't require them to be after the main header.
+ - lowercase everything, just in case.
+
+ Args:
+ header_path: Path to be canonicalized.
+
+ Returns:
+ Canonicalized path.
+ """
+ return header_path.replace('-inl.h', '.h').replace('-', '_').lower()
+
+ def IsInAlphabeticalOrder(self, header_path):
+ """Check if a header is in alphabetical order with the previous header.
+
+ Args:
+ header_path: Header to be checked.
+
+ Returns:
+ Returns true if the header is in alphabetical order.
+ """
+ canonical_header = self.CanonicalizeAlphabeticalOrder(header_path)
+ if self._last_header > canonical_header:
+ return False
+ self._last_header = canonical_header
+ return True
+
+ def CheckNextIncludeOrder(self, header_type):
+ """Returns a non-empty error message if the next header is out of order.
+
+ This function also updates the internal state to be ready to check
+ the next include.
+
+ Args:
+ header_type: One of the _XXX_HEADER constants defined above.
+
+ Returns:
+ The empty string if the header is in the right order, or an
+ error message describing what's wrong.
+
+ """
+ error_message = ('Found %s after %s' %
+ (self._TYPE_NAMES[header_type],
+ self._SECTION_NAMES[self._section]))
+
+ last_section = self._section
+
+ if header_type == _C_SYS_HEADER:
+ if self._section <= self._C_SECTION:
+ self._section = self._C_SECTION
+ else:
+ self._last_header = ''
+ return error_message
+ elif header_type == _CPP_SYS_HEADER:
+ if self._section <= self._CPP_SECTION:
+ self._section = self._CPP_SECTION
+ else:
+ self._last_header = ''
+ return error_message
+ elif header_type == _LIKELY_MY_HEADER:
+ if self._section <= self._MY_H_SECTION:
+ self._section = self._MY_H_SECTION
+ else:
+ self._section = self._OTHER_H_SECTION
+ elif header_type == _POSSIBLE_MY_HEADER:
+ if self._section <= self._MY_H_SECTION:
+ self._section = self._MY_H_SECTION
+ else:
+ # This will always be the fallback because we're not sure
+ # enough that the header is associated with this file.
+ self._section = self._OTHER_H_SECTION
+ else:
+ assert header_type == _OTHER_HEADER
+ self._section = self._OTHER_H_SECTION
+
+ if last_section != self._section:
+ self._last_header = ''
+
+ return ''
+
+
+class _CppLintState(object):
+ """Maintains module-wide state.."""
+
+ def __init__(self):
+ self.verbose_level = 1 # global setting.
+ self.error_count = 0 # global count of reported errors
+ # filters to apply when emitting error messages
+ self.filters = _DEFAULT_FILTERS[:]
+ self.counting = 'total' # In what way are we counting errors?
+ self.errors_by_category = {} # string to int dict storing error counts
+
+ # output format:
+ # "emacs" - format that emacs can parse (default)
+ # "vs7" - format that Microsoft Visual Studio 7 can parse
+ self.output_format = 'emacs'
+
+ def SetOutputFormat(self, output_format):
+ """Sets the output format for errors."""
+ self.output_format = output_format
+
+ def SetVerboseLevel(self, level):
+ """Sets the module's verbosity, and returns the previous setting."""
+ last_verbose_level = self.verbose_level
+ self.verbose_level = level
+ return last_verbose_level
+
+ def SetCountingStyle(self, counting_style):
+ """Sets the module's counting options."""
+ self.counting = counting_style
+
+ def SetFilters(self, filters):
+ """Sets the error-message filters.
+
+ These filters are applied when deciding whether to emit a given
+ error message.
+
+ Args:
+ filters: A string of comma-separated filters (eg "+whitespace/indent").
+ Each filter should start with + or -; else we die.
+
+ Raises:
+ ValueError: The comma-separated filters did not all start with '+' or '-'.
+ E.g. "-,+whitespace,-whitespace/indent,whitespace/badfilter"
+ """
+ # Default filters always have less priority than the flag ones.
+ self.filters = _DEFAULT_FILTERS[:]
+ for filt in filters.split(','):
+ clean_filt = filt.strip()
+ if clean_filt:
+ self.filters.append(clean_filt)
+ for filt in self.filters:
+ if not (filt.startswith('+') or filt.startswith('-')):
+ raise ValueError('Every filter in --filters must start with + or -'
+ ' (%s does not)' % filt)
+
+ def ResetErrorCounts(self):
+ """Sets the module's error statistic back to zero."""
+ self.error_count = 0
+ self.errors_by_category = {}
+
+ def IncrementErrorCount(self, category):
+ """Bumps the module's error statistic."""
+ self.error_count += 1
+ if self.counting in ('toplevel', 'detailed'):
+ if self.counting != 'detailed':
+ category = category.split('/')[0]
+ if category not in self.errors_by_category:
+ self.errors_by_category[category] = 0
+ self.errors_by_category[category] += 1
+
+ def PrintErrorCounts(self):
+ """Print a summary of errors by category, and the total."""
+ for category, count in self.errors_by_category.iteritems():
+ sys.stderr.write('Category \'%s\' errors found: %d\n' %
+ (category, count))
+ sys.stderr.write('Total errors found: %d\n' % self.error_count)
+
+_cpplint_state = _CppLintState()
+
+
+def _OutputFormat():
+ """Gets the module's output format."""
+ return _cpplint_state.output_format
+
+
+def _SetOutputFormat(output_format):
+ """Sets the module's output format."""
+ _cpplint_state.SetOutputFormat(output_format)
+
+
+def _VerboseLevel():
+ """Returns the module's verbosity setting."""
+ return _cpplint_state.verbose_level
+
+
+def _SetVerboseLevel(level):
+ """Sets the module's verbosity, and returns the previous setting."""
+ return _cpplint_state.SetVerboseLevel(level)
+
+
+def _SetCountingStyle(level):
+ """Sets the module's counting options."""
+ _cpplint_state.SetCountingStyle(level)
+
+
+def _Filters():
+ """Returns the module's list of output filters, as a list."""
+ return _cpplint_state.filters
+
+
+def _SetFilters(filters):
+ """Sets the module's error-message filters.
+
+ These filters are applied when deciding whether to emit a given
+ error message.
+
+ Args:
+ filters: A string of comma-separated filters (eg "whitespace/indent").
+ Each filter should start with + or -; else we die.
+ """
+ _cpplint_state.SetFilters(filters)
+
+
+class _FunctionState(object):
+ """Tracks current function name and the number of lines in its body."""
+
+ _NORMAL_TRIGGER = 250 # for --v=0, 500 for --v=1, etc.
+ _TEST_TRIGGER = 400 # about 50% more than _NORMAL_TRIGGER.
+
+ def __init__(self):
+ self.in_a_function = False
+ self.lines_in_function = 0
+ self.current_function = ''
+
+ def Begin(self, function_name):
+ """Start analyzing function body.
+
+ Args:
+ function_name: The name of the function being tracked.
+ """
+ self.in_a_function = True
+ self.lines_in_function = 0
+ self.current_function = function_name
+
+ def Count(self):
+ """Count line in current function body."""
+ if self.in_a_function:
+ self.lines_in_function += 1
+
+ def Check(self, error, filename, linenum):
+ """Report if too many lines in function body.
+
+ Args:
+ error: The function to call with any errors found.
+ filename: The name of the current file.
+ linenum: The number of the line to check.
+ """
+ if Match(r'T(EST|est)', self.current_function):
+ base_trigger = self._TEST_TRIGGER
+ else:
+ base_trigger = self._NORMAL_TRIGGER
+ trigger = base_trigger * 2**_VerboseLevel()
+
+ if self.lines_in_function > trigger:
+ error_level = int(math.log(self.lines_in_function / base_trigger, 2))
+ # 50 => 0, 100 => 1, 200 => 2, 400 => 3, 800 => 4, 1600 => 5, ...
+ if error_level > 5:
+ error_level = 5
+ error(filename, linenum, 'readability/fn_size', error_level,
+ 'Small and focused functions are preferred:'
+ ' %s has %d non-comment lines'
+ ' (error triggered by exceeding %d lines).' % (
+ self.current_function, self.lines_in_function, trigger))
+
+ def End(self):
+ """Stop analyzing function body."""
+ self.in_a_function = False
+
+
+class _IncludeError(Exception):
+ """Indicates a problem with the include order in a file."""
+ pass
+
+
+class FileInfo:
+ """Provides utility functions for filenames.
+
+ FileInfo provides easy access to the components of a file's path
+ relative to the project root.
+ """
+
+ def __init__(self, filename):
+ self._filename = filename
+
+ def FullName(self):
+ """Make Windows paths like Unix."""
+ return os.path.abspath(self._filename).replace('\\', '/')
+
+ def RepositoryName(self):
+ """FullName after removing the local path to the repository.
+
+ If we have a real absolute path name here we can try to do something smart:
+ detecting the root of the checkout and truncating /path/to/checkout from
+ the name so that we get header guards that don't include things like
+ "C:\Documents and Settings\..." or "/home/username/..." in them and thus
+ people on different computers who have checked the source out to different
+ locations won't see bogus errors.
+ """
+ fullname = self.FullName()
+
+ if os.path.exists(fullname):
+ project_dir = os.path.dirname(fullname)
+
+ if os.path.exists(os.path.join(project_dir, ".svn")):
+ # If there's a .svn file in the current directory, we recursively look
+ # up the directory tree for the top of the SVN checkout
+ root_dir = project_dir
+ one_up_dir = os.path.dirname(root_dir)
+ while os.path.exists(os.path.join(one_up_dir, ".svn")):
+ root_dir = os.path.dirname(root_dir)
+ one_up_dir = os.path.dirname(one_up_dir)
+
+ prefix = os.path.commonprefix([root_dir, project_dir])
+ return fullname[len(prefix) + 1:]
+
+ # Not SVN <= 1.6? Try to find a git, hg, or svn top level directory by
+ # searching up from the current path.
+ root_dir = os.path.dirname(fullname)
+ while (root_dir != os.path.dirname(root_dir) and
+ not os.path.exists(os.path.join(root_dir, ".git")) and
+ not os.path.exists(os.path.join(root_dir, ".hg")) and
+ not os.path.exists(os.path.join(root_dir, ".svn"))):
+ root_dir = os.path.dirname(root_dir)
+
+ if (os.path.exists(os.path.join(root_dir, ".git")) or
+ os.path.exists(os.path.join(root_dir, ".hg")) or
+ os.path.exists(os.path.join(root_dir, ".svn"))):
+ prefix = os.path.commonprefix([root_dir, project_dir])
+ return fullname[len(prefix) + 1:]
+
+ # Don't know what to do; header guard warnings may be wrong...
+ return fullname
+
+ def Split(self):
+ """Splits the file into the directory, basename, and extension.
+
+ For 'chrome/browser/browser.cc', Split() would
+ return ('chrome/browser', 'browser', '.cc')
+
+ Returns:
+ A tuple of (directory, basename, extension).
+ """
+
+ googlename = self.RepositoryName()
+ project, rest = os.path.split(googlename)
+ return (project,) + os.path.splitext(rest)
+
+ def BaseName(self):
+ """File base name - text after the final slash, before the final period."""
+ return self.Split()[1]
+
+ def Extension(self):
+ """File extension - text following the final period."""
+ return self.Split()[2]
+
+ def NoExtension(self):
+ """File has no source file extension."""
+ return '/'.join(self.Split()[0:2])
+
+ def IsSource(self):
+ """File has a source file extension."""
+ return self.Extension()[1:] in ('c', 'cc', 'cpp', 'cxx')
+
+
+def _ShouldPrintError(category, confidence, linenum):
+ """If confidence >= verbose, category passes filter and is not suppressed."""
+
+ # There are three ways we might decide not to print an error message:
+ # a "NOLINT(category)" comment appears in the source,
+ # the verbosity level isn't high enough, or the filters filter it out.
+ if IsErrorSuppressedByNolint(category, linenum):
+ return False
+ if confidence < _cpplint_state.verbose_level:
+ return False
+
+ is_filtered = False
+ for one_filter in _Filters():
+ if one_filter.startswith('-'):
+ if category.startswith(one_filter[1:]):
+ is_filtered = True
+ elif one_filter.startswith('+'):
+ if category.startswith(one_filter[1:]):
+ is_filtered = False
+ else:
+ assert False # should have been checked for in SetFilter.
+ if is_filtered:
+ return False
+
+ return True
+
+
+def Error(filename, linenum, category, confidence, message):
+ """Logs the fact we've found a lint error.
+
+ We log where the error was found, and also our confidence in the error,
+ that is, how certain we are this is a legitimate style regression, and
+ not a misidentification or a use that's sometimes justified.
+
+ False positives can be suppressed by the use of
+ "cpplint(category)" comments on the offending line. These are
+ parsed into _error_suppressions.
+
+ Args:
+ filename: The name of the file containing the error.
+ linenum: The number of the line containing the error.
+ category: A string used to describe the "category" this bug
+ falls under: "whitespace", say, or "runtime". Categories
+ may have a hierarchy separated by slashes: "whitespace/indent".
+ confidence: A number from 1-5 representing a confidence score for
+ the error, with 5 meaning that we are certain of the problem,
+ and 1 meaning that it could be a legitimate construct.
+ message: The error message.
+ """
+ if _ShouldPrintError(category, confidence, linenum):
+ _cpplint_state.IncrementErrorCount(category)
+ if _cpplint_state.output_format == 'vs7':
+ sys.stderr.write('%s(%s): %s [%s] [%d]\n' % (
+ filename, linenum, message, category, confidence))
+ else:
+ sys.stderr.write('%s:%s: %s [%s] [%d]\n' % (
+ filename, linenum, message, category, confidence))
+
+
+# Matches standard C++ escape esequences per 2.13.2.3 of the C++ standard.
+_RE_PATTERN_CLEANSE_LINE_ESCAPES = re.compile(
+ r'\\([abfnrtv?"\\\']|\d+|x[0-9a-fA-F]+)')
+# Matches strings. Escape codes should already be removed by ESCAPES.
+_RE_PATTERN_CLEANSE_LINE_DOUBLE_QUOTES = re.compile(r'"[^"]*"')
+# Matches characters. Escape codes should already be removed by ESCAPES.
+_RE_PATTERN_CLEANSE_LINE_SINGLE_QUOTES = re.compile(r"'.'")
+# Matches multi-line C++ comments.
+# This RE is a little bit more complicated than one might expect, because we
+# have to take care of space removals tools so we can handle comments inside
+# statements better.
+# The current rule is: We only clear spaces from both sides when we're at the
+# end of the line. Otherwise, we try to remove spaces from the right side,
+# if this doesn't work we try on left side but only if there's a non-character
+# on the right.
+_RE_PATTERN_CLEANSE_LINE_C_COMMENTS = re.compile(
+ r"""(\s*/\*.*\*/\s*$|
+ /\*.*\*/\s+|
+ \s+/\*.*\*/(?=\W)|
+ /\*.*\*/)""", re.VERBOSE)
+
+
+def IsCppString(line):
+ """Does line terminate so, that the next symbol is in string constant.
+
+ This function does not consider single-line nor multi-line comments.
+
+ Args:
+ line: is a partial line of code starting from the 0..n.
+
+ Returns:
+ True, if next character appended to 'line' is inside a
+ string constant.
+ """
+
+ line = line.replace(r'\\', 'XX') # after this, \\" does not match to \"
+ return ((line.count('"') - line.count(r'\"') - line.count("'\"'")) & 1) == 1
+
+
+def FindNextMultiLineCommentStart(lines, lineix):
+ """Find the beginning marker for a multiline comment."""
+ while lineix < len(lines):
+ if lines[lineix].strip().startswith('/*'):
+ # Only return this marker if the comment goes beyond this line
+ if lines[lineix].strip().find('*/', 2) < 0:
+ return lineix
+ lineix += 1
+ return len(lines)
+
+
+def FindNextMultiLineCommentEnd(lines, lineix):
+ """We are inside a comment, find the end marker."""
+ while lineix < len(lines):
+ if lines[lineix].strip().endswith('*/'):
+ return lineix
+ lineix += 1
+ return len(lines)
+
+
+def RemoveMultiLineCommentsFromRange(lines, begin, end):
+ """Clears a range of lines for multi-line comments."""
+ # Having // dummy comments makes the lines non-empty, so we will not get
+ # unnecessary blank line warnings later in the code.
+ for i in range(begin, end):
+ lines[i] = '// dummy'
+
+
+def RemoveMultiLineComments(filename, lines, error):
+ """Removes multiline (c-style) comments from lines."""
+ lineix = 0
+ while lineix < len(lines):
+ lineix_begin = FindNextMultiLineCommentStart(lines, lineix)
+ if lineix_begin >= len(lines):
+ return
+ lineix_end = FindNextMultiLineCommentEnd(lines, lineix_begin)
+ if lineix_end >= len(lines):
+ error(filename, lineix_begin + 1, 'readability/multiline_comment', 5,
+ 'Could not find end of multi-line comment')
+ return
+ RemoveMultiLineCommentsFromRange(lines, lineix_begin, lineix_end + 1)
+ lineix = lineix_end + 1
+
+
+def CleanseComments(line):
+ """Removes //-comments and single-line C-style /* */ comments.
+
+ Args:
+ line: A line of C++ source.
+
+ Returns:
+ The line with single-line comments removed.
+ """
+ commentpos = line.find('//')
+ if commentpos != -1 and not IsCppString(line[:commentpos]):
+ line = line[:commentpos].rstrip()
+ # get rid of /* ... */
+ return _RE_PATTERN_CLEANSE_LINE_C_COMMENTS.sub('', line)
+
+
+class CleansedLines(object):
+ """Holds 3 copies of all lines with different preprocessing applied to them.
+
+ 1) elided member contains lines without strings and comments,
+ 2) lines member contains lines without comments, and
+ 3) raw member contains all the lines without processing.
+ All these three members are of <type 'list'>, and of the same length.
+ """
+
+ def __init__(self, lines):
+ self.elided = []
+ self.lines = []
+ self.raw_lines = lines
+ self.num_lines = len(lines)
+ for linenum in range(len(lines)):
+ self.lines.append(CleanseComments(lines[linenum]))
+ elided = self._CollapseStrings(lines[linenum])
+ self.elided.append(CleanseComments(elided))
+
+ def NumLines(self):
+ """Returns the number of lines represented."""
+ return self.num_lines
+
+ @staticmethod
+ def _CollapseStrings(elided):
+ """Collapses strings and chars on a line to simple "" or '' blocks.
+
+ We nix strings first so we're not fooled by text like '"http://"'
+
+ Args:
+ elided: The line being processed.
+
+ Returns:
+ The line with collapsed strings.
+ """
+ if not _RE_PATTERN_INCLUDE.match(elided):
+ # Remove escaped characters first to make quote/single quote collapsing
+ # basic. Things that look like escaped characters shouldn't occur
+ # outside of strings and chars.
+ elided = _RE_PATTERN_CLEANSE_LINE_ESCAPES.sub('', elided)
+ elided = _RE_PATTERN_CLEANSE_LINE_SINGLE_QUOTES.sub("''", elided)
+ elided = _RE_PATTERN_CLEANSE_LINE_DOUBLE_QUOTES.sub('""', elided)
+ return elided
+
+
+def CloseExpression(clean_lines, linenum, pos):
+ """If input points to ( or { or [, finds the position that closes it.
+
+ If lines[linenum][pos] points to a '(' or '{' or '[', finds the
+ linenum/pos that correspond to the closing of the expression.
+
+ Args:
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ pos: A position on the line.
+
+ Returns:
+ A tuple (line, linenum, pos) pointer *past* the closing brace, or
+ (line, len(lines), -1) if we never find a close. Note we ignore
+ strings and comments when matching; and the line we return is the
+ 'cleansed' line at linenum.
+ """
+
+ line = clean_lines.elided[linenum]
+ startchar = line[pos]
+ if startchar not in '({[':
+ return (line, clean_lines.NumLines(), -1)
+ if startchar == '(': endchar = ')'
+ if startchar == '[': endchar = ']'
+ if startchar == '{': endchar = '}'
+
+ num_open = line.count(startchar) - line.count(endchar)
+ while linenum < clean_lines.NumLines() and num_open > 0:
+ linenum += 1
+ line = clean_lines.elided[linenum]
+ num_open += line.count(startchar) - line.count(endchar)
+ # OK, now find the endchar that actually got us back to even
+ endpos = len(line)
+ while num_open >= 0:
+ endpos = line.rfind(')', 0, endpos)
+ num_open -= 1 # chopped off another )
+ return (line, linenum, endpos + 1)
+
+
+def CheckForCopyright(filename, lines, error):
+ """Logs an error if no Copyright message appears at the top of the file."""
+
+ # We'll say it should occur by line 10. Don't forget there's a
+ # dummy line at the front.
+ for line in xrange(1, min(len(lines), 11)):
+ if re.search(r'Copyright', lines[line], re.I): break
+ else: # means no copyright line was found
+ error(filename, 0, 'legal/copyright', 5,
+ 'No copyright message found. '
+ 'You should have a line: "Copyright [year] <Copyright Owner>"')
+
+
+def GetHeaderGuardCPPVariable(filename):
+ """Returns the CPP variable that should be used as a header guard.
+
+ Args:
+ filename: The name of a C++ header file.
+
+ Returns:
+ The CPP variable that should be used as a header guard in the
+ named file.
+
+ """
+
+ # Restores original filename in case that cpplint is invoked from Emacs's
+ # flymake.
+ filename = re.sub(r'_flymake\.h$', '.h', filename)
+
+ fileinfo = FileInfo(filename)
+ return re.sub(r'[-./\s]', '_', fileinfo.RepositoryName()).upper() + '_'
+
+
+def CheckForHeaderGuard(filename, lines, error):
+ """Checks that the file contains a header guard.
+
+ Logs an error if no #ifndef header guard is present. For other
+ headers, checks that the full pathname is used.
+
+ Args:
+ filename: The name of the C++ header file.
+ lines: An array of strings, each representing a line of the file.
+ error: The function to call with any errors found.
+ """
+
+ cppvar = GetHeaderGuardCPPVariable(filename)
+
+ ifndef = None
+ ifndef_linenum = 0
+ define = None
+ endif = None
+ endif_linenum = 0
+ for linenum, line in enumerate(lines):
+ linesplit = line.split()
+ if len(linesplit) >= 2:
+ # find the first occurrence of #ifndef and #define, save arg
+ if not ifndef and linesplit[0] == '#ifndef':
+ # set ifndef to the header guard presented on the #ifndef line.
+ ifndef = linesplit[1]
+ ifndef_linenum = linenum
+ if not define and linesplit[0] == '#define':
+ define = linesplit[1]
+ # find the last occurrence of #endif, save entire line
+ if line.startswith('#endif'):
+ endif = line
+ endif_linenum = linenum
+
+ if not ifndef:
+ error(filename, 0, 'build/header_guard', 5,
+ 'No #ifndef header guard found, suggested CPP variable is: %s' %
+ cppvar)
+ return
+
+ if not define:
+ error(filename, 0, 'build/header_guard', 5,
+ 'No #define header guard found, suggested CPP variable is: %s' %
+ cppvar)
+ return
+
+ # The guard should be PATH_FILE_H_, but we also allow PATH_FILE_H__
+ # for backward compatibility.
+ if ifndef != cppvar:
+ error_level = 0
+ if ifndef != cppvar + '_':
+ error_level = 5
+
+ ParseNolintSuppressions(filename, lines[ifndef_linenum], ifndef_linenum,
+ error)
+ error(filename, ifndef_linenum, 'build/header_guard', error_level,
+ '#ifndef header guard has wrong style, please use: %s' % cppvar)
+
+ if define != ifndef:
+ error(filename, 0, 'build/header_guard', 5,
+ '#ifndef and #define don\'t match, suggested CPP variable is: %s' %
+ cppvar)
+ return
+
+ if endif != ('#endif // %s' % cppvar):
+ error_level = 0
+ if endif != ('#endif // %s' % (cppvar + '_')):
+ error_level = 5
+
+ ParseNolintSuppressions(filename, lines[endif_linenum], endif_linenum,
+ error)
+ error(filename, endif_linenum, 'build/header_guard', error_level,
+ '#endif line should be "#endif // %s"' % cppvar)
+
+
+def CheckForUnicodeReplacementCharacters(filename, lines, error):
+ """Logs an error for each line containing Unicode replacement characters.
+
+ These indicate that either the file contained invalid UTF-8 (likely)
+ or Unicode replacement characters (which it shouldn't). Note that
+ it's possible for this to throw off line numbering if the invalid
+ UTF-8 occurred adjacent to a newline.
+
+ Args:
+ filename: The name of the current file.
+ lines: An array of strings, each representing a line of the file.
+ error: The function to call with any errors found.
+ """
+ for linenum, line in enumerate(lines):
+ if u'\ufffd' in line:
+ error(filename, linenum, 'readability/utf8', 5,
+ 'Line contains invalid UTF-8 (or Unicode replacement character).')
+
+
+def CheckForNewlineAtEOF(filename, lines, error):
+ """Logs an error if there is no newline char at the end of the file.
+
+ Args:
+ filename: The name of the current file.
+ lines: An array of strings, each representing a line of the file.
+ error: The function to call with any errors found.
+ """
+
+ # The array lines() was created by adding two newlines to the
+ # original file (go figure), then splitting on \n.
+ # To verify that the file ends in \n, we just have to make sure the
+ # last-but-two element of lines() exists and is empty.
+ if len(lines) < 3 or lines[-2]:
+ error(filename, len(lines) - 2, 'whitespace/ending_newline', 5,
+ 'Could not find a newline character at the end of the file.')
+
+
+def CheckForMultilineCommentsAndStrings(filename, clean_lines, linenum, error):
+ """Logs an error if we see /* ... */ or "..." that extend past one line.
+
+ /* ... */ comments are legit inside macros, for one line.
+ Otherwise, we prefer // comments, so it's ok to warn about the
+ other. Likewise, it's ok for strings to extend across multiple
+ lines, as long as a line continuation character (backslash)
+ terminates each line. Although not currently prohibited by the C++
+ style guide, it's ugly and unnecessary. We don't do well with either
+ in this lint program, so we warn about both.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ error: The function to call with any errors found.
+ """
+ line = clean_lines.elided[linenum]
+
+ # Remove all \\ (escaped backslashes) from the line. They are OK, and the
+ # second (escaped) slash may trigger later \" detection erroneously.
+ line = line.replace('\\\\', '')
+
+ if line.count('/*') > line.count('*/'):
+ error(filename, linenum, 'readability/multiline_comment', 5,
+ 'Complex multi-line /*...*/-style comment found. '
+ 'Lint may give bogus warnings. '
+ 'Consider replacing these with //-style comments, '
+ 'with #if 0...#endif, '
+ 'or with more clearly structured multi-line comments.')
+
+ if (line.count('"') - line.count('\\"')) % 2:
+ error(filename, linenum, 'readability/multiline_string', 5,
+ 'Multi-line string ("...") found. This lint script doesn\'t '
+ 'do well with such strings, and may give bogus warnings. They\'re '
+ 'ugly and unnecessary, and you should use concatenation instead".')
+
+
+threading_list = (
+ ('asctime(', 'asctime_r('),
+ ('ctime(', 'ctime_r('),
+ ('getgrgid(', 'getgrgid_r('),
+ ('getgrnam(', 'getgrnam_r('),
+ ('getlogin(', 'getlogin_r('),
+ ('getpwnam(', 'getpwnam_r('),
+ ('getpwuid(', 'getpwuid_r('),
+ ('gmtime(', 'gmtime_r('),
+ ('localtime(', 'localtime_r('),
+ ('rand(', 'rand_r('),
+ ('readdir(', 'readdir_r('),
+ ('strtok(', 'strtok_r('),
+ ('ttyname(', 'ttyname_r('),
+ )
+
+
+def CheckPosixThreading(filename, clean_lines, linenum, error):
+ """Checks for calls to thread-unsafe functions.
+
+ Much code has been originally written without consideration of
+ multi-threading. Also, engineers are relying on their old experience;
+ they have learned posix before threading extensions were added. These
+ tests guide the engineers to use thread-safe functions (when using
+ posix directly).
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ error: The function to call with any errors found.
+ """
+ line = clean_lines.elided[linenum]
+ for single_thread_function, multithread_safe_function in threading_list:
+ ix = line.find(single_thread_function)
+ # Comparisons made explicit for clarity -- pylint: disable-msg=C6403
+ if ix >= 0 and (ix == 0 or (not line[ix - 1].isalnum() and
+ line[ix - 1] not in ('_', '.', '>'))):
+ error(filename, linenum, 'runtime/threadsafe_fn', 2,
+ 'Consider using ' + multithread_safe_function +
+ '...) instead of ' + single_thread_function +
+ '...) for improved thread safety.')
+
+
+# Matches invalid increment: *count++, which moves pointer instead of
+# incrementing a value.
+_RE_PATTERN_INVALID_INCREMENT = re.compile(
+ r'^\s*\*\w+(\+\+|--);')
+
+
+def CheckInvalidIncrement(filename, clean_lines, linenum, error):
+ """Checks for invalid increment *count++.
+
+ For example following function:
+ void increment_counter(int* count) {
+ *count++;
+ }
+ is invalid, because it effectively does count++, moving pointer, and should
+ be replaced with ++*count, (*count)++ or *count += 1.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ error: The function to call with any errors found.
+ """
+ line = clean_lines.elided[linenum]
+ if _RE_PATTERN_INVALID_INCREMENT.match(line):
+ error(filename, linenum, 'runtime/invalid_increment', 5,
+ 'Changing pointer instead of value (or unused value of operator*).')
+
+
+class _ClassInfo(object):
+ """Stores information about a class."""
+
+ def __init__(self, name, clean_lines, linenum):
+ self.name = name
+ self.linenum = linenum
+ self.seen_open_brace = False
+ self.is_derived = False
+ self.virtual_method_linenumber = None
+ self.has_virtual_destructor = False
+ self.brace_depth = 0
+
+ # Try to find the end of the class. This will be confused by things like:
+ # class A {
+ # } *x = { ...
+ #
+ # But it's still good enough for CheckSectionSpacing.
+ self.last_line = 0
+ depth = 0
+ for i in range(linenum, clean_lines.NumLines()):
+ line = clean_lines.lines[i]
+ depth += line.count('{') - line.count('}')
+ if not depth:
+ self.last_line = i
+ break
+
+
+class _ClassState(object):
+ """Holds the current state of the parse relating to class declarations.
+
+ It maintains a stack of _ClassInfos representing the parser's guess
+ as to the current nesting of class declarations. The innermost class
+ is at the top (back) of the stack. Typically, the stack will either
+ be empty or have exactly one entry.
+ """
+
+ def __init__(self):
+ self.classinfo_stack = []
+
+ def CheckFinished(self, filename, error):
+ """Checks that all classes have been completely parsed.
+
+ Call this when all lines in a file have been processed.
+ Args:
+ filename: The name of the current file.
+ error: The function to call with any errors found.
+ """
+ if self.classinfo_stack:
+ # Note: This test can result in false positives if #ifdef constructs
+ # get in the way of brace matching. See the testBuildClass test in
+ # cpplint_unittest.py for an example of this.
+ error(filename, self.classinfo_stack[0].linenum, 'build/class', 5,
+ 'Failed to find complete declaration of class %s' %
+ self.classinfo_stack[0].name)
+
+
+def CheckForNonStandardConstructs(filename, clean_lines, linenum,
+ class_state, error):
+ """Logs an error if we see certain non-ANSI constructs ignored by gcc-2.
+
+ Complain about several constructs which gcc-2 accepts, but which are
+ not standard C++. Warning about these in lint is one way to ease the
+ transition to new compilers.
+ - put storage class first (e.g. "static const" instead of "const static").
+ - "%lld" instead of %qd" in printf-type functions.
+ - "%1$d" is non-standard in printf-type functions.
+ - "\%" is an undefined character escape sequence.
+ - text after #endif is not allowed.
+ - invalid inner-style forward declaration.
+ - >? and <? operators, and their >?= and <?= cousins.
+ - classes with virtual methods need virtual destructors (compiler warning
+ available, but not turned on yet.)
+
+ Additionally, check for constructor/destructor style violations and reference
+ members, as it is very convenient to do so while checking for
+ gcc-2 compliance.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ class_state: A _ClassState instance which maintains information about
+ the current stack of nested class declarations being parsed.
+ error: A callable to which errors are reported, which takes 4 arguments:
+ filename, line number, error level, and message
+ """
+
+ # Remove comments from the line, but leave in strings for now.
+ line = clean_lines.lines[linenum]
+
+ if Search(r'printf\s*\(.*".*%[-+ ]?\d*q', line):
+ error(filename, linenum, 'runtime/printf_format', 3,
+ '%q in format strings is deprecated. Use %ll instead.')
+
+ if Search(r'printf\s*\(.*".*%\d+\$', line):
+ error(filename, linenum, 'runtime/printf_format', 2,
+ '%N$ formats are unconventional. Try rewriting to avoid them.')
+
+ # Remove escaped backslashes before looking for undefined escapes.
+ line = line.replace('\\\\', '')
+
+ if Search(r'("|\').*\\(%|\[|\(|{)', line):
+ error(filename, linenum, 'build/printf_format', 3,
+ '%, [, (, and { are undefined character escapes. Unescape them.')
+
+ # For the rest, work with both comments and strings removed.
+ line = clean_lines.elided[linenum]
+
+ if Search(r'\b(const|volatile|void|char|short|int|long'
+ r'|float|double|signed|unsigned'
+ r'|schar|u?int8|u?int16|u?int32|u?int64)'
+ r'\s+(auto|register|static|extern|typedef)\b',
+ line):
+ error(filename, linenum, 'build/storage_class', 5,
+ 'Storage class (static, extern, typedef, etc) should be first.')
+
+ if Match(r'\s*#\s*endif\s*[^/\s]+', line):
+ error(filename, linenum, 'build/endif_comment', 5,
+ 'Uncommented text after #endif is non-standard. Use a comment.')
+
+ if Match(r'\s*class\s+(\w+\s*::\s*)+\w+\s*;', line):
+ error(filename, linenum, 'build/forward_decl', 5,
+ 'Inner-style forward declarations are invalid. Remove this line.')
+
+ if Search(r'(\w+|[+-]?\d+(\.\d*)?)\s*(<|>)\?=?\s*(\w+|[+-]?\d+)(\.\d*)?',
+ line):
+ error(filename, linenum, 'build/deprecated', 3,
+ '>? and <? (max and min) operators are non-standard and deprecated.')
+
+ if Search(r'^\s*const\s*string\s*&\s*\w+\s*;', line):
+ # TODO(unknown): Could it be expanded safely to arbitrary references,
+ # without triggering too many false positives? The first
+ # attempt triggered 5 warnings for mostly benign code in the regtest, hence
+ # the restriction.
+ # Here's the original regexp, for the reference:
+ # type_name = r'\w+((\s*::\s*\w+)|(\s*<\s*\w+?\s*>))?'
+ # r'\s*const\s*' + type_name + '\s*&\s*\w+\s*;'
+ error(filename, linenum, 'runtime/member_string_references', 2,
+ 'const string& members are dangerous. It is much better to use '
+ 'alternatives, such as pointers or simple constants.')
+
+ # Track class entry and exit, and attempt to find cases within the
+ # class declaration that don't meet the C++ style
+ # guidelines. Tracking is very dependent on the code matching Google
+ # style guidelines, but it seems to perform well enough in testing
+ # to be a worthwhile addition to the checks.
+ classinfo_stack = class_state.classinfo_stack
+ # Look for a class declaration. The regexp accounts for decorated classes
+ # such as in:
+ # class LOCKABLE API Object {
+ # };
+ class_decl_match = Match(
+ r'\s*(template\s*<[\w\s<>,:]*>\s*)?'
+ '(class|struct)\s+([A-Z_]+\s+)*(\w+(::\w+)*)', line)
+ if class_decl_match:
+ classinfo_stack.append(_ClassInfo(
+ class_decl_match.group(4), clean_lines, linenum))
+
+ # Everything else in this function uses the top of the stack if it's
+ # not empty.
+ if not classinfo_stack:
+ return
+
+ classinfo = classinfo_stack[-1]
+
+ # If the opening brace hasn't been seen look for it and also
+ # parent class declarations.
+ if not classinfo.seen_open_brace:
+ # If the line has a ';' in it, assume it's a forward declaration or
+ # a single-line class declaration, which we won't process.
+ if line.find(';') != -1:
+ classinfo_stack.pop()
+ return
+ classinfo.seen_open_brace = (line.find('{') != -1)
+ # Look for a bare ':'
+ if Search('(^|[^:]):($|[^:])', line):
+ classinfo.is_derived = True
+ if not classinfo.seen_open_brace:
+ return # Everything else in this function is for after open brace
+
+ # The class may have been declared with namespace or classname qualifiers.
+ # The constructor and destructor will not have those qualifiers.
+ base_classname = classinfo.name.split('::')[-1]
+
+ # Look for single-argument constructors that aren't marked explicit.
+ # Technically a valid construct, but against style.
+ args = Match(r'\s+(?:inline\s+)?%s\s*\(([^,()]+)\)'
+ % re.escape(base_classname),
+ line)
+ if (args and
+ args.group(1) != 'void' and
+ not Match(r'(const\s+)?%s\s*(?:<\w+>\s*)?&' % re.escape(base_classname),
+ args.group(1).strip())):
+ error(filename, linenum, 'runtime/explicit', 5,
+ 'Single-argument constructors should be marked explicit.')
+
+ # Look for methods declared virtual.
+ if Search(r'\bvirtual\b', line):
+ classinfo.virtual_method_linenumber = linenum
+ # Only look for a destructor declaration on the same line. It would
+ # be extremely unlikely for the destructor declaration to occupy
+ # more than one line.
+ if Search(r'~%s\s*\(' % base_classname, line):
+ classinfo.has_virtual_destructor = True
+
+ # Look for class end.
+ brace_depth = classinfo.brace_depth
+ brace_depth = brace_depth + line.count('{') - line.count('}')
+ if brace_depth <= 0:
+ classinfo = classinfo_stack.pop()
+ # Try to detect missing virtual destructor declarations.
+ # For now, only warn if a non-derived class with virtual methods lacks
+ # a virtual destructor. This is to make it less likely that people will
+ # declare derived virtual destructors without declaring the base
+ # destructor virtual.
+ if ((classinfo.virtual_method_linenumber is not None) and
+ (not classinfo.has_virtual_destructor) and
+ (not classinfo.is_derived)): # Only warn for base classes
+ error(filename, classinfo.linenum, 'runtime/virtual', 4,
+ 'The class %s probably needs a virtual destructor due to '
+ 'having virtual method(s), one declared at line %d.'
+ % (classinfo.name, classinfo.virtual_method_linenumber))
+ else:
+ classinfo.brace_depth = brace_depth
+
+
+def CheckSpacingForFunctionCall(filename, line, linenum, error):
+ """Checks for the correctness of various spacing around function calls.
+
+ Args:
+ filename: The name of the current file.
+ line: The text of the line to check.
+ linenum: The number of the line to check.
+ error: The function to call with any errors found.
+ """
+
+ # Since function calls often occur inside if/for/while/switch
+ # expressions - which have their own, more liberal conventions - we
+ # first see if we should be looking inside such an expression for a
+ # function call, to which we can apply more strict standards.
+ fncall = line # if there's no control flow construct, look at whole line
+ for pattern in (r'\bif\s*\((.*)\)\s*{',
+ r'\bfor\s*\((.*)\)\s*{',
+ r'\bwhile\s*\((.*)\)\s*[{;]',
+ r'\bswitch\s*\((.*)\)\s*{'):
+ match = Search(pattern, line)
+ if match:
+ fncall = match.group(1) # look inside the parens for function calls
+ break
+
+ # Except in if/for/while/switch, there should never be space
+ # immediately inside parens (eg "f( 3, 4 )"). We make an exception
+ # for nested parens ( (a+b) + c ). Likewise, there should never be
+ # a space before a ( when it's a function argument. I assume it's a
+ # function argument when the char before the whitespace is legal in
+ # a function name (alnum + _) and we're not starting a macro. Also ignore
+ # pointers and references to arrays and functions coz they're too tricky:
+ # we use a very simple way to recognize these:
+ # " (something)(maybe-something)" or
+ # " (something)(maybe-something," or
+ # " (something)[something]"
+ # Note that we assume the contents of [] to be short enough that
+ # they'll never need to wrap.
+ if ( # Ignore control structures.
+ not Search(r'\b(if|for|while|switch|return|delete)\b', fncall) and
+ # Ignore pointers/references to functions.
+ not Search(r' \([^)]+\)\([^)]*(\)|,$)', fncall) and
+ # Ignore pointers/references to arrays.
+ not Search(r' \([^)]+\)\[[^\]]+\]', fncall)):
+ if Search(r'\w\s*\(\s(?!\s*\\$)', fncall): # a ( used for a fn call
+ error(filename, linenum, 'whitespace/parens', 4,
+ 'Extra space after ( in function call')
+ elif Search(r'\(\s+(?!(\s*\\)|\()', fncall):
+ error(filename, linenum, 'whitespace/parens', 2,
+ 'Extra space after (')
+ if (Search(r'\w\s+\(', fncall) and
+ not Search(r'#\s*define|typedef', fncall)):
+ error(filename, linenum, 'whitespace/parens', 4,
+ 'Extra space before ( in function call')
+ # If the ) is followed only by a newline or a { + newline, assume it's
+ # part of a control statement (if/while/etc), and don't complain
+ if Search(r'[^)]\s+\)\s*[^{\s]', fncall):
+ # If the closing parenthesis is preceded by only whitespaces,
+ # try to give a more descriptive error message.
+ if Search(r'^\s+\)', fncall):
+ error(filename, linenum, 'whitespace/parens', 2,
+ 'Closing ) should be moved to the previous line')
+ else:
+ error(filename, linenum, 'whitespace/parens', 2,
+ 'Extra space before )')
+
+
+def IsBlankLine(line):
+ """Returns true if the given line is blank.
+
+ We consider a line to be blank if the line is empty or consists of
+ only white spaces.
+
+ Args:
+ line: A line of a string.
+
+ Returns:
+ True, if the given line is blank.
+ """
+ return not line or line.isspace()
+
+
+def CheckForFunctionLengths(filename, clean_lines, linenum,
+ function_state, error):
+ """Reports for long function bodies.
+
+ For an overview why this is done, see:
+ http://google-styleguide.googlecode.com/svn/trunk/cppguide.xml#Write_Short_Functions
+
+ Uses a simplistic algorithm assuming other style guidelines
+ (especially spacing) are followed.
+ Only checks unindented functions, so class members are unchecked.
+ Trivial bodies are unchecked, so constructors with huge initializer lists
+ may be missed.
+ Blank/comment lines are not counted so as to avoid encouraging the removal
+ of vertical space and comments just to get through a lint check.
+ NOLINT *on the last line of a function* disables this check.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ function_state: Current function name and lines in body so far.
+ error: The function to call with any errors found.
+ """
+ lines = clean_lines.lines
+ line = lines[linenum]
+ raw = clean_lines.raw_lines
+ raw_line = raw[linenum]
+ joined_line = ''
+
+ starting_func = False
+ regexp = r'(\w(\w|::|\*|\&|\s)*)\(' # decls * & space::name( ...
+ match_result = Match(regexp, line)
+ if match_result:
+ # If the name is all caps and underscores, figure it's a macro and
+ # ignore it, unless it's TEST or TEST_F.
+ function_name = match_result.group(1).split()[-1]
+ if function_name == 'TEST' or function_name == 'TEST_F' or (
+ not Match(r'[A-Z_]+$', function_name)):
+ starting_func = True
+
+ if starting_func:
+ body_found = False
+ for start_linenum in xrange(linenum, clean_lines.NumLines()):
+ start_line = lines[start_linenum]
+ joined_line += ' ' + start_line.lstrip()
+ if Search(r'(;|})', start_line): # Declarations and trivial functions
+ body_found = True
+ break # ... ignore
+ elif Search(r'{', start_line):
+ body_found = True
+ function = Search(r'((\w|:)*)\(', line).group(1)
+ if Match(r'TEST', function): # Handle TEST... macros
+ parameter_regexp = Search(r'(\(.*\))', joined_line)
+ if parameter_regexp: # Ignore bad syntax
+ function += parameter_regexp.group(1)
+ else:
+ function += '()'
+ function_state.Begin(function)
+ break
+ if not body_found:
+ # No body for the function (or evidence of a non-function) was found.
+ error(filename, linenum, 'readability/fn_size', 5,
+ 'Lint failed to find start of function body.')
+ elif Match(r'^\}\s*$', line): # function end
+ function_state.Check(error, filename, linenum)
+ function_state.End()
+ elif not Match(r'^\s*$', line):
+ function_state.Count() # Count non-blank/non-comment lines.
+
+
+_RE_PATTERN_TODO = re.compile(r'^//(\s*)TODO(\(.+?\))?:?(\s|$)?')
+
+
+def CheckComment(comment, filename, linenum, error):
+ """Checks for common mistakes in TODO comments.
+
+ Args:
+ comment: The text of the comment from the line in question.
+ filename: The name of the current file.
+ linenum: The number of the line to check.
+ error: The function to call with any errors found.
+ """
+ match = _RE_PATTERN_TODO.match(comment)
+ if match:
+ # One whitespace is correct; zero whitespace is handled elsewhere.
+ leading_whitespace = match.group(1)
+ if len(leading_whitespace) > 1:
+ error(filename, linenum, 'whitespace/todo', 2,
+ 'Too many spaces before TODO')
+
+ username = match.group(2)
+ if not username:
+ error(filename, linenum, 'readability/todo', 2,
+ 'Missing username in TODO; it should look like '
+ '"// TODO(my_username): Stuff."')
+
+ middle_whitespace = match.group(3)
+ # Comparisons made explicit for correctness -- pylint: disable-msg=C6403
+ if middle_whitespace != ' ' and middle_whitespace != '':
+ error(filename, linenum, 'whitespace/todo', 2,
+ 'TODO(my_username) should be followed by a space')
+
+
+def CheckSpacing(filename, clean_lines, linenum, error):
+ """Checks for the correctness of various spacing issues in the code.
+
+ Things we check for: spaces around operators, spaces after
+ if/for/while/switch, no spaces around parens in function calls, two
+ spaces between code and comment, don't start a block with a blank
+ line, don't end a function with a blank line, don't add a blank line
+ after public/protected/private, don't have too many blank lines in a row.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ error: The function to call with any errors found.
+ """
+
+ raw = clean_lines.raw_lines
+ line = raw[linenum]
+
+ # Before nixing comments, check if the line is blank for no good
+ # reason. This includes the first line after a block is opened, and
+ # blank lines at the end of a function (ie, right before a line like '}'
+ if IsBlankLine(line):
+ elided = clean_lines.elided
+ prev_line = elided[linenum - 1]
+ prevbrace = prev_line.rfind('{')
+ # TODO(unknown): Don't complain if line before blank line, and line after,
+ # both start with alnums and are indented the same amount.
+ # This ignores whitespace at the start of a namespace block
+ # because those are not usually indented.
+ if (prevbrace != -1 and prev_line[prevbrace:].find('}') == -1
+ and prev_line[:prevbrace].find('namespace') == -1):
+ # OK, we have a blank line at the start of a code block. Before we
+ # complain, we check if it is an exception to the rule: The previous
+ # non-empty line has the parameters of a function header that are indented
+ # 4 spaces (because they did not fit in a 80 column line when placed on
+ # the same line as the function name). We also check for the case where
+ # the previous line is indented 6 spaces, which may happen when the
+ # initializers of a constructor do not fit into a 80 column line.
+ exception = False
+ if Match(r' {6}\w', prev_line): # Initializer list?
+ # We are looking for the opening column of initializer list, which
+ # should be indented 4 spaces to cause 6 space indentation afterwards.
+ search_position = linenum-2
+ while (search_position >= 0
+ and Match(r' {6}\w', elided[search_position])):
+ search_position -= 1
+ exception = (search_position >= 0
+ and elided[search_position][:5] == ' :')
+ else:
+ # Search for the function arguments or an initializer list. We use a
+ # simple heuristic here: If the line is indented 4 spaces; and we have a
+ # closing paren, without the opening paren, followed by an opening brace
+ # or colon (for initializer lists) we assume that it is the last line of
+ # a function header. If we have a colon indented 4 spaces, it is an
+ # initializer list.
+ exception = (Match(r' {4}\w[^\(]*\)\s*(const\s*)?(\{\s*$|:)',
+ prev_line)
+ or Match(r' {4}:', prev_line))
+
+ if not exception:
+ error(filename, linenum, 'whitespace/blank_line', 2,
+ 'Blank line at the start of a code block. Is this needed?')
+ # This doesn't ignore whitespace at the end of a namespace block
+ # because that is too hard without pairing open/close braces;
+ # however, a special exception is made for namespace closing
+ # brackets which have a comment containing "namespace".
+ #
+ # Also, ignore blank lines at the end of a block in a long if-else
+ # chain, like this:
+ # if (condition1) {
+ # // Something followed by a blank line
+ #
+ # } else if (condition2) {
+ # // Something else
+ # }
+ if linenum + 1 < clean_lines.NumLines():
+ next_line = raw[linenum + 1]
+ if (next_line
+ and Match(r'\s*}', next_line)
+ and next_line.find('namespace') == -1
+ and next_line.find('} else ') == -1):
+ error(filename, linenum, 'whitespace/blank_line', 3,
+ 'Blank line at the end of a code block. Is this needed?')
+
+ matched = Match(r'\s*(public|protected|private):', prev_line)
+ if matched:
+ error(filename, linenum, 'whitespace/blank_line', 3,
+ 'Do not leave a blank line after "%s:"' % matched.group(1))
+
+ # Next, we complain if there's a comment too near the text
+ commentpos = line.find('//')
+ if commentpos != -1:
+ # Check if the // may be in quotes. If so, ignore it
+ # Comparisons made explicit for clarity -- pylint: disable-msg=C6403
+ if (line.count('"', 0, commentpos) -
+ line.count('\\"', 0, commentpos)) % 2 == 0: # not in quotes
+ # Allow one space for new scopes, two spaces otherwise:
+ if (not Match(r'^\s*{ //', line) and
+ ((commentpos >= 1 and
+ line[commentpos-1] not in string.whitespace) or
+ (commentpos >= 2 and
+ line[commentpos-2] not in string.whitespace))):
+ error(filename, linenum, 'whitespace/comments', 2,
+ 'At least two spaces is best between code and comments')
+ # There should always be a space between the // and the comment
+ commentend = commentpos + 2
+ if commentend < len(line) and not line[commentend] == ' ':
+ # but some lines are exceptions -- e.g. if they're big
+ # comment delimiters like:
+ # //----------------------------------------------------------
+ # or are an empty C++ style Doxygen comment, like:
+ # ///
+ # or they begin with multiple slashes followed by a space:
+ # //////// Header comment
+ match = (Search(r'[=/-]{4,}\s*$', line[commentend:]) or
+ Search(r'^/$', line[commentend:]) or
+ Search(r'^/+ ', line[commentend:]))
+ if not match:
+ error(filename, linenum, 'whitespace/comments', 4,
+ 'Should have a space between // and comment')
+ CheckComment(line[commentpos:], filename, linenum, error)
+
+ line = clean_lines.elided[linenum] # get rid of comments and strings
+
+ # Don't try to do spacing checks for operator methods
+ line = re.sub(r'operator(==|!=|<|<<|<=|>=|>>|>)\(', 'operator\(', line)
+
+ # We allow no-spaces around = within an if: "if ( (a=Foo()) == 0 )".
+ # Otherwise not. Note we only check for non-spaces on *both* sides;
+ # sometimes people put non-spaces on one side when aligning ='s among
+ # many lines (not that this is behavior that I approve of...)
+ if Search(r'[\w.]=[\w.]', line) and not Search(r'\b(if|while) ', line):
+ error(filename, linenum, 'whitespace/operators', 4,
+ 'Missing spaces around =')
+
+ # It's ok not to have spaces around binary operators like + - * /, but if
+ # there's too little whitespace, we get concerned. It's hard to tell,
+ # though, so we punt on this one for now. TODO.
+
+ # You should always have whitespace around binary operators.
+ # Alas, we can't test < or > because they're legitimately used sans spaces
+ # (a->b, vector<int> a). The only time we can tell is a < with no >, and
+ # only if it's not template params list spilling into the next line.
+ match = Search(r'[^<>=!\s](==|!=|<=|>=)[^<>=!\s]', line)
+ if not match:
+ # Note that while it seems that the '<[^<]*' term in the following
+ # regexp could be simplified to '<.*', which would indeed match
+ # the same class of strings, the [^<] means that searching for the
+ # regexp takes linear rather than quadratic time.
+ if not Search(r'<[^<]*,\s*$', line): # template params spill
+ match = Search(r'[^<>=!\s](<)[^<>=!\s]([^>]|->)*$', line)
+ if match:
+ error(filename, linenum, 'whitespace/operators', 3,
+ 'Missing spaces around %s' % match.group(1))
+ # We allow no-spaces around << and >> when used like this: 10<<20, but
+ # not otherwise (particularly, not when used as streams)
+ match = Search(r'[^0-9\s](<<|>>)[^0-9\s]', line)
+ if match:
+ error(filename, linenum, 'whitespace/operators', 3,
+ 'Missing spaces around %s' % match.group(1))
+
+ # There shouldn't be space around unary operators
+ match = Search(r'(!\s|~\s|[\s]--[\s;]|[\s]\+\+[\s;])', line)
+ if match:
+ error(filename, linenum, 'whitespace/operators', 4,
+ 'Extra space for operator %s' % match.group(1))
+
+ # A pet peeve of mine: no spaces after an if, while, switch, or for
+ match = Search(r' (if\(|for\(|while\(|switch\()', line)
+ if match:
+ error(filename, linenum, 'whitespace/parens', 5,
+ 'Missing space before ( in %s' % match.group(1))
+
+ # For if/for/while/switch, the left and right parens should be
+ # consistent about how many spaces are inside the parens, and
+ # there should either be zero or one spaces inside the parens.
+ # We don't want: "if ( foo)" or "if ( foo )".
+ # Exception: "for ( ; foo; bar)" and "for (foo; bar; )" are allowed.
+ match = Search(r'\b(if|for|while|switch)\s*'
+ r'\(([ ]*)(.).*[^ ]+([ ]*)\)\s*{\s*$',
+ line)
+ if match:
+ if len(match.group(2)) != len(match.group(4)):
+ if not (match.group(3) == ';' and
+ len(match.group(2)) == 1 + len(match.group(4)) or
+ not match.group(2) and Search(r'\bfor\s*\(.*; \)', line)):
+ error(filename, linenum, 'whitespace/parens', 5,
+ 'Mismatching spaces inside () in %s' % match.group(1))
+ if not len(match.group(2)) in [0, 1]:
+ error(filename, linenum, 'whitespace/parens', 5,
+ 'Should have zero or one spaces inside ( and ) in %s' %
+ match.group(1))
+
+ # You should always have a space after a comma (either as fn arg or operator)
+ if Search(r',[^\s]', line):
+ error(filename, linenum, 'whitespace/comma', 3,
+ 'Missing space after ,')
+
+ # You should always have a space after a semicolon
+ # except for few corner cases
+ # TODO(unknown): clarify if 'if (1) { return 1;}' is requires one more
+ # space after ;
+ if Search(r';[^\s};\\)/]', line):
+ error(filename, linenum, 'whitespace/semicolon', 3,
+ 'Missing space after ;')
+
+ # Next we will look for issues with function calls.
+ CheckSpacingForFunctionCall(filename, line, linenum, error)
+
+ # Except after an opening paren, or after another opening brace (in case of
+ # an initializer list, for instance), you should have spaces before your
+ # braces. And since you should never have braces at the beginning of a line,
+ # this is an easy test.
+ if Search(r'[^ ({]{', line):
+ error(filename, linenum, 'whitespace/braces', 5,
+ 'Missing space before {')
+
+ # Make sure '} else {' has spaces.
+ if Search(r'}else', line):
+ error(filename, linenum, 'whitespace/braces', 5,
+ 'Missing space before else')
+
+ # You shouldn't have spaces before your brackets, except maybe after
+ # 'delete []' or 'new char * []'.
+ if Search(r'\w\s+\[', line) and not Search(r'delete\s+\[', line):
+ error(filename, linenum, 'whitespace/braces', 5,
+ 'Extra space before [')
+
+ # You shouldn't have a space before a semicolon at the end of the line.
+ # There's a special case for "for" since the style guide allows space before
+ # the semicolon there.
+ if Search(r':\s*;\s*$', line):
+ error(filename, linenum, 'whitespace/semicolon', 5,
+ 'Semicolon defining empty statement. Use { } instead.')
+ elif Search(r'^\s*;\s*$', line):
+ error(filename, linenum, 'whitespace/semicolon', 5,
+ 'Line contains only semicolon. If this should be an empty statement, '
+ 'use { } instead.')
+ elif (Search(r'\s+;\s*$', line) and
+ not Search(r'\bfor\b', line)):
+ error(filename, linenum, 'whitespace/semicolon', 5,
+ 'Extra space before last semicolon. If this should be an empty '
+ 'statement, use { } instead.')
+
+
+def CheckSectionSpacing(filename, clean_lines, class_info, linenum, error):
+ """Checks for additional blank line issues related to sections.
+
+ Currently the only thing checked here is blank line before protected/private.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ class_info: A _ClassInfo objects.
+ linenum: The number of the line to check.
+ error: The function to call with any errors found.
+ """
+ # Skip checks if the class is small, where small means 25 lines or less.
+ # 25 lines seems like a good cutoff since that's the usual height of
+ # terminals, and any class that can't fit in one screen can't really
+ # be considered "small".
+ #
+ # Also skip checks if we are on the first line. This accounts for
+ # classes that look like
+ # class Foo { public: ... };
+ #
+ # If we didn't find the end of the class, last_line would be zero,
+ # and the check will be skipped by the first condition.
+ if (class_info.last_line - class_info.linenum <= 24 or
+ linenum <= class_info.linenum):
+ return
+
+ matched = Match(r'\s*(public|protected|private):', clean_lines.lines[linenum])
+ if matched:
+ # Issue warning if the line before public/protected/private was
+ # not a blank line, but don't do this if the previous line contains
+ # "class" or "struct". This can happen two ways:
+ # - We are at the beginning of the class.
+ # - We are forward-declaring an inner class that is semantically
+ # private, but needed to be public for implementation reasons.
+ prev_line = clean_lines.lines[linenum - 1]
+ if (not IsBlankLine(prev_line) and
+ not Search(r'\b(class|struct)\b', prev_line)):
+ # Try a bit harder to find the beginning of the class. This is to
+ # account for multi-line base-specifier lists, e.g.:
+ # class Derived
+ # : public Base {
+ end_class_head = class_info.linenum
+ for i in range(class_info.linenum, linenum):
+ if Search(r'\{\s*$', clean_lines.lines[i]):
+ end_class_head = i
+ break
+ if end_class_head < linenum - 1:
+ error(filename, linenum, 'whitespace/blank_line', 3,
+ '"%s:" should be preceded by a blank line' % matched.group(1))
+
+
+def GetPreviousNonBlankLine(clean_lines, linenum):
+ """Return the most recent non-blank line and its line number.
+
+ Args:
+ clean_lines: A CleansedLines instance containing the file contents.
+ linenum: The number of the line to check.
+
+ Returns:
+ A tuple with two elements. The first element is the contents of the last
+ non-blank line before the current line, or the empty string if this is the
+ first non-blank line. The second is the line number of that line, or -1
+ if this is the first non-blank line.
+ """
+
+ prevlinenum = linenum - 1
+ while prevlinenum >= 0:
+ prevline = clean_lines.elided[prevlinenum]
+ if not IsBlankLine(prevline): # if not a blank line...
+ return (prevline, prevlinenum)
+ prevlinenum -= 1
+ return ('', -1)
+
+
+def CheckBraces(filename, clean_lines, linenum, error):
+ """Looks for misplaced braces (e.g. at the end of line).
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ error: The function to call with any errors found.
+ """
+
+ line = clean_lines.elided[linenum] # get rid of comments and strings
+
+ if Match(r'\s*{\s*$', line):
+ # We allow an open brace to start a line in the case where someone
+ # is using braces in a block to explicitly create a new scope,
+ # which is commonly used to control the lifetime of
+ # stack-allocated variables. We don't detect this perfectly: we
+ # just don't complain if the last non-whitespace character on the
+ # previous non-blank line is ';', ':', '{', or '}'.
+ prevline = GetPreviousNonBlankLine(clean_lines, linenum)[0]
+ if not Search(r'[;:}{]\s*$', prevline):
+ error(filename, linenum, 'whitespace/braces', 4,
+ '{ should almost always be at the end of the previous line')
+
+ # An else clause should be on the same line as the preceding closing brace.
+ if Match(r'\s*else\s*', line):
+ prevline = GetPreviousNonBlankLine(clean_lines, linenum)[0]
+ if Match(r'\s*}\s*$', prevline):
+ error(filename, linenum, 'whitespace/newline', 4,
+ 'An else should appear on the same line as the preceding }')
+
+ # If braces come on one side of an else, they should be on both.
+ # However, we have to worry about "else if" that spans multiple lines!
+ if Search(r'}\s*else[^{]*$', line) or Match(r'[^}]*else\s*{', line):
+ if Search(r'}\s*else if([^{]*)$', line): # could be multi-line if
+ # find the ( after the if
+ pos = line.find('else if')
+ pos = line.find('(', pos)
+ if pos > 0:
+ (endline, _, endpos) = CloseExpression(clean_lines, linenum, pos)
+ if endline[endpos:].find('{') == -1: # must be brace after if
+ error(filename, linenum, 'readability/braces', 5,
+ 'If an else has a brace on one side, it should have it on both')
+ else: # common case: else not followed by a multi-line if
+ error(filename, linenum, 'readability/braces', 5,
+ 'If an else has a brace on one side, it should have it on both')
+
+ # Likewise, an else should never have the else clause on the same line
+ if Search(r'\belse [^\s{]', line) and not Search(r'\belse if\b', line):
+ error(filename, linenum, 'whitespace/newline', 4,
+ 'Else clause should never be on same line as else (use 2 lines)')
+
+ # In the same way, a do/while should never be on one line
+ if Match(r'\s*do [^\s{]', line):
+ error(filename, linenum, 'whitespace/newline', 4,
+ 'do/while clauses should not be on a single line')
+
+ # Braces shouldn't be followed by a ; unless they're defining a struct
+ # or initializing an array.
+ # We can't tell in general, but we can for some common cases.
+ prevlinenum = linenum
+ while True:
+ (prevline, prevlinenum) = GetPreviousNonBlankLine(clean_lines, prevlinenum)
+ if Match(r'\s+{.*}\s*;', line) and not prevline.count(';'):
+ line = prevline + line
+ else:
+ break
+ if (Search(r'{.*}\s*;', line) and
+ line.count('{') == line.count('}') and
+ not Search(r'struct|class|enum|\s*=\s*{', line)):
+ error(filename, linenum, 'readability/braces', 4,
+ "You don't need a ; after a }")
+
+
+def ReplaceableCheck(operator, macro, line):
+ """Determine whether a basic CHECK can be replaced with a more specific one.
+
+ For example suggest using CHECK_EQ instead of CHECK(a == b) and
+ similarly for CHECK_GE, CHECK_GT, CHECK_LE, CHECK_LT, CHECK_NE.
+
+ Args:
+ operator: The C++ operator used in the CHECK.
+ macro: The CHECK or EXPECT macro being called.
+ line: The current source line.
+
+ Returns:
+ True if the CHECK can be replaced with a more specific one.
+ """
+
+ # This matches decimal and hex integers, strings, and chars (in that order).
+ match_constant = r'([-+]?(\d+|0[xX][0-9a-fA-F]+)[lLuU]{0,3}|".*"|\'.*\')'
+
+ # Expression to match two sides of the operator with something that
+ # looks like a literal, since CHECK(x == iterator) won't compile.
+ # This means we can't catch all the cases where a more specific
+ # CHECK is possible, but it's less annoying than dealing with
+ # extraneous warnings.
+ match_this = (r'\s*' + macro + r'\((\s*' +
+ match_constant + r'\s*' + operator + r'[^<>].*|'
+ r'.*[^<>]' + operator + r'\s*' + match_constant +
+ r'\s*\))')
+
+ # Don't complain about CHECK(x == NULL) or similar because
+ # CHECK_EQ(x, NULL) won't compile (requires a cast).
+ # Also, don't complain about more complex boolean expressions
+ # involving && or || such as CHECK(a == b || c == d).
+ return Match(match_this, line) and not Search(r'NULL|&&|\|\|', line)
+
+
+def CheckCheck(filename, clean_lines, linenum, error):
+ """Checks the use of CHECK and EXPECT macros.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ error: The function to call with any errors found.
+ """
+
+ # Decide the set of replacement macros that should be suggested
+ raw_lines = clean_lines.raw_lines
+ current_macro = ''
+ for macro in _CHECK_MACROS:
+ if raw_lines[linenum].find(macro) >= 0:
+ current_macro = macro
+ break
+ if not current_macro:
+ # Don't waste time here if line doesn't contain 'CHECK' or 'EXPECT'
+ return
+
+ line = clean_lines.elided[linenum] # get rid of comments and strings
+
+ # Encourage replacing plain CHECKs with CHECK_EQ/CHECK_NE/etc.
+ for operator in ['==', '!=', '>=', '>', '<=', '<']:
+ if ReplaceableCheck(operator, current_macro, line):
+ error(filename, linenum, 'readability/check', 2,
+ 'Consider using %s instead of %s(a %s b)' % (
+ _CHECK_REPLACEMENT[current_macro][operator],
+ current_macro, operator))
+ break
+
+
+def GetLineWidth(line):
+ """Determines the width of the line in column positions.
+
+ Args:
+ line: A string, which may be a Unicode string.
+
+ Returns:
+ The width of the line in column positions, accounting for Unicode
+ combining characters and wide characters.
+ """
+ if isinstance(line, unicode):
+ width = 0
+ for uc in unicodedata.normalize('NFC', line):
+ if unicodedata.east_asian_width(uc) in ('W', 'F'):
+ width += 2
+ elif not unicodedata.combining(uc):
+ width += 1
+ return width
+ else:
+ return len(line)
+
+
+def CheckStyle(filename, clean_lines, linenum, file_extension, class_state,
+ error):
+ """Checks rules from the 'C++ style rules' section of cppguide.html.
+
+ Most of these rules are hard to test (naming, comment style), but we
+ do what we can. In particular we check for 2-space indents, line lengths,
+ tab usage, spaces inside code, etc.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ file_extension: The extension (without the dot) of the filename.
+ error: The function to call with any errors found.
+ """
+
+ raw_lines = clean_lines.raw_lines
+ line = raw_lines[linenum]
+
+ if line.find('\t') != -1:
+ error(filename, linenum, 'whitespace/tab', 1,
+ 'Tab found; better to use spaces')
+
+ # One or three blank spaces at the beginning of the line is weird; it's
+ # hard to reconcile that with 2-space indents.
+ # NOTE: here are the conditions rob pike used for his tests. Mine aren't
+ # as sophisticated, but it may be worth becoming so: RLENGTH==initial_spaces
+ # if(RLENGTH > 20) complain = 0;
+ # if(match($0, " +(error|private|public|protected):")) complain = 0;
+ # if(match(prev, "&& *$")) complain = 0;
+ # if(match(prev, "\\|\\| *$")) complain = 0;
+ # if(match(prev, "[\",=><] *$")) complain = 0;
+ # if(match($0, " <<")) complain = 0;
+ # if(match(prev, " +for \\(")) complain = 0;
+ # if(prevodd && match(prevprev, " +for \\(")) complain = 0;
+ initial_spaces = 0
+ cleansed_line = clean_lines.elided[linenum]
+ while initial_spaces < len(line) and line[initial_spaces] == ' ':
+ initial_spaces += 1
+ if line and line[-1].isspace():
+ error(filename, linenum, 'whitespace/end_of_line', 4,
+ 'Line ends in whitespace. Consider deleting these extra spaces.')
+ # There are certain situations we allow one space, notably for labels
+ elif ((initial_spaces == 1 or initial_spaces == 3) and
+ not Match(r'\s*\w+\s*:\s*$', cleansed_line)):
+ error(filename, linenum, 'whitespace/indent', 3,
+ 'Weird number of spaces at line-start. '
+ 'Are you using a 2-space indent?')
+ # Labels should always be indented at least one space.
+ elif not initial_spaces and line[:2] != '//' and Search(r'[^:]:\s*$',
+ line):
+ error(filename, linenum, 'whitespace/labels', 4,
+ 'Labels should always be indented at least one space. '
+ 'If this is a member-initializer list in a constructor or '
+ 'the base class list in a class definition, the colon should '
+ 'be on the following line.')
+
+
+ # Check if the line is a header guard.
+ is_header_guard = False
+ if file_extension == 'h':
+ cppvar = GetHeaderGuardCPPVariable(filename)
+ if (line.startswith('#ifndef %s' % cppvar) or
+ line.startswith('#define %s' % cppvar) or
+ line.startswith('#endif // %s' % cppvar)):
+ is_header_guard = True
+ # #include lines and header guards can be long, since there's no clean way to
+ # split them.
+ #
+ # URLs can be long too. It's possible to split these, but it makes them
+ # harder to cut&paste.
+ #
+ # The "$Id:...$" comment may also get very long without it being the
+ # developers fault.
+ if (not line.startswith('#include') and not is_header_guard and
+ not Match(r'^\s*//.*http(s?)://\S*$', line) and
+ not Match(r'^// \$Id:.*#[0-9]+ \$$', line)):
+ line_width = GetLineWidth(line)
+ if line_width > 100:
+ error(filename, linenum, 'whitespace/line_length', 4,
+ 'Lines should very rarely be longer than 100 characters')
+ elif line_width > 80:
+ error(filename, linenum, 'whitespace/line_length', 2,
+ 'Lines should be <= 80 characters long')
+
+ if (cleansed_line.count(';') > 1 and
+ # for loops are allowed two ;'s (and may run over two lines).
+ cleansed_line.find('for') == -1 and
+ (GetPreviousNonBlankLine(clean_lines, linenum)[0].find('for') == -1 or
+ GetPreviousNonBlankLine(clean_lines, linenum)[0].find(';') != -1) and
+ # It's ok to have many commands in a switch case that fits in 1 line
+ not ((cleansed_line.find('case ') != -1 or
+ cleansed_line.find('default:') != -1) and
+ cleansed_line.find('break;') != -1)):
+ error(filename, linenum, 'whitespace/newline', 4,
+ 'More than one command on the same line')
+
+ # Some more style checks
+ CheckBraces(filename, clean_lines, linenum, error)
+ CheckSpacing(filename, clean_lines, linenum, error)
+ CheckCheck(filename, clean_lines, linenum, error)
+ if class_state and class_state.classinfo_stack:
+ CheckSectionSpacing(filename, clean_lines,
+ class_state.classinfo_stack[-1], linenum, error)
+
+
+_RE_PATTERN_INCLUDE_NEW_STYLE = re.compile(r'#include +"[^/]+\.h"')
+_RE_PATTERN_INCLUDE = re.compile(r'^\s*#\s*include\s*([<"])([^>"]*)[>"].*$')
+# Matches the first component of a filename delimited by -s and _s. That is:
+# _RE_FIRST_COMPONENT.match('foo').group(0) == 'foo'
+# _RE_FIRST_COMPONENT.match('foo.cc').group(0) == 'foo'
+# _RE_FIRST_COMPONENT.match('foo-bar_baz.cc').group(0) == 'foo'
+# _RE_FIRST_COMPONENT.match('foo_bar-baz.cc').group(0) == 'foo'
+_RE_FIRST_COMPONENT = re.compile(r'^[^-_.]+')
+
+
+def _DropCommonSuffixes(filename):
+ """Drops common suffixes like _test.cc or -inl.h from filename.
+
+ For example:
+ >>> _DropCommonSuffixes('foo/foo-inl.h')
+ 'foo/foo'
+ >>> _DropCommonSuffixes('foo/bar/foo.cc')
+ 'foo/bar/foo'
+ >>> _DropCommonSuffixes('foo/foo_internal.h')
+ 'foo/foo'
+ >>> _DropCommonSuffixes('foo/foo_unusualinternal.h')
+ 'foo/foo_unusualinternal'
+
+ Args:
+ filename: The input filename.
+
+ Returns:
+ The filename with the common suffix removed.
+ """
+ for suffix in ('test.cc', 'regtest.cc', 'unittest.cc',
+ 'inl.h', 'impl.h', 'internal.h'):
+ if (filename.endswith(suffix) and len(filename) > len(suffix) and
+ filename[-len(suffix) - 1] in ('-', '_')):
+ return filename[:-len(suffix) - 1]
+ return os.path.splitext(filename)[0]
+
+
+def _IsTestFilename(filename):
+ """Determines if the given filename has a suffix that identifies it as a test.
+
+ Args:
+ filename: The input filename.
+
+ Returns:
+ True if 'filename' looks like a test, False otherwise.
+ """
+ if (filename.endswith('_test.cc') or
+ filename.endswith('_unittest.cc') or
+ filename.endswith('_regtest.cc')):
+ return True
+ else:
+ return False
+
+
+def _ClassifyInclude(fileinfo, include, is_system):
+ """Figures out what kind of header 'include' is.
+
+ Args:
+ fileinfo: The current file cpplint is running over. A FileInfo instance.
+ include: The path to a #included file.
+ is_system: True if the #include used <> rather than "".
+
+ Returns:
+ One of the _XXX_HEADER constants.
+
+ For example:
+ >>> _ClassifyInclude(FileInfo('foo/foo.cc'), 'stdio.h', True)
+ _C_SYS_HEADER
+ >>> _ClassifyInclude(FileInfo('foo/foo.cc'), 'string', True)
+ _CPP_SYS_HEADER
+ >>> _ClassifyInclude(FileInfo('foo/foo.cc'), 'foo/foo.h', False)
+ _LIKELY_MY_HEADER
+ >>> _ClassifyInclude(FileInfo('foo/foo_unknown_extension.cc'),
+ ... 'bar/foo_other_ext.h', False)
+ _POSSIBLE_MY_HEADER
+ >>> _ClassifyInclude(FileInfo('foo/foo.cc'), 'foo/bar.h', False)
+ _OTHER_HEADER
+ """
+ # This is a list of all standard c++ header files, except
+ # those already checked for above.
+ is_stl_h = include in _STL_HEADERS
+ is_cpp_h = is_stl_h or include in _CPP_HEADERS
+
+ if is_system:
+ if is_cpp_h:
+ return _CPP_SYS_HEADER
+ else:
+ return _C_SYS_HEADER
+
+ # If the target file and the include we're checking share a
+ # basename when we drop common extensions, and the include
+ # lives in . , then it's likely to be owned by the target file.
+ target_dir, target_base = (
+ os.path.split(_DropCommonSuffixes(fileinfo.RepositoryName())))
+ include_dir, include_base = os.path.split(_DropCommonSuffixes(include))
+ if target_base == include_base and (
+ include_dir == target_dir or
+ include_dir == os.path.normpath(target_dir + '/../public')):
+ return _LIKELY_MY_HEADER
+
+ # If the target and include share some initial basename
+ # component, it's possible the target is implementing the
+ # include, so it's allowed to be first, but we'll never
+ # complain if it's not there.
+ target_first_component = _RE_FIRST_COMPONENT.match(target_base)
+ include_first_component = _RE_FIRST_COMPONENT.match(include_base)
+ if (target_first_component and include_first_component and
+ target_first_component.group(0) ==
+ include_first_component.group(0)):
+ return _POSSIBLE_MY_HEADER
+
+ return _OTHER_HEADER
+
+
+
+def CheckIncludeLine(filename, clean_lines, linenum, include_state, error):
+ """Check rules that are applicable to #include lines.
+
+ Strings on #include lines are NOT removed from elided line, to make
+ certain tasks easier. However, to prevent false positives, checks
+ applicable to #include lines in CheckLanguage must be put here.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ include_state: An _IncludeState instance in which the headers are inserted.
+ error: The function to call with any errors found.
+ """
+ fileinfo = FileInfo(filename)
+
+ line = clean_lines.lines[linenum]
+
+ # "include" should use the new style "foo/bar.h" instead of just "bar.h"
+ if _RE_PATTERN_INCLUDE_NEW_STYLE.search(line):
+ error(filename, linenum, 'build/include', 4,
+ 'Include the directory when naming .h files')
+
+ # we shouldn't include a file more than once. actually, there are a
+ # handful of instances where doing so is okay, but in general it's
+ # not.
+ match = _RE_PATTERN_INCLUDE.search(line)
+ if match:
+ include = match.group(2)
+ is_system = (match.group(1) == '<')
+ if include in include_state:
+ error(filename, linenum, 'build/include', 4,
+ '"%s" already included at %s:%s' %
+ (include, filename, include_state[include]))
+ else:
+ include_state[include] = linenum
+
+ # We want to ensure that headers appear in the right order:
+ # 1) for foo.cc, foo.h (preferred location)
+ # 2) c system files
+ # 3) cpp system files
+ # 4) for foo.cc, foo.h (deprecated location)
+ # 5) other google headers
+ #
+ # We classify each include statement as one of those 5 types
+ # using a number of techniques. The include_state object keeps
+ # track of the highest type seen, and complains if we see a
+ # lower type after that.
+ error_message = include_state.CheckNextIncludeOrder(
+ _ClassifyInclude(fileinfo, include, is_system))
+ if error_message:
+ error(filename, linenum, 'build/include_order', 4,
+ '%s. Should be: %s.h, c system, c++ system, other.' %
+ (error_message, fileinfo.BaseName()))
+ if not include_state.IsInAlphabeticalOrder(include):
+ error(filename, linenum, 'build/include_alpha', 4,
+ 'Include "%s" not in alphabetical order' % include)
+
+ # Look for any of the stream classes that are part of standard C++.
+ match = _RE_PATTERN_INCLUDE.match(line)
+ if match:
+ include = match.group(2)
+ if Match(r'(f|ind|io|i|o|parse|pf|stdio|str|)?stream$', include):
+ # Many unit tests use cout, so we exempt them.
+ if not _IsTestFilename(filename):
+ error(filename, linenum, 'readability/streams', 3,
+ 'Streams are highly discouraged.')
+
+
+def _GetTextInside(text, start_pattern):
+ """Retrieves all the text between matching open and close parentheses.
+
+ Given a string of lines and a regular expression string, retrieve all the text
+ following the expression and between opening punctuation symbols like
+ (, [, or {, and the matching close-punctuation symbol. This properly nested
+ occurrences of the punctuations, so for the text like
+ printf(a(), b(c()));
+ a call to _GetTextInside(text, r'printf\(') will return 'a(), b(c())'.
+ start_pattern must match string having an open punctuation symbol at the end.
+
+ Args:
+ text: The lines to extract text. Its comments and strings must be elided.
+ It can be single line and can span multiple lines.
+ start_pattern: The regexp string indicating where to start extracting
+ the text.
+ Returns:
+ The extracted text.
+ None if either the opening string or ending punctuation could not be found.
+ """
+ # TODO(sugawarayu): Audit cpplint.py to see what places could be profitably
+ # rewritten to use _GetTextInside (and use inferior regexp matching today).
+
+ # Give opening punctuations to get the matching close-punctuations.
+ matching_punctuation = {'(': ')', '{': '}', '[': ']'}
+ closing_punctuation = set(matching_punctuation.itervalues())
+
+ # Find the position to start extracting text.
+ match = re.search(start_pattern, text, re.M)
+ if not match: # start_pattern not found in text.
+ return None
+ start_position = match.end(0)
+
+ assert start_position > 0, (
+ 'start_pattern must ends with an opening punctuation.')
+ assert text[start_position - 1] in matching_punctuation, (
+ 'start_pattern must ends with an opening punctuation.')
+ # Stack of closing punctuations we expect to have in text after position.
+ punctuation_stack = [matching_punctuation[text[start_position - 1]]]
+ position = start_position
+ while punctuation_stack and position < len(text):
+ if text[position] == punctuation_stack[-1]:
+ punctuation_stack.pop()
+ elif text[position] in closing_punctuation:
+ # A closing punctuation without matching opening punctuations.
+ return None
+ elif text[position] in matching_punctuation:
+ punctuation_stack.append(matching_punctuation[text[position]])
+ position += 1
+ if punctuation_stack:
+ # Opening punctuations left without matching close-punctuations.
+ return None
+ # punctuations match.
+ return text[start_position:position - 1]
+
+
+def CheckLanguage(filename, clean_lines, linenum, file_extension, include_state,
+ error):
+ """Checks rules from the 'C++ language rules' section of cppguide.html.
+
+ Some of these rules are hard to test (function overloading, using
+ uint32 inappropriately), but we do the best we can.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ file_extension: The extension (without the dot) of the filename.
+ include_state: An _IncludeState instance in which the headers are inserted.
+ error: The function to call with any errors found.
+ """
+ # If the line is empty or consists of entirely a comment, no need to
+ # check it.
+ line = clean_lines.elided[linenum]
+ if not line:
+ return
+
+ match = _RE_PATTERN_INCLUDE.search(line)
+ if match:
+ CheckIncludeLine(filename, clean_lines, linenum, include_state, error)
+ return
+
+ # Create an extended_line, which is the concatenation of the current and
+ # next lines, for more effective checking of code that may span more than one
+ # line.
+ if linenum + 1 < clean_lines.NumLines():
+ extended_line = line + clean_lines.elided[linenum + 1]
+ else:
+ extended_line = line
+
+ # Make Windows paths like Unix.
+ fullname = os.path.abspath(filename).replace('\\', '/')
+
+ # TODO(unknown): figure out if they're using default arguments in fn proto.
+
+ # Check for non-const references in functions. This is tricky because &
+ # is also used to take the address of something. We allow <> for templates,
+ # (ignoring whatever is between the braces) and : for classes.
+ # These are complicated re's. They try to capture the following:
+ # paren (for fn-prototype start), typename, &, varname. For the const
+ # version, we're willing for const to be before typename or after
+ # Don't check the implementation on same line.
+ fnline = line.split('{', 1)[0]
+ if (len(re.findall(r'\([^()]*\b(?:[\w:]|<[^()]*>)+(\s?&|&\s?)\w+', fnline)) >
+ len(re.findall(r'\([^()]*\bconst\s+(?:typename\s+)?(?:struct\s+)?'
+ r'(?:[\w:]|<[^()]*>)+(\s?&|&\s?)\w+', fnline)) +
+ len(re.findall(r'\([^()]*\b(?:[\w:]|<[^()]*>)+\s+const(\s?&|&\s?)[\w]+',
+ fnline))):
+
+ # We allow non-const references in a few standard places, like functions
+ # called "swap()" or iostream operators like "<<" or ">>".
+ if not Search(
+ r'(swap|Swap|operator[<>][<>])\s*\(\s*(?:[\w:]|<.*>)+\s*&',
+ fnline):
+ error(filename, linenum, 'runtime/references', 2,
+ 'Is this a non-const reference? '
+ 'If so, make const or use a pointer.')
+
+ # Check to see if they're using an conversion function cast.
+ # I just try to capture the most common basic types, though there are more.
+ # Parameterless conversion functions, such as bool(), are allowed as they are
+ # probably a member operator declaration or default constructor.
+ match = Search(
+ r'(\bnew\s+)?\b' # Grab 'new' operator, if it's there
+ r'(int|float|double|bool|char|int32|uint32|int64|uint64)\([^)]', line)
+ if match:
+ # gMock methods are defined using some variant of MOCK_METHODx(name, type)
+ # where type may be float(), int(string), etc. Without context they are
+ # virtually indistinguishable from int(x) casts. Likewise, gMock's
+ # MockCallback takes a template parameter of the form return_type(arg_type),
+ # which looks much like the cast we're trying to detect.
+ if (match.group(1) is None and # If new operator, then this isn't a cast
+ not (Match(r'^\s*MOCK_(CONST_)?METHOD\d+(_T)?\(', line) or
+ Match(r'^\s*MockCallback<.*>', line))):
+ error(filename, linenum, 'readability/casting', 4,
+ 'Using deprecated casting style. '
+ 'Use static_cast<%s>(...) instead' %
+ match.group(2))
+
+ CheckCStyleCast(filename, linenum, line, clean_lines.raw_lines[linenum],
+ 'static_cast',
+ r'\((int|float|double|bool|char|u?int(16|32|64))\)', error)
+
+ # This doesn't catch all cases. Consider (const char * const)"hello".
+ #
+ # (char *) "foo" should always be a const_cast (reinterpret_cast won't
+ # compile).
+ if CheckCStyleCast(filename, linenum, line, clean_lines.raw_lines[linenum],
+ 'const_cast', r'\((char\s?\*+\s?)\)\s*"', error):
+ pass
+ else:
+ # Check pointer casts for other than string constants
+ CheckCStyleCast(filename, linenum, line, clean_lines.raw_lines[linenum],
+ 'reinterpret_cast', r'\((\w+\s?\*+\s?)\)', error)
+
+ # In addition, we look for people taking the address of a cast. This
+ # is dangerous -- casts can assign to temporaries, so the pointer doesn't
+ # point where you think.
+ if Search(
+ r'(&\([^)]+\)[\w(])|(&(static|dynamic|reinterpret)_cast\b)', line):
+ error(filename, linenum, 'runtime/casting', 4,
+ ('Are you taking an address of a cast? '
+ 'This is dangerous: could be a temp var. '
+ 'Take the address before doing the cast, rather than after'))
+
+ # Check for people declaring static/global STL strings at the top level.
+ # This is dangerous because the C++ language does not guarantee that
+ # globals with constructors are initialized before the first access.
+ match = Match(
+ r'((?:|static +)(?:|const +))string +([a-zA-Z0-9_:]+)\b(.*)',
+ line)
+ # Make sure it's not a function.
+ # Function template specialization looks like: "string foo<Type>(...".
+ # Class template definitions look like: "string Foo<Type>::Method(...".
+ if match and not Match(r'\s*(<.*>)?(::[a-zA-Z0-9_]+)?\s*\(([^"]|$)',
+ match.group(3)):
+ error(filename, linenum, 'runtime/string', 4,
+ 'For a static/global string constant, use a C style string instead: '
+ '"%schar %s[]".' %
+ (match.group(1), match.group(2)))
+
+ # Check that we're not using RTTI outside of testing code.
+ if Search(r'\bdynamic_cast<', line) and not _IsTestFilename(filename):
+ error(filename, linenum, 'runtime/rtti', 5,
+ 'Do not use dynamic_cast<>. If you need to cast within a class '
+ "hierarchy, use static_cast<> to upcast. Google doesn't support "
+ 'RTTI.')
+
+ if Search(r'\b([A-Za-z0-9_]*_)\(\1\)', line):
+ error(filename, linenum, 'runtime/init', 4,
+ 'You seem to be initializing a member variable with itself.')
+
+ if file_extension == 'h':
+ # TODO(unknown): check that 1-arg constructors are explicit.
+ # How to tell it's a constructor?
+ # (handled in CheckForNonStandardConstructs for now)
+ # TODO(unknown): check that classes have DISALLOW_EVIL_CONSTRUCTORS
+ # (level 1 error)
+ pass
+
+ # Check if people are using the verboten C basic types. The only exception
+ # we regularly allow is "unsigned short port" for port.
+ if Search(r'\bshort port\b', line):
+ if not Search(r'\bunsigned short port\b', line):
+ error(filename, linenum, 'runtime/int', 4,
+ 'Use "unsigned short" for ports, not "short"')
+ else:
+ match = Search(r'\b(short|long(?! +double)|long long)\b', line)
+ if match:
+ error(filename, linenum, 'runtime/int', 4,
+ 'Use int16/int64/etc, rather than the C type %s' % match.group(1))
+
+ # When snprintf is used, the second argument shouldn't be a literal.
+ match = Search(r'snprintf\s*\(([^,]*),\s*([0-9]*)\s*,', line)
+ if match and match.group(2) != '0':
+ # If 2nd arg is zero, snprintf is used to calculate size.
+ error(filename, linenum, 'runtime/printf', 3,
+ 'If you can, use sizeof(%s) instead of %s as the 2nd arg '
+ 'to snprintf.' % (match.group(1), match.group(2)))
+
+ # Check if some verboten C functions are being used.
+ if Search(r'\bsprintf\b', line):
+ error(filename, linenum, 'runtime/printf', 5,
+ 'Never use sprintf. Use snprintf instead.')
+ match = Search(r'\b(strcpy|strcat)\b', line)
+ if match:
+ error(filename, linenum, 'runtime/printf', 4,
+ 'Almost always, snprintf is better than %s' % match.group(1))
+
+ if Search(r'\bsscanf\b', line):
+ error(filename, linenum, 'runtime/printf', 1,
+ 'sscanf can be ok, but is slow and can overflow buffers.')
+
+ # Check if some verboten operator overloading is going on
+ # TODO(unknown): catch out-of-line unary operator&:
+ # class X {};
+ # int operator&(const X& x) { return 42; } // unary operator&
+ # The trick is it's hard to tell apart from binary operator&:
+ # class Y { int operator&(const Y& x) { return 23; } }; // binary operator&
+ if Search(r'\boperator\s*&\s*\(\s*\)', line):
+ error(filename, linenum, 'runtime/operator', 4,
+ 'Unary operator& is dangerous. Do not use it.')
+
+ # Check for suspicious usage of "if" like
+ # } if (a == b) {
+ if Search(r'\}\s*if\s*\(', line):
+ error(filename, linenum, 'readability/braces', 4,
+ 'Did you mean "else if"? If not, start a new line for "if".')
+
+ # Check for potential format string bugs like printf(foo).
+ # We constrain the pattern not to pick things like DocidForPrintf(foo).
+ # Not perfect but it can catch printf(foo.c_str()) and printf(foo->c_str())
+ # TODO(sugawarayu): Catch the following case. Need to change the calling
+ # convention of the whole function to process multiple line to handle it.
+ # printf(
+ # boy_this_is_a_really_long_variable_that_cannot_fit_on_the_prev_line);
+ printf_args = _GetTextInside(line, r'(?i)\b(string)?printf\s*\(')
+ if printf_args:
+ match = Match(r'([\w.\->()]+)$', printf_args)
+ if match:
+ function_name = re.search(r'\b((?:string)?printf)\s*\(',
+ line, re.I).group(1)
+ error(filename, linenum, 'runtime/printf', 4,
+ 'Potential format string bug. Do %s("%%s", %s) instead.'
+ % (function_name, match.group(1)))
+
+ # Check for potential memset bugs like memset(buf, sizeof(buf), 0).
+ match = Search(r'memset\s*\(([^,]*),\s*([^,]*),\s*0\s*\)', line)
+ if match and not Match(r"^''|-?[0-9]+|0x[0-9A-Fa-f]$", match.group(2)):
+ error(filename, linenum, 'runtime/memset', 4,
+ 'Did you mean "memset(%s, 0, %s)"?'
+ % (match.group(1), match.group(2)))
+
+ if Search(r'\busing namespace\b', line):
+ error(filename, linenum, 'build/namespaces', 5,
+ 'Do not use namespace using-directives. '
+ 'Use using-declarations instead.')
+
+ # Detect variable-length arrays.
+ match = Match(r'\s*(.+::)?(\w+) [a-z]\w*\[(.+)];', line)
+ if (match and match.group(2) != 'return' and match.group(2) != 'delete' and
+ match.group(3).find(']') == -1):
+ # Split the size using space and arithmetic operators as delimiters.
+ # If any of the resulting tokens are not compile time constants then
+ # report the error.
+ tokens = re.split(r'\s|\+|\-|\*|\/|<<|>>]', match.group(3))
+ is_const = True
+ skip_next = False
+ for tok in tokens:
+ if skip_next:
+ skip_next = False
+ continue
+
+ if Search(r'sizeof\(.+\)', tok): continue
+ if Search(r'arraysize\(\w+\)', tok): continue
+
+ tok = tok.lstrip('(')
+ tok = tok.rstrip(')')
+ if not tok: continue
+ if Match(r'\d+', tok): continue
+ if Match(r'0[xX][0-9a-fA-F]+', tok): continue
+ if Match(r'k[A-Z0-9]\w*', tok): continue
+ if Match(r'(.+::)?k[A-Z0-9]\w*', tok): continue
+ if Match(r'(.+::)?[A-Z][A-Z0-9_]*', tok): continue
+ # A catch all for tricky sizeof cases, including 'sizeof expression',
+ # 'sizeof(*type)', 'sizeof(const type)', 'sizeof(struct StructName)'
+ # requires skipping the next token because we split on ' ' and '*'.
+ if tok.startswith('sizeof'):
+ skip_next = True
+ continue
+ is_const = False
+ break
+ if not is_const:
+ error(filename, linenum, 'runtime/arrays', 1,
+ 'Do not use variable-length arrays. Use an appropriately named '
+ "('k' followed by CamelCase) compile-time constant for the size.")
+
+ # If DISALLOW_EVIL_CONSTRUCTORS, DISALLOW_COPY_AND_ASSIGN, or
+ # DISALLOW_IMPLICIT_CONSTRUCTORS is present, then it should be the last thing
+ # in the class declaration.
+ match = Match(
+ (r'\s*'
+ r'(DISALLOW_(EVIL_CONSTRUCTORS|COPY_AND_ASSIGN|IMPLICIT_CONSTRUCTORS))'
+ r'\(.*\);$'),
+ line)
+ if match and linenum + 1 < clean_lines.NumLines():
+ next_line = clean_lines.elided[linenum + 1]
+ # We allow some, but not all, declarations of variables to be present
+ # in the statement that defines the class. The [\w\*,\s]* fragment of
+ # the regular expression below allows users to declare instances of
+ # the class or pointers to instances, but not less common types such
+ # as function pointers or arrays. It's a tradeoff between allowing
+ # reasonable code and avoiding trying to parse more C++ using regexps.
+ if not Search(r'^\s*}[\w\*,\s]*;', next_line):
+ error(filename, linenum, 'readability/constructors', 3,
+ match.group(1) + ' should be the last thing in the class')
+
+ # Check for use of unnamed namespaces in header files. Registration
+ # macros are typically OK, so we allow use of "namespace {" on lines
+ # that end with backslashes.
+ if (file_extension == 'h'
+ and Search(r'\bnamespace\s*{', line)
+ and line[-1] != '\\'):
+ error(filename, linenum, 'build/namespaces', 4,
+ 'Do not use unnamed namespaces in header files. See '
+ 'http://google-styleguide.googlecode.com/svn/trunk/cppguide.xml#Namespaces'
+ ' for more information.')
+
+
+def CheckCStyleCast(filename, linenum, line, raw_line, cast_type, pattern,
+ error):
+ """Checks for a C-style cast by looking for the pattern.
+
+ This also handles sizeof(type) warnings, due to similarity of content.
+
+ Args:
+ filename: The name of the current file.
+ linenum: The number of the line to check.
+ line: The line of code to check.
+ raw_line: The raw line of code to check, with comments.
+ cast_type: The string for the C++ cast to recommend. This is either
+ reinterpret_cast, static_cast, or const_cast, depending.
+ pattern: The regular expression used to find C-style casts.
+ error: The function to call with any errors found.
+
+ Returns:
+ True if an error was emitted.
+ False otherwise.
+ """
+ match = Search(pattern, line)
+ if not match:
+ return False
+
+ # e.g., sizeof(int)
+ sizeof_match = Match(r'.*sizeof\s*$', line[0:match.start(1) - 1])
+ if sizeof_match:
+ error(filename, linenum, 'runtime/sizeof', 1,
+ 'Using sizeof(type). Use sizeof(varname) instead if possible')
+ return True
+
+ remainder = line[match.end(0):]
+
+ # The close paren is for function pointers as arguments to a function.
+ # eg, void foo(void (*bar)(int));
+ # The semicolon check is a more basic function check; also possibly a
+ # function pointer typedef.
+ # eg, void foo(int); or void foo(int) const;
+ # The equals check is for function pointer assignment.
+ # eg, void *(*foo)(int) = ...
+ # The > is for MockCallback<...> ...
+ #
+ # Right now, this will only catch cases where there's a single argument, and
+ # it's unnamed. It should probably be expanded to check for multiple
+ # arguments with some unnamed.
+ function_match = Match(r'\s*(\)|=|(const)?\s*(;|\{|throw\(\)|>))', remainder)
+ if function_match:
+ if (not function_match.group(3) or
+ function_match.group(3) == ';' or
+ ('MockCallback<' not in raw_line and
+ '/*' not in raw_line)):
+ error(filename, linenum, 'readability/function', 3,
+ 'All parameters should be named in a function')
+ return True
+
+ # At this point, all that should be left is actual casts.
+ error(filename, linenum, 'readability/casting', 4,
+ 'Using C-style cast. Use %s<%s>(...) instead' %
+ (cast_type, match.group(1)))
+
+ return True
+
+
+_HEADERS_CONTAINING_TEMPLATES = (
+ ('<deque>', ('deque',)),
+ ('<functional>', ('unary_function', 'binary_function',
+ 'plus', 'minus', 'multiplies', 'divides', 'modulus',
+ 'negate',
+ 'equal_to', 'not_equal_to', 'greater', 'less',
+ 'greater_equal', 'less_equal',
+ 'logical_and', 'logical_or', 'logical_not',
+ 'unary_negate', 'not1', 'binary_negate', 'not2',
+ 'bind1st', 'bind2nd',
+ 'pointer_to_unary_function',
+ 'pointer_to_binary_function',
+ 'ptr_fun',
+ 'mem_fun_t', 'mem_fun', 'mem_fun1_t', 'mem_fun1_ref_t',
+ 'mem_fun_ref_t',
+ 'const_mem_fun_t', 'const_mem_fun1_t',
+ 'const_mem_fun_ref_t', 'const_mem_fun1_ref_t',
+ 'mem_fun_ref',
+ )),
+ ('<limits>', ('numeric_limits',)),
+ ('<list>', ('list',)),
+ ('<map>', ('map', 'multimap',)),
+ ('<memory>', ('allocator',)),
+ ('<queue>', ('queue', 'priority_queue',)),
+ ('<set>', ('set', 'multiset',)),
+ ('<stack>', ('stack',)),
+ ('<string>', ('char_traits', 'basic_string',)),
+ ('<utility>', ('pair',)),
+ ('<vector>', ('vector',)),
+
+ # gcc extensions.
+ # Note: std::hash is their hash, ::hash is our hash
+ ('<hash_map>', ('hash_map', 'hash_multimap',)),
+ ('<hash_set>', ('hash_set', 'hash_multiset',)),
+ ('<slist>', ('slist',)),
+ )
+
+_RE_PATTERN_STRING = re.compile(r'\bstring\b')
+
+_re_pattern_algorithm_header = []
+for _template in ('copy', 'max', 'min', 'min_element', 'sort', 'swap',
+ 'transform'):
+ # Match max<type>(..., ...), max(..., ...), but not foo->max, foo.max or
+ # type::max().
+ _re_pattern_algorithm_header.append(
+ (re.compile(r'[^>.]\b' + _template + r'(<.*?>)?\([^\)]'),
+ _template,
+ '<algorithm>'))
+
+_re_pattern_templates = []
+for _header, _templates in _HEADERS_CONTAINING_TEMPLATES:
+ for _template in _templates:
+ _re_pattern_templates.append(
+ (re.compile(r'(\<|\b)' + _template + r'\s*\<'),
+ _template + '<>',
+ _header))
+
+
+def FilesBelongToSameModule(filename_cc, filename_h):
+ """Check if these two filenames belong to the same module.
+
+ The concept of a 'module' here is a as follows:
+ foo.h, foo-inl.h, foo.cc, foo_test.cc and foo_unittest.cc belong to the
+ same 'module' if they are in the same directory.
+ some/path/public/xyzzy and some/path/internal/xyzzy are also considered
+ to belong to the same module here.
+
+ If the filename_cc contains a longer path than the filename_h, for example,
+ '/absolute/path/to/base/sysinfo.cc', and this file would include
+ 'base/sysinfo.h', this function also produces the prefix needed to open the
+ header. This is used by the caller of this function to more robustly open the
+ header file. We don't have access to the real include paths in this context,
+ so we need this guesswork here.
+
+ Known bugs: tools/base/bar.cc and base/bar.h belong to the same module
+ according to this implementation. Because of this, this function gives
+ some false positives. This should be sufficiently rare in practice.
+
+ Args:
+ filename_cc: is the path for the .cc file
+ filename_h: is the path for the header path
+
+ Returns:
+ Tuple with a bool and a string:
+ bool: True if filename_cc and filename_h belong to the same module.
+ string: the additional prefix needed to open the header file.
+ """
+
+ if not filename_cc.endswith('.cc'):
+ return (False, '')
+ filename_cc = filename_cc[:-len('.cc')]
+ if filename_cc.endswith('_unittest'):
+ filename_cc = filename_cc[:-len('_unittest')]
+ elif filename_cc.endswith('_test'):
+ filename_cc = filename_cc[:-len('_test')]
+ filename_cc = filename_cc.replace('/public/', '/')
+ filename_cc = filename_cc.replace('/internal/', '/')
+
+ if not filename_h.endswith('.h'):
+ return (False, '')
+ filename_h = filename_h[:-len('.h')]
+ if filename_h.endswith('-inl'):
+ filename_h = filename_h[:-len('-inl')]
+ filename_h = filename_h.replace('/public/', '/')
+ filename_h = filename_h.replace('/internal/', '/')
+
+ files_belong_to_same_module = filename_cc.endswith(filename_h)
+ common_path = ''
+ if files_belong_to_same_module:
+ common_path = filename_cc[:-len(filename_h)]
+ return files_belong_to_same_module, common_path
+
+
+def UpdateIncludeState(filename, include_state, io=codecs):
+ """Fill up the include_state with new includes found from the file.
+
+ Args:
+ filename: the name of the header to read.
+ include_state: an _IncludeState instance in which the headers are inserted.
+ io: The io factory to use to read the file. Provided for testability.
+
+ Returns:
+ True if a header was succesfully added. False otherwise.
+ """
+ headerfile = None
+ try:
+ headerfile = io.open(filename, 'r', 'utf8', 'replace')
+ except IOError:
+ return False
+ linenum = 0
+ for line in headerfile:
+ linenum += 1
+ clean_line = CleanseComments(line)
+ match = _RE_PATTERN_INCLUDE.search(clean_line)
+ if match:
+ include = match.group(2)
+ # The value formatting is cute, but not really used right now.
+ # What matters here is that the key is in include_state.
+ include_state.setdefault(include, '%s:%d' % (filename, linenum))
+ return True
+
+
+def CheckForIncludeWhatYouUse(filename, clean_lines, include_state, error,
+ io=codecs):
+ """Reports for missing stl includes.
+
+ This function will output warnings to make sure you are including the headers
+ necessary for the stl containers and functions that you use. We only give one
+ reason to include a header. For example, if you use both equal_to<> and
+ less<> in a .h file, only one (the latter in the file) of these will be
+ reported as a reason to include the <functional>.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ include_state: An _IncludeState instance.
+ error: The function to call with any errors found.
+ io: The IO factory to use to read the header file. Provided for unittest
+ injection.
+ """
+ required = {} # A map of header name to linenumber and the template entity.
+ # Example of required: { '<functional>': (1219, 'less<>') }
+
+ for linenum in xrange(clean_lines.NumLines()):
+ line = clean_lines.elided[linenum]
+ if not line or line[0] == '#':
+ continue
+
+ # String is special -- it is a non-templatized type in STL.
+ matched = _RE_PATTERN_STRING.search(line)
+ if matched:
+ # Don't warn about strings in non-STL namespaces:
+ # (We check only the first match per line; good enough.)
+ prefix = line[:matched.start()]
+ if prefix.endswith('std::') or not prefix.endswith('::'):
+ required['<string>'] = (linenum, 'string')
+
+ for pattern, template, header in _re_pattern_algorithm_header:
+ if pattern.search(line):
+ required[header] = (linenum, template)
+
+ # The following function is just a speed up, no semantics are changed.
+ if not '<' in line: # Reduces the cpu time usage by skipping lines.
+ continue
+
+ for pattern, template, header in _re_pattern_templates:
+ if pattern.search(line):
+ required[header] = (linenum, template)
+
+ # The policy is that if you #include something in foo.h you don't need to
+ # include it again in foo.cc. Here, we will look at possible includes.
+ # Let's copy the include_state so it is only messed up within this function.
+ include_state = include_state.copy()
+
+ # Did we find the header for this file (if any) and succesfully load it?
+ header_found = False
+
+ # Use the absolute path so that matching works properly.
+ abs_filename = FileInfo(filename).FullName()
+
+ # For Emacs's flymake.
+ # If cpplint is invoked from Emacs's flymake, a temporary file is generated
+ # by flymake and that file name might end with '_flymake.cc'. In that case,
+ # restore original file name here so that the corresponding header file can be
+ # found.
+ # e.g. If the file name is 'foo_flymake.cc', we should search for 'foo.h'
+ # instead of 'foo_flymake.h'
+ abs_filename = re.sub(r'_flymake\.cc$', '.cc', abs_filename)
+
+ # include_state is modified during iteration, so we iterate over a copy of
+ # the keys.
+ header_keys = include_state.keys()
+ for header in header_keys:
+ (same_module, common_path) = FilesBelongToSameModule(abs_filename, header)
+ fullpath = common_path + header
+ if same_module and UpdateIncludeState(fullpath, include_state, io):
+ header_found = True
+
+ # If we can't find the header file for a .cc, assume it's because we don't
+ # know where to look. In that case we'll give up as we're not sure they
+ # didn't include it in the .h file.
+ # TODO(unknown): Do a better job of finding .h files so we are confident that
+ # not having the .h file means there isn't one.
+ if filename.endswith('.cc') and not header_found:
+ return
+
+ # All the lines have been processed, report the errors found.
+ for required_header_unstripped in required:
+ template = required[required_header_unstripped][1]
+ if required_header_unstripped.strip('<>"') not in include_state:
+ error(filename, required[required_header_unstripped][0],
+ 'build/include_what_you_use', 4,
+ 'Add #include ' + required_header_unstripped + ' for ' + template)
+
+
+_RE_PATTERN_EXPLICIT_MAKEPAIR = re.compile(r'\bmake_pair\s*<')
+
+
+def CheckMakePairUsesDeduction(filename, clean_lines, linenum, error):
+ """Check that make_pair's template arguments are deduced.
+
+ G++ 4.6 in C++0x mode fails badly if make_pair's template arguments are
+ specified explicitly, and such use isn't intended in any case.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ error: The function to call with any errors found.
+ """
+ raw = clean_lines.raw_lines
+ line = raw[linenum]
+ match = _RE_PATTERN_EXPLICIT_MAKEPAIR.search(line)
+ if match:
+ error(filename, linenum, 'build/explicit_make_pair',
+ 4, # 4 = high confidence
+ 'Omit template arguments from make_pair OR use pair directly OR'
+ ' if appropriate, construct a pair directly')
+
+
+def ProcessLine(filename, file_extension,
+ clean_lines, line, include_state, function_state,
+ class_state, error, extra_check_functions=[]):
+ """Processes a single line in the file.
+
+ Args:
+ filename: Filename of the file that is being processed.
+ file_extension: The extension (dot not included) of the file.
+ clean_lines: An array of strings, each representing a line of the file,
+ with comments stripped.
+ line: Number of line being processed.
+ include_state: An _IncludeState instance in which the headers are inserted.
+ function_state: A _FunctionState instance which counts function lines, etc.
+ class_state: A _ClassState instance which maintains information about
+ the current stack of nested class declarations being parsed.
+ error: A callable to which errors are reported, which takes 4 arguments:
+ filename, line number, error level, and message
+ extra_check_functions: An array of additional check functions that will be
+ run on each source line. Each function takes 4
+ arguments: filename, clean_lines, line, error
+ """
+ raw_lines = clean_lines.raw_lines
+ ParseNolintSuppressions(filename, raw_lines[line], line, error)
+ CheckForFunctionLengths(filename, clean_lines, line, function_state, error)
+ CheckForMultilineCommentsAndStrings(filename, clean_lines, line, error)
+ CheckStyle(filename, clean_lines, line, file_extension, class_state, error)
+ CheckLanguage(filename, clean_lines, line, file_extension, include_state,
+ error)
+ CheckForNonStandardConstructs(filename, clean_lines, line,
+ class_state, error)
+ CheckPosixThreading(filename, clean_lines, line, error)
+ CheckInvalidIncrement(filename, clean_lines, line, error)
+ CheckMakePairUsesDeduction(filename, clean_lines, line, error)
+ for check_fn in extra_check_functions:
+ check_fn(filename, clean_lines, line, error)
+
+def ProcessFileData(filename, file_extension, lines, error,
+ extra_check_functions=[]):
+ """Performs lint checks and reports any errors to the given error function.
+
+ Args:
+ filename: Filename of the file that is being processed.
+ file_extension: The extension (dot not included) of the file.
+ lines: An array of strings, each representing a line of the file, with the
+ last element being empty if the file is terminated with a newline.
+ error: A callable to which errors are reported, which takes 4 arguments:
+ filename, line number, error level, and message
+ extra_check_functions: An array of additional check functions that will be
+ run on each source line. Each function takes 4
+ arguments: filename, clean_lines, line, error
+ """
+ lines = (['// marker so line numbers and indices both start at 1'] + lines +
+ ['// marker so line numbers end in a known way'])
+
+ include_state = _IncludeState()
+ function_state = _FunctionState()
+ class_state = _ClassState()
+
+ ResetNolintSuppressions()
+
+ CheckForCopyright(filename, lines, error)
+
+ if file_extension == 'h':
+ CheckForHeaderGuard(filename, lines, error)
+
+ RemoveMultiLineComments(filename, lines, error)
+ clean_lines = CleansedLines(lines)
+ for line in xrange(clean_lines.NumLines()):
+ ProcessLine(filename, file_extension, clean_lines, line,
+ include_state, function_state, class_state, error,
+ extra_check_functions)
+ class_state.CheckFinished(filename, error)
+
+ CheckForIncludeWhatYouUse(filename, clean_lines, include_state, error)
+
+ # We check here rather than inside ProcessLine so that we see raw
+ # lines rather than "cleaned" lines.
+ CheckForUnicodeReplacementCharacters(filename, lines, error)
+
+ CheckForNewlineAtEOF(filename, lines, error)
+
+def ProcessFile(filename, vlevel, extra_check_functions=[]):
+ """Does google-lint on a single file.
+
+ Args:
+ filename: The name of the file to parse.
+
+ vlevel: The level of errors to report. Every error of confidence
+ >= verbose_level will be reported. 0 is a good default.
+
+ extra_check_functions: An array of additional check functions that will be
+ run on each source line. Each function takes 4
+ arguments: filename, clean_lines, line, error
+ """
+
+ _SetVerboseLevel(vlevel)
+
+ try:
+ # Support the UNIX convention of using "-" for stdin. Note that
+ # we are not opening the file with universal newline support
+ # (which codecs doesn't support anyway), so the resulting lines do
+ # contain trailing '\r' characters if we are reading a file that
+ # has CRLF endings.
+ # If after the split a trailing '\r' is present, it is removed
+ # below. If it is not expected to be present (i.e. os.linesep !=
+ # '\r\n' as in Windows), a warning is issued below if this file
+ # is processed.
+
+ if filename == '-':
+ lines = codecs.StreamReaderWriter(sys.stdin,
+ codecs.getreader('utf8'),
+ codecs.getwriter('utf8'),
+ 'replace').read().split('\n')
+ else:
+ lines = codecs.open(filename, 'r', 'utf8', 'replace').read().split('\n')
+
+ carriage_return_found = False
+ # Remove trailing '\r'.
+ for linenum in range(len(lines)):
+ if lines[linenum].endswith('\r'):
+ lines[linenum] = lines[linenum].rstrip('\r')
+ carriage_return_found = True
+
+ except IOError:
+ sys.stderr.write(
+ "Skipping input '%s': Can't open for reading\n" % filename)
+ return
+
+ # Note, if no dot is found, this will give the entire filename as the ext.
+ file_extension = filename[filename.rfind('.') + 1:]
+
+ # When reading from stdin, the extension is unknown, so no cpplint tests
+ # should rely on the extension.
+ if (filename != '-' and file_extension != 'cc' and file_extension != 'h'
+ and file_extension != 'cpp'):
+ sys.stderr.write('Ignoring %s; not a .cc or .h file\n' % filename)
+ else:
+ ProcessFileData(filename, file_extension, lines, Error,
+ extra_check_functions)
+ if carriage_return_found and os.linesep != '\r\n':
+ # Use 0 for linenum since outputting only one error for potentially
+ # several lines.
+ Error(filename, 0, 'whitespace/newline', 1,
+ 'One or more unexpected \\r (^M) found;'
+ 'better to use only a \\n')
+
+ sys.stderr.write('Done processing %s\n' % filename)
+
+
+def PrintUsage(message):
+ """Prints a brief usage string and exits, optionally with an error message.
+
+ Args:
+ message: The optional error message.
+ """
+ sys.stderr.write(_USAGE)
+ if message:
+ sys.exit('\nFATAL ERROR: ' + message)
+ else:
+ sys.exit(1)
+
+
+def PrintCategories():
+ """Prints a list of all the error-categories used by error messages.
+
+ These are the categories used to filter messages via --filter.
+ """
+ sys.stderr.write(''.join(' %s\n' % cat for cat in _ERROR_CATEGORIES))
+ sys.exit(0)
+
+
+def ParseArguments(args):
+ """Parses the command line arguments.
+
+ This may set the output format and verbosity level as side-effects.
+
+ Args:
+ args: The command line arguments:
+
+ Returns:
+ The list of filenames to lint.
+ """
+ try:
+ (opts, filenames) = getopt.getopt(args, '', ['help', 'output=', 'verbose=',
+ 'counting=',
+ 'filter='])
+ except getopt.GetoptError:
+ PrintUsage('Invalid arguments.')
+
+ verbosity = _VerboseLevel()
+ output_format = _OutputFormat()
+ filters = ''
+ counting_style = ''
+
+ for (opt, val) in opts:
+ if opt == '--help':
+ PrintUsage(None)
+ elif opt == '--output':
+ if not val in ('emacs', 'vs7'):
+ PrintUsage('The only allowed output formats are emacs and vs7.')
+ output_format = val
+ elif opt == '--verbose':
+ verbosity = int(val)
+ elif opt == '--filter':
+ filters = val
+ if not filters:
+ PrintCategories()
+ elif opt == '--counting':
+ if val not in ('total', 'toplevel', 'detailed'):
+ PrintUsage('Valid counting options are total, toplevel, and detailed')
+ counting_style = val
+
+ if not filenames:
+ PrintUsage('No files were specified.')
+
+ _SetOutputFormat(output_format)
+ _SetVerboseLevel(verbosity)
+ _SetFilters(filters)
+ _SetCountingStyle(counting_style)
+
+ return filenames
+
+
+def main():
+ filenames = ParseArguments(sys.argv[1:])
+
+ # Change stderr to write with replacement characters so we don't die
+ # if we try to print something containing non-ASCII characters.
+ sys.stderr = codecs.StreamReaderWriter(sys.stderr,
+ codecs.getreader('utf8'),
+ codecs.getwriter('utf8'),
+ 'replace')
+
+ _cpplint_state.ResetErrorCounts()
+ for filename in filenames:
+ ProcessFile(filename, _cpplint_state.verbose_level)
+ _cpplint_state.PrintErrorCounts()
+
+ sys.exit(_cpplint_state.error_count > 0)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/Tools/Fragments/README.txt b/Tools/Fragments/README.txt
new file mode 100644
index 0000000..0888930
--- /dev/null
+++ b/Tools/Fragments/README.txt
@@ -0,0 +1,4 @@
+Scripts in here might have a very specific use case or application, or there might be a need to preprocess or postprocess to utilize them these could be refactored into something more general.
+
+merge.py:
+ Merge can take a reference list of vertices from a .ply file and add texture coordinates from a .obj file without changing the order of the vertices in the .ply description. It does not implement a full .ply/.obj parser. See the documentation in the file for more information
\ No newline at end of file
diff --git a/Tools/Fragments/merge-testdata/expected-testout.txt b/Tools/Fragments/merge-testdata/expected-testout.txt
new file mode 100644
index 0000000..e35a23d
--- /dev/null
+++ b/Tools/Fragments/merge-testdata/expected-testout.txt
@@ -0,0 +1,9 @@
+This should be the result of doing either
+
+python ../merge.py testvertices.txt testobj.txt testout.txt
+python ../merge.py testvertices.txt testobj-indices.txt testout.txt
+
+4.1 4.2 4.3 4.4 4.5
+1.1 1.2 1.3 1.4 1.5
+2.1 2.2 2.3 0.0 0.0
+3.1 3.2 3.3 3.4 3.5
diff --git a/Tools/Fragments/merge-testdata/testobj-indices.txt b/Tools/Fragments/merge-testdata/testobj-indices.txt
new file mode 100644
index 0000000..4b975af
--- /dev/null
+++ b/Tools/Fragments/merge-testdata/testobj-indices.txt
@@ -0,0 +1,8 @@
+v 1.1 1.3 -1.2
+v 3.1 3.3 -3.2
+v 4.1 4.3 -4.2
+vt 4.4 4.5
+vt 3.4 3.5
+vt 1.4 1.5
+vt 0.1 0.2
+f 1/3 2/2 3/1
diff --git a/Tools/Fragments/merge-testdata/testobj.txt b/Tools/Fragments/merge-testdata/testobj.txt
new file mode 100644
index 0000000..232b00a
--- /dev/null
+++ b/Tools/Fragments/merge-testdata/testobj.txt
@@ -0,0 +1,7 @@
+v 1.1 1.3 -1.2
+v 3.1 3.3 -3.2
+v 4.1 4.3 -4.2
+vt 1.4 1.5
+vt 3.4 3.5
+vt 4.4 4.5
+vt 0.1 0.2
\ No newline at end of file
diff --git a/Tools/Fragments/merge-testdata/testvertices.txt b/Tools/Fragments/merge-testdata/testvertices.txt
new file mode 100644
index 0000000..330ef80
--- /dev/null
+++ b/Tools/Fragments/merge-testdata/testvertices.txt
@@ -0,0 +1,4 @@
+4.1 4.2 4.3
+1.1 1.2 1.3
+2.1 2.2 2.3
+3.1 3.2 3.3
\ No newline at end of file
diff --git a/Tools/Fragments/merge.py b/Tools/Fragments/merge.py
new file mode 100644
index 0000000..7578186
--- /dev/null
+++ b/Tools/Fragments/merge.py
@@ -0,0 +1,128 @@
+#!/usr/bin/python
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Merge a list of input vertices and information from an .obj file
+
+Give a file with a list of input vertices, look through the obj file and find
+corresponding vertices, add texture information to the input vertices and store
+the resulting list in an output file. The order in the vertex file is maintained
+and vertices that cannot be found in the obj file will receive default information
+This can be used to add information to a .ply file where the order of the vertices
+is already established
+E.g.
+
+vertex file (fragment from .ply file)
+1.0 1.0 1.0
+2.0 2.0 2.0
+3.0 3.0 3.0
+
+obj file (fragment from .obj file)
+v 1.0 1.0 -1.0
+v 2.0 2.0 -2.0
+vt 1.0 1.0
+vt 0.5 0.5
+
+output (fragment, paste this into the vertex section of the .ply file)
+1.0 1.0 1.0 1.0 1.0
+2.0 2.0 2.0 0.5 0.5
+3.0 3.0 3.0 0.0 0.0
+
+Typical usage:
+ merge.py input.txt object.obj output.txt
+"""
+
+import csv
+import argparse
+
+def find(item, vertices) :
+ index = 0
+ for vertex in vertices:
+ if (item == vertex) :
+ return index
+ index += 1
+ return -1
+
+vertices = []
+uvs = []
+uvIndices = []
+
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser(
+ description="Merge a list of input vertices and information from an .obj file.")
+ parser.add_argument('vertices', help='Filename for input vertices.')
+ parser.add_argument('obj', help='The .obj file to use.' )
+ parser.add_argument('output', help='Filename for output.')
+ args = parser.parse_args()
+
+ # this can be a basic obj file, we are only looking at 'v' and 'vt' for now
+ with open(args.obj, 'rb') as csvfile:
+ reader = csv.reader(csvfile, delimiter = ' ')
+
+ vertexIndex = 0
+ for row in reader:
+ if (row[0] == 'v') :
+ #Go from obj to ply coordinate system, change the sign on the z coord
+ if (row[3][0] == '-'):
+ row[3] = row[3][1:]
+ else:
+ row[3] = '-'+row[3]
+
+ #swap y and z
+ tmp = row[3]
+ row[3] = row[2]
+ row[2] = tmp
+ vertices.append(row[1:])
+ elif (row[0] == 'vt') :
+ uvs.append(row[1:])
+ uvIndices.append(vertexIndex)
+ vertexIndex += 1
+ elif (row[0] == 'f') :
+ # These are either
+ # f v v v
+ # f v/vt v/vt v/vt
+ # f v/vt/vn v/vt/vn v/vt/vn
+ for item in row[1:]:
+ indices = item.split("/")
+ if (len(indices) > 1) :
+ # .obj indices start with 1
+ # put the index of the texture coordinate into the indices map
+ uvIndices[int(indices[0])-1] = int(indices[1])-1
+
+ result = []
+
+ # Reference is just a list of vertices x,y,z separated by ' '
+ # The comparison is done via the strings of the numbers, no accuracy needed, this relies
+ # on the fact that the output format is the same and no transformation was done on the
+ # vertices
+ with open(args.vertices, 'rb') as csvfile:
+ reader = csv.reader(csvfile, delimiter = ' ')
+ count = 0
+ for row in reader:
+ texCoord = ["0.0","0.0"]
+ index = find(row, vertices)
+ if (index != -1) :
+ texCoord = uvs[uvIndices[index]]
+ row.append(texCoord[0])
+ row.append(texCoord[1])
+ result.append(row)
+
+
+ with open(args.output, 'wb') as outfile:
+ writer = csv.writer(outfile, delimiter = ' ', quoting=csv.QUOTE_NONE)
+ for row in result:
+ writer.writerow(row)
\ No newline at end of file
diff --git a/Tools/alphabetize_cmakelists.py b/Tools/alphabetize_cmakelists.py
new file mode 100644
index 0000000..3134ad5
--- /dev/null
+++ b/Tools/alphabetize_cmakelists.py
@@ -0,0 +1,109 @@
+#!/usr/bin/python
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Alphabetize the CMake lists of headers and sources.
+
+Typical usage:
+ Tools/alphabetize-cmakelists.py SurgSim/CMakeLists.py
+or:
+ find . -name 'build*' -prune -o -name CMakeLists.txt -print \
+ | xargs python Tools/alphabetize-cmakelists.py
+"""
+
+import argparse
+import sys
+import re
+
+def slurp_lines(file):
+ try:
+ with open(file, 'r') as f:
+ return f.read().splitlines()
+ except IOError as e:
+ print >> sys.stderr, e
+ return None
+
+def spew_lines(file, lines):
+ try:
+ with open(file, 'wb') as f:
+ for line in lines:
+ f.write(line + "\n")
+ return True
+ except IOError as e:
+ print >> sys.stderr, e
+ return False
+
+def filename_sort_filter(file):
+# return re.sub(r'Tests?$', '', re.sub(r'\.(?:h|cpp)$', '', file)).lower()
+# Taking out the string 'Tests' screws up the file ordering test
+ return re.sub(r'\.(?:h|cpp)$', '', file).lower()
+
+def alphabetize(file, lines):
+ if lines is None:
+ return None
+ (result, indent, var_name, content) = ([], '', None, None)
+ for line in lines:
+ if content is not None:
+ (xline, had_closing_paren) = re.subn(r'\)\s*$', '', line)
+ if re.search(r'[(){}]', xline):
+ print >> sys.stderr, file + ": Bad line '" + line + "'"
+ # put the content lines back unmodified
+ result.extend(content)
+ result.append(line)
+ continue
+ content.append(xline)
+ if had_closing_paren:
+ content = content[1:] # strip the original set(... line
+ content = filter(lambda x: len(x), map(lambda x: x.strip(), content))
+ #print var_name + ":", content
+ content = sorted(content, key=filename_sort_filter)
+ result.append(indent + "set(" + var_name)
+ result.extend(map(lambda x: indent + "\t" + x, content))
+ result.append(indent + ")")
+ content = None
+ else:
+ match = re.search(
+ r'^(\s*)[Ss][Ee][Tt]\s*\(\s*(\w+(?:HEADERS|SOURCES))\s*$', line)
+ if match:
+ indent = match.group(1)
+ var_name = match.group(2)
+ content = [ line ] # include original line, will be removed later
+ else:
+ result.append(line)
+ if result == lines:
+ return None
+ return result
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser(
+ description='Alphabetize CMake lists of headers and sources.')
+ parser.add_argument('files', metavar='FILE', nargs='*',
+ help='The file names to reformat.')
+ args = parser.parse_args()
+
+ touched = False
+ for file in args.files:
+ if not re.search(r'\.(?:txt|cmake)$', file):
+ print >> sys.stderr, file + ": not a CMake input file!"
+ else:
+ lines = alphabetize(file, slurp_lines(file))
+ if lines is not None:
+ spew_lines(file, lines)
+ print "Updated", file
+ touched = True
+
+ if not touched:
+ print "{}: Nothing to update!".format(sys.argv[0])
diff --git a/Tools/fix-style.py b/Tools/fix-style.py
new file mode 100644
index 0000000..a548df2
--- /dev/null
+++ b/Tools/fix-style.py
@@ -0,0 +1,70 @@
+#!/usr/bin/python
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Runs AStyle to reformat the specified source files."""
+
+import argparse
+import sys
+import os
+import subprocess
+
+ASTYLE_FLAGS = ('--indent=force-tab=4', '--style=allman', '--add-brackets',
+ '--unpad-paren', '--pad-header', '--align-pointer=type',
+ '--indent-preprocessor',
+ '--min-conditional-indent=0', '--max-instatement-indent=60',
+ # The following flags require AStyle 2.03 or later:
+ '--max-code-length=120', '--break-after-logical',
+ )
+
+def adjust_environment():
+ """Adjust the environment with some likely AStyle locations."""
+ if 'SYSTEMDRIVE' in os.environ:
+ for prefix in [os.environ['SYSTEMDRIVE'], 'c:', 'd:', 's:']:
+ for location in 'SimQuest', 'simquest', 'tools':
+ path = prefix + '/' + location + "/AStyle/bin"
+ if os.path.isdir(path):
+ os.environ['PATH'] += os.pathsep + path
+
+def run_astyle(files):
+ command = ['astyle']
+ command.extend(ASTYLE_FLAGS)
+ command.extend(files)
+ try:
+ retcode = subprocess.call(command)
+ if retcode < 0:
+ print sys.argv[0] + ": AStyle terminated by signal", -retcode
+ sys.exit(1)
+ elif retcode > 0:
+ print sys.argv[0] + ": AStyle returned status ", retcode
+ sys.exit(1)
+ except OSError as e:
+ print sys.argv[0] + ": AStyle execution failed:", e
+ sys.exit(1)
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser(
+ description='Reformat source files using AStyle with our options.')
+ parser.add_argument('files', metavar='FILE', nargs='*',
+ help='The file names to reformat.')
+ args = parser.parse_args()
+
+ adjust_environment()
+
+ if len(args.files) == 0:
+ print "{}: Nothing to reformat!".format(sys.argv[0])
+ else:
+ run_astyle(args.files)
diff --git a/Tools/fix_header_guards.py b/Tools/fix_header_guards.py
new file mode 100644
index 0000000..1a7a8f3
--- /dev/null
+++ b/Tools/fix_header_guards.py
@@ -0,0 +1,144 @@
+#!/usr/bin/python
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Fix the header include guards in files."""
+
+import argparse
+import sys
+import subprocess
+import re
+import os
+
+def slurp_numbered_lines(file):
+ try:
+ with open(file, 'r') as f:
+ lines = list(enumerate(f.read().splitlines(), start=0))
+ if not lines:
+ print >> sys.stderr, file + ": warning, file is empty."
+ return lines
+ except IOError as e:
+ print >> sys.stderr, e
+ return None
+
+def spew_raw_lines(file, lines):
+ try:
+ with open(file, 'wb') as f:
+ for line in lines:
+ f.write(line)
+ return True
+ except IOError as e:
+ print >> sys.stderr, e
+ return False
+
+HEADER_GUARD_DIRECTORY_PREFIX_CACHE = {}
+
+def get_header_guard_directory_prefix(dir_path):
+ if dir_path not in HEADER_GUARD_DIRECTORY_PREFIX_CACHE:
+ prefix = ""
+ path = dir_path
+ while not (os.path.exists(os.path.join(path, 'LICENSE')) or
+ os.path.exists(os.path.join(path, '.git'))):
+ (parent, dirname) = os.path.split(path)
+ assert parent != path
+ prefix = re.sub(r'\W+', '', dirname).upper() + '_' + prefix
+ path = parent
+ HEADER_GUARD_DIRECTORY_PREFIX_CACHE[dir_path] = prefix
+
+ return HEADER_GUARD_DIRECTORY_PREFIX_CACHE[dir_path]
+
+def get_expected_header_guard(file):
+ (dir_path, filename) = os.path.split(os.path.abspath(file))
+ guard = re.sub(r'\W+', '_', filename).upper()
+
+ while not (os.path.exists(os.path.join(dir_path, 'LICENSE')) or
+ os.path.exists(os.path.join(dir_path, '.git'))):
+ (parent, dirname) = os.path.split(dir_path)
+ assert parent != dir_path
+ guard = re.sub(r'\W+', '', dirname).upper() + '_' + guard
+ dir_path = parent
+
+ return guard
+
+def fix_header_guards(flags, file, lines):
+ ifndef = filter(lambda x: re.search(r'^\s*#\s*ifndef\b', x[1]), lines)
+ if not ifndef:
+ print >> sys.stderr, file + ": no #ifndef lines!"
+ return None
+
+ guard_expected = get_expected_header_guard(file)
+
+ lines[ifndef[0][0]] = (ifndef[0][0],
+ re.sub(r'^(\s*#\s*ifndef)\s+\w+((?:\s.*)?)$',
+ r'\1 ' + guard_expected + r'\2',
+ ifndef[0][1]))
+
+ define = filter(lambda x: re.search(r'^\s*#\s*define\b', x[1]), lines)
+ if not define:
+ print >> sys.stderr, file + ": no #define lines!"
+ return None
+
+ lines[define[0][0]] = (define[0][0],
+ re.sub(r'^(\s*#\s*define)\s+\w+((?:\s.*)?)$',
+ r'\1 ' + guard_expected + r'\2',
+ define[0][1]))
+
+ endif = filter(lambda x: re.search(r'^\s*#\s*endif\b', x[1]), lines)
+ if not endif:
+ print >> sys.stderr, file + ": no #endif lines!"
+ return None
+
+ update_endif = False
+ if not args.lenient_endif:
+ update_endif = True
+ if (re.search(r'^\s*#\s*endif\s*//', endif[-1][1]) and
+ not re.search(r'^\s*#\s*endif\s+//\s+' + guard_expected + r'\s*$',
+ endif[-1][1])):
+ update_endif = True # The comment exists but isn't right.
+
+ if update_endif:
+ lines[endif[-1][0]] = (endif[-1][0],
+ re.sub(r'^(\s*#\s*endif)\b.*$',
+ r'\1 // ' + guard_expected,
+ endif[-1][1]))
+ return lines
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser(
+ description='Fix the header include guards in files.')
+ parser.add_argument('--lenient-endif', action='store_true',
+ help='Fix #endif comments only if present and wrong.')
+ parser.add_argument('files', metavar='FILE', nargs='*',
+ help='The file names to check.')
+ args = parser.parse_args()
+
+ for file in args.files:
+ if not re.search(r'\.h$', file):
+ print >> sys.stderr, file + ": Ignored."
+ continue
+
+ lines = slurp_numbered_lines(file)
+ if not lines:
+ continue
+ updated = fix_header_guards(args, file,
+ map(lambda x: (x[0], x[1]), lines))
+ if not updated:
+ continue
+ if updated == lines:
+ continue
+ output = map(lambda x: x[1] + "\n", updated)
+ spew_raw_lines(file, output)
+ print >> sys.stderr, file + ": Updated."
diff --git a/Tools/memcheck.supp b/Tools/memcheck.supp
new file mode 100644
index 0000000..a9df12f
--- /dev/null
+++ b/Tools/memcheck.supp
@@ -0,0 +1,7 @@
+{
+ wcscmp/.../~locale
+ Memcheck:Addr8
+ fun:wcscmp
+ ...
+ fun:_ZNSt6localeD*Ev
+}
diff --git a/Tools/remove-trailing-whitespace.py b/Tools/remove-trailing-whitespace.py
new file mode 100644
index 0000000..70f2983
--- /dev/null
+++ b/Tools/remove-trailing-whitespace.py
@@ -0,0 +1,80 @@
+#!/usr/bin/python
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Remove all trailing whitespace from a file.
+
+Typical usage:
+ Tools/remove-trailing-whitespace.py Foo.h
+or:
+ find SurgSim \( -name '*.h' -o -name '*.cpp' \) -print \
+ | xargs python Tools/remove-trailing-whitespace.py
+"""
+
+import argparse
+import sys
+import re
+
+def slurp_raw_lines(file):
+ try:
+ with open(file, 'rb') as f:
+ return f.readlines()
+ except IOError as e:
+ print >> sys.stderr, e
+ return None
+
+def spew_raw_lines(file, lines):
+ try:
+ with open(file, 'wb') as f:
+ for line in lines:
+ f.write(line)
+ return True
+ except IOError as e:
+ print >> sys.stderr, e
+ return False
+
+def update(file, lines):
+ if lines is None:
+ return None
+ eol = "\n"
+ if len(lines) and re.search(r'\r\n$', lines[0]):
+ eol = "\r\n"
+ result = map(lambda x: x.rstrip() + eol, lines)
+ # If the trailing newline was missing, but there's no other trailing
+ # whitespace, leave well enough alone.
+ if len(lines) and lines[-1] == lines[-1].rstrip():
+ result[-1] = lines[-1]
+ if result == lines:
+ return None
+ return result
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser(
+ description="Remove all trailing whitespace from a file.")
+ parser.add_argument('files', metavar='FILE', nargs='*',
+ help='The file names to modify.')
+ args = parser.parse_args()
+
+ touched = False
+ for file in args.files:
+ lines = update(file, slurp_raw_lines(file))
+ if lines is not None:
+ spew_raw_lines(file, lines)
+ print "Updated", file
+ touched = True
+
+ if not touched:
+ print "{}: Nothing to update!".format(sys.argv[0])
diff --git a/Tools/run-lint.py b/Tools/run-lint.py
new file mode 100644
index 0000000..5d2dcc1
--- /dev/null
+++ b/Tools/run-lint.py
@@ -0,0 +1,496 @@
+#!/usr/bin/python
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Runs Google's cpplint.py and some other checks on C++ files.
+
+This script tries to flag violations of the OpenSurgSim coding
+standards in C++ code.
+
+This is certainly not a substitute for looking at the code by hand:
+many violations will not be caught by the script, and some amount of
+flagged code will turn out to be correct. But it does make it easier
+to notice certain common errors.
+"""
+
+# TODO(bert): add some more checks.
+
+import argparse
+import sys
+import subprocess
+import re
+import os
+
+from fix_header_guards import get_expected_header_guard
+from alphabetize_cmakelists import filename_sort_filter
+
+STANDARD_WARNING_FORMAT = \
+ "{file}:{line}:{col}: {text} [{category}] [{level}]"
+VISUAL_STUDIO_WARNING_FORMAT = \
+ "{file}({line},{col}): {warning} {vscategory}[{level}]: {text}"
+WARNING_FORMAT = STANDARD_WARNING_FORMAT
+
+def emit_warning(fields):
+ fields = dict(fields)
+ fmt = WARNING_FORMAT
+ # add a "vscategory" field which is like "category" but never empty
+ if 'category' in fields and fields['category'] is not None:
+ fields['vscategory'] = fields['category']
+ else:
+ fmt = re.sub(r'\s*\[\{category[^\}]*\}\]', '', fmt)
+ fmt = re.sub(r'\s*\{category[^\}]*\}', '', fmt)
+ fields['category'] = '???' # panic button
+ fields['vscategory'] = 'RL'
+ if 'level' not in fields:
+ fmt = re.sub(r'\s*\[\{level[^\}]*\}\]', '', fmt)
+ fmt = re.sub(r'\s*\{level[^\}]*\}', '', fmt)
+ fields['level'] = '???' # panic button
+ if 'col' not in fields:
+ fmt = re.sub(r'\s*\{col[^\}]*\}:', '', fmt)
+ fmt = re.sub(r'\s*,\{col[^\}]*\}\)', ')', fmt)
+ fmt = re.sub(r'\s*\{col[^\}]*\}', '', fmt)
+ fields['col'] = '???' # panic button
+ if 'line' not in fields:
+ fmt = re.sub(r'\s*\{line[^\}]*\}:', '', fmt)
+ fmt = re.sub(r'\s*\(\{line[^\}]*\}\)', '', fmt)
+ fmt = re.sub(r'\s*\{line[^\}]*\}', '', fmt)
+ fields['line'] = '???' # panic button
+ if 'warning' not in fields:
+ fields['warning'] = 'warning'
+ print fmt.format(**fields)
+
+def emit_error(fields):
+ fields = dict(fields)
+ fields['warning'] = 'error'
+ emit_warning(fields)
+
+def slurp_numbered_lines(file):
+ try:
+ with open(file, 'r') as f:
+ lines = list(enumerate(f.read().splitlines(), start=1))
+ if not lines:
+ emit_warning({'file': file, 'category': "opensurgsim/no_lines",
+ 'text': "file is empty."})
+ return lines
+ except IOError as e:
+ print >> sys.stderr, e
+ emit_error({'file': file, 'category': "opensurgsim/no_file",
+ 'text': "file is missing or could not be opened."})
+ return None
+
+def run_cpplint(script, filter, files):
+ if not files:
+ return False
+ argv = [sys.executable, script]
+ if filter is not None:
+ argv.extend(['--filter', filter])
+ argv.extend(files)
+ try:
+ cmd = subprocess.Popen(argv, stdout=subprocess.PIPE,
+ stderr=subprocess.STDOUT)
+ except OSError as e:
+ print "Failed to run '" + script + "':\n\t", e
+ return False
+
+ saw_output = False
+ for line in cmd.stdout:
+ saw_output = True
+ line = line.rstrip()
+ match = re.search(r'^(.*?):(\d+):\s+(.*?)\s+\[(\S+)\]\s+\[(\d+)\]\s*$',
+ line)
+ if match:
+ fields = dict(zip(['file', 'line', 'text', 'category', 'level'],
+ match.groups()))
+ emit_warning(fields)
+ elif re.search(r'\[(\S+)\]\s+\[(\d+)\]\s*$', line):
+ emit_error({'file': "run-lint",
+ 'text': ("failed to parse likely warning line: '" +
+ line + "'")})
+ elif re.search(r':\d+:', line):
+ emit_error({'file': "run-lint",
+ 'text': ("failed to parse line with likely line number: '" +
+ line + "'")})
+ else:
+ print line
+
+ # The output is done, so the command should be finished.
+ assert cmd.wait() is not None
+ if not saw_output:
+ emit_error({'file': "run-lint", 'text': "cpplint produced no output!"})
+ return False
+ return cmd.returncode == 0
+
+def check_header_guard(flags, file, lines):
+ ifndef = filter(lambda x: re.search(r'^\s*#\s*ifndef\b', x[1]), lines)
+ if not ifndef:
+ emit_warning({'file': file, 'category': "opensurgsim/no_ifndef",
+ 'text': "no #ifndef lines!"})
+ return False
+# elif len(ifndef) > 1:
+# emit_warning({'file': file, 'line': ifndef[1][0],
+# 'category': "opensurgsim/several_ifndef",
+# 'text': "multiple #ifndef lines; using first."})
+
+ guard = ifndef[0][1]
+ guard = re.sub(r'//.*', '', guard)
+ guard = re.sub(r'/\*.*?\*/', '', guard)
+ guard_match = re.search(r'^\s*#\s*ifndef\s+(\w+)\s*$', guard)
+ if not guard_match:
+ emit_error({'file': file, 'line': ifndef[0][0],
+ 'category': "opensurgsim/internal",
+ 'text': "script error: failed to parse #ifndef line!"})
+ return False
+ guard = guard_match.group(1)
+
+ guard_expected = get_expected_header_guard(file)
+
+ if guard != guard_expected:
+ emit_warning({'file': file, 'line': ifndef[0][0],
+ 'category': "opensurgsim/header_guard",
+ 'text': ("unexpected guard '{}'! expected '{}'."
+ .format(guard, guard_expected)) })
+
+ define = filter(lambda x: re.search(r'^\s*#\s*define\b', x[1]), lines)
+ if not define:
+ emit_warning({'file': file, 'category': "opensurgsim/no_define",
+ 'text': "no #define lines!"})
+ else:
+ def_match = re.match(r'^\s*#\s*define\s+(\w+)\s*$',
+ re.sub(r'/\*.*?\*/', '',
+ re.sub(r'//.*', '', define[0][1])))
+ if not def_match:
+ emit_error({'file': file, 'line': define[0][0],
+ 'category': "opensurgsim/internal",
+ 'text': "script error: failed to parse #define line!"})
+ elif def_match.group(1) != guard:
+ emit_warning({'file': file, 'line': define[0][0],
+ 'category': "opensurgsim/guard_mismatch",
+ 'text': "#define doesn't match #ifndef!"})
+
+ endif = filter(lambda x: re.search(r'^\s*#\s*endif\b', x[1]), lines)
+ if not endif:
+ emit_warning({'file': file, 'category': "opensurgsim/no_endif",
+ 'text': "no #endif lines!"})
+ elif re.search(r'^\s*#\s*endif\s*$', endif[-1][1]):
+ if flags.missing_endif_comments:
+ emit_warning({'file': file, 'line': endif[-1][0],
+ 'category': "opensurgsim/endif_no_comment",
+ 'text': "#endif with no comment."})
+ else:
+ if re.search(r'^\s*#\s*endif\s*///', endif[-1][1]):
+ emit_warning({'file': file, 'line': endif[-1][0],
+ 'category': "opensurgsim/endif_doxygen",
+ 'text': "#endif with a Doxygen style /// comment."})
+ endif_match = re.search(r'^\s*#\s*endif\s*//+\s*(.*?)\s*$', endif[-1][1])
+ if not endif_match:
+ emit_error({'file': file, 'line': endif[-1][0],
+ 'category': "opensurgsim/internal",
+ 'text': "script error: failed to parse #endif comment."})
+ elif endif_match.group(1) != guard:
+ emit_warning({'file': file, 'line': endif[-1][0],
+ 'category': "opensurgsim/endif_mismatch",
+ 'text': ("#endif comment doesn't match #ifndef! (" + \
+ endif_match.group(1) + ")")})
+
+def find_column_char(text, column, tab_width=4):
+ """Find the character that corresponds to the specified column.
+
+ In the absence of tabs in the text, the return value will be equal
+ to the column. When tabs are present, it will be smaller.
+
+ """
+ text = text[:column]
+ while len(text.expandtabs(tab_width)) > column:
+ text = text[:(len(text)-1)]
+ return len(text)
+
+def get_column(flags, text, column):
+ if flags.visual_studio:
+ return find_column_char(text, column)
+ else:
+ return column
+
+def check_length(flags, file, lines):
+ xlines = map(lambda x: (x[0], re.sub(r'\r?\n$', '', x[1].expandtabs(4))),
+ lines)
+ for bad in filter(lambda x: len(x[1]) > flags.max_line_length, xlines):
+ col = get_column(flags, lines[bad[0]-1][1], flags.max_line_length)
+ emit_warning({'file': file, 'line': bad[0], 'col': col,
+ 'category': "opensurgsim/too_long",
+ 'text': ("the line is {} characters long, which is longer"
+ " than {}!"
+ .format(len(bad[1]), flags.max_line_length)) })
+
+def check_tab_indentation(flags, file, lines):
+ for line_number, line in lines:
+ # Not a blank line
+ if len(line) > 0:
+ # Check that lines start with 0 or more tab characters.
+ # 0 to 3 spaces is allowed for alignment, but anymore and a tab is expected.
+ if re.search(r'^\t* {0,3}\S' , line) is None:
+ emit_warning({'file': file, 'line': line_number,
+ 'category': "opensurgsim/tab_indention",
+ 'text': "the line does not correctly indent using tabs."})
+
+def check_oss_include(flags, file, lines):
+ for line_number, line in lines:
+ # Not a blank line
+ if len(line) > 0:
+ # Check that lines does not start with "#include <SurgSim"
+ if re.search(r'#include <SurgSim' , line) is not None:
+ emit_warning({'file': file, 'line': line_number,
+ 'category': "opensurgsim/oss_include",
+ 'text': "OSS header files should be included using #include \"\"."})
+
+def get_listed_files(flags, file, lines):
+ flines = filter(lambda x: re.search(r'^\s.*\.(?:h|cpp)\s*(?:\)\s*)?$', x[1]),
+ lines)
+ for f in flines:
+ if not re.search(r'^\t\S', f[1]):
+ emit_warning({'file': file, 'line': f[0],
+ 'category': "opensurgsim/cmake_file_indent",
+ 'text': "file name indentation is not a single tab."})
+ if re.search(r'\)\s*$', f[1]):
+ emit_warning({'file': file, 'line': f[0],
+ 'category': "opensurgsim/cmake_file_parenthesis",
+ 'text': "file name is followed by a right parenthesis."})
+ elif re.search(r'\s$', f[1]):
+ emit_warning({'file': file, 'line': f[0],
+ 'category': "opensurgsim/cmake_file_trailing",
+ 'text': "file name is followed by trailing whitespace."})
+ files = map(lambda x: (x[0], re.sub(r'\s*\)$', '', x[1].strip())), flines)
+ for f in files:
+ if re.search(r'\$', f[1]):
+ emit_warning({'file': file, 'line': f[0],
+ 'category': "opensurgsim/cmake_file_variable",
+ 'text': "file name contains a variable!"})
+ if re.search(r'\s', f[1]):
+ emit_warning({'file': file, 'line': f[0],
+ 'category': "opensurgsim/cmake_file_whitespace",
+ 'text': "file name contains whitespace!"})
+ return files
+
+def check_file_order(flags, file, files):
+ saw_duplicated = False
+ saw_out_of_order = False
+ for i in range(1, len(files)):
+ if files[i-1][0] == (files[i][0]-1):
+ first = filename_sort_filter(files[i-1][1])
+ second = filename_sort_filter(files[i][1])
+ if first == second and not saw_duplicated:
+ emit_warning({'file': file, 'line': files[i][0],
+ 'category': "opensurgsim/cmake_file_duplicate",
+ 'text': "file list contains duplicate entries." })
+ saw_duplicated = False
+ elif first > second and not saw_out_of_order:
+ emit_warning({'file': file, 'line': files[i][0],
+ 'category': "opensurgsim/cmake_file_order",
+ 'text': "file list is not correctly ordered." })
+ saw_out_of_order = False
+
+def decorate_listed_files(flags, file, files):
+ prefix = os.path.dirname(file)
+ paths = map(lambda x: (x[0], os.path.normpath(os.path.join(prefix, x[1]))),
+ files)
+ for p in paths:
+ if not os.path.exists(p[1]):
+ emit_warning({'file': file, 'line': p[0],
+ 'category': "opensurgsim/cmake_file_missing",
+ 'text': ("file " + p[1] +
+ " does not exist but is listed in CMakeLists!") })
+ return paths
+
+def get_listed_file_paths(flags, file, lines):
+ files = get_listed_files(flags, file, lines)
+ check_file_order(flags, file, files)
+ paths = decorate_listed_files(flags, file, files)
+ return map(lambda x: x[1], paths)
+
+def find_responsible_cmakelists(file):
+ dir_path = os.path.dirname(file)
+ while True:
+ guess = os.path.join(dir_path, 'CMakeLists.txt')
+ if os.path.exists(guess):
+ return guess
+ parent = os.path.dirname(dir_path)
+ if parent == dir_path:
+ # uh-oh, we didn't find ANY CMakeLists.txt file?!?
+ return file
+ dir_path = parent
+
+def check_file_lists(cmakelists_files, found_files):
+ while cmakelists_files or found_files:
+ if (cmakelists_files and found_files and
+ cmakelists_files[0] == found_files[0]):
+ # lists match. remove matching elements and keep going.
+ del cmakelists_files[0]
+ del found_files[0]
+ elif not found_files or cmakelists_files[0] < found_files[0]:
+ # in CMakeLists but not on disk. should already have been handled.
+ del cmakelists_files[0]
+ elif not cmakelists_files or found_files[0] < cmakelists_files[0]:
+ # in the list but not in CMakeLists.
+ emit_warning({'file': find_responsible_cmakelists(found_files[0]),
+ 'category': "opensurgsim/cmake_file_not_listed",
+ 'text': ("file " + found_files[0] +
+ " not present in CMakeLists.txt!") })
+ del found_files[0]
+ else:
+ assert False, "should not have reached here"
+
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser(
+ description='Check source files for coding standard violations.')
+ parser.add_argument('--traverse', metavar='DIR', action='append',
+ help='Find all applicable files in DIR and its subdirs.')
+ parser.add_argument('--cpplint-script', metavar='PATH_TO_SCRIPT',
+ help='The path to Google\'s cpplint.py script.',
+ default='cpplint.py')
+ parser.add_argument('--cpplint-filter', '--filter', metavar='FILTER',
+ help='The --filter argument to pass to cpplint.')
+ parser.add_argument('--no-cpplint', action='store_false', dest='do_cpplint',
+ help='Do not run the cpplint.py script.')
+ parser.add_argument('--missing-endif-comments', action='store_true',
+ help='Flag missing include guard #endif comments, too.')
+ parser.add_argument('--ignore-guards',
+ action='store_false', dest='do_check_guards',
+ help='Do not check the header file include guards.')
+ parser.add_argument('--ignore-tab-indentation',
+ action='store_false', dest='do_check_tab_indentation',
+ help='Do not check that lines are indented with tabs.')
+ parser.add_argument('--ignore-oss-include',
+ action='store_false', dest='do_check_oss_include',
+ help='Do not check that OSS related include use #include "".')
+ parser.add_argument('--max-line-length', type=int, default=120,
+ help='Maximum allowed line length for C++ code.')
+ parser.add_argument('--ignore-length',
+ action='store_false', dest='do_check_length',
+ help='Do not check the line length.')
+ parser.add_argument('--ignore-file-lists',
+ action='store_false', dest='do_check_file_lists',
+ help='Do not check the files in CMakeLists.txt.')
+ parser.add_argument('--ignore-ext',
+ action='store_false', dest='do_check_extension',
+ help='Do not check the file extensions.')
+ parser.add_argument('--format',
+ help='The format string for warnings and errors.')
+ parser.add_argument('--visual-studio', '--vs', action='store_true',
+ help='Use defaults tailored for Visual Studio.')
+ parser.add_argument('files', metavar='FILE', nargs='*',
+ help='The file names to check.')
+ args = parser.parse_args()
+
+ if args.format is not None:
+ WARNING_FORMAT = args.format
+ elif args.visual_studio:
+ WARNING_FORMAT = VISUAL_STUDIO_WARNING_FORMAT
+ else:
+ WARNING_FORMAT = STANDARD_WARNING_FORMAT
+
+ if args.traverse is not None:
+ for traverse_dir in args.traverse:
+ for current_dir, subdirs, files in os.walk(traverse_dir):
+ args.files.extend(map(lambda x: os.path.join(current_dir, x),
+ filter(lambda x: re.search(r'\.(?:h|cpp)$', x),
+ files)))
+ if 'CMakeLists.txt' in files:
+ args.files.append(os.path.join(current_dir, 'CMakeLists.txt'))
+ # always skip .git, build directories, ThirdParty...
+ for bad in filter(lambda x:
+ re.search(r'^(?:\.git|build.*|ThirdParty)$', x, re.IGNORECASE),
+ subdirs):
+ subdirs.remove(bad)
+
+ ok = True
+
+ if args.do_check_extension:
+ bad_ext = filter(lambda x:
+ not re.search(r'\.(?:h|cpp)$|CMakeLists\.txt$', x),
+ args.files)
+ for file in bad_ext:
+ emit_warning({'file': file,
+ 'text': "unknown extension (expected .h or .cpp)"})
+ ok = False
+
+ if args.do_cpplint:
+ # Batch the files through cpplint, 100 at a time so we don't exceed OS limits on
+ # command size or amount of parameters
+ index = 0
+ ok = True
+ while index < len(args.files) :
+ if not run_cpplint(args.cpplint_script, args.cpplint_filter,
+ filter(lambda x: re.search(r'\.(?:h|cpp)$', x),
+ args.files[index:index+100])):
+ ok = False
+ index += 100
+
+ cmakelists_files = None
+ for file in args.files:
+ lines = None
+
+ if args.do_check_guards and re.search(r'\.h$', file):
+ if lines is None:
+ lines = slurp_numbered_lines(file)
+ if not lines:
+ continue
+ if not check_header_guard(args, file, lines):
+ ok = False
+
+ if args.do_check_length and re.search(r'\.(h|cpp)$', file):
+ if lines is None:
+ lines = slurp_numbered_lines(file)
+ if not lines:
+ continue
+ if not check_length(args, file, lines):
+ ok = False
+
+ if args.do_check_file_lists and re.search(r'CMakeLists\.txt$', file):
+ if lines is None:
+ lines = slurp_numbered_lines(file)
+ if not lines:
+ continue
+ if cmakelists_files is None:
+ cmakelists_files = []
+ cmakelists_files.extend(get_listed_file_paths(args, file, lines))
+ # cmakelists_files will be checked later...
+
+ if args.do_check_tab_indentation and re.search(r'(\.h|\.cpp|CMakeLists\.txt$)', file):
+ if lines is None:
+ lines = slurp_numbered_lines(file)
+ if not lines:
+ continue
+ if not check_tab_indentation(args, file, lines):
+ ok = False
+
+ if args.do_check_oss_include and re.search(r'(\.h|\.cpp$)', file):
+ if lines is None:
+ lines = slurp_numbered_lines(file)
+ if not lines:
+ continue
+ if not check_oss_include(args, file, lines):
+ ok = False
+
+ if args.do_check_file_lists and cmakelists_files is not None:
+ cmakelists_files = sorted(cmakelists_files)
+ found_files = sorted(map(lambda x: os.path.normpath(x),
+ filter(lambda x: re.search(r'\.(?:h|cpp)$', x),
+ args.files)))
+ check_file_lists(cmakelists_files, found_files)
+
+
+# if not ok:
+# sys.exit(1)
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-med/opensurgsim.git
More information about the debian-med-commit
mailing list